From 06acf3cddc7e16e11dbea91940ae8b685d548329 Mon Sep 17 00:00:00 2001 From: DoinLakeFlyer Date: Fri, 7 Feb 2020 09:28:21 -0800 Subject: [PATCH] Switch to using BATTERY_STATUS for battery info --- src/Vehicle/Vehicle.cc | 126 +++++++++++++++++------------------------ src/Vehicle/Vehicle.h | 8 --- 2 files changed, 51 insertions(+), 83 deletions(-) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 558e761eb..6d92bde63 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -1536,32 +1536,6 @@ void Vehicle::_handleSysStatus(mavlink_message_t& message) mavlink_sys_status_t sysStatus; mavlink_msg_sys_status_decode(&message, &sysStatus); - if (sysStatus.current_battery == -1) { - _battery1FactGroup.current()->setRawValue(VehicleBatteryFactGroup::_currentUnavailable); - } else { - // Current is in Amps, current_battery is 10 * milliamperes (1 = 10 milliampere) - _battery1FactGroup.current()->setRawValue((float)sysStatus.current_battery / 100.0f); - } - if (sysStatus.voltage_battery == UINT16_MAX) { - _battery1FactGroup.voltage()->setRawValue(VehicleBatteryFactGroup::_voltageUnavailable); - } else { - _battery1FactGroup.voltage()->setRawValue((double)sysStatus.voltage_battery / 1000.0); - // current_battery is 10 mA and voltage_battery is 1mV. (10/1e3 times 1/1e3 = 1/1e5) - _battery1FactGroup.instantPower()->setRawValue((float)(sysStatus.current_battery*sysStatus.voltage_battery)/(100000.0)); - } - _battery1FactGroup.percentRemaining()->setRawValue(sysStatus.battery_remaining); - - //-- Low battery warning - if (sysStatus.battery_remaining > 0) { - int warnThreshold = _settingsManager->appSettings()->batteryPercentRemainingAnnounce()->rawValue().toInt(); - if (sysStatus.battery_remaining < warnThreshold && - sysStatus.battery_remaining < _lastAnnouncedLowBatteryPercent && - _lastBatteryAnnouncement.elapsed() > (sysStatus.battery_remaining < warnThreshold * 0.5 ? 15000 : 30000)) { - _say(tr("%1 low battery: %2 percent remaining").arg(_vehicleIdSpeech()).arg(sysStatus.battery_remaining)); - _lastBatteryAnnouncement.start(); - _lastAnnouncedLowBatteryPercent = sysStatus.battery_remaining; - } - } if (_onboardControlSensorsPresent != sysStatus.onboard_control_sensors_present) { _onboardControlSensorsPresent = sysStatus.onboard_control_sensors_present; @@ -1596,44 +1570,54 @@ void Vehicle::_handleBatteryStatus(mavlink_message_t& message) mavlink_battery_status_t bat_status; mavlink_msg_battery_status_decode(&message, &bat_status); - VehicleBatteryFactGroup& batteryFactGroup = bat_status.id == 0 ? _battery1FactGroup : _battery2FactGroup; - - if (bat_status.temperature == INT16_MAX) { - batteryFactGroup.temperature()->setRawValue(VehicleBatteryFactGroup::_temperatureUnavailable); - } else { - batteryFactGroup.temperature()->setRawValue(static_cast(bat_status.temperature)/100.0); - } - if (bat_status.current_consumed == -1) { - batteryFactGroup.mahConsumed()->setRawValue(VehicleBatteryFactGroup::_mahConsumedUnavailable); + VehicleBatteryFactGroup* pBatteryFactGroup = nullptr; + if (bat_status.id == 0) { + pBatteryFactGroup = &_battery1FactGroup; + } else if (bat_status.id == 1) { + pBatteryFactGroup = &_battery2FactGroup; } else { - batteryFactGroup.mahConsumed()->setRawValue(bat_status.current_consumed); + return; } int cellCount = 0; + double voltage = qQNaN(); for (int i=0; i<10; i++) { - if (bat_status.voltages[i] != UINT16_MAX) { - cellCount++; + double cellVoltage = bat_status.voltages[i] == UINT16_MAX ? qQNaN() : (double)bat_status.voltages[i] / 1000.0; + if (qIsNaN(cellVoltage)) { + break; + } + cellCount++; + if (i == 0) { + voltage = cellVoltage; + } else { + voltage += cellVoltage; } } - if (cellCount == 0) { - cellCount = -1; - } + pBatteryFactGroup->cellCount()->setRawValue(cellCount == 0 ? qQNaN() : cellCount); - batteryFactGroup.cellCount()->setRawValue(cellCount); - //-- Time remaining in seconds (0 means not supported) - batteryFactGroup.timeRemaining()->setRawValue(bat_status.time_remaining); - //-- Battery charge state (0 means not supported) - if(bat_status.charge_state <= MAV_BATTERY_CHARGE_STATE_UNHEALTHY) { - batteryFactGroup.chargeState()->setRawValue(bat_status.charge_state); - } else { - batteryFactGroup.chargeState()->setRawValue(0); + 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); + + //-- 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; + } + } } - //-- TODO: Somewhere, actions would be taken based on this chargeState: - // MAV_BATTERY_CHARGE_STATE_CRITICAL: Battery state is critical, return / abort immediately - // MAV_BATTERY_CHARGE_STATE_EMERGENCY: Battery state is too low for ordinary abortion, fastest possible emergency stop preventing damage - // MAV_BATTERY_CHARGE_STATE_FAILED: Battery failed, damage unavoidable - // MAV_BATTERY_CHARGE_STATE_UNHEALTHY: Battery is diagnosed to be broken or an error occurred, usage is discouraged / prohibited } void Vehicle::_setHomePosition(QGeoCoordinate& homeCoord) @@ -4264,24 +4248,16 @@ const char* VehicleBatteryFactGroup::_chargeStateFactName = "cha const char* VehicleBatteryFactGroup::_settingsGroup = "Vehicle.battery"; -const double VehicleBatteryFactGroup::_voltageUnavailable = -1.0; -const int VehicleBatteryFactGroup::_percentRemainingUnavailable = -1; -const int VehicleBatteryFactGroup::_mahConsumedUnavailable = -1; -const int VehicleBatteryFactGroup::_currentUnavailable = -1; -const double VehicleBatteryFactGroup::_temperatureUnavailable = -1.0; -const int VehicleBatteryFactGroup::_cellCountUnavailable = -1.0; -const double VehicleBatteryFactGroup::_instantPowerUnavailable = -1.0; - VehicleBatteryFactGroup::VehicleBatteryFactGroup(QObject* parent) : FactGroup(1000, ":/json/Vehicle/BatteryFact.json", parent) , _voltageFact (0, _voltageFactName, FactMetaData::valueTypeDouble) - , _percentRemainingFact (0, _percentRemainingFactName, FactMetaData::valueTypeInt32) - , _mahConsumedFact (0, _mahConsumedFactName, FactMetaData::valueTypeInt32) - , _currentFact (0, _currentFactName, FactMetaData::valueTypeFloat) + , _percentRemainingFact (0, _percentRemainingFactName, FactMetaData::valueTypeDouble) + , _mahConsumedFact (0, _mahConsumedFactName, FactMetaData::valueTypeDouble) + , _currentFact (0, _currentFactName, FactMetaData::valueTypeDouble) , _temperatureFact (0, _temperatureFactName, FactMetaData::valueTypeDouble) - , _cellCountFact (0, _cellCountFactName, FactMetaData::valueTypeInt32) - , _instantPowerFact (0, _instantPowerFactName, FactMetaData::valueTypeFloat) - , _timeRemainingFact (0, _timeRemainingFactName, FactMetaData::valueTypeInt32) + , _cellCountFact (0, _cellCountFactName, FactMetaData::valueTypeDouble) + , _instantPowerFact (0, _instantPowerFactName, FactMetaData::valueTypeDouble) + , _timeRemainingFact (0, _timeRemainingFactName, FactMetaData::valueTypeDouble) , _chargeStateFact (0, _chargeStateFactName, FactMetaData::valueTypeUint8) { _addFact(&_voltageFact, _voltageFactName); @@ -4295,13 +4271,13 @@ VehicleBatteryFactGroup::VehicleBatteryFactGroup(QObject* parent) _addFact(&_chargeStateFact, _chargeStateFactName); // Start out as not available - _voltageFact.setRawValue (_voltageUnavailable); - _percentRemainingFact.setRawValue (_percentRemainingUnavailable); - _mahConsumedFact.setRawValue (_mahConsumedUnavailable); - _currentFact.setRawValue (_currentUnavailable); - _temperatureFact.setRawValue (_temperatureUnavailable); - _cellCountFact.setRawValue (_cellCountUnavailable); - _instantPowerFact.setRawValue (_instantPowerUnavailable); + _voltageFact.setRawValue (qQNaN()); + _percentRemainingFact.setRawValue (qQNaN()); + _mahConsumedFact.setRawValue (qQNaN()); + _currentFact.setRawValue (qQNaN()); + _temperatureFact.setRawValue (qQNaN()); + _cellCountFact.setRawValue (qQNaN()); + _instantPowerFact.setRawValue (qQNaN()); } const char* VehicleWindFactGroup::_directionFactName = "direction"; diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 1376fce9e..10585e084 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -283,14 +283,6 @@ public: static const char* _settingsGroup; - static const double _voltageUnavailable; - static const int _percentRemainingUnavailable; - static const int _mahConsumedUnavailable; - static const int _currentUnavailable; - static const double _temperatureUnavailable; - static const int _cellCountUnavailable; - static const double _instantPowerUnavailable; - private: Fact _voltageFact; Fact _percentRemainingFact; -- 2.22.0