From 4e61315d070385b9d859fea78bff91c486c2d13b Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Sat, 22 Aug 2020 13:31:19 -0700 Subject: [PATCH] Full BATTERY_STATUS support --- qgroundcontrol.pro | 2 + src/FactSystem/FactGroup.cc | 11 +- src/FactSystem/FactGroup.h | 20 ++- src/QmlControls/InstrumentValueData.cc | 58 +++--- src/QmlControls/InstrumentValueData.h | 12 +- src/Settings/AppSettings.h | 2 +- src/Vehicle/BatteryFact.json | 48 +++-- src/Vehicle/Vehicle.cc | 184 +++++++------------ src/Vehicle/Vehicle.h | 86 ++------- src/Vehicle/VehicleBatteryFactGroup.cc | 193 ++++++++++++++++++++ src/Vehicle/VehicleBatteryFactGroup.h | 94 ++++++++++ src/comm/MockLink.cc | 82 ++++++++- src/comm/MockLink.h | 11 +- src/ui/preferences/GeneralSettings.qml | 23 --- src/ui/toolbar/BatteryIndicator.qml | 233 +++++++++++++++++-------- 15 files changed, 712 insertions(+), 347 deletions(-) create mode 100644 src/Vehicle/VehicleBatteryFactGroup.cc create mode 100644 src/Vehicle/VehicleBatteryFactGroup.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 880e4cda0..2cd5bdb58 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -705,6 +705,7 @@ HEADERS += \ src/Vehicle/TrajectoryPoints.h \ src/Vehicle/Vehicle.h \ src/Vehicle/VehicleObjectAvoidance.h \ + src/Vehicle/VehicleBatteryFactGroup.h \ src/VehicleSetup/JoystickConfigController.h \ src/comm/LinkConfiguration.h \ src/comm/LinkInterface.h \ @@ -924,6 +925,7 @@ SOURCES += \ src/Vehicle/TrajectoryPoints.cc \ src/Vehicle/Vehicle.cc \ src/Vehicle/VehicleObjectAvoidance.cc \ + src/Vehicle/VehicleBatteryFactGroup.cc \ src/VehicleSetup/JoystickConfigController.cc \ src/comm/LinkConfiguration.cc \ src/comm/LinkInterface.cc \ diff --git a/src/FactSystem/FactGroup.cc b/src/FactSystem/FactGroup.cc index 985dea5b0..edb4e8f24 100644 --- a/src/FactSystem/FactGroup.cc +++ b/src/FactSystem/FactGroup.cc @@ -18,8 +18,6 @@ #include #include -QGC_LOGGING_CATEGORY(FactGroupLog, "FactGroupLog") - FactGroup::FactGroup(int updateRateMsecs, const QString& metaDataFile, QObject* parent, bool ignoreCamelCase) : QObject(parent) , _updateRateMSecs(updateRateMsecs) @@ -135,6 +133,8 @@ void FactGroup::_addFact(Fact* fact, const QString& name) } _nameToFactMap[name] = fact; _factNames.append(name); + + emit factNamesChanged(); } void FactGroup::_addFactGroup(FactGroup* factGroup, const QString& name) @@ -145,6 +145,8 @@ void FactGroup::_addFactGroup(FactGroup* factGroup, const QString& name) } _nameToFactGroupMap[name] = factGroup; + + emit factGroupNamesChanged(); } void FactGroup::_updateAllValues(void) @@ -175,3 +177,8 @@ QString FactGroup::_camelCase(const QString& text) { return text[0].toLower() + text.right(text.length() - 1); } + +void FactGroup::handleMessage(Vehicle* /* vehicle */, mavlink_message_t& /* message */) +{ + // Default implementation does nothing +} diff --git a/src/FactSystem/FactGroup.h b/src/FactSystem/FactGroup.h index 00614a8ca..6ece49957 100644 --- a/src/FactSystem/FactGroup.h +++ b/src/FactSystem/FactGroup.h @@ -7,18 +7,17 @@ * ****************************************************************************/ - -#ifndef FactGroup_H -#define FactGroup_H +#pragma once #include "Fact.h" +#include "QGCMAVLink.h" #include "QGCLoggingCategory.h" #include #include #include -Q_DECLARE_LOGGING_CATEGORY(VehicleLog) +class Vehicle; /// Used to group Facts together into an object hierarachy. class FactGroup : public QObject @@ -29,8 +28,8 @@ public: FactGroup(int updateRateMsecs, const QString& metaDataFile, QObject* parent = nullptr, bool ignoreCamelCase = false); FactGroup(int updateRateMsecs, QObject* parent = nullptr, bool ignoreCamelCase = false); - Q_PROPERTY(QStringList factNames READ factNames CONSTANT) - Q_PROPERTY(QStringList factGroupNames READ factGroupNames CONSTANT) + Q_PROPERTY(QStringList factNames READ factNames NOTIFY factNamesChanged) + Q_PROPERTY(QStringList factGroupNames READ factGroupNames NOTIFY factGroupNamesChanged) /// @ return true: if the fact exists in the group Q_INVOKABLE bool factExists(const QString& name); @@ -49,6 +48,13 @@ public: QStringList factNames(void) const { return _factNames; } QStringList factGroupNames(void) const { return _nameToFactGroupMap.keys(); } + /// Allows a FactGroup to parse incoming messages and fill in values + virtual void handleMessage(Vehicle* vehicle, mavlink_message_t& message); + +signals: + void factNamesChanged (void); + void factGroupNamesChanged (void); + protected slots: virtual void _updateAllValues(void); @@ -71,5 +77,3 @@ private: bool _ignoreCamelCase = false; QTimer _updateTimer; }; - -#endif diff --git a/src/QmlControls/InstrumentValueData.cc b/src/QmlControls/InstrumentValueData.cc index 498d014e9..efde15431 100644 --- a/src/QmlControls/InstrumentValueData.cc +++ b/src/QmlControls/InstrumentValueData.cc @@ -52,13 +52,7 @@ void InstrumentValueData::_activeVehicleChanged(Vehicle* activeVehicle) _activeVehicle = activeVehicle; - _factGroupNames.clear(); - _factGroupNames = _activeVehicle->factGroupNames(); - for (QString& name: _factGroupNames) { - name[0] = name[0].toUpper(); - } - _factGroupNames.prepend(vehicleFactGroupName); - emit factGroupNamesChanged(_factGroupNames); + emit factGroupNamesChanged(); if (_fact) { _fact = nullptr; @@ -85,13 +79,11 @@ void InstrumentValueData::clearFact(void) { _fact = nullptr; _factName.clear(); - _factGroupName.clear(); - _factValueNames.clear(); _text.clear(); _icon.clear(); _showUnits = true; - emit factValueNamesChanged (_factValueNames); + emit factValueNamesChanged (); emit factChanged (_fact); emit factNameChanged (_factName); emit factGroupNameChanged (_factGroupName); @@ -113,17 +105,12 @@ void InstrumentValueData::setFact(const QString& factGroupName, const QString& f } else { factGroup = _activeVehicle->getFactGroup(factGroupName); } - - _factValueNames.clear(); - _factValueNames = factGroup->factNames(); - for (QString& name: _factValueNames) { - name[0] = name[0].toUpper(); - } + _factGroupName = factGroupName; QString nonEmptyFactName; if (factGroup) { if (factName.isEmpty()) { - nonEmptyFactName = _factValueNames[0]; + nonEmptyFactName = factValueNames()[0]; } else { nonEmptyFactName = factName; } @@ -131,16 +118,14 @@ void InstrumentValueData::setFact(const QString& factGroupName, const QString& f } if (_fact) { - _factGroupName = factGroupName; - _factName = nonEmptyFactName; - + _factName = nonEmptyFactName; connect(_fact, &Fact::rawValueChanged, this, &InstrumentValueData::_updateRanges); } else { - _factName.clear(); _factGroupName.clear(); + _factName.clear(); } - emit factValueNamesChanged (_factValueNames); + emit factValueNamesChanged (); emit factChanged (_fact); emit factNameChanged (_factName); emit factGroupNameChanged (_factGroupName); @@ -364,3 +349,32 @@ int InstrumentValueData::_currentRangeIndex(const QVariant& value) } return _rangeValues.count(); } + +QStringList InstrumentValueData::factGroupNames(void) const +{ + QStringList groupNames = _activeVehicle->factGroupNames(); + + for (QString& name: groupNames) { + name[0] = name[0].toUpper(); + } + groupNames.prepend(vehicleFactGroupName); + + return groupNames; +} + +QStringList InstrumentValueData::factValueNames(void) const +{ + FactGroup* factGroup = nullptr; + if (_factGroupName == vehicleFactGroupName) { + factGroup = _activeVehicle; + } else { + factGroup = _activeVehicle->getFactGroup(_factGroupName); + } + + QStringList valueNames = factGroup->factNames(); + for (QString& name: valueNames) { + name[0] = name[0].toUpper(); + } + + return valueNames; +} diff --git a/src/QmlControls/InstrumentValueData.h b/src/QmlControls/InstrumentValueData.h index cb6489dbf..4598d1b21 100644 --- a/src/QmlControls/InstrumentValueData.h +++ b/src/QmlControls/InstrumentValueData.h @@ -33,8 +33,8 @@ public: explicit InstrumentValueData(FactValueGrid* factValueGrid, QObject* parent); Q_PROPERTY(FactValueGrid* factValueGrid MEMBER _factValueGrid CONSTANT) - Q_PROPERTY(QStringList factGroupNames MEMBER _factGroupNames NOTIFY factGroupNamesChanged) - Q_PROPERTY(QStringList factValueNames MEMBER _factValueNames NOTIFY factValueNamesChanged) + Q_PROPERTY(QStringList factGroupNames READ factGroupNames NOTIFY factGroupNamesChanged) + Q_PROPERTY(QStringList factValueNames READ factValueNames NOTIFY factValueNamesChanged) Q_PROPERTY(QString factGroupName READ factGroupName NOTIFY factGroupNameChanged) Q_PROPERTY(QString factName READ factName NOTIFY factNameChanged) Q_PROPERTY(Fact* fact READ fact NOTIFY factChanged) @@ -58,6 +58,8 @@ public: Q_INVOKABLE void addRangeValue (void); Q_INVOKABLE void removeRangeValue(int index); + QStringList factGroupNames (void) const; + QStringList factValueNames (void) const; QString factGroupName (void) const { return _factGroupName; } QString factName (void) const { return _factName; } Fact* fact (void) { return _fact; } @@ -88,8 +90,8 @@ signals: void textChanged (QString text); void showUnitsChanged (bool showUnits); void iconChanged (const QString& icon); - void factGroupNamesChanged (const QStringList& factGroupNames); - void factValueNamesChanged (const QStringList& factValueNames); + void factGroupNamesChanged (void); + void factValueNamesChanged (void); void rangeTypeChanged (RangeType rangeType); void rangeValuesChanged (const QVariantList& rangeValues); void rangeColorsChanged (const QVariantList& rangeColors); @@ -119,8 +121,6 @@ private: QString _text; bool _showUnits = true; QString _icon; - QStringList _factGroupNames; - QStringList _factValueNames; QColor _currentColor; double _currentOpacity = 1.0; QString _currentIcon; diff --git a/src/Settings/AppSettings.h b/src/Settings/AppSettings.h index 20a676d87..89c366ca2 100644 --- a/src/Settings/AppSettings.h +++ b/src/Settings/AppSettings.h @@ -32,7 +32,7 @@ public: DEFINE_SETTINGFACT(offlineEditingHoverSpeed) DEFINE_SETTINGFACT(offlineEditingAscentSpeed) DEFINE_SETTINGFACT(offlineEditingDescentSpeed) - DEFINE_SETTINGFACT(batteryPercentRemainingAnnounce) + DEFINE_SETTINGFACT(batteryPercentRemainingAnnounce) // Important: This is only used to calculate battery swaps DEFINE_SETTINGFACT(defaultMissionItemAltitude) DEFINE_SETTINGFACT(telemetrySave) DEFINE_SETTINGFACT(telemetrySaveNotArmed) diff --git a/src/Vehicle/BatteryFact.json b/src/Vehicle/BatteryFact.json index 14980e8b2..695d1dfb6 100644 --- a/src/Vehicle/BatteryFact.json +++ b/src/Vehicle/BatteryFact.json @@ -1,63 +1,89 @@ { "version": 1, - "fileType": "FactMetaData", + "fileType": "FactMetaData", "QGC.MetaData.Facts": [ +{ + "name": "id", + "shortDesc": "Battery Id", + "type": "uint8" +}, +{ + "name": "batteryFunction", + "shortDesc": "Battery Function", + "type": "uint8", + "enumStrings": "n/a,All Flight Systems,Propulsion,Avionics,Payload", + "enumValues": "0,1,2,3,4", + "decimalPlaces": 0 +}, +{ + "name": "batteryType", + "shortDesc": "Battery Type", + "type": "uint8", + "enumStrings": "n/a,LIPO,LIFE,LION,NIMH", + "enumValues": "0,1,2,3,4", + "decimalPlaces": 0 +}, { "name": "voltage", - "shortDesc": "Voltage", + "shortDesc": "Voltage", "type": "double", "decimalPlaces": 2, "units": "v" }, { "name": "percentRemaining", - "shortDesc": "Percent", + "shortDesc": "Percent", "type": "double", "decimalPlaces": 0, "units": "%" }, { "name": "mahConsumed", - "shortDesc": "Consumed", + "shortDesc": "Consumed", "type": "double", "decimalPlaces": 0, "units": "mAh" }, { "name": "current", - "shortDesc": "Current", + "shortDesc": "Current", "type": "double", "decimalPlaces": 2, "units": "A" }, { "name": "temperature", - "shortDesc": "Temperature", + "shortDesc": "Temperature", "type": "double", "decimalPlaces": 0, "units": "C" }, { "name": "instantPower", - "shortDesc": "Watts", + "shortDesc": "Watts", "type": "double", "decimalPlaces": 2, "units": "W" }, { "name": "timeRemaining", - "shortDesc": "Time Remaining", + "shortDesc": "Time Remaining", "type": "double", "decimalPlaces": 0, "units": "s" }, +{ + "name": "timeRemainingStr", + "shortDesc": "Time Remaining", + "type": "string" +}, { "name": "chargeState", - "shortDesc": "Charge State", + "shortDesc": "Charge State", "type": "uint8", - "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", + "enumStrings": "n/a,Ok,Low,Critical,Emergency,Failed,Unhealthy,Charging", + "enumValues": "0,1,2,3,4,5,6,7", "decimalPlaces": 0 } ] diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index cc9363c20..580598bc7 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -48,6 +48,7 @@ #include "FTPManager.h" #include "ComponentInformationManager.h" #include "InitialConnectStateMachine.h" +#include "VehicleBatteryFactGroup.h" #if defined(QGC_AIRMAP_ENABLED) #include "AirspaceVehicleManager.h" @@ -86,8 +87,6 @@ const char* Vehicle::_hobbsFactName = "hobbs"; const char* Vehicle::_throttlePctFactName = "throttlePct"; const char* Vehicle::_gpsFactGroupName = "gps"; -const char* Vehicle::_battery1FactGroupName = "battery"; -const char* Vehicle::_battery2FactGroupName = "battery2"; const char* Vehicle::_windFactGroupName = "wind"; const char* Vehicle::_vibrationFactGroupName = "vibration"; const char* Vehicle::_temperatureFactGroupName = "temperature"; @@ -182,7 +181,6 @@ Vehicle::Vehicle(LinkInterface* link, , _firmwareVersionType(FIRMWARE_VERSION_TYPE_OFFICIAL) , _gitHash(versionNotSetValue) , _uid(0) - , _lastAnnouncedLowBatteryPercent(100) , _priorityLinkCommanded(false) , _orbitActive(false) , _pidTuningTelemetryMode(false) @@ -208,8 +206,6 @@ Vehicle::Vehicle(LinkInterface* link, , _hobbsFact (0, _hobbsFactName, FactMetaData::valueTypeString) , _throttlePctFact (0, _throttlePctFactName, FactMetaData::valueTypeUint16) , _gpsFactGroup(this) - , _battery1FactGroup(this) - , _battery2FactGroup(this) , _windFactGroup(this) , _vibrationFactGroup(this) , _temperatureFactGroup(this) @@ -287,7 +283,6 @@ Vehicle::Vehicle(LinkInterface* link, // Start csv logger connect(&_csvLogTimer, &QTimer::timeout, this, &Vehicle::_writeCsvLine); _csvLogTimer.start(1000); - _lastBatteryAnnouncement.start(); } // Disconnected Vehicle for offline editing @@ -371,7 +366,6 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _firmwareVersionType(FIRMWARE_VERSION_TYPE_OFFICIAL) , _gitHash(versionNotSetValue) , _uid(0) - , _lastAnnouncedLowBatteryPercent(100) , _orbitActive(false) , _pidTuningTelemetryMode(false) , _pidTuningWaitingForRates(false) @@ -396,8 +390,6 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _hobbsFact (0, _hobbsFactName, FactMetaData::valueTypeString) , _throttlePctFact (0, _throttlePctFactName, FactMetaData::valueTypeUint16) , _gpsFactGroup(this) - , _battery1FactGroup(this) - , _battery2FactGroup(this) , _windFactGroup(this) , _vibrationFactGroup(this) , _clockFactGroup(this) @@ -504,8 +496,6 @@ void Vehicle::_commonInit() _addFact(&_hobbsFact, _hobbsFactName); _addFactGroup(&_gpsFactGroup, _gpsFactGroupName); - _addFactGroup(&_battery1FactGroup, _battery1FactGroupName); - _addFactGroup(&_battery2FactGroup, _battery2FactGroupName); _addFactGroup(&_windFactGroup, _windFactGroupName); _addFactGroup(&_vibrationFactGroup, _vibrationFactGroupName); _addFactGroup(&_temperatureFactGroup, _temperatureFactGroupName); @@ -714,6 +704,16 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes _waitForMavlinkMessageMessageReceived(message); + // Battery fact groups are created dynamically as new batteries are discovered + VehicleBatteryFactGroup::handleMessageForFactGroupCreation(this, message); + + // Let the fact groups take a whack at the mavlink traffic + QStringList groupNames = factGroupNames(); + for (int i=0; ihandleMessage(this, message); + } + switch (message.msgid) { case MAVLINK_MSG_ID_HOME_POSITION: _handleHomePosition(message); @@ -1343,8 +1343,6 @@ void Vehicle::_handleHighLatency(mavlink_message_t& message) _windFactGroup.speed()->setRawValue((double)highLatency.airspeed / 5.0); - _battery1FactGroup.percentRemaining()->setRawValue(highLatency.battery_remaining); - _temperatureFactGroup.temperature1()->setRawValue(highLatency.temperature_air); _gpsFactGroup.lat()->setRawValue(coordinate.latitude); @@ -1391,8 +1389,6 @@ void Vehicle::_handleHighLatency2(mavlink_message_t& message) _windFactGroup.direction()->setRawValue((double)highLatency2.wind_heading * 2.0); _windFactGroup.speed()->setRawValue((double)highLatency2.windspeed / 5.0); - _battery1FactGroup.percentRemaining()->setRawValue(highLatency2.battery); - _temperatureFactGroup.temperature1()->setRawValue(highLatency2.temperature_air); _gpsFactGroup.lat()->setRawValue(highLatency2.latitude * 1e-7); @@ -1617,35 +1613,6 @@ 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; @@ -1697,52 +1664,64 @@ void Vehicle::_handleSysStatus(mavlink_message_t& message) _onboardControlSensorsUnhealthy = newSensorsUnhealthy; 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) { - mavlink_battery_status_t bat_status; - mavlink_msg_battery_status_decode(&message, &bat_status); + mavlink_battery_status_t batteryStatus; + mavlink_msg_battery_status_decode(&message, &batteryStatus); - VehicleBatteryFactGroup* pBatteryFactGroup = nullptr; - if (bat_status.id == 0) { - pBatteryFactGroup = &_battery1FactGroup; - } else if (bat_status.id == 1) { - pBatteryFactGroup = &_battery2FactGroup; - } else { - return; + if (!_lowestBatteryChargeStateAnnouncedMap.contains(batteryStatus.id)) { + _lowestBatteryChargeStateAnnouncedMap[batteryStatus.id] = batteryStatus.charge_state; } - double voltage = qQNaN(); - for (int i=0; i<10; i++) { - double cellVoltage = bat_status.voltages[i] == UINT16_MAX ? qQNaN() : static_cast(bat_status.voltages[i]) / 1000.0; - if (qIsNaN(cellVoltage)) { - break; + QString batteryMessage; + + switch (batteryStatus.charge_state) { + case MAV_BATTERY_CHARGE_STATE_OK: + _lowestBatteryChargeStateAnnouncedMap[batteryStatus.id] = batteryStatus.charge_state; + break; + case MAV_BATTERY_CHARGE_STATE_LOW: + if (batteryStatus.charge_state > _lowestBatteryChargeStateAnnouncedMap[batteryStatus.id]) { + _lowestBatteryChargeStateAnnouncedMap[batteryStatus.id] = batteryStatus.charge_state; + batteryMessage = tr("battery %1 level low"); } - if (i == 0) { - voltage = cellVoltage; - } else { - voltage += cellVoltage; + break; + case MAV_BATTERY_CHARGE_STATE_CRITICAL: + if (batteryStatus.charge_state > _lowestBatteryChargeStateAnnouncedMap[batteryStatus.id]) { + _lowestBatteryChargeStateAnnouncedMap[batteryStatus.id] = batteryStatus.charge_state; + 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; + 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; + 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; + batteryMessage = tr("battery %1 unhealthy"); + } + break; } - 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->timeRemaining()->setRawValue(bat_status.time_remaining == 0 ? qQNaN() : bat_status.time_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); + if (!batteryMessage.isEmpty()) { + QString batteryIdStr("%1"); + if (_batteryFactGroupListModel.count() > 1) { + batteryIdStr = batteryIdStr.arg(batteryStatus.id); + } else { + batteryIdStr = batteryIdStr.arg(""); + } + _say(tr("warning")); + _say(QStringLiteral("%1 %2 ").arg(_vehicleIdSpeech()).arg(batteryMessage.arg(batteryIdStr))); } } @@ -1777,7 +1756,7 @@ void Vehicle::_updateArmed(bool armed) _flightTimerStart(); _clearCameraTriggerPoints(); // Reset battery warning - _lastAnnouncedLowBatteryPercent = 100; + _lowestBatteryChargeStateAnnouncedMap.clear(); } else { _trajectoryPoints->stop(); _flightTimerStop(); @@ -4401,51 +4380,6 @@ void Vehicle::sendJoystickDataThreadSafe(float roll, float pitch, float yaw, flo sendMessageOnLinkThreadSafe(priorityLink(), message); } -//----------------------------------------------------------------------------- -//----------------------------------------------------------------------------- - -const char* VehicleBatteryFactGroup::_voltageFactName = "voltage"; -const char* VehicleBatteryFactGroup::_percentRemainingFactName = "percentRemaining"; -const char* VehicleBatteryFactGroup::_mahConsumedFactName = "mahConsumed"; -const char* VehicleBatteryFactGroup::_currentFactName = "current"; -const char* VehicleBatteryFactGroup::_temperatureFactName = "temperature"; -const char* VehicleBatteryFactGroup::_instantPowerFactName = "instantPower"; -const char* VehicleBatteryFactGroup::_timeRemainingFactName = "timeRemaining"; -const char* VehicleBatteryFactGroup::_chargeStateFactName = "chargeState"; - -const char* VehicleBatteryFactGroup::_settingsGroup = "Vehicle.battery"; - -VehicleBatteryFactGroup::VehicleBatteryFactGroup(QObject* parent) - : FactGroup(1000, ":/json/Vehicle/BatteryFact.json", parent) - , _voltageFact (0, _voltageFactName, FactMetaData::valueTypeDouble) - , _percentRemainingFact (0, _percentRemainingFactName, FactMetaData::valueTypeDouble) - , _mahConsumedFact (0, _mahConsumedFactName, FactMetaData::valueTypeDouble) - , _currentFact (0, _currentFactName, FactMetaData::valueTypeDouble) - , _temperatureFact (0, _temperatureFactName, FactMetaData::valueTypeDouble) - , _instantPowerFact (0, _instantPowerFactName, FactMetaData::valueTypeDouble) - , _timeRemainingFact (0, _timeRemainingFactName, FactMetaData::valueTypeDouble) - , _chargeStateFact (0, _chargeStateFactName, FactMetaData::valueTypeUint8) -{ - _addFact(&_voltageFact, _voltageFactName); - _addFact(&_percentRemainingFact, _percentRemainingFactName); - _addFact(&_mahConsumedFact, _mahConsumedFactName); - _addFact(&_currentFact, _currentFactName); - _addFact(&_temperatureFact, _temperatureFactName); - _addFact(&_instantPowerFact, _instantPowerFactName); - _addFact(&_timeRemainingFact, _timeRemainingFactName); - _addFact(&_chargeStateFact, _chargeStateFactName); - - // Start out as not available - _voltageFact.setRawValue (qQNaN()); - _percentRemainingFact.setRawValue (qQNaN()); - _mahConsumedFact.setRawValue (qQNaN()); - _currentFact.setRawValue (qQNaN()); - _temperatureFact.setRawValue (qQNaN()); - _instantPowerFact.setRawValue (qQNaN()); - _timeRemainingFact.setRawValue (qQNaN()); - _chargeStateFact.setRawValue (MAV_BATTERY_CHARGE_STATE_UNDEFINED); -} - const char* VehicleWindFactGroup::_directionFactName = "direction"; const char* VehicleWindFactGroup::_speedFactName = "speed"; const char* VehicleWindFactGroup::_verticalSpeedFactName = "verticalSpeed"; diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index dcdd35cc3..add186dd9 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -47,6 +47,7 @@ class TerrainProtocolHandler; class ComponentInformationManager; class FTPManager; class InitialConnectStateMachine; +class VehicleBatteryFactGroup; #if defined(QGC_AIRMAP_ENABLED) class AirspaceVehicleManager; @@ -257,53 +258,6 @@ private: Fact _lockFact; }; -class VehicleBatteryFactGroup : public FactGroup -{ - Q_OBJECT - -public: - VehicleBatteryFactGroup(QObject* parent = nullptr); - - Q_PROPERTY(Fact* voltage READ voltage CONSTANT) - Q_PROPERTY(Fact* percentRemaining READ percentRemaining CONSTANT) - Q_PROPERTY(Fact* mahConsumed READ mahConsumed CONSTANT) - Q_PROPERTY(Fact* current READ current CONSTANT) - Q_PROPERTY(Fact* temperature READ temperature CONSTANT) - Q_PROPERTY(Fact* instantPower READ instantPower CONSTANT) - Q_PROPERTY(Fact* timeRemaining READ timeRemaining CONSTANT) - Q_PROPERTY(Fact* chargeState READ chargeState CONSTANT) - - 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* chargeState () { return &_chargeStateFact; } - - static const char* _voltageFactName; - static const char* _percentRemainingFactName; - static const char* _mahConsumedFactName; - static const char* _currentFactName; - static const char* _temperatureFactName; - static const char* _instantPowerFactName; - static const char* _timeRemainingFactName; - static const char* _chargeStateFactName; - - static const char* _settingsGroup; - -private: - Fact _voltageFact; - Fact _percentRemainingFact; - Fact _mahConsumedFact; - Fact _currentFact; - Fact _temperatureFact; - Fact _instantPowerFact; - Fact _timeRemainingFact; - Fact _chargeStateFact; -}; - class VehicleTemperatureFactGroup : public FactGroup { Q_OBJECT @@ -692,17 +646,16 @@ public: Q_PROPERTY(Fact* hobbs READ hobbs CONSTANT) Q_PROPERTY(Fact* throttlePct READ throttlePct CONSTANT) - Q_PROPERTY(FactGroup* gps READ gpsFactGroup CONSTANT) - Q_PROPERTY(FactGroup* battery READ battery1FactGroup CONSTANT) - Q_PROPERTY(FactGroup* battery2 READ battery2FactGroup CONSTANT) - Q_PROPERTY(FactGroup* wind READ windFactGroup CONSTANT) - Q_PROPERTY(FactGroup* vibration READ vibrationFactGroup CONSTANT) - Q_PROPERTY(FactGroup* temperature READ temperatureFactGroup CONSTANT) - Q_PROPERTY(FactGroup* clock READ clockFactGroup CONSTANT) - Q_PROPERTY(FactGroup* setpoint READ setpointFactGroup CONSTANT) - Q_PROPERTY(FactGroup* estimatorStatus READ estimatorStatusFactGroup CONSTANT) - Q_PROPERTY(FactGroup* terrain READ terrainFactGroup CONSTANT) - Q_PROPERTY(FactGroup* distanceSensors READ distanceSensorFactGroup CONSTANT) + Q_PROPERTY(FactGroup* gps READ gpsFactGroup CONSTANT) + Q_PROPERTY(FactGroup* wind READ windFactGroup CONSTANT) + Q_PROPERTY(FactGroup* vibration READ vibrationFactGroup CONSTANT) + Q_PROPERTY(FactGroup* temperature READ temperatureFactGroup CONSTANT) + Q_PROPERTY(FactGroup* clock READ clockFactGroup CONSTANT) + Q_PROPERTY(FactGroup* setpoint READ setpointFactGroup CONSTANT) + Q_PROPERTY(FactGroup* estimatorStatus READ estimatorStatusFactGroup CONSTANT) + Q_PROPERTY(FactGroup* terrain READ terrainFactGroup CONSTANT) + Q_PROPERTY(FactGroup* distanceSensors READ distanceSensorFactGroup CONSTANT) + Q_PROPERTY(QmlObjectListModel* batteries READ batteries CONSTANT) Q_PROPERTY(int firmwareMajorVersion READ firmwareMajorVersion NOTIFY firmwareVersionChanged) Q_PROPERTY(int firmwareMinorVersion READ firmwareMinorVersion NOTIFY firmwareVersionChanged) @@ -720,7 +673,7 @@ public: Q_INVOKABLE void resetCounters (); // Called when the message drop-down is invoked to clear current count - Q_INVOKABLE void resetMessages(); + Q_INVOKABLE void resetMessages(); Q_INVOKABLE void virtualTabletJoystickValue(double roll, double pitch, double yaw, double thrust); Q_INVOKABLE void disconnectInactiveVehicle(); @@ -1027,8 +980,6 @@ public: Fact* throttlePct () { return &_throttlePctFact; } FactGroup* gpsFactGroup () { return &_gpsFactGroup; } - FactGroup* battery1FactGroup () { return &_battery1FactGroup; } - FactGroup* battery2FactGroup () { return &_battery2FactGroup; } FactGroup* windFactGroup () { return &_windFactGroup; } FactGroup* vibrationFactGroup () { return &_vibrationFactGroup; } FactGroup* temperatureFactGroup () { return &_temperatureFactGroup; } @@ -1037,6 +988,7 @@ public: FactGroup* distanceSensorFactGroup () { return &_distanceSensorFactGroup; } FactGroup* estimatorStatusFactGroup () { return &_estimatorStatusFactGroup; } FactGroup* terrainFactGroup () { return &_terrainFactGroup; } + QmlObjectListModel* batteries () { return &_batteryFactGroupListModel; } void setConnectionLostEnabled(bool connectionLostEnabled); @@ -1404,7 +1356,6 @@ private: void _writeCsvLine (); void _flightTimerStart (); void _flightTimerStop (); - void _batteryStatusWorker (int batteryId, double voltage, double current, double batteryRemainingPct); void _chunkedStatusTextTimeout (void); void _chunkedStatusTextCompleted (uint8_t compId); @@ -1566,9 +1517,6 @@ private: QString _gitHash; quint64 _uid; - QElapsedTimer _lastBatteryAnnouncement; - int _lastAnnouncedLowBatteryPercent; - SharedLinkInterfacePointer _priorityLink; // We always keep a reference to the priority link to manage shutdown ordering bool _priorityLinkCommanded; @@ -1653,6 +1601,8 @@ private: void _sendMavCommandWorker(bool commandInt, bool requestMessage, bool showError, MavCmdResultHandler resultHandler, void* resultHandlerData, int compId, MAV_CMD command, MAV_FRAME frame, float param1, float param2, float param3, float param4, float param5, float param6, float param7); + QMap _lowestBatteryChargeStateAnnouncedMap; + // FactGroup facts Fact _rollFact; @@ -1677,8 +1627,6 @@ private: Fact _throttlePctFact; VehicleGPSFactGroup _gpsFactGroup; - VehicleBatteryFactGroup _battery1FactGroup; - VehicleBatteryFactGroup _battery2FactGroup; VehicleWindFactGroup _windFactGroup; VehicleVibrationFactGroup _vibrationFactGroup; VehicleTemperatureFactGroup _temperatureFactGroup; @@ -1687,6 +1635,7 @@ private: VehicleDistanceSensorFactGroup _distanceSensorFactGroup; VehicleEstimatorStatusFactGroup _estimatorStatusFactGroup; TerrainFactGroup _terrainFactGroup; + QmlObjectListModel _batteryFactGroupListModel; TerrainProtocolHandler* _terrainProtocolHandler = nullptr; @@ -1712,8 +1661,6 @@ private: static const char* _throttlePctFactName; static const char* _gpsFactGroupName; - static const char* _battery1FactGroupName; - static const char* _battery2FactGroupName; static const char* _windFactGroupName; static const char* _vibrationFactGroupName; static const char* _temperatureFactGroupName; @@ -1729,4 +1676,5 @@ private: static const char* _joystickEnabledSettingsKey; friend class InitialConnectStateMachine; + friend class VehicleBatteryFactGroup; // Allow VehicleBatteryFactGroup to call _addFactGroup }; diff --git a/src/Vehicle/VehicleBatteryFactGroup.cc b/src/Vehicle/VehicleBatteryFactGroup.cc new file mode 100644 index 000000000..5a23c2243 --- /dev/null +++ b/src/Vehicle/VehicleBatteryFactGroup.cc @@ -0,0 +1,193 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "VehicleBatteryFactGroup.h" +#include "QmlObjectListModel.h" +#include "Vehicle.h" + +const char* VehicleBatteryFactGroup::_batteryFactGroupNamePrefix = "battery"; + +const char* VehicleBatteryFactGroup::_batteryIdFactName = "id"; +const char* VehicleBatteryFactGroup::_batteryFunctionFactName = "batteryFunction"; +const char* VehicleBatteryFactGroup::_batteryTypeFactName = "batteryType"; +const char* VehicleBatteryFactGroup::_voltageFactName = "voltage"; +const char* VehicleBatteryFactGroup::_percentRemainingFactName = "percentRemaining"; +const char* VehicleBatteryFactGroup::_mahConsumedFactName = "mahConsumed"; +const char* VehicleBatteryFactGroup::_currentFactName = "current"; +const char* VehicleBatteryFactGroup::_temperatureFactName = "temperature"; +const char* VehicleBatteryFactGroup::_instantPowerFactName = "instantPower"; +const char* VehicleBatteryFactGroup::_timeRemainingFactName = "timeRemaining"; +const char* VehicleBatteryFactGroup::_timeRemainingStrFactName = "timeRemainingStr"; +const char* VehicleBatteryFactGroup::_chargeStateFactName = "chargeState"; + +const char* VehicleBatteryFactGroup::_settingsGroup = "Vehicle.battery"; + +VehicleBatteryFactGroup::VehicleBatteryFactGroup(uint8_t batteryId, QObject* parent) + : FactGroup (1000, ":/json/Vehicle/BatteryFact.json", parent) + , _batteryIdFact (0, _batteryIdFactName, FactMetaData::valueTypeUint8) + , _batteryFunctionFact (0, _batteryFunctionFactName, FactMetaData::valueTypeUint8) + , _batteryTypeFact (0, _batteryTypeFactName, FactMetaData::valueTypeUint8) + , _voltageFact (0, _voltageFactName, FactMetaData::valueTypeDouble) + , _currentFact (0, _currentFactName, FactMetaData::valueTypeDouble) + , _mahConsumedFact (0, _mahConsumedFactName, FactMetaData::valueTypeDouble) + , _temperatureFact (0, _temperatureFactName, FactMetaData::valueTypeDouble) + , _percentRemainingFact (0, _percentRemainingFactName, FactMetaData::valueTypeDouble) + , _timeRemainingFact (0, _timeRemainingFactName, FactMetaData::valueTypeDouble) + , _timeRemainingStrFact (0, _timeRemainingStrFactName, FactMetaData::valueTypeString) + , _chargeStateFact (0, _chargeStateFactName, FactMetaData::valueTypeUint8) + , _instantPowerFact (0, _instantPowerFactName, FactMetaData::valueTypeDouble) +{ + _addFact(&_batteryIdFact, _batteryIdFactName); + _addFact(&_batteryFunctionFact, _batteryFunctionFactName); + _addFact(&_batteryTypeFact, _batteryTypeFactName); + _addFact(&_voltageFact, _voltageFactName); + _addFact(&_currentFact, _currentFactName); + _addFact(&_mahConsumedFact, _mahConsumedFactName); + _addFact(&_temperatureFact, _temperatureFactName); + _addFact(&_percentRemainingFact, _percentRemainingFactName); + _addFact(&_timeRemainingFact, _timeRemainingFactName); + _addFact(&_timeRemainingStrFact, _timeRemainingStrFactName); + _addFact(&_chargeStateFact, _chargeStateFactName); + _addFact(&_instantPowerFact, _instantPowerFactName); + + _batteryIdFact.setRawValue (batteryId); + _batteryFunctionFact.setRawValue (MAV_BATTERY_FUNCTION_UNKNOWN); + _batteryTypeFact.setRawValue (MAV_BATTERY_TYPE_UNKNOWN); + _voltageFact.setRawValue (qQNaN()); + _currentFact.setRawValue (qQNaN()); + _mahConsumedFact.setRawValue (qQNaN()); + _temperatureFact.setRawValue (qQNaN()); + _percentRemainingFact.setRawValue (qQNaN()); + _timeRemainingFact.setRawValue (qQNaN()); + _chargeStateFact.setRawValue (MAV_BATTERY_CHARGE_STATE_UNDEFINED); + _instantPowerFact.setRawValue (qQNaN()); + + connect(&_timeRemainingFact, &Fact::rawValueChanged, this, &VehicleBatteryFactGroup::_timeRemainingChanged); +} + +void VehicleBatteryFactGroup::handleMessageForFactGroupCreation(Vehicle* vehicle, mavlink_message_t& message) +{ + switch (message.msgid) { + case MAVLINK_MSG_ID_HIGH_LATENCY: + case MAVLINK_MSG_ID_HIGH_LATENCY2: + _findOrAddBatteryGroupById(vehicle, 0); + break; + case MAVLINK_MSG_ID_BATTERY_STATUS: + { + mavlink_battery_status_t batteryStatus; + mavlink_msg_battery_status_decode(&message, &batteryStatus); + _findOrAddBatteryGroupById(vehicle, batteryStatus.id); + } + break; + } +} + +void VehicleBatteryFactGroup::handleMessage(Vehicle* vehicle, mavlink_message_t& message) +{ + switch (message.msgid) { + case MAVLINK_MSG_ID_HIGH_LATENCY: + _handleHighLatency(vehicle, message); + break; + case MAVLINK_MSG_ID_HIGH_LATENCY2: + _handleHighLatency2(vehicle, message); + break; + case MAVLINK_MSG_ID_BATTERY_STATUS: + _handleBatteryStatus(vehicle, message); + break; + } +} + +void VehicleBatteryFactGroup::_handleHighLatency(Vehicle* vehicle, mavlink_message_t& message) +{ + mavlink_high_latency_t highLatency; + mavlink_msg_high_latency_decode(&message, &highLatency); + + VehicleBatteryFactGroup* group = _findOrAddBatteryGroupById(vehicle, 0); + group->percentRemaining()->setRawValue(highLatency.battery_remaining == UINT8_MAX ? qQNaN() : highLatency.battery_remaining); +} + +void VehicleBatteryFactGroup::_handleHighLatency2(Vehicle* vehicle, mavlink_message_t& message) +{ + mavlink_high_latency2_t highLatency2; + mavlink_msg_high_latency2_decode(&message, &highLatency2); + + VehicleBatteryFactGroup* group = _findOrAddBatteryGroupById(vehicle, 0); + group->percentRemaining()->setRawValue(highLatency2.battery == -1 ? qQNaN() : highLatency2.battery); +} + +void VehicleBatteryFactGroup::_handleBatteryStatus(Vehicle* vehicle, mavlink_message_t& message) +{ + mavlink_battery_status_t batteryStatus; + mavlink_msg_battery_status_decode(&message, &batteryStatus); + + VehicleBatteryFactGroup* group = _findOrAddBatteryGroupById(vehicle, batteryStatus.id); + + double totalVoltage = qQNaN(); + for (int i=0; i<10; i++) { + double cellVoltage = batteryStatus.voltages[i] == UINT16_MAX ? qQNaN() : static_cast(batteryStatus.voltages[i]) / 1000.0; + if (qIsNaN(cellVoltage)) { + break; + } + if (i == 0) { + totalVoltage = cellVoltage; + } else { + totalVoltage += cellVoltage; + } + } + + group->function()->setRawValue (batteryStatus.battery_function); + group->type()->setRawValue (batteryStatus.type); + group->temperature()->setRawValue (batteryStatus.temperature == INT16_MAX ? qQNaN() : static_cast(batteryStatus.temperature) / 100.0); + group->voltage()->setRawValue (totalVoltage); + group->current()->setRawValue (batteryStatus.current_battery == -1 ? qQNaN() : batteryStatus.current_battery); + group->mahConsumed()->setRawValue (batteryStatus.current_consumed == -1 ? qQNaN() : batteryStatus.current_consumed); + group->percentRemaining()->setRawValue (batteryStatus.battery_remaining == -1 ? qQNaN() : batteryStatus.battery_remaining); + group->timeRemaining()->setRawValue (batteryStatus.time_remaining == 0 ? qQNaN() : batteryStatus.time_remaining); + group->chargeState()->setRawValue (batteryStatus.charge_state); + group->instantPower()->setRawValue (totalVoltage * group->current()->rawValue().toDouble()); +} + +VehicleBatteryFactGroup* VehicleBatteryFactGroup::_findOrAddBatteryGroupById(Vehicle* vehicle, uint8_t batteryId) +{ + QmlObjectListModel* batteries = vehicle->batteries(); + + // We maintain the list in order sorted by battery id so the ui shows them sorted. + for (int i=0; icount(); i++) { + VehicleBatteryFactGroup* group = batteries->value(i); + int listBatteryId = group->id()->rawValue().toInt(); + if (listBatteryId > batteryId) { + VehicleBatteryFactGroup* newBatteryGroup = new VehicleBatteryFactGroup(batteryId, batteries); + batteries->insert(i, newBatteryGroup); + vehicle->_addFactGroup(newBatteryGroup, QStringLiteral("%1%2").arg(_batteryFactGroupNamePrefix).arg(batteryId)); + return newBatteryGroup; + } else if (listBatteryId == batteryId) { + return group; + } + } + + VehicleBatteryFactGroup* newBatteryGroup = new VehicleBatteryFactGroup(batteryId, batteries); + batteries->append(newBatteryGroup); + vehicle->_addFactGroup(newBatteryGroup, QStringLiteral("%1%2").arg(_batteryFactGroupNamePrefix).arg(batteryId)); + + return newBatteryGroup; +} + +void VehicleBatteryFactGroup::_timeRemainingChanged(QVariant value) +{ + if (qIsNaN(value.toDouble())) { + _timeRemainingStrFact.setRawValue("--:--:--"); + } else { + int totalSeconds = value.toInt(); + int hours = totalSeconds / 3600; + int minutes = (totalSeconds % 3600) / 60; + int seconds = totalSeconds % 60; + + _timeRemainingStrFact.setRawValue(QString::asprintf("%02dH:%02dM:%02dS", hours, minutes, seconds)); + } +} diff --git a/src/Vehicle/VehicleBatteryFactGroup.h b/src/Vehicle/VehicleBatteryFactGroup.h new file mode 100644 index 000000000..3b678b45a --- /dev/null +++ b/src/Vehicle/VehicleBatteryFactGroup.h @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "FactGroup.h" +#include "QGCMAVLink.h" + +class Vehicle; + +class VehicleBatteryFactGroup : public FactGroup +{ + Q_OBJECT + +public: + VehicleBatteryFactGroup(uint8_t batteryId, QObject* parent = nullptr); + + Q_PROPERTY(Fact* id READ id 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) + + Fact* id () { return &_batteryIdFact; } + 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; } + + static const char* _batteryIdFactName; + static const char* _batteryFunctionFactName; + static const char* _batteryTypeFactName; + static const char* _temperatureFactName; + static const char* _voltageFactName; + static const char* _currentFactName; + static const char* _mahConsumedFactName; + static const char* _percentRemainingFactName; + static const char* _timeRemainingFactName; + static const char* _timeRemainingStrFactName; + static const char* _chargeStateFactName; + static const char* _instantPowerFactName; + + static const char* _settingsGroup; + + /// Creates a new fact group for the battery id as needed and updates the Vehicle with it + static void handleMessageForFactGroupCreation(Vehicle* vehicle, mavlink_message_t& message); + + // Overrides from FactGroup + void handleMessage(Vehicle* vehicle, mavlink_message_t& message) override; + +private slots: + void _timeRemainingChanged(QVariant value); + +private: + static void _handleHighLatency (Vehicle* vehicle, mavlink_message_t& message); + static void _handleHighLatency2 (Vehicle* vehicle, mavlink_message_t& message); + static void _handleBatteryStatus (Vehicle* vehicle, mavlink_message_t& message); + static VehicleBatteryFactGroup* _findOrAddBatteryGroupById (Vehicle* vehicle, uint8_t batteryId); + + Fact _batteryIdFact; + Fact _batteryFunctionFact; + Fact _batteryTypeFact; + Fact _voltageFact; + Fact _currentFact; + Fact _mahConsumedFact; + Fact _temperatureFact; + Fact _percentRemainingFact; + Fact _timeRemainingFact; + Fact _timeRemainingStrFact; + Fact _chargeStateFact; + Fact _instantPowerFact; + + static const char* _batteryFactGroupNamePrefix; +}; diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 354421710..5786cadd1 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -176,6 +176,7 @@ void MockLink::_run1HzTasks(void) _sendHighLatency2(); } else { _sendVibration(); + _sendBatteryStatus(); _sendSysStatus(); _sendADSBVehicles(); if (!qgcApp()->runningUnitTests()) { @@ -360,9 +361,6 @@ void MockLink::_sendHighLatency2(void) void MockLink::_sendSysStatus(void) { - if(_batteryRemaining > 50) { - _batteryRemaining = static_cast(100 - (_runningTime.elapsed() / 1000)); - } mavlink_message_t msg; mavlink_msg_sys_status_pack_chan( _vehicleSystemId, @@ -375,11 +373,87 @@ void MockLink::_sendSysStatus(void) 250, // load 4200 * 4, // voltage_battery 8000, // current_battery - _batteryRemaining, // battery_remaining + _battery1PctRemaining, // battery_remaining 0,0,0,0,0,0); respondWithMavlinkMessage(msg); } +void MockLink::_sendBatteryStatus(void) +{ + if(_battery1PctRemaining > 1) { + _battery1PctRemaining = static_cast(100 - (_runningTime.elapsed() / 1000)); + _battery1TimeRemaining = static_cast(_batteryMaxTimeRemaining) * (static_cast(_battery1PctRemaining) / 100.0); + if (_battery1PctRemaining > 50) { + _battery1ChargeState = MAV_BATTERY_CHARGE_STATE_OK; + } else if (_battery1PctRemaining > 30) { + _battery1ChargeState = MAV_BATTERY_CHARGE_STATE_LOW; + } else if (_battery1PctRemaining > 20) { + _battery1ChargeState = MAV_BATTERY_CHARGE_STATE_CRITICAL; + } else { + _battery1ChargeState = MAV_BATTERY_CHARGE_STATE_EMERGENCY; + } + } + if(_battery2PctRemaining > 1) { + _battery2PctRemaining = static_cast(100 - ((_runningTime.elapsed() / 1000) / 2)); + _battery2TimeRemaining = static_cast(_batteryMaxTimeRemaining) * (static_cast(_battery2PctRemaining) / 100.0); + if (_battery2PctRemaining > 50) { + _battery2ChargeState = MAV_BATTERY_CHARGE_STATE_OK; + } else if (_battery2PctRemaining > 30) { + _battery2ChargeState = MAV_BATTERY_CHARGE_STATE_LOW; + } else if (_battery2PctRemaining > 20) { + _battery2ChargeState = MAV_BATTERY_CHARGE_STATE_CRITICAL; + } else { + _battery2ChargeState = MAV_BATTERY_CHARGE_STATE_EMERGENCY; + } + } + + mavlink_message_t msg; + uint16_t rgVoltages[10]; + uint16_t rgVoltagesNone[10]; + + for (int i=0; i<10; i++) { + rgVoltages[i] = UINT16_MAX; + rgVoltagesNone[i] = UINT16_MAX; + } + rgVoltages[0] = rgVoltages[1] = rgVoltages[2] = 4200; + + mavlink_msg_battery_status_pack_chan( + _vehicleSystemId, + _vehicleComponentId, + static_cast(_mavlinkChannel), + &msg, + 1, // battery id + MAV_BATTERY_FUNCTION_ALL, + MAV_BATTERY_TYPE_LIPO, + 2100, // temp cdegC + rgVoltages, + 600, // battery cA + 100, // current consumed mAh + -1, // energy consumed not supported + _battery1PctRemaining, + _battery1TimeRemaining, + _battery1ChargeState); + respondWithMavlinkMessage(msg); + + mavlink_msg_battery_status_pack_chan( + _vehicleSystemId, + _vehicleComponentId, + static_cast(_mavlinkChannel), + &msg, + 2, // battery id + MAV_BATTERY_FUNCTION_ALL, + MAV_BATTERY_TYPE_LIPO, + INT16_MAX, // temp cdegC + rgVoltagesNone, + 600, // battery cA + 100, // current consumed mAh + -1, // energy consumed not supported + _battery2PctRemaining, + _battery2TimeRemaining, + _battery2ChargeState); + respondWithMavlinkMessage(msg); +} + void MockLink::_sendVibration(void) { mavlink_message_t msg; diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index ed75463ec..f7e4dc5cb 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -224,6 +224,7 @@ private: void _sendGpsRawInt (void); void _sendVibration (void); void _sendSysStatus (void); + void _sendBatteryStatus (void); void _sendStatusTextMessages (void); void _sendChunkedStatusText (uint16_t chunkId, bool missingChunks); void _respondWithAutopilotVersion (void); @@ -257,8 +258,14 @@ private: uint32_t _mavCustomMode; uint8_t _mavState; - QElapsedTimer _runningTime; - int8_t _batteryRemaining = 100; + QElapsedTimer _runningTime; + static const int32_t _batteryMaxTimeRemaining = 15 * 60; + int8_t _battery1PctRemaining = 100; + int32_t _battery1TimeRemaining = _batteryMaxTimeRemaining; + MAV_BATTERY_CHARGE_STATE _battery1ChargeState = MAV_BATTERY_CHARGE_STATE_OK; + int8_t _battery2PctRemaining = 100; + int32_t _battery2TimeRemaining = _batteryMaxTimeRemaining; + MAV_BATTERY_CHARGE_STATE _battery2ChargeState = MAV_BATTERY_CHARGE_STATE_OK; MAV_AUTOPILOT _firmwareType; MAV_TYPE _vehicleType; diff --git a/src/ui/preferences/GeneralSettings.qml b/src/ui/preferences/GeneralSettings.qml index 55fafcff3..e9d2a9b99 100644 --- a/src/ui/preferences/GeneralSettings.qml +++ b/src/ui/preferences/GeneralSettings.qml @@ -30,7 +30,6 @@ Rectangle { anchors.fill: parent anchors.margins: ScreenTools.defaultFontPixelWidth - property Fact _percentRemainingAnnounce: QGroundControl.settingsManager.appSettings.batteryPercentRemainingAnnounce property Fact _savePath: QGroundControl.settingsManager.appSettings.savePath property Fact _appFontPointSize: QGroundControl.settingsManager.appSettings.appFontPointSize property Fact _userBrandImageIndoor: QGroundControl.settingsManager.brandImageSettings.userBrandImageIndoor @@ -623,28 +622,6 @@ Rectangle { } } } - - RowLayout { - visible: QGroundControl.settingsManager.appSettings.batteryPercentRemainingAnnounce.visible - - QGCCheckBox { - id: announcePercentCheckbox - text: qsTr("Announce battery lower than") - checked: _percentRemainingAnnounce.value !== 0 - onClicked: { - if (checked) { - _percentRemainingAnnounce.value = _percentRemainingAnnounce.defaultValueString - } else { - _percentRemainingAnnounce.value = 0 - } - } - } - FactTextField { - fact: _percentRemainingAnnounce - Layout.preferredWidth: _valueFieldWidth - enabled: announcePercentCheckbox.checked - } - } } } diff --git a/src/ui/toolbar/BatteryIndicator.qml b/src/ui/toolbar/BatteryIndicator.qml index a83a6be07..f164b16c5 100644 --- a/src/ui/toolbar/BatteryIndicator.qml +++ b/src/ui/toolbar/BatteryIndicator.qml @@ -15,6 +15,7 @@ import QGroundControl.Controls 1.0 import QGroundControl.MultiVehicleManager 1.0 import QGroundControl.ScreenTools 1.0 import QGroundControl.Palette 1.0 +import MAVLink 1.0 //------------------------------------------------------------------------- //-- Battery Indicator @@ -24,103 +25,187 @@ Item { anchors.bottom: parent.bottom width: batteryIndicatorRow.width + Component.onCompleted: console.log("mavlink", MAVLink.MAV_BATTERY_CHARGE_STATE_CRITICAL, MAVLink.MAV_BATTERY_FUNCTION_ALL) + property bool showIndicator: true - function getBatteryColor() { - if(activeVehicle) { - if(activeVehicle.battery.percentRemaining.value > 75) { - return qgcPal.text - } - if(activeVehicle.battery.percentRemaining.value > 50) { - return qgcPal.colorOrange - } - if(activeVehicle.battery.percentRemaining.value > 0.1) { - return qgcPal.colorRed + Row { + id: batteryIndicatorRow + anchors.top: parent.top + anchors.bottom: parent.bottom + + Repeater { + model: activeVehicle ? activeVehicle.batteries : 0 + + Loader { + anchors.top: parent.top + anchors.bottom: parent.bottom + sourceComponent: batteryVisual + + property var battery: object } } - return qgcPal.colorGrey } + MouseArea { + anchors.fill: parent + onClicked: { + mainWindow.showIndicatorPopup(_root, batteryPopup) + } + } + + Component { + id: batteryVisual + + Row { + anchors.top: parent.top + anchors.bottom: parent.bottom - function getBatteryPercentageText() { - if(activeVehicle) { - if(activeVehicle.battery.percentRemaining.value > 98.9) { - return "100%" + function getBatteryColor() { + switch (battery.chargeState.rawValue) { + case MAVLink.MAV_BATTERY_CHARGE_STATE_OK: + return qgcPal.text + case MAVLink.MAV_BATTERY_CHARGE_STATE_LOW: + return qgcPal.colorOrange + case MAVLink.MAV_BATTERY_CHARGE_STATE_CRITICAL: + case MAVLink.MAV_BATTERY_CHARGE_STATE_EMERGENCY: + case MAVLink.MAV_BATTERY_CHARGE_STATE_FAILED: + case MAVLink.MAV_BATTERY_CHARGE_STATE_UNHEALTHY: + return qgcPal.colorRed + default: + return qgcPal.text + } } - if(activeVehicle.battery.percentRemaining.value > 0.1) { - return activeVehicle.battery.percentRemaining.valueString + activeVehicle.battery.percentRemaining.units + + function getBatteryPercentageText() { + if (!isNaN(battery.percentRemaining.rawValue)) { + if (battery.percentRemaining.rawValue > 98.9) { + return qsTr("100%") + } else { + return battery.percentRemaining.valueString + battery.percentRemaining.units + } + } else if (!isNaN(battery.voltage.rawValue)) { + return battery.voltage.valueString + battery.voltage.units + } else if (battery.chargeState.rawValue !== MAVLink.MAV_BATTERY_CHARGE_STATE_UNDEFINED) { + return battery.chargeState.enumStringValue + } + return "" } - if(activeVehicle.battery.voltage.value >= 0) { - return activeVehicle.battery.voltage.valueString + activeVehicle.battery.voltage.units + + QGCColoredImage { + anchors.top: parent.top + anchors.bottom: parent.bottom + width: height + sourceSize.width: width + source: "/qmlimages/Battery.svg" + fillMode: Image.PreserveAspectFit + color: getBatteryColor() + } + + QGCLabel { + text: getBatteryPercentageText() + font.pointSize: ScreenTools.mediumFontPointSize + color: getBatteryColor() + anchors.verticalCenter: parent.verticalCenter } } - return "N/A" } Component { - id: batteryInfo + id: batteryValuesAvailableComponent + + QtObject { + property bool functionAvailable: battery.function.rawValue !== MAVLink.MAV_BATTERY_FUNCTION_UNKNOWN + property bool temperatureAvailable: !isNaN(battery.temperature.rawValue) + property bool currentAvailable: !isNaN(battery.current.rawValue) + property bool mahConsumedAvailable: !isNaN(battery.mahConsumed.rawValue) + property bool timeRemainingAvailable: !isNaN(battery.timeRemaining.rawValue) + property bool chargeStateAvailable: battery.chargeState.rawValue !== MAVLink.MAV_BATTERY_CHARGE_STATE_UNDEFINED + } + } + + Component { + id: batteryPopup Rectangle { - width: battCol.width + ScreenTools.defaultFontPixelWidth * 3 - height: battCol.height + ScreenTools.defaultFontPixelHeight * 2 - radius: ScreenTools.defaultFontPixelHeight * 0.5 - color: qgcPal.window + width: mainLayout.width + mainLayout.anchors.margins * 2 + height: mainLayout.height + mainLayout.anchors.margins * 2 + radius: ScreenTools.defaultFontPixelHeight / 2 + color: qgcPal.window border.color: qgcPal.text - Column { - id: battCol - spacing: ScreenTools.defaultFontPixelHeight * 0.5 - width: Math.max(battGrid.width, battLabel.width) - anchors.margins: ScreenTools.defaultFontPixelHeight - anchors.centerIn: parent + ColumnLayout { + id: mainLayout + anchors.margins: ScreenTools.defaultFontPixelWidth + anchors.top: parent.top + anchors.right: parent.right + spacing: ScreenTools.defaultFontPixelHeight QGCLabel { - id: battLabel - text: qsTr("Battery Status") - font.family: ScreenTools.demiboldFontFamily - anchors.horizontalCenter: parent.horizontalCenter + Layout.alignment: Qt.AlignCenter + text: qsTr("Battery Status") + font.family: ScreenTools.demiboldFontFamily } - GridLayout { - id: battGrid - anchors.margins: ScreenTools.defaultFontPixelHeight - columnSpacing: ScreenTools.defaultFontPixelWidth - columns: 2 - anchors.horizontalCenter: parent.horizontalCenter - - QGCLabel { text: qsTr("Voltage:") } - QGCLabel { text: (activeVehicle && activeVehicle.battery.voltage.value !== -1) ? (activeVehicle.battery.voltage.valueString + " " + activeVehicle.battery.voltage.units) : "N/A" } - QGCLabel { text: qsTr("Accumulated Consumption:") } - QGCLabel { text: (activeVehicle && activeVehicle.battery.mahConsumed.value !== -1) ? (activeVehicle.battery.mahConsumed.valueString + " " + activeVehicle.battery.mahConsumed.units) : "N/A" } + RowLayout { + spacing: ScreenTools.defaultFontPixelWidth + + ColumnLayout { + Repeater { + model: activeVehicle ? activeVehicle.batteries : 0 + + ColumnLayout { + spacing: 0 + + property var batteryValuesAvailable: nameAvailableLoader.item + + Loader { + id: nameAvailableLoader + sourceComponent: batteryValuesAvailableComponent + + property var battery: object + } + + QGCLabel { text: qsTr("Battery %1").arg(object.id.rawValue) } + QGCLabel { text: qsTr("Charge State"); visible: batteryValuesAvailable.chargeStateAvailable } + QGCLabel { text: qsTr("Remaining"); visible: batteryValuesAvailable.timeRemainingAvailable } + QGCLabel { text: qsTr("Remaining") } + QGCLabel { text: qsTr("Voltage") } + QGCLabel { text: qsTr("Consumed"); visible: batteryValuesAvailable.mahConsumedAvailable } + QGCLabel { text: qsTr("Temperature"); visible: batteryValuesAvailable.temperatureAvailable } + QGCLabel { text: qsTr("Function"); visible: batteryValuesAvailable.functionAvailable } + } + } + } + + ColumnLayout { + Repeater { + model: activeVehicle ? activeVehicle.batteries : 0 + + ColumnLayout { + spacing: 0 + + property var batteryValuesAvailable: valueAvailableLoader.item + + Loader { + id: valueAvailableLoader + sourceComponent: batteryValuesAvailableComponent + + property var battery: object + } + + QGCLabel { text: "" } + QGCLabel { text: object.chargeState.enumStringValue; visible: batteryValuesAvailable.chargeStateAvailable } + QGCLabel { text: object.timeRemainingStr.value; visible: batteryValuesAvailable.timeRemainingAvailable } + QGCLabel { text: object.percentRemaining.valueString + " " + object.percentRemaining.units } + QGCLabel { text: object.voltage.valueString + " " + object.voltage.units } + QGCLabel { text: object.mahConsumed.valueString + " " + object.mahConsumed.units; visible: batteryValuesAvailable.mahConsumedAvailable } + QGCLabel { text: object.temperature.valueString + " " + object.temperature.units; visible: batteryValuesAvailable.temperatureAvailable } + QGCLabel { text: object.function.enumStringValue; visible: batteryValuesAvailable.functionAvailable } + } + } + } } } } } - - Row { - id: batteryIndicatorRow - anchors.top: parent.top - anchors.bottom: parent.bottom - opacity: (activeVehicle && activeVehicle.battery.voltage.value >= 0) ? 1 : 0.5 - QGCColoredImage { - anchors.top: parent.top - anchors.bottom: parent.bottom - width: height - sourceSize.width: width - source: "/qmlimages/Battery.svg" - fillMode: Image.PreserveAspectFit - color: qgcPal.text - } - QGCLabel { - text: getBatteryPercentageText() - font.pointSize: ScreenTools.mediumFontPointSize - color: getBatteryColor() - anchors.verticalCenter: parent.verticalCenter - } - } - MouseArea { - anchors.fill: parent - onClicked: { - mainWindow.showIndicatorPopup(_root, batteryInfo) - } - } } -- 2.22.0