diff --git a/src/Camera/MavlinkCameraControlInterface.h b/src/Camera/MavlinkCameraControlInterface.h index d393d31d332f..a764bbb39b73 100644 --- a/src/Camera/MavlinkCameraControlInterface.h +++ b/src/Camera/MavlinkCameraControlInterface.h @@ -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 ¶mExtAck) = 0; virtual void handleParamExtValue(const mavlink_param_ext_value_t ¶mExtValue) = 0; diff --git a/src/Camera/QGCCameraManager.cc b/src/Camera/QGCCameraManager.cc index 1df349fc8130..42ca2248af69 100644 --- a/src/Camera/QGCCameraManager.cc +++ b/src/Camera/QGCCameraManager.cc @@ -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; @@ -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); diff --git a/src/Camera/QGCCameraManager.h b/src/Camera/QGCCameraManager.h index 5366d94df49a..cfcc32a1f50c 100644 --- a/src/Camera/QGCCameraManager.h +++ b/src/Camera/QGCCameraManager.h @@ -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); diff --git a/src/Camera/SimulatedCameraControl.h b/src/Camera/SimulatedCameraControl.h index 0cf1aad785f3..e62eea7ef883 100644 --- a/src/Camera/SimulatedCameraControl.h +++ b/src/Camera/SimulatedCameraControl.h @@ -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 {} diff --git a/src/Camera/VehicleCameraControl.cc b/src/Camera/VehicleCameraControl.cc index a6b37709c847..996fda07b829 100644 --- a/src/Camera/VehicleCameraControl.cc +++ b/src/Camera/VehicleCameraControl.cc @@ -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(bs.percent_remaining)) { + _batteryRemaining = static_cast(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:" diff --git a/src/Camera/VehicleCameraControl.h b/src/Camera/VehicleCameraControl.h index 6ff3470eded6..cf5575dfd703 100644 --- a/src/Camera/VehicleCameraControl.h +++ b/src/Camera/VehicleCameraControl.h @@ -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); diff --git a/src/FactSystem/FactGroupListModel.h b/src/FactSystem/FactGroupListModel.h index 839d941bd139..62a3c0e41f33 100644 --- a/src/FactSystem/FactGroupListModel.h +++ b/src/FactSystem/FactGroupListModel.h @@ -6,6 +6,8 @@ #include +class Vehicle; + /// Dynamically manages FactGroupWithIds based on incoming messages. class FactGroupListModel : public QmlObjectListModel { @@ -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: virtual bool _shouldHandleMessage(const mavlink_message_t &message, QList &ids) const = 0; diff --git a/src/Vehicle/FactGroups/BatteryFact.json b/src/Vehicle/FactGroups/BatteryFact.json index 1f4899affc32..3ba2cf029917 100644 --- a/src/Vehicle/FactGroups/BatteryFact.json +++ b/src/Vehicle/FactGroups/BatteryFact.json @@ -29,7 +29,7 @@ "shortDesc": "Voltage", "type": "double", "decimalPlaces": 2, - "units": "v" + "units": "V" }, { "name": "percentRemaining", @@ -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" +}, +{ + "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 } ] } diff --git a/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc b/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc index 2f40176be393..6354401b7d77 100644 --- a/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc +++ b/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc @@ -1,4 +1,7 @@ #include "BatteryFactGroupListModel.h" +#include "Vehicle.h" + +#include // strnlen BatteryFactGroupListModel::BatteryFactGroupListModel(QObject* parent) : FactGroupListModel("battery", parent) @@ -22,6 +25,20 @@ bool BatteryFactGroupListModel::_shouldHandleMessage(const mavlink_message_t &me ids.append(batteryStatus.id); return true; } + case MAVLINK_MSG_ID_BATTERY_STATUS_V2: + { + mavlink_battery_status_v2_t bs{}; + mavlink_msg_battery_status_v2_decode(&message, &bs); + ids.append(bs.id); + return true; + } + case MAVLINK_MSG_ID_BATTERY_INFO: + { + mavlink_battery_info_t bi{}; + mavlink_msg_battery_info_decode(&message, &bi); + ids.append(bi.id); + return true; + } default: return false; // Not a message we care about } @@ -32,6 +49,83 @@ FactGroupWithId *BatteryFactGroupListModel::_createFactGroupWithId(uint32_t id) return new BatteryFactGroup(id, this); } +void BatteryFactGroupListModel::handleMessageForFactGroupCreation( + Vehicle *vehicle, const mavlink_message_t &message) +{ + // Only drive negotiation state from the autopilot component. A camera + // peripheral on a different compid may also send BATTERY_STATUS_V2, but + // must not trigger V2 activation or cause BATTERY_STATUS to be suppressed + // for the autopilot (which may not be sending V2 at all). + if (message.compid == static_cast(vehicle->defaultComponentId())) { + if (message.msgid == MAVLINK_MSG_ID_BATTERY_STATUS_V2) { + _v2StatusReceived = true; + if (_v2State != V2NegotiationActive) { + _activateV2(vehicle); + } + } else if (message.msgid == MAVLINK_MSG_ID_BATTERY_INFO) { + _infoReceived = true; + } + } + + FactGroupListModel::handleMessageForFactGroupCreation(vehicle, message); +} + +void BatteryFactGroupListModel::startV2Negotiation(Vehicle *vehicle) +{ + // --- BATTERY_STATUS_V2 --- + // Guard is scoped to this block only: a V2 frame arriving before + // initialConnectComplete can call _activateV2 early (state → Active), + // but must not prevent the BATTERY_INFO request below from running. + if (_v2State == V2NegotiationUnknown) { + if (_v2StatusReceived) { + // Already streaming — activate immediately, no request needed + _activateV2(vehicle); + } else { + _v2State = V2NegotiationRequesting; + + // sendMavCommandWithLambdaFallback calls the lambda only on MAV_RESULT_UNSUPPORTED. + // On ACCEPTED (or other), we simply wait: handleMessageForFactGroupCreation will + // call _activateV2 when the first V2 frame arrives. If no frame ever arrives we + // stay in Requesting and V1 continues to be used (safe fallback). + vehicle->sendMavCommandWithLambdaFallback( + [this]() { + _v2State = V2NegotiationUnsupported; + }, + vehicle->defaultComponentId(), + MAV_CMD_SET_MESSAGE_INTERVAL, + false, // showError = false + static_cast(MAVLINK_MSG_ID_BATTERY_STATUS_V2), + 2000000.0f // 0.5 Hz + ); + } + } + + // --- BATTERY_INFO (independent, fire-and-forget) --- + if (!_infoReceived) { + vehicle->sendMavCommand( + vehicle->defaultComponentId(), + MAV_CMD_SET_MESSAGE_INTERVAL, + false, // showError = false + static_cast(MAVLINK_MSG_ID_BATTERY_INFO), + 10000000.0f // 0.1 Hz + ); + } +} + +void BatteryFactGroupListModel::_activateV2(Vehicle *vehicle) +{ + _v2State = V2NegotiationActive; + + // Ask the flight stack to stop streaming BATTERY_STATUS (V1) + vehicle->sendMavCommand( + vehicle->defaultComponentId(), + MAV_CMD_SET_MESSAGE_INTERVAL, + false, // showError = false + static_cast(MAVLINK_MSG_ID_BATTERY_STATUS), + -1.0f // interval −1 = disable + ); +} + BatteryFactGroup::BatteryFactGroup(uint32_t batteryId, QObject *parent) : FactGroupWithId(1000, QStringLiteral(":/json/Vehicle/BatteryFact.json"), parent) { @@ -46,6 +140,26 @@ BatteryFactGroup::BatteryFactGroup(uint32_t batteryId, QObject *parent) _addFact(&_timeRemainingStrFact); _addFact(&_chargeStateFact); _addFact(&_instantPowerFact); + _addFact(&_capacityRemainingFact); + _addFact(&_capacityRemainingIsInferredFact); + _addFact(&_statusFlagsFact); + _addFact(&_batteryNameFact); + _addFact(&_serialNumberFact); + _addFact(&_manufactureDateFact); + _addFact(&_fullChargeCapacityFact); + _addFact(&_designCapacityFact); + _addFact(&_nominalVoltageFact); + _addFact(&_dischargeMinimumVoltageFact); + _addFact(&_chargingMinimumVoltageFact); + _addFact(&_restingMinimumVoltageFact); + _addFact(&_chargingMaximumVoltageFact); + _addFact(&_chargingMaximumCurrentFact); + _addFact(&_dischargeMaximumCurrentFact); + _addFact(&_dischargeMaximumBurstCurrentFact); + _addFact(&_cycleCountFact); + _addFact(&_weightFact); + _addFact(&_stateOfHealthFact); + _addFact(&_cellsInSeriesFact); _idFact.setRawValue(batteryId); _batteryFunctionFact.setRawValue(MAV_BATTERY_FUNCTION_UNKNOWN); @@ -58,12 +172,68 @@ BatteryFactGroup::BatteryFactGroup(uint32_t batteryId, QObject *parent) _timeRemainingFact.setRawValue(qQNaN()); _chargeStateFact.setRawValue(MAV_BATTERY_CHARGE_STATE_UNDEFINED); _instantPowerFact.setRawValue(qQNaN()); + _capacityRemainingFact.setRawValue(qQNaN()); + _capacityRemainingIsInferredFact.setRawValue(false); + _statusFlagsFact.setRawValue(0U); + _batteryNameFact.setRawValue(QString()); + _serialNumberFact.setRawValue(QString()); + _manufactureDateFact.setRawValue(QString()); + _fullChargeCapacityFact.setRawValue(qQNaN()); + _designCapacityFact.setRawValue(qQNaN()); + _nominalVoltageFact.setRawValue(qQNaN()); + _dischargeMinimumVoltageFact.setRawValue(qQNaN()); + _chargingMinimumVoltageFact.setRawValue(qQNaN()); + _restingMinimumVoltageFact.setRawValue(qQNaN()); + _chargingMaximumVoltageFact.setRawValue(qQNaN()); + _chargingMaximumCurrentFact.setRawValue(qQNaN()); + _dischargeMaximumCurrentFact.setRawValue(qQNaN()); + _dischargeMaximumBurstCurrentFact.setRawValue(qQNaN()); + _cycleCountFact.setRawValue(qQNaN()); + _weightFact.setRawValue(qQNaN()); + _stateOfHealthFact.setRawValue(qQNaN()); + _cellsInSeriesFact.setRawValue(qQNaN()); (void) connect(&_timeRemainingFact, &Fact::rawValueChanged, this, &BatteryFactGroup::_timeRemainingChanged); } +// static +uint8_t BatteryFactGroup::chargeStateFromV2(uint32_t statusFlags, uint8_t percentRemaining) +{ + constexpr uint32_t faultMask = + MAV_BATTERY_STATUS_FLAGS_FAULT_CELL_IMBALANCE | + MAV_BATTERY_STATUS_FLAGS_FAULT_PROTECTION_SYSTEM | + MAV_BATTERY_STATUS_FLAGS_FAULT_OVER_VOLT | + MAV_BATTERY_STATUS_FLAGS_FAULT_UNDER_VOLT | + MAV_BATTERY_STATUS_FLAGS_FAULT_OVER_TEMPERATURE | + MAV_BATTERY_STATUS_FLAGS_FAULT_UNDER_TEMPERATURE | + MAV_BATTERY_STATUS_FLAGS_FAULT_OVER_CURRENT | + MAV_BATTERY_STATUS_FLAGS_FAULT_SHORT_CIRCUIT | + MAV_BATTERY_STATUS_FLAGS_FAULT_INCOMPATIBLE_VOLTAGE | + MAV_BATTERY_STATUS_FLAGS_FAULT_INCOMPATIBLE_FIRMWARE | + MAV_BATTERY_STATUS_FLAGS_FAULT_INCOMPATIBLE_CELLS_CONFIGURATION; + + if (statusFlags & faultMask) return MAV_BATTERY_CHARGE_STATE_FAILED; + if (statusFlags & MAV_BATTERY_STATUS_FLAGS_NOT_READY_TO_USE) return MAV_BATTERY_CHARGE_STATE_EMERGENCY; + + // BATTERY_STATUS_V2 has no LOW/CRITICAL flags — derive from percentRemaining + // using conventional alarm thresholds. CHARGING is intentionally excluded here; + // UI callers add it for display; audio callers skip it (not a voice-alertable state). + if (percentRemaining != UINT8_MAX) { + constexpr int kCriticalPct = 10; + constexpr int kLowPct = 25; + if (static_cast(percentRemaining) <= kCriticalPct) return MAV_BATTERY_CHARGE_STATE_CRITICAL; + if (static_cast(percentRemaining) <= kLowPct) return MAV_BATTERY_CHARGE_STATE_LOW; + } + return MAV_BATTERY_CHARGE_STATE_OK; +} + void BatteryFactGroup::handleMessage(Vehicle *vehicle, const mavlink_message_t &message) { + if (message.msgid == MAVLINK_MSG_ID_BATTERY_STATUS) { + const auto *listModel = qobject_cast(parent()); + if (listModel && listModel->isV2Active()) return; + } + switch (message.msgid) { case MAVLINK_MSG_ID_HIGH_LATENCY: _handleHighLatency(vehicle, message); @@ -74,6 +244,12 @@ void BatteryFactGroup::handleMessage(Vehicle *vehicle, const mavlink_message_t & case MAVLINK_MSG_ID_BATTERY_STATUS: _handleBatteryStatus(vehicle, message); break; + case MAVLINK_MSG_ID_BATTERY_STATUS_V2: + _handleBatteryStatusV2(vehicle, message); + break; + case MAVLINK_MSG_ID_BATTERY_INFO: + _handleBatteryInfo(vehicle, message); + break; default: break; } @@ -157,3 +333,92 @@ void BatteryFactGroup::_timeRemainingChanged(const QVariant &value) _timeRemainingStrFact.setRawValue(QString::asprintf("%02dH:%02dM:%02dS", hours, minutes, seconds)); } } + +void BatteryFactGroup::_handleBatteryStatusV2(Vehicle * /*vehicle*/, const mavlink_message_t &message) +{ + mavlink_battery_status_v2_t bs{}; + mavlink_msg_battery_status_v2_decode(&message, &bs); + + if (bs.id != id()->rawValue().toUInt()) { + return; + } + + temperature()->setRawValue((bs.temperature == INT16_MAX) ? qQNaN() : static_cast(bs.temperature) / 100.0); + + const double v = qIsNaN(bs.voltage) ? qQNaN() : static_cast(bs.voltage); + const double i = qIsNaN(bs.current) ? qQNaN() : static_cast(bs.current); + voltage()->setRawValue(v); + current()->setRawValue(i); + + double consumed = qIsNaN(bs.capacity_consumed) ? qQNaN() : static_cast(bs.capacity_consumed); + double remaining = qIsNaN(bs.capacity_remaining) ? qQNaN() : static_cast(bs.capacity_remaining); + const double fcc = fullChargeCapacity()->rawValue().toDouble(); // NaN if BATTERY_INFO not yet received + + bool remainingInferred = false; + if (qIsNaN(remaining) && !qIsNaN(consumed) && !qIsNaN(fcc)) { + remaining = fcc - consumed; + remainingInferred = true; + } else if (qIsNaN(consumed) && !qIsNaN(remaining) && !qIsNaN(fcc)) { + consumed = fcc - remaining; + // remaining was measured directly — remainingInferred stays false + } + + mahConsumed()->setRawValue(qIsNaN(consumed) ? qQNaN() : consumed * 1000.0); + capacityRemaining()->setRawValue(remaining); + capacityRemainingIsInferred()->setRawValue(remainingInferred); + + percentRemaining()->setRawValue((bs.percent_remaining == UINT8_MAX) ? qQNaN() : static_cast(bs.percent_remaining)); + + statusFlags()->setRawValue(bs.status_flags); + + // Derive charge state for UI display. CHARGING is added here (display-only; + // audio alerts skip it — see Vehicle::_handleBatteryStatusV2). + uint8_t derivedChargeState = chargeStateFromV2(bs.status_flags, bs.percent_remaining); + if (derivedChargeState == MAV_BATTERY_CHARGE_STATE_OK + && (bs.status_flags & MAV_BATTERY_STATUS_FLAGS_CHARGING)) { + derivedChargeState = MAV_BATTERY_CHARGE_STATE_CHARGING; + } + chargeState()->setRawValue(derivedChargeState); + + instantPower()->setRawValue(v * i); + + _setTelemetryAvailable(true); +} + +void BatteryFactGroup::_handleBatteryInfo(Vehicle * /*vehicle*/, const mavlink_message_t &message) +{ + mavlink_battery_info_t bi{}; + mavlink_msg_battery_info_decode(&message, &bi); + + if (bi.id != id()->rawValue().toUInt()) { + return; + } + + function()->setRawValue(bi.battery_function); + type()->setRawValue(bi.type); + + // Fields using 0 as "not provided" are converted to NaN for consistent invalid-value handling + auto zeroAsNaN = [](float v) -> double { return (v == 0.0f) ? qQNaN() : static_cast(v); }; + + fullChargeCapacity()->setRawValue(qIsNaN(bi.full_charge_capacity) ? qQNaN() : static_cast(bi.full_charge_capacity)); + designCapacity()->setRawValue(zeroAsNaN(bi.design_capacity)); + nominalVoltage()->setRawValue(zeroAsNaN(bi.nominal_voltage)); + dischargeMinimumVoltage()->setRawValue(zeroAsNaN(bi.discharge_minimum_voltage)); + chargingMinimumVoltage()->setRawValue(zeroAsNaN(bi.charging_minimum_voltage)); + restingMinimumVoltage()->setRawValue(zeroAsNaN(bi.resting_minimum_voltage)); + chargingMaximumVoltage()->setRawValue(zeroAsNaN(bi.charging_maximum_voltage)); + chargingMaximumCurrent()->setRawValue(zeroAsNaN(bi.charging_maximum_current)); + dischargeMaximumCurrent()->setRawValue(zeroAsNaN(bi.discharge_maximum_current)); + dischargeMaximumBurstCurrent()->setRawValue(zeroAsNaN(bi.discharge_maximum_burst_current)); + cycleCount()->setRawValue((bi.cycle_count == UINT16_MAX) ? qQNaN() : static_cast(bi.cycle_count)); + weight()->setRawValue(zeroAsNaN(static_cast(bi.weight))); + stateOfHealth()->setRawValue((bi.state_of_health == 255) ? qQNaN() : static_cast(bi.state_of_health)); + cellsInSeries()->setRawValue((bi.cells_in_series == 0) ? qQNaN() : static_cast(bi.cells_in_series)); + + // String fields: bound by field size in case the sender omits the null terminator + batteryName()->setRawValue( QString::fromLatin1(bi.name, strnlen(bi.name, sizeof(bi.name)))); + serialNumber()->setRawValue( QString::fromLatin1(bi.serial_number, strnlen(bi.serial_number, sizeof(bi.serial_number)))); + manufactureDate()->setRawValue(QString::fromLatin1(bi.manufacture_date, strnlen(bi.manufacture_date, sizeof(bi.manufacture_date)))); + + _setTelemetryAvailable(true); +} diff --git a/src/Vehicle/FactGroups/BatteryFactGroupListModel.h b/src/Vehicle/FactGroups/BatteryFactGroupListModel.h index 5237a0de2b06..e1b90f322570 100644 --- a/src/Vehicle/FactGroups/BatteryFactGroupListModel.h +++ b/src/Vehicle/FactGroups/BatteryFactGroupListModel.h @@ -2,6 +2,8 @@ #include "FactGroupListModel.h" +class Vehicle; + class BatteryFactGroupListModel : public FactGroupListModel { Q_OBJECT @@ -11,41 +13,115 @@ class BatteryFactGroupListModel : public FactGroupListModel public: explicit BatteryFactGroupListModel(QObject* parent = nullptr); + /// Called after initial connect completes. Requests BATTERY_STATUS_V2 and + /// BATTERY_INFO streams if they are not already being received, and suppresses + /// BATTERY_STATUS once V2 is active. + void startV2Negotiation(Vehicle *vehicle); + + /// Returns true once BATTERY_STATUS_V2 has been confirmed active. + /// When true, BATTERY_STATUS messages should be ignored. + bool isV2Active() const { return _v2State == V2NegotiationActive; } + + // Override to detect arriving V2/INFO frames during negotiation + void handleMessageForFactGroupCreation(Vehicle *vehicle, + const mavlink_message_t &message) override; + protected: // Overrides from FactGroupListModel bool _shouldHandleMessage(const mavlink_message_t &message, QList &ids) const final; FactGroupWithId *_createFactGroupWithId(uint32_t id) final; + +private: + enum V2NegotiationState { + V2NegotiationUnknown, ///< Initial — negotiation not yet started + V2NegotiationRequesting, ///< SET_MESSAGE_INTERVAL sent, awaiting first V2 frame + V2NegotiationActive, ///< V2 confirmed; BATTERY_STATUS suppressed + V2NegotiationUnsupported ///< Flight stack returned UNSUPPORTED; use V1 + }; + + void _activateV2(Vehicle *vehicle); + + V2NegotiationState _v2State = V2NegotiationUnknown; + bool _v2StatusReceived = false; ///< true once any BATTERY_STATUS_V2 frame arrives + bool _infoReceived = false; ///< true once any BATTERY_INFO frame arrives }; class BatteryFactGroup : public FactGroupWithId { Q_OBJECT - Q_PROPERTY(Fact *function READ function CONSTANT) - Q_PROPERTY(Fact *type READ type CONSTANT) - Q_PROPERTY(Fact *temperature READ temperature CONSTANT) - Q_PROPERTY(Fact *voltage READ voltage CONSTANT) - Q_PROPERTY(Fact *current READ current CONSTANT) - Q_PROPERTY(Fact *mahConsumed READ mahConsumed CONSTANT) - Q_PROPERTY(Fact *percentRemaining READ percentRemaining CONSTANT) - Q_PROPERTY(Fact *timeRemaining READ timeRemaining CONSTANT) - Q_PROPERTY(Fact *timeRemainingStr READ timeRemainingStr CONSTANT) - Q_PROPERTY(Fact *chargeState READ chargeState CONSTANT) - Q_PROPERTY(Fact *instantPower READ instantPower CONSTANT) + Q_PROPERTY(Fact *function READ function CONSTANT) + Q_PROPERTY(Fact *type READ type CONSTANT) + Q_PROPERTY(Fact *temperature READ temperature CONSTANT) + Q_PROPERTY(Fact *voltage READ voltage CONSTANT) + Q_PROPERTY(Fact *current READ current CONSTANT) + Q_PROPERTY(Fact *mahConsumed READ mahConsumed CONSTANT) + Q_PROPERTY(Fact *percentRemaining READ percentRemaining CONSTANT) + Q_PROPERTY(Fact *timeRemaining READ timeRemaining CONSTANT) + Q_PROPERTY(Fact *timeRemainingStr READ timeRemainingStr CONSTANT) + Q_PROPERTY(Fact *chargeState READ chargeState CONSTANT) + Q_PROPERTY(Fact *instantPower READ instantPower CONSTANT) + // BATTERY_STATUS_V2 facts + Q_PROPERTY(Fact *capacityRemaining READ capacityRemaining CONSTANT) + Q_PROPERTY(Fact *capacityRemainingIsInferred READ capacityRemainingIsInferred CONSTANT) + Q_PROPERTY(Fact *statusFlags READ statusFlags CONSTANT) + // BATTERY_INFO facts + Q_PROPERTY(Fact *batteryName READ batteryName CONSTANT) + Q_PROPERTY(Fact *serialNumber READ serialNumber CONSTANT) + Q_PROPERTY(Fact *manufactureDate READ manufactureDate CONSTANT) + Q_PROPERTY(Fact *fullChargeCapacity READ fullChargeCapacity CONSTANT) + Q_PROPERTY(Fact *designCapacity READ designCapacity CONSTANT) + Q_PROPERTY(Fact *nominalVoltage READ nominalVoltage CONSTANT) + Q_PROPERTY(Fact *dischargeMinimumVoltage READ dischargeMinimumVoltage CONSTANT) + Q_PROPERTY(Fact *chargingMinimumVoltage READ chargingMinimumVoltage CONSTANT) + Q_PROPERTY(Fact *restingMinimumVoltage READ restingMinimumVoltage CONSTANT) + Q_PROPERTY(Fact *chargingMaximumVoltage READ chargingMaximumVoltage CONSTANT) + Q_PROPERTY(Fact *chargingMaximumCurrent READ chargingMaximumCurrent CONSTANT) + Q_PROPERTY(Fact *dischargeMaximumCurrent READ dischargeMaximumCurrent CONSTANT) + Q_PROPERTY(Fact *dischargeMaximumBurstCurrent READ dischargeMaximumBurstCurrent CONSTANT) + Q_PROPERTY(Fact *cycleCount READ cycleCount CONSTANT) + Q_PROPERTY(Fact *weight READ weight CONSTANT) + Q_PROPERTY(Fact *stateOfHealth READ stateOfHealth CONSTANT) + Q_PROPERTY(Fact *cellsInSeries READ cellsInSeries CONSTANT) public: explicit BatteryFactGroup(uint32_t batteryId, QObject *parent = nullptr); - Fact *function() { return &_batteryFunctionFact; } - Fact *type() { return &_batteryTypeFact; } - Fact *voltage() { return &_voltageFact; } - Fact *percentRemaining() { return &_percentRemainingFact; } - Fact *mahConsumed() { return &_mahConsumedFact; } - Fact *current() { return &_currentFact; } - Fact *temperature() { return &_temperatureFact; } - Fact *instantPower() { return &_instantPowerFact; } - Fact *timeRemaining() { return &_timeRemainingFact; } - Fact *timeRemainingStr() { return &_timeRemainingStrFact; } - Fact *chargeState() { return &_chargeStateFact; } + Fact *function() { return &_batteryFunctionFact; } + Fact *type() { return &_batteryTypeFact; } + Fact *voltage() { return &_voltageFact; } + Fact *percentRemaining() { return &_percentRemainingFact; } + Fact *mahConsumed() { return &_mahConsumedFact; } + Fact *current() { return &_currentFact; } + Fact *temperature() { return &_temperatureFact; } + Fact *instantPower() { return &_instantPowerFact; } + Fact *timeRemaining() { return &_timeRemainingFact; } + Fact *timeRemainingStr() { return &_timeRemainingStrFact; } + Fact *chargeState() { return &_chargeStateFact; } + Fact *capacityRemaining() { return &_capacityRemainingFact; } + Fact *capacityRemainingIsInferred() { return &_capacityRemainingIsInferredFact; } + Fact *statusFlags() { return &_statusFlagsFact; } + Fact *batteryName() { return &_batteryNameFact; } + Fact *serialNumber() { return &_serialNumberFact; } + Fact *manufactureDate() { return &_manufactureDateFact; } + Fact *fullChargeCapacity() { return &_fullChargeCapacityFact; } + Fact *designCapacity() { return &_designCapacityFact; } + Fact *nominalVoltage() { return &_nominalVoltageFact; } + Fact *dischargeMinimumVoltage() { return &_dischargeMinimumVoltageFact; } + Fact *chargingMinimumVoltage() { return &_chargingMinimumVoltageFact; } + Fact *restingMinimumVoltage() { return &_restingMinimumVoltageFact; } + Fact *chargingMaximumVoltage() { return &_chargingMaximumVoltageFact; } + Fact *chargingMaximumCurrent() { return &_chargingMaximumCurrentFact; } + Fact *dischargeMaximumCurrent() { return &_dischargeMaximumCurrentFact; } + Fact *dischargeMaximumBurstCurrent() { return &_dischargeMaximumBurstCurrentFact; } + Fact *cycleCount() { return &_cycleCountFact; } + Fact *weight() { return &_weightFact; } + Fact *stateOfHealth() { return &_stateOfHealthFact; } + Fact *cellsInSeries() { return &_cellsInSeriesFact; } + + /// Derives MAV_BATTERY_CHARGE_STATE from BATTERY_STATUS_V2 status_flags and + /// percent_remaining. CHARGING is excluded — callers that display it (UI) add + /// it themselves; callers that only trigger audio alerts (Vehicle) skip it. + static uint8_t chargeStateFromV2(uint32_t statusFlags, uint8_t percentRemaining); // Overrides from FactGroup void handleMessage(Vehicle *vehicle, const mavlink_message_t &message) final; @@ -57,16 +133,41 @@ private slots: void _handleHighLatency(Vehicle *vehicle, const mavlink_message_t &message); void _handleHighLatency2(Vehicle *vehicle, const mavlink_message_t &message); void _handleBatteryStatus(Vehicle *vehicle, const mavlink_message_t &message); + void _handleBatteryStatusV2(Vehicle *vehicle, const mavlink_message_t &message); + void _handleBatteryInfo(Vehicle *vehicle, const mavlink_message_t &message); - Fact _batteryFunctionFact = Fact(0, QStringLiteral("batteryFunction"), FactMetaData::valueTypeUint8); - Fact _batteryTypeFact = Fact(0, QStringLiteral("batteryType"), FactMetaData::valueTypeUint8); - Fact _voltageFact = Fact(0, QStringLiteral("voltage"), FactMetaData::valueTypeDouble); - Fact _currentFact = Fact(0, QStringLiteral("current"), FactMetaData::valueTypeDouble); - Fact _mahConsumedFact = Fact(0, QStringLiteral("mahConsumed"), FactMetaData::valueTypeDouble); - Fact _temperatureFact = Fact(0, QStringLiteral("temperature"), FactMetaData::valueTypeDouble); - Fact _percentRemainingFact = Fact(0, QStringLiteral("percentRemaining"), FactMetaData::valueTypeDouble); - Fact _timeRemainingFact = Fact(0, QStringLiteral("timeRemaining"), FactMetaData::valueTypeDouble); - Fact _timeRemainingStrFact = Fact(0, QStringLiteral("timeRemainingStr"), FactMetaData::valueTypeString); - Fact _chargeStateFact = Fact(0, QStringLiteral("chargeState"), FactMetaData::valueTypeUint8); - Fact _instantPowerFact = Fact(0, QStringLiteral("instantPower"), FactMetaData::valueTypeDouble); + // BATTERY_STATUS / shared facts + Fact _batteryFunctionFact = Fact(0, QStringLiteral("batteryFunction"), FactMetaData::valueTypeUint8); + Fact _batteryTypeFact = Fact(0, QStringLiteral("batteryType"), FactMetaData::valueTypeUint8); + Fact _voltageFact = Fact(0, QStringLiteral("voltage"), FactMetaData::valueTypeDouble); + Fact _currentFact = Fact(0, QStringLiteral("current"), FactMetaData::valueTypeDouble); + Fact _mahConsumedFact = Fact(0, QStringLiteral("mahConsumed"), FactMetaData::valueTypeDouble); + Fact _temperatureFact = Fact(0, QStringLiteral("temperature"), FactMetaData::valueTypeDouble); + Fact _percentRemainingFact = Fact(0, QStringLiteral("percentRemaining"), FactMetaData::valueTypeDouble); + Fact _timeRemainingFact = Fact(0, QStringLiteral("timeRemaining"), FactMetaData::valueTypeDouble); + Fact _timeRemainingStrFact = Fact(0, QStringLiteral("timeRemainingStr"), FactMetaData::valueTypeString); + Fact _chargeStateFact = Fact(0, QStringLiteral("chargeState"), FactMetaData::valueTypeUint8); + Fact _instantPowerFact = Fact(0, QStringLiteral("instantPower"), FactMetaData::valueTypeDouble); + // BATTERY_STATUS_V2 facts + Fact _capacityRemainingFact = Fact(0, QStringLiteral("capacityRemaining"), FactMetaData::valueTypeDouble); + Fact _capacityRemainingIsInferredFact = Fact(0, QStringLiteral("capacityRemainingIsInferred"), FactMetaData::valueTypeBool); + Fact _statusFlagsFact = Fact(0, QStringLiteral("statusFlags"), FactMetaData::valueTypeUint32); + // BATTERY_INFO facts + Fact _batteryNameFact = Fact(0, QStringLiteral("batteryName"), FactMetaData::valueTypeString); + Fact _serialNumberFact = Fact(0, QStringLiteral("serialNumber"), FactMetaData::valueTypeString); + Fact _manufactureDateFact = Fact(0, QStringLiteral("manufactureDate"), FactMetaData::valueTypeString); + Fact _fullChargeCapacityFact = Fact(0, QStringLiteral("fullChargeCapacity"), FactMetaData::valueTypeDouble); + Fact _designCapacityFact = Fact(0, QStringLiteral("designCapacity"), FactMetaData::valueTypeDouble); + Fact _nominalVoltageFact = Fact(0, QStringLiteral("nominalVoltage"), FactMetaData::valueTypeDouble); + Fact _dischargeMinimumVoltageFact = Fact(0, QStringLiteral("dischargeMinimumVoltage"), FactMetaData::valueTypeDouble); + Fact _chargingMinimumVoltageFact = Fact(0, QStringLiteral("chargingMinimumVoltage"), FactMetaData::valueTypeDouble); + Fact _restingMinimumVoltageFact = Fact(0, QStringLiteral("restingMinimumVoltage"), FactMetaData::valueTypeDouble); + Fact _chargingMaximumVoltageFact = Fact(0, QStringLiteral("chargingMaximumVoltage"), FactMetaData::valueTypeDouble); + Fact _chargingMaximumCurrentFact = Fact(0, QStringLiteral("chargingMaximumCurrent"), FactMetaData::valueTypeDouble); + Fact _dischargeMaximumCurrentFact = Fact(0, QStringLiteral("dischargeMaximumCurrent"), FactMetaData::valueTypeDouble); + Fact _dischargeMaximumBurstCurrentFact = Fact(0, QStringLiteral("dischargeMaximumBurstCurrent"), FactMetaData::valueTypeDouble); + Fact _cycleCountFact = Fact(0, QStringLiteral("cycleCount"), FactMetaData::valueTypeDouble); + Fact _weightFact = Fact(0, QStringLiteral("weight"), FactMetaData::valueTypeDouble); + Fact _stateOfHealthFact = Fact(0, QStringLiteral("stateOfHealth"), FactMetaData::valueTypeDouble); + Fact _cellsInSeriesFact = Fact(0, QStringLiteral("cellsInSeries"), FactMetaData::valueTypeDouble); }; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 4653133712d1..f28add08563a 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -271,6 +271,9 @@ void Vehicle::_commonInit(LinkInterface* link) }); connect(_initialConnectStateMachine, &InitialConnectStateMachine::progressUpdate, this, &Vehicle::_gotProgressUpdate); + connect(this, &Vehicle::initialConnectComplete, this, [this]() { + _batteryFactGroupListModel->startV2Negotiation(this); + }); connect(_parameterManager, &ParameterManager::loadProgressChanged, this, &Vehicle::_gotProgressUpdate); _objectAvoidance = new VehicleObjectAvoidance(this, this); @@ -626,6 +629,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_BATTERY_STATUS: _handleBatteryStatus(message); break; + case MAVLINK_MSG_ID_BATTERY_STATUS_V2: + _handleBatteryStatusV2(message); + break; case MAVLINK_MSG_ID_SYS_STATUS: _handleSysStatus(message); break; @@ -1110,46 +1116,55 @@ void Vehicle::_handleSysStatus(mavlink_message_t& message) void Vehicle::_handleBatteryStatus(mavlink_message_t& message) { + if (message.compid != _defaultComponentId) return; + if (_batteryFactGroupListModel->isV2Active()) return; mavlink_battery_status_t batteryStatus; mavlink_msg_battery_status_decode(&message, &batteryStatus); + _announceBatteryChargeState(batteryStatus.id, batteryStatus.charge_state); +} - if (!_lowestBatteryChargeStateAnnouncedMap.contains(batteryStatus.id)) { - _lowestBatteryChargeStateAnnouncedMap[batteryStatus.id] = batteryStatus.charge_state; +void Vehicle::_announceBatteryChargeState(uint8_t batteryId, uint8_t chargeState) +{ + if (!_lowestBatteryChargeStateAnnouncedMap.contains(batteryId)) { + // Seed at OK (1), not chargeState. CHARGING (7) is numerically the highest + // enum value, so initialising with it would cause all subsequent LOW/CRITICAL + // alerts to fail the "chargeState > tracked" guard permanently. + _lowestBatteryChargeStateAnnouncedMap[batteryId] = MAV_BATTERY_CHARGE_STATE_OK; } QString batteryMessage; - switch (batteryStatus.charge_state) { + switch (chargeState) { case MAV_BATTERY_CHARGE_STATE_OK: - _lowestBatteryChargeStateAnnouncedMap[batteryStatus.id] = batteryStatus.charge_state; + _lowestBatteryChargeStateAnnouncedMap[batteryId] = chargeState; break; case MAV_BATTERY_CHARGE_STATE_LOW: - if (batteryStatus.charge_state > _lowestBatteryChargeStateAnnouncedMap[batteryStatus.id]) { - _lowestBatteryChargeStateAnnouncedMap[batteryStatus.id] = batteryStatus.charge_state; + if (chargeState > _lowestBatteryChargeStateAnnouncedMap[batteryId]) { + _lowestBatteryChargeStateAnnouncedMap[batteryId] = chargeState; batteryMessage = tr("battery %1 level low"); } break; case MAV_BATTERY_CHARGE_STATE_CRITICAL: - if (batteryStatus.charge_state > _lowestBatteryChargeStateAnnouncedMap[batteryStatus.id]) { - _lowestBatteryChargeStateAnnouncedMap[batteryStatus.id] = batteryStatus.charge_state; + if (chargeState > _lowestBatteryChargeStateAnnouncedMap[batteryId]) { + _lowestBatteryChargeStateAnnouncedMap[batteryId] = chargeState; batteryMessage = tr("battery %1 level is critical"); } break; case MAV_BATTERY_CHARGE_STATE_EMERGENCY: - if (batteryStatus.charge_state > _lowestBatteryChargeStateAnnouncedMap[batteryStatus.id]) { - _lowestBatteryChargeStateAnnouncedMap[batteryStatus.id] = batteryStatus.charge_state; + if (chargeState > _lowestBatteryChargeStateAnnouncedMap[batteryId]) { + _lowestBatteryChargeStateAnnouncedMap[batteryId] = chargeState; batteryMessage = tr("battery %1 level emergency"); } break; case MAV_BATTERY_CHARGE_STATE_FAILED: - if (batteryStatus.charge_state > _lowestBatteryChargeStateAnnouncedMap[batteryStatus.id]) { - _lowestBatteryChargeStateAnnouncedMap[batteryStatus.id] = batteryStatus.charge_state; + if (chargeState > _lowestBatteryChargeStateAnnouncedMap[batteryId]) { + _lowestBatteryChargeStateAnnouncedMap[batteryId] = chargeState; batteryMessage = tr("battery %1 failed"); } break; case MAV_BATTERY_CHARGE_STATE_UNHEALTHY: - if (batteryStatus.charge_state > _lowestBatteryChargeStateAnnouncedMap[batteryStatus.id]) { - _lowestBatteryChargeStateAnnouncedMap[batteryStatus.id] = batteryStatus.charge_state; + if (chargeState > _lowestBatteryChargeStateAnnouncedMap[batteryId]) { + _lowestBatteryChargeStateAnnouncedMap[batteryId] = chargeState; batteryMessage = tr("battery %1 unhealthy"); } break; @@ -1158,7 +1173,7 @@ void Vehicle::_handleBatteryStatus(mavlink_message_t& message) if (!batteryMessage.isEmpty()) { QString batteryIdStr("%1"); if (_batteryFactGroupListModel->count() > 1) { - batteryIdStr = batteryIdStr.arg(batteryStatus.id); + batteryIdStr = batteryIdStr.arg(batteryId); } else { batteryIdStr = batteryIdStr.arg(""); } @@ -1167,6 +1182,17 @@ void Vehicle::_handleBatteryStatus(mavlink_message_t& message) } } +void Vehicle::_handleBatteryStatusV2(mavlink_message_t& message) +{ + if (message.compid != _defaultComponentId) return; + mavlink_battery_status_v2_t bs; + mavlink_msg_battery_status_v2_decode(&message, &bs); + + // CHARGING is not a voice-alertable state; fault/level states handled by chargeStateFromV2. + const uint8_t chargeState = BatteryFactGroup::chargeStateFromV2(bs.status_flags, bs.percent_remaining); + _announceBatteryChargeState(bs.id, chargeState); +} + void Vehicle::_setHomePosition(QGeoCoordinate& homeCoord) { if (homeCoord != _homePosition) { diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 8382de856a20..5833000b45d2 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -908,6 +908,8 @@ void _activeVehicleChanged (Vehicle* newActiveVehicle); void _handleRadioStatus (mavlink_message_t& message); void _handleRCChannels (mavlink_message_t& message); void _handleBatteryStatus (mavlink_message_t& message); + void _handleBatteryStatusV2 (mavlink_message_t& message); + void _announceBatteryChargeState (uint8_t batteryId, uint8_t chargeState); void _handleSysStatus (mavlink_message_t& message); void _handleExtendedSysState (mavlink_message_t& message); void _handleCommandAck (mavlink_message_t& message);