From dda740b7094454c45e7e1f2d015c688390eab213 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 Oct 2015 11:35:46 +0200 Subject: [PATCH] MAVLink app: Ensure to report battery status correctly according to MAVLink standards --- src/modules/mavlink/mavlink_messages.cpp | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c6dde06677..fed42bef2b 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -556,7 +556,7 @@ protected: msg.errors_count2 = status.errors_count2; msg.errors_count3 = status.errors_count3; msg.errors_count4 = status.errors_count4; - msg.battery_remaining = (msg.voltage_battery > 0) ? + msg.battery_remaining = (status.condition_battery_voltage_valid) ? status.battery_remaining * 100.0f : -1; _mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg); @@ -571,14 +571,22 @@ protected: if (i < status.battery_cell_count) { bat_msg.voltages[i] = (status.battery_voltage / status.battery_cell_count) * 1000.0f; } else { - bat_msg.voltages[i] = 0; + bat_msg.voltages[i] = UINT16_MAX; } } - bat_msg.current_battery = status.battery_current * 100.0f; - bat_msg.current_consumed = status.battery_discharged_mah; + + if (status.condition_battery_voltage_valid) { + bat_msg.current_battery = (bat_msg.current_battery >= 0.0f) ? + status.battery_current * 100.0f : -1; + bat_msg.current_consumed = (bat_msg.current_consumed >= 0.0f) ? + status.battery_discharged_mah : -1; + bat_msg.battery_remaining = status.battery_remaining * 100.0f; + } else { + bat_msg.current_battery = -1.0f; + bat_msg.current_consumed = -1.0f; + bat_msg.battery_remaining = -1.0f; + } bat_msg.energy_consumed = -1.0f; - bat_msg.battery_remaining = (status.battery_voltage > 0) ? - status.battery_remaining * 100.0f : -1; _mavlink->send_message(MAVLINK_MSG_ID_BATTERY_STATUS, &bat_msg); }