From f754d23a0fe5c77e1d69570c467f1f535f947c7d Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 24 Jun 2016 08:08:53 +0200 Subject: [PATCH] mavlink receiver: fixed computation of cell count Signed-off-by: Roman --- src/modules/mavlink/mavlink_receiver.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8cbcdf682d..b973cf3629 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -258,6 +258,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) case MAVLINK_MSG_ID_GPS_RTCM_DATA: handle_message_gps_rtcm_data(msg); break; + case MAVLINK_MSG_ID_BATTERY_STATUS: handle_message_battery_status(msg); break; @@ -1169,13 +1170,10 @@ MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg) float voltage_sum = 0.0f; uint8_t cell_count = 0; - for (unsigned i = 0; i < 10;i++) { - if (battery_mavlink.voltages[i] < UINT16_MAX) { - voltage_sum += (float)(battery_mavlink.voltages[i]) / 1000.0f; - } else { - cell_count = i; - break; - } + + while (battery_mavlink.voltages[cell_count] < UINT16_MAX && cell_count < 10) { + voltage_sum += (float)(battery_mavlink.voltages[cell_count]) / 1000.0f; + cell_count++; } battery_status.voltage_v = voltage_sum;