diff --git a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp index 2bf4821c02..5f5f517e9f 100644 --- a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp +++ b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp @@ -1221,11 +1221,20 @@ void TelemetryImpl::process_battery_status(const mavlink_message_t& message) static_cast(NAN) : bat_status.temperature * 1e-2f; // cdegC to degC new_battery.voltage_v = 0.0f; - for (int i = 0; i < 255; i++) { - if (bat_status.voltages[i] == std::numeric_limits::max()) + for (int i = 0; i < 10; ++i) { + if (bat_status.voltages[i] == std::numeric_limits::max()) { break; + } new_battery.voltage_v += static_cast(bat_status.voltages[i]) * 1e-3f; } + + for (int i = 0; i < 4; ++i) { + // A value of 1 means 0 mV. + if (bat_status.voltages_ext[i] > 1) { + new_battery.voltage_v += static_cast(bat_status.voltages_ext[i]) * 1e-3f; + } + } + new_battery.remaining_percent = bat_status.battery_remaining; new_battery.current_battery_a = (bat_status.current_battery == -1) ? static_cast(NAN) :