forked from Archive/PX4-Autopilot
mavlink receiver: fixed computation of cell count
Signed-off-by: Roman <bapstr@ethz.ch>
This commit is contained in:
parent
925c340915
commit
f754d23a0f
|
@ -258,6 +258,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
||||||
case MAVLINK_MSG_ID_GPS_RTCM_DATA:
|
case MAVLINK_MSG_ID_GPS_RTCM_DATA:
|
||||||
handle_message_gps_rtcm_data(msg);
|
handle_message_gps_rtcm_data(msg);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_BATTERY_STATUS:
|
case MAVLINK_MSG_ID_BATTERY_STATUS:
|
||||||
handle_message_battery_status(msg);
|
handle_message_battery_status(msg);
|
||||||
break;
|
break;
|
||||||
|
@ -1169,13 +1170,10 @@ MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg)
|
||||||
|
|
||||||
float voltage_sum = 0.0f;
|
float voltage_sum = 0.0f;
|
||||||
uint8_t cell_count = 0;
|
uint8_t cell_count = 0;
|
||||||
for (unsigned i = 0; i < 10;i++) {
|
|
||||||
if (battery_mavlink.voltages[i] < UINT16_MAX) {
|
while (battery_mavlink.voltages[cell_count] < UINT16_MAX && cell_count < 10) {
|
||||||
voltage_sum += (float)(battery_mavlink.voltages[i]) / 1000.0f;
|
voltage_sum += (float)(battery_mavlink.voltages[cell_count]) / 1000.0f;
|
||||||
} else {
|
cell_count++;
|
||||||
cell_count = i;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
battery_status.voltage_v = voltage_sum;
|
battery_status.voltage_v = voltage_sum;
|
||||||
|
|
Loading…
Reference in New Issue