diff --git a/src/modules/mavlink/streams/BATTERY_STATUS.hpp b/src/modules/mavlink/streams/BATTERY_STATUS.hpp index 5824c534eb..055dce5e2e 100644 --- a/src/modules/mavlink/streams/BATTERY_STATUS.hpp +++ b/src/modules/mavlink/streams/BATTERY_STATUS.hpp @@ -72,10 +72,13 @@ private: bat_msg.id = battery_status.id - 1; bat_msg.battery_function = MAV_BATTERY_FUNCTION_ALL; bat_msg.type = MAV_BATTERY_TYPE_LIPO; - bat_msg.current_consumed = (battery_status.connected) ? battery_status.discharged_mah : -1; + bat_msg.current_consumed = (battery_status.connected + && fabsf(battery_status.discharged_mah + 1.f) > FLT_EPSILON) ? battery_status.discharged_mah : -1; bat_msg.energy_consumed = -1; - bat_msg.current_battery = (battery_status.connected) ? battery_status.current_a * 100 : -1; - bat_msg.battery_remaining = (battery_status.connected) ? roundf(battery_status.remaining * 100.f) : -1; + bat_msg.current_battery = (battery_status.connected + && fabsf(battery_status.current_a + 1.f) > FLT_EPSILON) ? battery_status.current_a * 100 : -1; + bat_msg.battery_remaining = (battery_status.connected + && fabsf(battery_status.remaining + 1.f) > FLT_EPSILON) ? roundf(battery_status.remaining * 100.f) : -1; // 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; diff --git a/src/modules/mavlink/streams/SYS_STATUS.hpp b/src/modules/mavlink/streams/SYS_STATUS.hpp index 7a89501793..fbde745dc2 100644 --- a/src/modules/mavlink/streams/SYS_STATUS.hpp +++ b/src/modules/mavlink/streams/SYS_STATUS.hpp @@ -170,8 +170,8 @@ private: if (lowest_battery.connected) { msg.voltage_battery = lowest_battery.voltage_v * 1000.0f; - msg.current_battery = lowest_battery.current_a * 100.0f; - msg.battery_remaining = ceilf(lowest_battery.remaining * 100.0f); + msg.current_battery = (fabsf(lowest_battery.current_a + 1.f) > FLT_EPSILON) ? lowest_battery.current_a * 100.0f : -1; + msg.battery_remaining = (fabsf(lowest_battery.remaining + 1.f) > FLT_EPSILON) ? ceilf(lowest_battery.remaining * 100.0f) : -1; } else { msg.voltage_battery = UINT16_MAX;