From 3302d7c7bfc88243d2468c4faa788f7b2a91b192 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Wed, 8 Apr 2026 15:32:26 +1000 Subject: [PATCH 01/13] Add support for BATTERY_STATUS_V2 and BATTERY_INFO --- src/Camera/MavlinkCameraControlInterface.h | 1 + src/Camera/QGCCameraManager.cc | 13 ++ src/Camera/QGCCameraManager.h | 1 + src/Camera/SimulatedCameraControl.h | 1 + src/Camera/VehicleCameraControl.cc | 11 ++ src/Camera/VehicleCameraControl.h | 3 +- src/Vehicle/FactGroups/BatteryFact.json | 129 ++++++++++++++ .../FactGroups/BatteryFactGroupListModel.cc | 163 ++++++++++++++++++ .../FactGroups/BatteryFactGroupListModel.h | 133 ++++++++++---- src/Vehicle/Vehicle.cc | 66 +++++-- src/Vehicle/Vehicle.h | 2 + 11 files changed, 474 insertions(+), 49 deletions(-) diff --git a/src/Camera/MavlinkCameraControlInterface.h b/src/Camera/MavlinkCameraControlInterface.h index 865be1f91d43..028215ae3a0e 100644 --- a/src/Camera/MavlinkCameraControlInterface.h +++ b/src/Camera/MavlinkCameraControlInterface.h @@ -252,6 +252,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 b697c0b625f5..3501a0210497 100644 --- a/src/Camera/QGCCameraManager.cc +++ b/src/Camera/QGCCameraManager.cc @@ -186,6 +186,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; @@ -493,6 +496,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 4486d3adb923..611d0d6d9f13 100644 --- a/src/Camera/QGCCameraManager.h +++ b/src/Camera/QGCCameraManager.h @@ -126,6 +126,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 e30569a1adf7..9e0c1bbfa480 100644 --- a/src/Camera/SimulatedCameraControl.h +++ b/src/Camera/SimulatedCameraControl.h @@ -123,6 +123,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 3656bf2d78cb..a18ce43677c0 100644 --- a/src/Camera/VehicleCameraControl.cc +++ b/src/Camera/VehicleCameraControl.cc @@ -1622,6 +1622,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(VehicleCameraControlLog).noquote() << "Received CAMERA_CAPTURE_STATUS - stopping timer, resetting retries:" diff --git a/src/Camera/VehicleCameraControl.h b/src/Camera/VehicleCameraControl.h index ae8679371cf3..0886b8069e13 100644 --- a/src/Camera/VehicleCameraControl.h +++ b/src/Camera/VehicleCameraControl.h @@ -129,7 +129,8 @@ class VehicleCameraControl : public MavlinkCameraControlInterface void handleParamExtAck (const mavlink_param_ext_ack_t& paramExtAck) override; void handleParamExtValue (const mavlink_param_ext_value_t& paramExtValue) override; void handleStorageInformation(const mavlink_storage_information_t& storageInformation) override; - void handleBatteryStatus (const mavlink_battery_status_t& bs) override; + void handleBatteryStatus (const mavlink_battery_status_t& bs) override; + void handleBatteryStatusV2 (const mavlink_battery_status_v2_t& bs) override; void handleTrackingImageStatus(const mavlink_camera_tracking_image_status_t &trackingImageStatus) override; void handleVideoStreamInformation(const mavlink_video_stream_information_t &videoStreamInformation) override; void handleVideoStreamStatus(const mavlink_video_stream_status_t &videoStreamStatus) override; diff --git a/src/Vehicle/FactGroups/BatteryFact.json b/src/Vehicle/FactGroups/BatteryFact.json index 1f4899affc32..e804c6afc96d 100644 --- a/src/Vehicle/FactGroups/BatteryFact.json +++ b/src/Vehicle/FactGroups/BatteryFact.json @@ -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 7b2937ddb299..9e0cda43a207 100644 --- a/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc +++ b/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc @@ -23,6 +23,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 } @@ -47,6 +61,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); @@ -59,6 +93,26 @@ 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); } @@ -75,6 +129,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; } @@ -158,3 +218,106 @@ 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 a MAV_BATTERY_CHARGE_STATE from the status_flags bitmask for display and audio alerts. + // Note: LOW/CRITICAL states have no equivalent in BATTERY_STATUS_V2 status_flags. + constexpr uint32_t faultMask = + MAV_BATTERY_STATUS_FLAGS_FAULT_BATTERY_OVER_TEMPERATURE | + MAV_BATTERY_STATUS_FLAGS_FAULT_BATTERY_UNDER_TEMPERATURE | + MAV_BATTERY_STATUS_FLAGS_FAULT_BATTERY_OVER_VOLTAGE | + MAV_BATTERY_STATUS_FLAGS_FAULT_BATTERY_UNDER_VOLTAGE | + MAV_BATTERY_STATUS_FLAGS_FAULT_BATTERY_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_LEGACY; + + uint8_t derivedChargeState = MAV_BATTERY_CHARGE_STATE_OK; + if (bs.status_flags & faultMask) { + derivedChargeState = MAV_BATTERY_CHARGE_STATE_FAILED; + } else if (bs.status_flags & MAV_BATTERY_STATUS_FLAGS_NOT_READY_TO_USE) { + derivedChargeState = MAV_BATTERY_CHARGE_STATE_EMERGENCY; + } else if (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: fromLatin1 stops at the null terminator; all-zero yields an empty string + batteryName()->setRawValue(QString::fromLatin1(bi.name)); + serialNumber()->setRawValue(QString::fromLatin1(bi.serial_number)); + manufactureDate()->setRawValue(QString::fromLatin1(bi.manufacture_date)); + + _setTelemetryAvailable(true); +} diff --git a/src/Vehicle/FactGroups/BatteryFactGroupListModel.h b/src/Vehicle/FactGroups/BatteryFactGroupListModel.h index 5237a0de2b06..48b05ed590f1 100644 --- a/src/Vehicle/FactGroups/BatteryFactGroupListModel.h +++ b/src/Vehicle/FactGroups/BatteryFactGroupListModel.h @@ -20,32 +20,74 @@ class BatteryFactGroupListModel : public FactGroupListModel 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; } // Overrides from FactGroup void handleMessage(Vehicle *vehicle, const mavlink_message_t &message) final; @@ -57,16 +99,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 3e0cf405f260..96de21be2281 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -630,6 +630,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; @@ -1130,44 +1133,48 @@ void Vehicle::_handleBatteryStatus(mavlink_message_t& message) { 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)) { + _lowestBatteryChargeStateAnnouncedMap[batteryId] = chargeState; } 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; @@ -1176,7 +1183,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(""); } @@ -1185,6 +1192,35 @@ void Vehicle::_handleBatteryStatus(mavlink_message_t& message) } } +void Vehicle::_handleBatteryStatusV2(mavlink_message_t& message) +{ + mavlink_battery_status_v2_t bs; + mavlink_msg_battery_status_v2_decode(&message, &bs); + + // Derive a MAV_BATTERY_CHARGE_STATE from the status_flags bitmask for audio alerts. + // Note: LOW/CRITICAL states have no equivalent in BATTERY_STATUS_V2 status_flags. + constexpr uint32_t faultMask = + MAV_BATTERY_STATUS_FLAGS_FAULT_BATTERY_OVER_TEMPERATURE | + MAV_BATTERY_STATUS_FLAGS_FAULT_BATTERY_UNDER_TEMPERATURE | + MAV_BATTERY_STATUS_FLAGS_FAULT_BATTERY_OVER_VOLTAGE | + MAV_BATTERY_STATUS_FLAGS_FAULT_BATTERY_UNDER_VOLTAGE | + MAV_BATTERY_STATUS_FLAGS_FAULT_BATTERY_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_LEGACY; + + uint8_t chargeState = MAV_BATTERY_CHARGE_STATE_OK; + if (bs.status_flags & faultMask) { + chargeState = MAV_BATTERY_CHARGE_STATE_FAILED; + } else if (bs.status_flags & MAV_BATTERY_STATUS_FLAGS_NOT_READY_TO_USE) { + chargeState = MAV_BATTERY_CHARGE_STATE_EMERGENCY; + } + // CHARGING is not an alertable state — intentionally not mapped here + + _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 51076c5479d0..e6e265a656c7 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -854,6 +854,8 @@ private slots: void _handleCurrentMode (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); From f61bcc5e6f51eff59cd0db2557cf0cc8d6314d94 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Thu, 9 Apr 2026 07:24:17 +1000 Subject: [PATCH 02/13] Fix Battery_status_v2 display --- .../FactGroups/BatteryFactGroupListModel.cc | 20 ++++++++++--------- src/Vehicle/Vehicle.cc | 20 ++++++++++--------- 2 files changed, 22 insertions(+), 18 deletions(-) diff --git a/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc b/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc index 9e0cda43a207..257a1cc15820 100644 --- a/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc +++ b/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc @@ -259,15 +259,17 @@ void BatteryFactGroup::_handleBatteryStatusV2(Vehicle * /*vehicle*/, const mavli // Derive a MAV_BATTERY_CHARGE_STATE from the status_flags bitmask for display and audio alerts. // Note: LOW/CRITICAL states have no equivalent in BATTERY_STATUS_V2 status_flags. constexpr uint32_t faultMask = - MAV_BATTERY_STATUS_FLAGS_FAULT_BATTERY_OVER_TEMPERATURE | - MAV_BATTERY_STATUS_FLAGS_FAULT_BATTERY_UNDER_TEMPERATURE | - MAV_BATTERY_STATUS_FLAGS_FAULT_BATTERY_OVER_VOLTAGE | - MAV_BATTERY_STATUS_FLAGS_FAULT_BATTERY_UNDER_VOLTAGE | - MAV_BATTERY_STATUS_FLAGS_FAULT_BATTERY_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_LEGACY; + 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; uint8_t derivedChargeState = MAV_BATTERY_CHARGE_STATE_OK; if (bs.status_flags & faultMask) { diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 96de21be2281..951f3c03554b 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -1200,15 +1200,17 @@ void Vehicle::_handleBatteryStatusV2(mavlink_message_t& message) // Derive a MAV_BATTERY_CHARGE_STATE from the status_flags bitmask for audio alerts. // Note: LOW/CRITICAL states have no equivalent in BATTERY_STATUS_V2 status_flags. constexpr uint32_t faultMask = - MAV_BATTERY_STATUS_FLAGS_FAULT_BATTERY_OVER_TEMPERATURE | - MAV_BATTERY_STATUS_FLAGS_FAULT_BATTERY_UNDER_TEMPERATURE | - MAV_BATTERY_STATUS_FLAGS_FAULT_BATTERY_OVER_VOLTAGE | - MAV_BATTERY_STATUS_FLAGS_FAULT_BATTERY_UNDER_VOLTAGE | - MAV_BATTERY_STATUS_FLAGS_FAULT_BATTERY_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_LEGACY; + 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; uint8_t chargeState = MAV_BATTERY_CHARGE_STATE_OK; if (bs.status_flags & faultMask) { From 4eaf2708fecaae8877452af3622a2dc1f70e2ea5 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Thu, 9 Apr 2026 08:57:38 +1000 Subject: [PATCH 03/13] Update to enable/disable battery status messages to use latest --- src/FactSystem/FactGroupListModel.h | 2 +- .../FactGroups/BatteryFactGroupListModel.cc | 78 ++++++++++++++++++- .../FactGroups/BatteryFactGroupListModel.h | 30 +++++++ src/Vehicle/Vehicle.cc | 4 + 4 files changed, 112 insertions(+), 2 deletions(-) diff --git a/src/FactSystem/FactGroupListModel.h b/src/FactSystem/FactGroupListModel.h index 2dfbe31a0347..816c61e65e2a 100644 --- a/src/FactSystem/FactGroupListModel.h +++ b/src/FactSystem/FactGroupListModel.h @@ -18,7 +18,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/BatteryFactGroupListModel.cc b/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc index 257a1cc15820..0ce17594f438 100644 --- a/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc +++ b/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc @@ -1,5 +1,5 @@ #include "BatteryFactGroupListModel.h" -#include "MAVLinkLib.h" +#include "Vehicle.h" BatteryFactGroupListModel::BatteryFactGroupListModel(QObject* parent) : FactGroupListModel("battery", parent) @@ -47,6 +47,77 @@ FactGroupWithId *BatteryFactGroupListModel::_createFactGroupWithId(uint32_t id) return new BatteryFactGroup(id, this); } +void BatteryFactGroupListModel::handleMessageForFactGroupCreation( + Vehicle *vehicle, const mavlink_message_t &message) +{ + if (message.msgid == MAVLINK_MSG_ID_BATTERY_STATUS_V2) { + _v2StatusReceived = true; + if (_v2State == V2NegotiationRequesting) { + _activateV2(vehicle); + } + } else if (message.msgid == MAVLINK_MSG_ID_BATTERY_INFO) { + _infoReceived = true; + } + + FactGroupListModel::handleMessageForFactGroupCreation(vehicle, message); +} + +void BatteryFactGroupListModel::startV2Negotiation(Vehicle *vehicle) +{ + if (_v2State != V2NegotiationUnknown) return; + + // --- BATTERY_STATUS_V2 --- + if (_v2StatusReceived) { + // Already streaming — activate immediately, no request needed + _activateV2(vehicle); + } else { + _v2State = V2NegotiationRequesting; + _negotiationVehicle = vehicle; + + // 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; + _negotiationVehicle = nullptr; + }, + 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; + _negotiationVehicle = nullptr; + + // 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) { @@ -119,6 +190,11 @@ BatteryFactGroup::BatteryFactGroup(uint32_t batteryId, QObject *parent) 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); diff --git a/src/Vehicle/FactGroups/BatteryFactGroupListModel.h b/src/Vehicle/FactGroups/BatteryFactGroupListModel.h index 48b05ed590f1..be7c7734504c 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,10 +13,38 @@ 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 + Vehicle *_negotiationVehicle = nullptr; }; class BatteryFactGroup : public FactGroupWithId diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 951f3c03554b..c8cdb1311469 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -287,6 +287,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); @@ -1131,6 +1134,7 @@ void Vehicle::_handleSysStatus(mavlink_message_t& message) void Vehicle::_handleBatteryStatus(mavlink_message_t& message) { + if (_batteryFactGroupListModel->isV2Active()) return; mavlink_battery_status_t batteryStatus; mavlink_msg_battery_status_decode(&message, &batteryStatus); _announceBatteryChargeState(batteryStatus.id, batteryStatus.charge_state); From 7ac5af7d9ebc653939d22e2d282b665765113d85 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Sat, 11 Apr 2026 14:46:10 +1000 Subject: [PATCH 04/13] Battery V2: derive LOW/CRITICAL from percentRemaining; remove dead _negotiationVehicle MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit BATTERY_STATUS_V2 status_flags has no bits for MAV_BATTERY_CHARGE_STATE_LOW or _CRITICAL, so once V2 is active the existing voice alerts and charge-state fact could never reach those states. Fix by falling back to a percent-based derivation using the BatteryIndicatorSettings thresholds (threshold1/threshold2) when no fault flag is set — applied both in Vehicle::_handleBatteryStatusV2 (audio alerts) and BatteryFactGroup::_handleBatteryStatusV2 (chargeState fact). Also removes the _negotiationVehicle member which was written in three places but never read (dead state, flagged by Copilot review r3061146854). Addresses Copilot review comments: r3061146802, r3061146835 — missing LOW/CRITICAL in V2 path r3061146854 — unused _negotiationVehicle field Co-Authored-By: Claude Sonnet 4.6 --- .../FactGroups/BatteryFactGroupListModel.cc | 22 ++++++++++++++----- .../FactGroups/BatteryFactGroupListModel.h | 7 +++--- src/Vehicle/Vehicle.cc | 12 ++++++++++ 3 files changed, 31 insertions(+), 10 deletions(-) diff --git a/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc b/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc index 0ce17594f438..f84f400963ea 100644 --- a/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc +++ b/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc @@ -1,4 +1,6 @@ #include "BatteryFactGroupListModel.h" +#include "BatteryIndicatorSettings.h" +#include "SettingsManager.h" #include "Vehicle.h" BatteryFactGroupListModel::BatteryFactGroupListModel(QObject* parent) @@ -71,8 +73,7 @@ void BatteryFactGroupListModel::startV2Negotiation(Vehicle *vehicle) // Already streaming — activate immediately, no request needed _activateV2(vehicle); } else { - _v2State = V2NegotiationRequesting; - _negotiationVehicle = vehicle; + _v2State = V2NegotiationRequesting; // sendMavCommandWithLambdaFallback calls the lambda only on MAV_RESULT_UNSUPPORTED. // On ACCEPTED (or other), we simply wait: handleMessageForFactGroupCreation will @@ -80,8 +81,7 @@ void BatteryFactGroupListModel::startV2Negotiation(Vehicle *vehicle) // stay in Requesting and V1 continues to be used (safe fallback). vehicle->sendMavCommandWithLambdaFallback( [this]() { - _v2State = V2NegotiationUnsupported; - _negotiationVehicle = nullptr; + _v2State = V2NegotiationUnsupported; }, vehicle->defaultComponentId(), MAV_CMD_SET_MESSAGE_INTERVAL, @@ -105,8 +105,7 @@ void BatteryFactGroupListModel::startV2Negotiation(Vehicle *vehicle) void BatteryFactGroupListModel::_activateV2(Vehicle *vehicle) { - _v2State = V2NegotiationActive; - _negotiationVehicle = nullptr; + _v2State = V2NegotiationActive; // Ask the flight stack to stop streaming BATTERY_STATUS (V1) vehicle->sendMavCommand( @@ -354,6 +353,17 @@ void BatteryFactGroup::_handleBatteryStatusV2(Vehicle * /*vehicle*/, const mavli derivedChargeState = MAV_BATTERY_CHARGE_STATE_EMERGENCY; } else if (bs.status_flags & MAV_BATTERY_STATUS_FLAGS_CHARGING) { derivedChargeState = MAV_BATTERY_CHARGE_STATE_CHARGING; + } else if (bs.percent_remaining != UINT8_MAX) { + // BATTERY_STATUS_V2 has no LOW/CRITICAL flags — derive from percentRemaining + // using the same thresholds as the battery indicator UI. + const auto *battSettings = SettingsManager::instance()->batteryIndicatorSettings(); + const int thr1 = battSettings->threshold1()->rawValue().toInt(); + const int thr2 = battSettings->threshold2()->rawValue().toInt(); + if (static_cast(bs.percent_remaining) <= thr2) { + derivedChargeState = MAV_BATTERY_CHARGE_STATE_CRITICAL; + } else if (static_cast(bs.percent_remaining) <= thr1) { + derivedChargeState = MAV_BATTERY_CHARGE_STATE_LOW; + } } chargeState()->setRawValue(derivedChargeState); diff --git a/src/Vehicle/FactGroups/BatteryFactGroupListModel.h b/src/Vehicle/FactGroups/BatteryFactGroupListModel.h index be7c7734504c..82accb7f519f 100644 --- a/src/Vehicle/FactGroups/BatteryFactGroupListModel.h +++ b/src/Vehicle/FactGroups/BatteryFactGroupListModel.h @@ -41,10 +41,9 @@ class BatteryFactGroupListModel : public FactGroupListModel 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 - Vehicle *_negotiationVehicle = nullptr; + 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 diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index c8cdb1311469..ecd455dd4d6d 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -59,6 +59,7 @@ #include "RequestMessageCoordinator.h" #include "SettingsManager.h" #include "AppSettings.h" +#include "BatteryIndicatorSettings.h" #include "FlyViewSettings.h" #include "StandardModes.h" #include "TerrainProtocolHandler.h" @@ -1221,6 +1222,17 @@ void Vehicle::_handleBatteryStatusV2(mavlink_message_t& message) chargeState = MAV_BATTERY_CHARGE_STATE_FAILED; } else if (bs.status_flags & MAV_BATTERY_STATUS_FLAGS_NOT_READY_TO_USE) { chargeState = MAV_BATTERY_CHARGE_STATE_EMERGENCY; + } else if (bs.percent_remaining != UINT8_MAX) { + // BATTERY_STATUS_V2 has no LOW/CRITICAL flags — derive from percentRemaining + // using the same thresholds as the battery indicator UI. + const auto *battSettings = SettingsManager::instance()->batteryIndicatorSettings(); + const int thr1 = battSettings->threshold1()->rawValue().toInt(); + const int thr2 = battSettings->threshold2()->rawValue().toInt(); + if (static_cast(bs.percent_remaining) <= thr2) { + chargeState = MAV_BATTERY_CHARGE_STATE_CRITICAL; + } else if (static_cast(bs.percent_remaining) <= thr1) { + chargeState = MAV_BATTERY_CHARGE_STATE_LOW; + } } // CHARGING is not an alertable state — intentionally not mapped here From 012334a034b483b75a018fe668e13451d014ef8e Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Sat, 11 Apr 2026 15:09:58 +1000 Subject: [PATCH 05/13] Fix const qualifier on BatteryIndicatorSettings pointer threshold1()/threshold2() are non-const Fact* accessors; using 'const auto *' on the settings pointer caused a const-qualification mismatch caught by -Werror. Remove the const qualifier. Co-Authored-By: Claude Sonnet 4.6 --- src/Vehicle/FactGroups/BatteryFactGroupListModel.cc | 2 +- src/Vehicle/Vehicle.cc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc b/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc index f84f400963ea..5b714f7525de 100644 --- a/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc +++ b/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc @@ -356,7 +356,7 @@ void BatteryFactGroup::_handleBatteryStatusV2(Vehicle * /*vehicle*/, const mavli } else if (bs.percent_remaining != UINT8_MAX) { // BATTERY_STATUS_V2 has no LOW/CRITICAL flags — derive from percentRemaining // using the same thresholds as the battery indicator UI. - const auto *battSettings = SettingsManager::instance()->batteryIndicatorSettings(); + auto *battSettings = SettingsManager::instance()->batteryIndicatorSettings(); const int thr1 = battSettings->threshold1()->rawValue().toInt(); const int thr2 = battSettings->threshold2()->rawValue().toInt(); if (static_cast(bs.percent_remaining) <= thr2) { diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index ecd455dd4d6d..db71125ae766 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -1225,7 +1225,7 @@ void Vehicle::_handleBatteryStatusV2(mavlink_message_t& message) } else if (bs.percent_remaining != UINT8_MAX) { // BATTERY_STATUS_V2 has no LOW/CRITICAL flags — derive from percentRemaining // using the same thresholds as the battery indicator UI. - const auto *battSettings = SettingsManager::instance()->batteryIndicatorSettings(); + auto *battSettings = SettingsManager::instance()->batteryIndicatorSettings(); const int thr1 = battSettings->threshold1()->rawValue().toInt(); const int thr2 = battSettings->threshold2()->rawValue().toInt(); if (static_cast(bs.percent_remaining) <= thr2) { From 8c8a8bb621d50c1fa079d0550f12fc74c527e503 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Sat, 11 Apr 2026 16:00:37 +1000 Subject: [PATCH 06/13] Battery V2: fix LOW/CRITICAL threshold derivation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Used BatteryIndicatorSettings.threshold1/threshold2 (defaults 80%/60%) as LOW/CRITICAL alarm triggers, which would fire voice alerts at 79% and 59% remaining — far too early. Replace with conventional alarm thresholds (25% LOW, 10% CRITICAL) that sit below the visual OK sub-colour range and match industry norms. Remove the now-unused BatteryIndicatorSettings.h includes. Co-Authored-By: Claude Sonnet 4.6 --- src/Vehicle/FactGroups/BatteryFactGroupListModel.cc | 13 +++++-------- src/Vehicle/Vehicle.cc | 12 +++++------- 2 files changed, 10 insertions(+), 15 deletions(-) diff --git a/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc b/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc index 5b714f7525de..c4a4b9dd796e 100644 --- a/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc +++ b/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc @@ -1,6 +1,4 @@ #include "BatteryFactGroupListModel.h" -#include "BatteryIndicatorSettings.h" -#include "SettingsManager.h" #include "Vehicle.h" BatteryFactGroupListModel::BatteryFactGroupListModel(QObject* parent) @@ -355,13 +353,12 @@ void BatteryFactGroup::_handleBatteryStatusV2(Vehicle * /*vehicle*/, const mavli derivedChargeState = MAV_BATTERY_CHARGE_STATE_CHARGING; } else if (bs.percent_remaining != UINT8_MAX) { // BATTERY_STATUS_V2 has no LOW/CRITICAL flags — derive from percentRemaining - // using the same thresholds as the battery indicator UI. - auto *battSettings = SettingsManager::instance()->batteryIndicatorSettings(); - const int thr1 = battSettings->threshold1()->rawValue().toInt(); - const int thr2 = battSettings->threshold2()->rawValue().toInt(); - if (static_cast(bs.percent_remaining) <= thr2) { + // using conventional alert thresholds (25 % LOW, 10 % CRITICAL). + constexpr int kLowPct = 25; + constexpr int kCriticalPct = 10; + if (static_cast(bs.percent_remaining) <= kCriticalPct) { derivedChargeState = MAV_BATTERY_CHARGE_STATE_CRITICAL; - } else if (static_cast(bs.percent_remaining) <= thr1) { + } else if (static_cast(bs.percent_remaining) <= kLowPct) { derivedChargeState = MAV_BATTERY_CHARGE_STATE_LOW; } } diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index db71125ae766..ff00ffce97dc 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -59,7 +59,6 @@ #include "RequestMessageCoordinator.h" #include "SettingsManager.h" #include "AppSettings.h" -#include "BatteryIndicatorSettings.h" #include "FlyViewSettings.h" #include "StandardModes.h" #include "TerrainProtocolHandler.h" @@ -1224,13 +1223,12 @@ void Vehicle::_handleBatteryStatusV2(mavlink_message_t& message) chargeState = MAV_BATTERY_CHARGE_STATE_EMERGENCY; } else if (bs.percent_remaining != UINT8_MAX) { // BATTERY_STATUS_V2 has no LOW/CRITICAL flags — derive from percentRemaining - // using the same thresholds as the battery indicator UI. - auto *battSettings = SettingsManager::instance()->batteryIndicatorSettings(); - const int thr1 = battSettings->threshold1()->rawValue().toInt(); - const int thr2 = battSettings->threshold2()->rawValue().toInt(); - if (static_cast(bs.percent_remaining) <= thr2) { + // using conventional alert thresholds (25 % LOW, 10 % CRITICAL). + constexpr int kLowPct = 25; + constexpr int kCriticalPct = 10; + if (static_cast(bs.percent_remaining) <= kCriticalPct) { chargeState = MAV_BATTERY_CHARGE_STATE_CRITICAL; - } else if (static_cast(bs.percent_remaining) <= thr1) { + } else if (static_cast(bs.percent_remaining) <= kLowPct) { chargeState = MAV_BATTERY_CHARGE_STATE_LOW; } } From ccea450abdcaa2af8038442c4fac7392adc17d41 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Sat, 11 Apr 2026 16:27:40 +1000 Subject: [PATCH 07/13] Battery V2: fix component-ID scoping, string safety, extract shared helper Three bugs fixed from Copilot re-review: 1. V2 activation was triggered by any component sending BATTERY_STATUS_V2, including camera peripherals. A camera on a different compid could cause QGC to suppress BATTERY_STATUS from the autopilot (which was never sending V2). Fix: gate _v2StatusReceived / _activateV2 on message.compid matching vehicle->defaultComponentId(). 2. QString::fromLatin1(bi.name) scanned for a null terminator past the MAVLink fixed-length field boundary if a sender omitted it. Fix: bound with strnlen(bi.name, sizeof(bi.name)) for all three BATTERY_INFO string fields. 3. The charge-state derivation (faultMask + percent thresholds) was duplicated identically in BatteryFactGroup and Vehicle, creating a maintenance risk. Extracted into BatteryFactGroup::chargeStateFromV2(statusFlags, percent). This also resolves the previously noted CHARGING/EMERGENCY asymmetry: CHARGING is applied on top of the helper result in the UI path only; the audio path intentionally omits it. Co-Authored-By: Claude Sonnet 4.6 --- .../FactGroups/BatteryFactGroupListModel.cc | 95 +++++++++++-------- .../FactGroups/BatteryFactGroupListModel.h | 5 + src/Vehicle/Vehicle.cc | 35 +------ 3 files changed, 61 insertions(+), 74 deletions(-) diff --git a/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc b/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc index c4a4b9dd796e..b21196b72f7c 100644 --- a/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc +++ b/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc @@ -1,6 +1,8 @@ #include "BatteryFactGroupListModel.h" #include "Vehicle.h" +#include // strnlen + BatteryFactGroupListModel::BatteryFactGroupListModel(QObject* parent) : FactGroupListModel("battery", parent) { @@ -50,13 +52,19 @@ FactGroupWithId *BatteryFactGroupListModel::_createFactGroupWithId(uint32_t id) void BatteryFactGroupListModel::handleMessageForFactGroupCreation( Vehicle *vehicle, const mavlink_message_t &message) { - if (message.msgid == MAVLINK_MSG_ID_BATTERY_STATUS_V2) { - _v2StatusReceived = true; - if (_v2State == V2NegotiationRequesting) { - _activateV2(vehicle); + // 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 == V2NegotiationRequesting) { + _activateV2(vehicle); + } + } else if (message.msgid == MAVLINK_MSG_ID_BATTERY_INFO) { + _infoReceived = true; } - } else if (message.msgid == MAVLINK_MSG_ID_BATTERY_INFO) { - _infoReceived = true; } FactGroupListModel::handleMessageForFactGroupCreation(vehicle, message); @@ -185,6 +193,37 @@ BatteryFactGroup::BatteryFactGroup(uint32_t batteryId, QObject *parent) (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) { @@ -329,38 +368,12 @@ void BatteryFactGroup::_handleBatteryStatusV2(Vehicle * /*vehicle*/, const mavli statusFlags()->setRawValue(bs.status_flags); - // Derive a MAV_BATTERY_CHARGE_STATE from the status_flags bitmask for display and audio alerts. - // Note: LOW/CRITICAL states have no equivalent in BATTERY_STATUS_V2 status_flags. - 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; - - uint8_t derivedChargeState = MAV_BATTERY_CHARGE_STATE_OK; - if (bs.status_flags & faultMask) { - derivedChargeState = MAV_BATTERY_CHARGE_STATE_FAILED; - } else if (bs.status_flags & MAV_BATTERY_STATUS_FLAGS_NOT_READY_TO_USE) { - derivedChargeState = MAV_BATTERY_CHARGE_STATE_EMERGENCY; - } else if (bs.status_flags & MAV_BATTERY_STATUS_FLAGS_CHARGING) { + // 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; - } else if (bs.percent_remaining != UINT8_MAX) { - // BATTERY_STATUS_V2 has no LOW/CRITICAL flags — derive from percentRemaining - // using conventional alert thresholds (25 % LOW, 10 % CRITICAL). - constexpr int kLowPct = 25; - constexpr int kCriticalPct = 10; - if (static_cast(bs.percent_remaining) <= kCriticalPct) { - derivedChargeState = MAV_BATTERY_CHARGE_STATE_CRITICAL; - } else if (static_cast(bs.percent_remaining) <= kLowPct) { - derivedChargeState = MAV_BATTERY_CHARGE_STATE_LOW; - } } chargeState()->setRawValue(derivedChargeState); @@ -399,10 +412,10 @@ void BatteryFactGroup::_handleBatteryInfo(Vehicle * /*vehicle*/, const mavlink_m 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: fromLatin1 stops at the null terminator; all-zero yields an empty string - batteryName()->setRawValue(QString::fromLatin1(bi.name)); - serialNumber()->setRawValue(QString::fromLatin1(bi.serial_number)); - manufactureDate()->setRawValue(QString::fromLatin1(bi.manufacture_date)); + // 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 82accb7f519f..e1b90f322570 100644 --- a/src/Vehicle/FactGroups/BatteryFactGroupListModel.h +++ b/src/Vehicle/FactGroups/BatteryFactGroupListModel.h @@ -118,6 +118,11 @@ class BatteryFactGroup : public FactGroupWithId 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; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index ff00ffce97dc..8f621e03329f 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -1201,39 +1201,8 @@ void Vehicle::_handleBatteryStatusV2(mavlink_message_t& message) mavlink_battery_status_v2_t bs; mavlink_msg_battery_status_v2_decode(&message, &bs); - // Derive a MAV_BATTERY_CHARGE_STATE from the status_flags bitmask for audio alerts. - // Note: LOW/CRITICAL states have no equivalent in BATTERY_STATUS_V2 status_flags. - 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; - - uint8_t chargeState = MAV_BATTERY_CHARGE_STATE_OK; - if (bs.status_flags & faultMask) { - chargeState = MAV_BATTERY_CHARGE_STATE_FAILED; - } else if (bs.status_flags & MAV_BATTERY_STATUS_FLAGS_NOT_READY_TO_USE) { - chargeState = MAV_BATTERY_CHARGE_STATE_EMERGENCY; - } else if (bs.percent_remaining != UINT8_MAX) { - // BATTERY_STATUS_V2 has no LOW/CRITICAL flags — derive from percentRemaining - // using conventional alert thresholds (25 % LOW, 10 % CRITICAL). - constexpr int kLowPct = 25; - constexpr int kCriticalPct = 10; - if (static_cast(bs.percent_remaining) <= kCriticalPct) { - chargeState = MAV_BATTERY_CHARGE_STATE_CRITICAL; - } else if (static_cast(bs.percent_remaining) <= kLowPct) { - chargeState = MAV_BATTERY_CHARGE_STATE_LOW; - } - } - // CHARGING is not an alertable state — intentionally not mapped here - + // 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); } From fbf1bd0048464a0e3b89af2e3459b6de7718586a Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Sat, 11 Apr 2026 16:54:31 +1000 Subject: [PATCH 08/13] Battery: fix _announceBatteryChargeState map initialisation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Seeded _lowestBatteryChargeStateAnnouncedMap with chargeState on first encounter. MAV_BATTERY_CHARGE_STATE_CHARGING = 7 is numerically the largest enum value, so if the first message for a battery carried CHARGING, all subsequent LOW (2) / CRITICAL (3) / EMERGENCY (4) / FAILED (5) checks failed `chargeState > 7` permanently — voice alerts were silently suppressed for the lifetime of the connection unless an OK state was received in between. Fix: seed the map at MAV_BATTERY_CHARGE_STATE_OK (1) unconditionally, which also ensures that connecting to a vehicle already in LOW/CRITICAL fires an immediate alert rather than swallowing the first occurrence. Pre-existing bug on master; surfaced by Copilot review of the V2 path. Co-Authored-By: Claude Sonnet 4.6 --- src/Vehicle/Vehicle.cc | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 8f621e03329f..bfe8588dec09 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -1143,7 +1143,10 @@ void Vehicle::_handleBatteryStatus(mavlink_message_t& message) void Vehicle::_announceBatteryChargeState(uint8_t batteryId, uint8_t chargeState) { if (!_lowestBatteryChargeStateAnnouncedMap.contains(batteryId)) { - _lowestBatteryChargeStateAnnouncedMap[batteryId] = chargeState; + // 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; From 7597ab0cb07a0ad941d0f4ce37762943012c684e Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Sun, 12 Apr 2026 15:26:41 +1000 Subject: [PATCH 09/13] Battery V2: activate V2 on any autopilot V2 frame, not just in Requesting state MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit If the vehicle ACKs SET_MESSAGE_INTERVAL as UNSUPPORTED but later starts streaming BATTERY_STATUS_V2 (e.g. an always-on stream not controlled by SET_MESSAGE_INTERVAL), the old guard kept _v2State at Unsupported and _activateV2 was never called — both V1 and V2 would update the same facts simultaneously and V1 was never suppressed. Changing the guard to != V2NegotiationActive means V2 activates on the first frame from the autopilot component regardless of prior negotiation state. _activateV2 sets the state to Active as its first action, so subsequent frames never re-trigger it and SET_MESSAGE_INTERVAL(147, -1) is sent exactly once. Co-Authored-By: Claude Sonnet 4.6 --- src/Vehicle/FactGroups/BatteryFactGroupListModel.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc b/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc index b21196b72f7c..e25d4386db92 100644 --- a/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc +++ b/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc @@ -59,7 +59,7 @@ void BatteryFactGroupListModel::handleMessageForFactGroupCreation( if (message.compid == static_cast(vehicle->defaultComponentId())) { if (message.msgid == MAVLINK_MSG_ID_BATTERY_STATUS_V2) { _v2StatusReceived = true; - if (_v2State == V2NegotiationRequesting) { + if (_v2State != V2NegotiationActive) { _activateV2(vehicle); } } else if (message.msgid == MAVLINK_MSG_ID_BATTERY_INFO) { From 516180e28161455b805b22e99f80aa7f0f856a10 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Sun, 12 Apr 2026 16:16:29 +1000 Subject: [PATCH 10/13] Battery V2: scope V2 audio alerts to autopilot compid; fix voltage unit case _handleBatteryStatusV2 was calling _announceBatteryChargeState without filtering on compid, so a camera peripheral sending BATTERY_STATUS_V2 would trigger voice battery alerts for the vehicle. Added the same `compid != _defaultComponentId` guard already used by _handleHomePosition and the V2 negotiation path. Also standardise the five new BATTERY_INFO voltage fact unit strings from "V" to "v" to match the existing "voltage" fact in BatteryFact.json. Co-Authored-By: Claude Sonnet 4.6 --- src/Vehicle/FactGroups/BatteryFact.json | 10 +++++----- src/Vehicle/Vehicle.cc | 1 + 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/Vehicle/FactGroups/BatteryFact.json b/src/Vehicle/FactGroups/BatteryFact.json index e804c6afc96d..4703cbbe30b3 100644 --- a/src/Vehicle/FactGroups/BatteryFact.json +++ b/src/Vehicle/FactGroups/BatteryFact.json @@ -138,35 +138,35 @@ "shortDesc": "Nominal Voltage", "type": "double", "decimalPlaces": 2, - "units": "V" + "units": "v" }, { "name": "dischargeMinimumVoltage", "shortDesc": "Discharge Min Voltage", "type": "double", "decimalPlaces": 2, - "units": "V" + "units": "v" }, { "name": "chargingMinimumVoltage", "shortDesc": "Charging Min Voltage", "type": "double", "decimalPlaces": 2, - "units": "V" + "units": "v" }, { "name": "restingMinimumVoltage", "shortDesc": "Resting Min Voltage", "type": "double", "decimalPlaces": 2, - "units": "V" + "units": "v" }, { "name": "chargingMaximumVoltage", "shortDesc": "Charging Max Voltage", "type": "double", "decimalPlaces": 2, - "units": "V" + "units": "v" }, { "name": "chargingMaximumCurrent", diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index bfe8588dec09..16cc5fa0ce57 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -1201,6 +1201,7 @@ void Vehicle::_announceBatteryChargeState(uint8_t batteryId, uint8_t chargeState 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); From 0fd6e90dbfc6486a4bc5b191fe06cd5aa3d6305b Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Sun, 12 Apr 2026 18:20:10 +1000 Subject: [PATCH 11/13] Battery facts: use uppercase V for all voltage units; add Vehicle forward-decl BatteryFact.json had a pre-existing outlier using "v" (lowercase) for the voltage fact while every other Fact metadata file in the codebase uses "V". Standardise the existing voltage fact and all new BATTERY_INFO voltage facts to uppercase "V". FactGroupListModel.h declared methods taking Vehicle* without a forward declaration, making the header rely on include-order to compile. Add the missing class Vehicle; forward declaration, consistent with FactGroup.h and ParameterManager.h in the same directory. Co-Authored-By: Claude Sonnet 4.6 --- src/FactSystem/FactGroupListModel.h | 5 +++-- src/Vehicle/FactGroups/BatteryFact.json | 12 ++++++------ 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/FactSystem/FactGroupListModel.h b/src/FactSystem/FactGroupListModel.h index 816c61e65e2a..62a3c0e41f33 100644 --- a/src/FactSystem/FactGroupListModel.h +++ b/src/FactSystem/FactGroupListModel.h @@ -6,8 +6,9 @@ #include -/// \brief Dynamically manages FactGroupWithIds based on incoming messages. -/// +class Vehicle; + +/// Dynamically manages FactGroupWithIds based on incoming messages. class FactGroupListModel : public QmlObjectListModel { Q_OBJECT diff --git a/src/Vehicle/FactGroups/BatteryFact.json b/src/Vehicle/FactGroups/BatteryFact.json index 4703cbbe30b3..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", @@ -138,35 +138,35 @@ "shortDesc": "Nominal Voltage", "type": "double", "decimalPlaces": 2, - "units": "v" + "units": "V" }, { "name": "dischargeMinimumVoltage", "shortDesc": "Discharge Min Voltage", "type": "double", "decimalPlaces": 2, - "units": "v" + "units": "V" }, { "name": "chargingMinimumVoltage", "shortDesc": "Charging Min Voltage", "type": "double", "decimalPlaces": 2, - "units": "v" + "units": "V" }, { "name": "restingMinimumVoltage", "shortDesc": "Resting Min Voltage", "type": "double", "decimalPlaces": 2, - "units": "v" + "units": "V" }, { "name": "chargingMaximumVoltage", "shortDesc": "Charging Max Voltage", "type": "double", "decimalPlaces": 2, - "units": "v" + "units": "V" }, { "name": "chargingMaximumCurrent", From 72920003f5f21e8e754497bc6464a9f78d5df286 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Mon, 13 Apr 2026 13:33:16 +1000 Subject: [PATCH 12/13] Battery V1: scope audio alerts to autopilot compid; matches V2 handler _handleBatteryStatus lacked the compid guard that was added to _handleBatteryStatusV2 in the previous commit. A camera peripheral streaming BATTERY_STATUS (V1) could trigger incorrect voice alerts for the autopilot battery. Add the same early-return pattern used by _handleSysStatus and _handleBatteryStatusV2. Co-Authored-By: Claude Sonnet 4.6 --- src/Vehicle/Vehicle.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 16cc5fa0ce57..a8a288adfb30 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -1134,6 +1134,7 @@ 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); From a18af7eae0255800c537af43df34d9893efc4ed0 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Mon, 13 Apr 2026 13:49:34 +1000 Subject: [PATCH 13/13] Battery V2: fix BATTERY_INFO not requested when V2 activates before initialConnectComplete If BATTERY_STATUS_V2 frames arrive before initialConnectComplete fires (e.g. autopilot already streaming at 0.5 Hz), _activateV2 is called from handleMessageForFactGroupCreation and _v2State becomes Active. The early return at the top of startV2Negotiation then skips the entire function, including the BATTERY_INFO sendMavCommand, leaving BATTERY_INFO unrequested even when _infoReceived is false. Scope the early-return to the V2 negotiation block only. The BATTERY_INFO request is independent and always runs when _infoReceived is still false. Co-Authored-By: Claude Sonnet 4.6 --- .../FactGroups/BatteryFactGroupListModel.cc | 47 ++++++++++--------- 1 file changed, 25 insertions(+), 22 deletions(-) diff --git a/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc b/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc index e25d4386db92..6354401b7d77 100644 --- a/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc +++ b/src/Vehicle/FactGroups/BatteryFactGroupListModel.cc @@ -72,29 +72,32 @@ void BatteryFactGroupListModel::handleMessageForFactGroupCreation( void BatteryFactGroupListModel::startV2Negotiation(Vehicle *vehicle) { - if (_v2State != V2NegotiationUnknown) return; - // --- BATTERY_STATUS_V2 --- - 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 - ); + // 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) ---