Skip to content
Open
Show file tree
Hide file tree
Changes from 11 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions src/Camera/MavlinkCameraControlInterface.h
Original file line number Diff line number Diff line change
Expand Up @@ -250,6 +250,7 @@ class MavlinkCameraControlInterface : public FactGroup
virtual bool validateParameter(Fact *pFact, QVariant &newValue) = 0; ///< Allow controller to modify or invalidate parameter change

virtual void handleBatteryStatus(const mavlink_battery_status_t &bs) = 0;
virtual void handleBatteryStatusV2(const mavlink_battery_status_v2_t &bs) = 0;
virtual void handleCameraCaptureStatus(const mavlink_camera_capture_status_t &cameraCaptureStatus) = 0;
virtual void handleParamExtAck(const mavlink_param_ext_ack_t &paramExtAck) = 0;
virtual void handleParamExtValue(const mavlink_param_ext_value_t &paramExtValue) = 0;
Expand Down
13 changes: 13 additions & 0 deletions src/Camera/QGCCameraManager.cc
Original file line number Diff line number Diff line change
Expand Up @@ -185,6 +185,9 @@ void QGCCameraManager::_mavlinkMessageReceived(const mavlink_message_t &message)
case MAVLINK_MSG_ID_BATTERY_STATUS:
_handleBatteryStatus(message);
break;
case MAVLINK_MSG_ID_BATTERY_STATUS_V2:
_handleBatteryStatusV2(message);
break;
case MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS:
_handleTrackingImageStatus(message);
break;
Expand Down Expand Up @@ -492,6 +495,16 @@ void QGCCameraManager::_handleBatteryStatus(const mavlink_message_t &message)
}
}

void QGCCameraManager::_handleBatteryStatusV2(const mavlink_message_t &message)
{
MavlinkCameraControlInterface *pCamera = _findCamera(message.compid);
if (pCamera) {
mavlink_battery_status_v2_t bs{};
mavlink_msg_battery_status_v2_decode(&message, &bs);
pCamera->handleBatteryStatusV2(bs);
}
}

void QGCCameraManager::_handleTrackingImageStatus(const mavlink_message_t &message)
{
MavlinkCameraControlInterface *pCamera = _findCamera(message.compid);
Expand Down
1 change: 1 addition & 0 deletions src/Camera/QGCCameraManager.h
Original file line number Diff line number Diff line change
Expand Up @@ -125,6 +125,7 @@ private slots:
void _handleVideoStreamInformation(const mavlink_message_t& message);
void _handleVideoStreamStatus(const mavlink_message_t& message);
void _handleBatteryStatus(const mavlink_message_t& message);
void _handleBatteryStatusV2(const mavlink_message_t& message);
void _handleTrackingImageStatus(const mavlink_message_t& message);
void _addCameraControlToLists(MavlinkCameraControlInterface* cameraControl);
void _handleCameraFovStatus(const mavlink_message_t& message);
Expand Down
1 change: 1 addition & 0 deletions src/Camera/SimulatedCameraControl.h
Original file line number Diff line number Diff line change
Expand Up @@ -118,6 +118,7 @@ class SimulatedCameraControl : public MavlinkCameraControlInterface
bool validateParameter(Fact* /*pFact*/, QVariant& /*newValue*/) override { return false; }

void handleBatteryStatus(const mavlink_battery_status_t& /*bs*/) override {}
void handleBatteryStatusV2(const mavlink_battery_status_v2_t& /*bs*/) override {}
void handleCameraCaptureStatus(const mavlink_camera_capture_status_t& /*cameraCaptureStatus*/) override {}
void handleParamExtAck(const mavlink_param_ext_ack_t& /*paramExtAck*/) override {}
void handleParamExtValue(const mavlink_param_ext_value_t& /*paramExtValue*/) override {}
Expand Down
11 changes: 11 additions & 0 deletions src/Camera/VehicleCameraControl.cc
Original file line number Diff line number Diff line change
Expand Up @@ -1568,6 +1568,17 @@ void VehicleCameraControl::handleBatteryStatus(const mavlink_battery_status_t& b
}
}

void VehicleCameraControl::handleBatteryStatusV2(const mavlink_battery_status_v2_t& bs)
{
qCDebug(CameraControlLog).noquote() << "Received BATTERY_STATUS_V2:"
<< "\n\tBattery remaining (%):" << bs.percent_remaining;

if (bs.percent_remaining != UINT8_MAX && _batteryRemaining != static_cast<int>(bs.percent_remaining)) {
_batteryRemaining = static_cast<int>(bs.percent_remaining);
emit batteryRemainingChanged();
}
}

