aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GCS_Common : Mavlink wrap when battery percentage is above 100

Browse Source 
When calculated battery percentage is above 100 we wrap it to INT8_MAX
This commit is contained in:
Shiv Tyagi 2021-09-21 19:23:24 +05:30 committed by Andrew Tridgell
parent 3ae8dfead4
commit 5f94986af1
2 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/GCS_MAVLink/GCS.h
View File

@ -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

18
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -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;