From 54f2e91775013a2138ea609a87cc0177f0bb3a18 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 25 Jun 2021 12:13:44 +0200 Subject: [PATCH] battery_status: report remaining flight time in seconds This allows more accurate reporting and is compliant with the MAVLink interface. --- msg/battery_status.msg | 2 +- src/drivers/batt_smbus/batt_smbus.cpp | 2 +- src/drivers/smart_battery/batmon/batmon.cpp | 2 +- src/drivers/uavcan/sensors/battery.cpp | 2 +- src/drivers/uavcan/sensors/cbat.cpp | 2 +- .../uavcan_v1/Subscribers/legacy/LegacyBatteryInfo.hpp | 2 +- src/modules/mavlink/streams/BATTERY_STATUS.hpp | 4 +++- 7 files changed, 9 insertions(+), 7 deletions(-) diff --git a/msg/battery_status.msg b/msg/battery_status.msg index b14be9dccc..e25be115db 100644 --- a/msg/battery_status.msg +++ b/msg/battery_status.msg @@ -8,6 +8,7 @@ float32 current_average_a # Battery current average in amperes, -1 if unknown float32 discharged_mah # Discharged amount in mAh, -1 if unknown float32 remaining # From 1 to 0, -1 if unknown float32 scale # Power scaling factor, >= 1, or -1 if unknown +float32 time_remaining_s # predicted time in seconds remaining until battery is empty under previous averaged load, -1 if unknown float32 temperature # temperature of the battery. NaN if unknown int32 cell_count # Number of cells @@ -18,7 +19,6 @@ uint8 source # Battery source uint8 priority # Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1 uint16 capacity # actual capacity of the battery uint16 cycle_count # number of discharge cycles the battery has experienced -uint16 run_time_to_empty # predicted remaining battery capacity based on the present rate of discharge in min uint16 average_time_to_empty # predicted remaining battery capacity based on the average rate of discharge in min uint16 serial_number # serial number of the battery pack uint16 manufacture_date # manufacture date, part of serial number of the battery pack. formated as: Day + Month×32 + (Year–1980)×512 diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 313d0ec75d..1dfac055b4 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -136,7 +136,7 @@ void BATT_SMBUS::RunImpl() // Read run time to empty (minutes). ret |= _interface->read_word(BATT_SMBUS_RUN_TIME_TO_EMPTY, result); - new_report.run_time_to_empty = result; + new_report.time_remaining_s = result * 60; // Read average time to empty (minutes). ret |= _interface->read_word(BATT_SMBUS_AVERAGE_TIME_TO_EMPTY, result); diff --git a/src/drivers/smart_battery/batmon/batmon.cpp b/src/drivers/smart_battery/batmon/batmon.cpp index eba6ce2469..e3e54db2f2 100644 --- a/src/drivers/smart_battery/batmon/batmon.cpp +++ b/src/drivers/smart_battery/batmon/batmon.cpp @@ -162,7 +162,7 @@ void Batmon::RunImpl() // Read run time to empty (minutes). ret |= _interface->read_word(BATT_SMBUS_RUN_TIME_TO_EMPTY, result); - new_report.run_time_to_empty = result; + new_report.time_remaining_s = result * 60; // Read average time to empty (minutes). ret |= _interface->read_word(BATT_SMBUS_AVERAGE_TIME_TO_EMPTY, result); diff --git a/src/drivers/uavcan/sensors/battery.cpp b/src/drivers/uavcan/sensors/battery.cpp index 23be52437f..eedf4b7c6b 100644 --- a/src/drivers/uavcan/sensors/battery.cpp +++ b/src/drivers/uavcan/sensors/battery.cpp @@ -83,7 +83,7 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure= 0.f)) ? + math::max((int)battery_status.time_remaining_s, 1) : 0; switch (battery_status.warning) { case (battery_status_s::BATTERY_WARNING_NONE):