Skip to content

Q
qgroundcontrol
Commit 06acf3cd authored by DoinLakeFlyer's avatar DoinLakeFlyer
Browse files
Options

parent 1d5c87d7
Hide whitespace changes
Inline Side-by-side
Showing with 51 additions and 83 deletions
src/Vehicle/Vehicle.cc
View file @ 06acf3cd
......@@ -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<double>(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<double>(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";
......
src/Vehicle/Vehicle.h
View file @ 06acf3cd
......@@ -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;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment