From 5f94986af1ca6cc5d85dde32fea825e79fa09754 Mon Sep 17 00:00:00 2001 From: Shiv Tyagi Date: Tue, 21 Sep 2021 19:23:24 +0530 Subject: [PATCH] GCS_Common : Mavlink wrap when battery percentage is above 100 When calculated battery percentage is above 100 we wrap it to INT8_MAX --- libraries/GCS_MAVLink/GCS.h | 2 ++ libraries/GCS_MAVLink/GCS_Common.cpp | 18 ++++++++++++------ 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index f2e0981fc1..b939c37d4e 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -281,6 +281,8 @@ public: void send_generator_status() const; virtual void send_winch_status() const {}; void send_water_depth() const; + int8_t battery_remaining_pct(const uint8_t instance) const; + #if HAL_HIGH_LATENCY2_ENABLED void send_high_latency() const; #endif // HAL_HIGH_LATENCY2_ENABLED diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 8fe132653b..8b72d7ae00 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -211,6 +211,12 @@ void GCS_MAVLINK::send_mcu_status(void) } #endif +// returns the battery remaining percentage if valid, -1 otherwise +int8_t GCS_MAVLINK::battery_remaining_pct(const uint8_t instance) const { + uint8_t percentage; + return AP::battery().capacity_remaining_pct(percentage, instance) ? MIN(percentage, INT8_MAX) : -1; +} + void GCS_MAVLINK::send_battery_status(const uint8_t instance) const { // catch the battery backend not supporting the required number of cells @@ -256,6 +262,8 @@ void GCS_MAVLINK::send_battery_status(const uint8_t instance) const } float current, consumed_mah, consumed_wh; + const int8_t percentage = battery_remaining_pct(instance); + if (battery.current_amps(current, instance)) { current = constrain_float(current * 100,-INT16_MAX,INT16_MAX); } else { @@ -269,8 +277,6 @@ void GCS_MAVLINK::send_battery_status(const uint8_t instance) const } else { consumed_wh = -1; } - uint8_t _percentage = -1; - const int8_t percentage = battery.capacity_remaining_pct(_percentage, instance) ? _percentage : -1; mavlink_msg_battery_status_send(chan, instance, // id @@ -4671,10 +4677,10 @@ void GCS_MAVLINK::send_sys_status() const AP_BattMonitor &battery = AP::battery(); float battery_current; - uint8_t battery_remaining = -1; + int8_t battery_remaining = -1; if (battery.healthy() && battery.current_amps(battery_current)) { - IGNORE_RETURN(battery.capacity_remaining_pct(battery_remaining)); + battery_remaining = battery_remaining_pct(AP_BATT_PRIMARY_INSTANCE); battery_current = constrain_float(battery_current * 100,-INT16_MAX,INT16_MAX); } else { battery_current = -1; @@ -5574,10 +5580,10 @@ void GCS_MAVLINK::send_high_latency() const const AP_BattMonitor &battery = AP::battery(); float battery_current; - uint8_t battery_remaining = -1; + int8_t battery_remaining = -1; if (battery.healthy() && battery.current_amps(battery_current)) { - IGNORE_RETURN(battery.capacity_remaining_pct(battery_remaining)); + battery_remaining = battery_remaining_pct(AP_BATT_PRIMARY_INSTANCE); battery_current = constrain_float(battery_current * 100,-INT16_MAX,INT16_MAX); } else { battery_current = -1;