Browse Source

GCS_Mavlink: constrain battery current to avoid wrap

c415-sdk
Peter Hall 5 years ago committed by Peter Barker
parent
commit
b1742b4e19
  1. 6
      libraries/GCS_MAVLink/GCS_Common.cpp

6
libraries/GCS_MAVLink/GCS_Common.cpp

@ -217,7 +217,7 @@ void GCS_MAVLINK::send_battery_status(const uint8_t instance) const
float current, consumed_mah, consumed_wh; float current, consumed_mah, consumed_wh;
if (battery.current_amps(current, instance)) { if (battery.current_amps(current, instance)) {
current *= 100; current = constrain_float(current * 100,-INT16_MAX,INT16_MAX);
} else { } else {
current = -1; current = -1;
} }
@ -2031,7 +2031,7 @@ void GCS_MAVLINK::send_battery2()
if (battery.num_instances() > 1) { if (battery.num_instances() > 1) {
float current; float current;
if (battery.current_amps(current, 1)) { if (battery.current_amps(current, 1)) {
current *= 100; // 10*mA current = constrain_float(current * 100,-INT16_MAX,INT16_MAX); // 10*mA
} else { } else {
current = -1; current = -1;
} }
@ -4167,7 +4167,7 @@ void GCS_MAVLINK::send_sys_status()
if (battery.healthy() && battery.current_amps(battery_current)) { if (battery.healthy() && battery.current_amps(battery_current)) {
battery_remaining = battery.capacity_remaining_pct(); battery_remaining = battery.capacity_remaining_pct();
battery_current *= 100; battery_current = constrain_float(battery_current * 100,-INT16_MAX,INT16_MAX);
} else { } else {
battery_current = -1; battery_current = -1;
battery_remaining = -1; battery_remaining = -1;

Loading…
Cancel
Save