void VehicleCameraControl::handleCameraCaptureStatus(const mavlink_camera_capture_status_t& cameraCaptureStatus)
{
qCDebug(CameraControlLog).noquote() << "Received CAMERA_CAPTURE_STATUS - stopping timer, resetting retries:"
Expand Down
3 changes: 2 additions & 1 deletion src/Camera/VehicleCameraControl.h
Original file line number Diff line number Diff line change
Expand Up @@ -122,7 +122,8 @@ class VehicleCameraControl : public MavlinkCameraControlInterface
virtual void handleParamExtAck (const mavlink_param_ext_ack_t& paramExtAck);
virtual void handleParamExtValue (const mavlink_param_ext_value_t& paramExtValue);
virtual void handleStorageInformation(const mavlink_storage_information_t& storageInformation);
virtual void handleBatteryStatus (const mavlink_battery_status_t& bs);
virtual void handleBatteryStatus (const mavlink_battery_status_t& bs);
virtual void handleBatteryStatusV2 (const mavlink_battery_status_v2_t& bs);
virtual void handleTrackingImageStatus(const mavlink_camera_tracking_image_status_t &trackingImageStatus);
virtual void handleVideoStreamInformation(const mavlink_video_stream_information_t &videoStreamInformation);
virtual void handleVideoStreamStatus(const mavlink_video_stream_status_t &videoStreamStatus);
Expand Down
4 changes: 3 additions & 1 deletion src/FactSystem/FactGroupListModel.h
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,8 @@

#include <QList>

class Vehicle;

/// Dynamically manages FactGroupWithIds based on incoming messages.
class FactGroupListModel : public QmlObjectListModel
{
Expand All @@ -17,7 +19,7 @@ class FactGroupListModel : public QmlObjectListModel
explicit FactGroupListModel(const char* factGroupNamePrefix, QObject* parent = nullptr);

/// Allows for creation/updating of dynamic FactGroups based on incoming messages
void handleMessageForFactGroupCreation(Vehicle *vehicle, const mavlink_message_t &message);
virtual void handleMessageForFactGroupCreation(Vehicle *vehicle, const mavlink_message_t &message);

protected:
Comment thread
hamishwillee marked this conversation as resolved.
virtual bool _shouldHandleMessage(const mavlink_message_t &message, QList<uint32_t> &ids) const = 0;
Expand Down
131 changes: 130 additions & 1 deletion src/Vehicle/FactGroups/BatteryFact.json
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@
"shortDesc": "Voltage",
"type": "double",
"decimalPlaces": 2,
"units": "v"
"units": "V"
},
{
"name": "percentRemaining",
Expand Down Expand Up @@ -85,6 +85,135 @@
"enumStrings": "n/a,Ok,Low,Critical,Emergency,Failed,Unhealthy,Charging",
"enumValues": "0,1,2,3,4,5,6,7",
"decimalPlaces": 0
},
{
"name": "capacityRemaining",
"shortDesc": "Capacity Remaining",
"type": "double",
"decimalPlaces": 2,
"units": "Ah"
},
{
"name": "capacityRemainingIsInferred",
"shortDesc": "Capacity Remaining Is Inferred",
"type": "bool"
},
{
"name": "statusFlags",
"shortDesc": "Status Flags",
"type": "uint32",
"decimalPlaces": 0
},
{
"name": "batteryName",
"shortDesc": "Battery Name",
"type": "string"
},
{
"name": "serialNumber",
"shortDesc": "Serial Number",
"type": "string"
},
{
"name": "manufactureDate",
"shortDesc": "Manufacture Date",
"type": "string"
},
{
"name": "fullChargeCapacity",
"shortDesc": "Full Charge Capacity",
"type": "double",
"decimalPlaces": 2,
"units": "Ah"
},
{
"name": "designCapacity",
"shortDesc": "Design Capacity",
"type": "double",
"decimalPlaces": 2,
"units": "Ah"
},
{
"name": "nominalVoltage",
"shortDesc": "Nominal Voltage",
"type": "double",
"decimalPlaces": 2,
"units": "V"
},
{
"name": "dischargeMinimumVoltage",
"shortDesc": "Discharge Min Voltage",
"type": "double",
"decimalPlaces": 2,
"units": "V"
},
{
"name": "chargingMinimumVoltage",
"shortDesc": "Charging Min Voltage",
"type": "double",
"decimalPlaces": 2,
"units": "V"
},
{
"name": "restingMinimumVoltage",
"shortDesc": "Resting Min Voltage",
"type": "double",
"decimalPlaces": 2,
"units": "V"
},
{
"name": "chargingMaximumVoltage",
"shortDesc": "Charging Max Voltage",
"type": "double",
"decimalPlaces": 2,
"units": "V"
Comment thread
hamishwillee marked this conversation as resolved.
},
{
"name": "chargingMaximumCurrent",
"shortDesc": "Charging Max Current",
"type": "double",
"decimalPlaces": 2,
"units": "A"
},
{
"name": "dischargeMaximumCurrent",
"shortDesc": "Discharge Max Current",
"type": "double",
"decimalPlaces": 2,
"units": "A"
},
{
"name": "dischargeMaximumBurstCurrent",
"shortDesc": "Discharge Max Burst Current",
"type": "double",
"decimalPlaces": 2,
"units": "A"
},
{
"name": "cycleCount",
"shortDesc": "Cycle Count",
"type": "double",
"decimalPlaces": 0
},
{
"name": "weight",
"shortDesc": "Weight",
"type": "double",
"decimalPlaces": 0,
"units": "g"
},
{
"name": "stateOfHealth",
"shortDesc": "State of Health",
"type": "double",
"decimalPlaces": 0,
"units": "%"
},
{
"name": "cellsInSeries",
"shortDesc": "Cells In Series",
"type": "double",
"decimalPlaces": 0
}
]
}
Loading
Loading