Skip to content

Commit

Permalink
battery_status: clearly define and handle zero remaining flight time
Browse files Browse the repository at this point in the history
  • Loading branch information
MaEtUgR committed Nov 11, 2021
1 parent b5d1636 commit 47329a0
Show file tree
Hide file tree
Showing 3 changed files with 4 additions and 4 deletions.
2 changes: 1 addition & 1 deletion msg/battery_status.msg
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +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 time_remaining_s # predicted time in seconds remaining until battery is empty under previous averaged load, NAN if unknown
float32 temperature # temperature of the battery. NaN if unknown
int32 cell_count # Number of cells

Expand Down
2 changes: 1 addition & 1 deletion src/lib/battery/battery.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -249,7 +249,7 @@ void Battery::computeScale()

float Battery::computeRemainingTime(float current_a)
{
float time_remaining_s = -1.f;
float time_remaining_s{NAN};

// Only estimate remaining time with useful in flight current measurements
if (_current_filter_a.getState() > 1.f) {
Expand Down
4 changes: 2 additions & 2 deletions src/modules/mavlink/streams/BATTERY_STATUS.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -76,8 +76,8 @@ class MavlinkStreamBatteryStatus : public MavlinkStream
bat_msg.energy_consumed = -1;
bat_msg.current_battery = (battery_status.connected) ? battery_status.current_filtered_a * 100 : -1;
bat_msg.battery_remaining = (battery_status.connected) ? ceilf(battery_status.remaining * 100.f) : -1;
// MAVLink extension: 0 is unsupported, in uORB it's -1
bat_msg.time_remaining = (battery_status.connected && (battery_status.time_remaining_s >= 0.f)) ?
// MAVLink extension: 0 is unsupported, in uORB it's NAN
bat_msg.time_remaining = (battery_status.connected && (PX4_ISFINITE(battery_status.time_remaining_s))) ?
math::max((int)battery_status.time_remaining_s, 1) : 0;

switch (battery_status.warning) {
Expand Down

0 comments on commit 47329a0

Please sign in to comment.