From 892d544c46e4a9029d8d4cc38bffaed5a66c3853 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 14 Aug 2023 13:57:17 +1200 Subject: [PATCH 1/2] Revert "Account for maximum 255 batteries, as limited by the mavlink specs" This reverts commit a8737b9284979edf7623adee1b831ae38e4feb60. Signed-off-by: Julian Oes --- src/mavsdk/plugins/telemetry/telemetry_impl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp index 2bf4821c02..ef27d74642 100644 --- a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp +++ b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp @@ -1221,7 +1221,7 @@ 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++) { + 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; From a09f836b4fcd9e9a5bd045ff1fe237e3fd7de0a2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 14 Aug 2023 14:09:31 +1200 Subject: [PATCH 2/2] telemetry: use extended cells for voltage calc Otherwise, the overall voltage is wrong. Signed-off-by: Julian Oes --- src/mavsdk/plugins/telemetry/telemetry_impl.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp index ef27d74642..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 < 10; 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) :