From 6ee2349dc9f379f37b837fec5b99bf44ef1ed9e0 Mon Sep 17 00:00:00 2001 From: DoinLakeFlyer Date: Fri, 7 Feb 2020 10:16:12 -0800 Subject: [PATCH] Handle unreliable PX4 support of BATTERY_STATUS --- src/Vehicle/BatteryFact.json | 2 +- src/Vehicle/Vehicle.cc | 64 +++++++++++++++++++++++++----------- src/Vehicle/Vehicle.h | 1 + 3 files changed, 47 insertions(+), 20 deletions(-) diff --git a/src/Vehicle/BatteryFact.json b/src/Vehicle/BatteryFact.json index 97b798c3c..7312ff446 100644 --- a/src/Vehicle/BatteryFact.json +++ b/src/Vehicle/BatteryFact.json @@ -59,7 +59,7 @@ "name": "chargeState", "shortDescription": "Charge State", "type": "uint8", - "enumStrings": "N/A,Normal Operation,Low Battery State,Critical Battery State,Emergency Battery State,Battery Failed,Battery Unhealthy", + "enumStrings": "n/a,Normal Operation,Low Battery State,Critical Battery State,Emergency Battery State,Battery Failed,Battery Unhealthy", "enumValues": "0,1,2,3,4,5,6", "decimalPlaces": 0 } diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 6d92bde63..8df222b5e 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -1531,12 +1531,40 @@ bool Vehicle::_apmArmingNotRequired() _parameterManager->getParameter(FactSystem::defaultComponentId, armingRequireParam)->rawValue().toInt() == 0; } +void Vehicle::_batteryStatusWorker(int batteryId, double voltage, double current, double batteryRemainingPct) +{ + VehicleBatteryFactGroup* pBatteryFactGroup = nullptr; + if (batteryId == 0) { + pBatteryFactGroup = &_battery1FactGroup; + } else if (batteryId == 1) { + pBatteryFactGroup = &_battery2FactGroup; + } else { + return; + } + + pBatteryFactGroup->voltage()->setRawValue(voltage); + pBatteryFactGroup->current()->setRawValue(current); + pBatteryFactGroup->instantPower()->setRawValue(voltage * current); + pBatteryFactGroup->percentRemaining()->setRawValue(batteryRemainingPct); + + //-- Low battery warning + if (batteryId == 0 && !qIsNaN(batteryRemainingPct)) { + int warnThreshold = _settingsManager->appSettings()->batteryPercentRemainingAnnounce()->rawValue().toInt(); + if (batteryRemainingPct < warnThreshold && + batteryRemainingPct < _lastAnnouncedLowBatteryPercent && + _lastBatteryAnnouncement.elapsed() > (batteryRemainingPct < warnThreshold * 0.5 ? 15000 : 30000)) { + _say(tr("%1 low battery: %2 percent remaining").arg(_vehicleIdSpeech()).arg(static_cast(batteryRemainingPct))); + _lastBatteryAnnouncement.start(); + _lastAnnouncedLowBatteryPercent = static_cast(batteryRemainingPct); + } + } +} + void Vehicle::_handleSysStatus(mavlink_message_t& message) { mavlink_sys_status_t sysStatus; mavlink_msg_sys_status_decode(&message, &sysStatus); - if (_onboardControlSensorsPresent != sysStatus.onboard_control_sensors_present) { _onboardControlSensorsPresent = sysStatus.onboard_control_sensors_present; emit sensorsPresentBitsChanged(_onboardControlSensorsPresent); @@ -1563,6 +1591,12 @@ void Vehicle::_handleSysStatus(mavlink_message_t& message) emit unhealthySensorsChanged(); emit sensorsUnhealthyBitsChanged(_onboardControlSensorsUnhealthy); } + + // BATTERY_STATUS is currently unreliable on PX4 stack so we rely on SYS_STATUS for partial battery 0 information to work around it + _batteryStatusWorker(0 /* batteryId */, + sysStatus.voltage_battery == UINT16_MAX ? qQNaN() : static_cast(sysStatus.voltage_battery) / 1000.0, + sysStatus.current_battery == -1 ? qQNaN() : static_cast(sysStatus.current_battery) / 100.0, + sysStatus.battery_remaining == -1 ? qQNaN() : sysStatus.battery_remaining); } void Vehicle::_handleBatteryStatus(mavlink_message_t& message) @@ -1582,7 +1616,7 @@ void Vehicle::_handleBatteryStatus(mavlink_message_t& message) int cellCount = 0; double voltage = qQNaN(); for (int i=0; i<10; i++) { - double cellVoltage = bat_status.voltages[i] == UINT16_MAX ? qQNaN() : (double)bat_status.voltages[i] / 1000.0; + double cellVoltage = bat_status.voltages[i] == UINT16_MAX ? qQNaN() : static_cast(bat_status.voltages[i]) / 1000.0; if (qIsNaN(cellVoltage)) { break; } @@ -1599,24 +1633,14 @@ void Vehicle::_handleBatteryStatus(mavlink_message_t& message) pBatteryFactGroup->temperature()->setRawValue(bat_status.temperature == INT16_MAX ? qQNaN() : static_cast(bat_status.temperature) / 100.0); pBatteryFactGroup->mahConsumed()->setRawValue(bat_status.current_consumed == -1 ? qQNaN() : bat_status.current_consumed); pBatteryFactGroup->chargeState()->setRawValue(bat_status.charge_state); - pBatteryFactGroup->current()->setRawValue(bat_status.current_battery == -1 ? qQNaN() : (double)bat_status.current_battery / 100.0); - pBatteryFactGroup->voltage()->setRawValue(voltage); - pBatteryFactGroup->instantPower()->setRawValue(pBatteryFactGroup->voltage()->rawValue().toDouble() * pBatteryFactGroup->current()->rawValue().toDouble()); - pBatteryFactGroup->timeRemaining()->setRawValue(bat_status.time_remaining); - pBatteryFactGroup->percentRemaining()->setRawValue(bat_status.battery_remaining); + pBatteryFactGroup->timeRemaining()->setRawValue(bat_status.time_remaining == 0 ? qQNaN() : bat_status.time_remaining); - //-- Low battery warning - if (bat_status.id == 0) { - if (bat_status.battery_remaining > 0) { - int warnThreshold = _settingsManager->appSettings()->batteryPercentRemainingAnnounce()->rawValue().toInt(); - if (bat_status.battery_remaining < warnThreshold && - bat_status.battery_remaining < _lastAnnouncedLowBatteryPercent && - _lastBatteryAnnouncement.elapsed() > (bat_status.battery_remaining < warnThreshold * 0.5 ? 15000 : 30000)) { - _say(tr("%1 low battery: %2 percent remaining").arg(_vehicleIdSpeech()).arg(bat_status.battery_remaining)); - _lastBatteryAnnouncement.start(); - _lastAnnouncedLowBatteryPercent = bat_status.battery_remaining; - } - } + // BATTERY_STATUS is currently unreliable on PX4 stack so we rely on SYS_STATUS for partial battery 0 information to work around it + if (bat_status.id != 0) { + _batteryStatusWorker(bat_status.id, + voltage, + bat_status.current_battery == -1 ? qQNaN() : (double)bat_status.current_battery / 100.0, + bat_status.battery_remaining == -1 ? qQNaN() : bat_status.battery_remaining); } } @@ -4278,6 +4302,8 @@ VehicleBatteryFactGroup::VehicleBatteryFactGroup(QObject* parent) _temperatureFact.setRawValue (qQNaN()); _cellCountFact.setRawValue (qQNaN()); _instantPowerFact.setRawValue (qQNaN()); + _timeRemainingFact.setRawValue (qQNaN()); + _chargeStateFact.setRawValue (MAV_BATTERY_CHARGE_STATE_UNDEFINED); } const char* VehicleWindFactGroup::_directionFactName = "direction"; diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 10585e084..3f342260c 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -1348,6 +1348,7 @@ private: void _writeCsvLine (); void _flightTimerStart (); void _flightTimerStop (); + void _batteryStatusWorker (int batteryId, double voltage, double current, double batteryRemainingPct); int _id; ///< Mavlink system id int _defaultComponentId; -- 2.22.0