From 773e9fcea1f1e74523bf8a7d9efaad3baf8f640f Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Thu, 26 May 2022 17:22:25 +1000 Subject: [PATCH 1/5] TEMP GIT_TAG update to get mavlink battery status v2 branch --- third_party/mavlink/CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/third_party/mavlink/CMakeLists.txt b/third_party/mavlink/CMakeLists.txt index 132aa3866d..1375b6b258 100644 --- a/third_party/mavlink/CMakeLists.txt +++ b/third_party/mavlink/CMakeLists.txt @@ -18,7 +18,9 @@ find_package(Python3 COMPONENTS Interpreter REQUIRED) ExternalProject_add( mavlink GIT_REPOSITORY https://github.com/mavlink/mavlink - GIT_TAG 3b52eac09c2e37325e4bc49cd2667ea37bf1d7d2 + GIT_TAG a5556816af29e348875cbcc26a1d36b42dc5e921 + # This GIT_TAG will need to update as it is for a temporary branch containing the new battery + # https://github.com/mavlink/mavlink/commit/a5556816af29e348875cbcc26a1d36b42dc5e921 PREFIX mavlink CONFIGURE_COMMAND Python3::Interpreter -m pymavlink.tools.mavgen From 2e5344ba2320a1d4c6281800ce3500914285d8e7 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Thu, 26 May 2022 17:58:26 +1000 Subject: [PATCH 2/5] battery_status_v2 message handling --- .../plugins/telemetry/telemetry_impl.cpp | 41 +++++++++++++++++++ src/mavsdk/plugins/telemetry/telemetry_impl.h | 1 + 2 files changed, 42 insertions(+) diff --git a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp index 4ab5b606a4..6699a37fc9 100644 --- a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp +++ b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp @@ -85,6 +85,11 @@ void TelemetryImpl::init() [this](const mavlink_message_t& message) { process_battery_status(message); }, this); + _parent->register_mavlink_message_handler( + MAVLINK_MSG_ID_BATTERY_STATUS_V2, + [this](const mavlink_message_t& message) { process_battery_status_v2(message); }, + this); + _parent->register_mavlink_message_handler( MAVLINK_MSG_ID_HEARTBEAT, [this](const mavlink_message_t& message) { process_heartbeat(message); }, @@ -1176,6 +1181,42 @@ void TelemetryImpl::process_battery_status(const mavlink_message_t& message) } } +void TelemetryImpl::process_battery_status_v2(const mavlink_message_t& message) +{ + mavlink_battery_status_v2_t bat_status; + mavlink_msg_battery_status_v2_decode(&message, &bat_status); + + _has_bat_status = true; + + Telemetry::Battery new_battery; + new_battery.id = bat_status.id; + new_battery.voltage_v = bat_status.voltage; + //Is it correct to set this to NaN for UINT32_MAX? HOw do you specify = NaN? + + // FIXME: it is strange calling it percent when the range goes from 0 to 1. + // Can we fix this? What about if value not provided? + new_battery.remaining_percent = bat_status.percent_remaining; // to test * 1e-2f; + + //bat_status also has: + //uint32_t current; /*< [mA] Battery current (through all cells/loads). UINT32_MAX: field not provided.*/ + // uint32_t current_consumed; /*< [mAh] Consumed charge (since vehicle powered on). UINT32_MAX: field not provided. Note: For power modules the expectation is that batteries are fully charged before turning on the vehicle.*/ + // uint32_t current_remaining; /*< [mAh] Remaining charge (until empty). UINT32_MAX: field not provided. Note: Power monitors should not set this value.*/ + // uint32_t status_flags; /*< Fault, health, and readiness status indications.*/ + // int16_t temperature; /*< [cdegC] Temperature of the whole battery pack (not internal electronics). INT16_MAX field not provided.*/ + + + set_battery(new_battery); + + { + std::lock_guard lock(_subscription_mutex); + if (_battery_subscription) { + auto callback = _battery_subscription; + auto arg = battery(); + _parent->call_user_callback([callback, arg]() { callback(arg); }); + } + } +} + void TelemetryImpl::process_heartbeat(const mavlink_message_t& message) { if (message.compid != MAV_COMP_ID_AUTOPILOT1) { diff --git a/src/mavsdk/plugins/telemetry/telemetry_impl.h b/src/mavsdk/plugins/telemetry/telemetry_impl.h index 6ee4ccfe20..b67ab2ba6e 100644 --- a/src/mavsdk/plugins/telemetry/telemetry_impl.h +++ b/src/mavsdk/plugins/telemetry/telemetry_impl.h @@ -201,6 +201,7 @@ class TelemetryImpl : public PluginImplBase { void process_fixedwing_metrics(const mavlink_message_t& message); void process_sys_status(const mavlink_message_t& message); void process_battery_status(const mavlink_message_t& message); + void process_battery_status_v2(const mavlink_message_t& message); void process_heartbeat(const mavlink_message_t& message); void process_rc_channels(const mavlink_message_t& message); void process_unix_epoch_time(const mavlink_message_t& message); From e80d2e1eec2fb474da7cdf07f4b8adc021628467 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Thu, 2 Jun 2022 18:07:42 +1000 Subject: [PATCH 3/5] Update the CMakeList with update changelist --- third_party/mavlink/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/third_party/mavlink/CMakeLists.txt b/third_party/mavlink/CMakeLists.txt index 1375b6b258..a7e386364e 100644 --- a/third_party/mavlink/CMakeLists.txt +++ b/third_party/mavlink/CMakeLists.txt @@ -18,9 +18,9 @@ find_package(Python3 COMPONENTS Interpreter REQUIRED) ExternalProject_add( mavlink GIT_REPOSITORY https://github.com/mavlink/mavlink - GIT_TAG a5556816af29e348875cbcc26a1d36b42dc5e921 + GIT_TAG daabb9a54d1fc3b8cc6a4f3517168d381d278678 # This GIT_TAG will need to update as it is for a temporary branch containing the new battery - # https://github.com/mavlink/mavlink/commit/a5556816af29e348875cbcc26a1d36b42dc5e921 + # https://github.com/mavlink/mavlink/commit/daabb9a54d1fc3b8cc6a4f3517168d381d278678 PREFIX mavlink CONFIGURE_COMMAND Python3::Interpreter -m pymavlink.tools.mavgen From 8bc6b6e2d2eb70298c3e56b01933b9a97cfd6be5 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Thu, 2 Jun 2022 18:10:01 +1000 Subject: [PATCH 4/5] Update src/mavsdk/plugins/telemetry/telemetry_impl.cpp --- 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 6699a37fc9..575b2372bf 100644 --- a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp +++ b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp @@ -1190,7 +1190,7 @@ void TelemetryImpl::process_battery_status_v2(const mavlink_message_t& message) Telemetry::Battery new_battery; new_battery.id = bat_status.id; - new_battery.voltage_v = bat_status.voltage; + new_battery.voltage_v = (std::numeric_limits::max() == bat_status.voltage) ? NAN : bat_status.voltage * 1e-3f; //Is it correct to set this to NaN for UINT32_MAX? HOw do you specify = NaN? // FIXME: it is strange calling it percent when the range goes from 0 to 1. From 731b9d5761eeac4ec18eb38c1ff2b231e8385ced Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Thu, 2 Jun 2022 18:48:08 +1000 Subject: [PATCH 5/5] Fix up remaining_percent if empty --- .../plugins/telemetry/telemetry_impl.cpp | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp index 575b2372bf..b1004787be 100644 --- a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp +++ b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp @@ -1191,19 +1191,13 @@ void TelemetryImpl::process_battery_status_v2(const mavlink_message_t& message) Telemetry::Battery new_battery; new_battery.id = bat_status.id; new_battery.voltage_v = (std::numeric_limits::max() == bat_status.voltage) ? NAN : bat_status.voltage * 1e-3f; - //Is it correct to set this to NaN for UINT32_MAX? HOw do you specify = NaN? - - // FIXME: it is strange calling it percent when the range goes from 0 to 1. - // Can we fix this? What about if value not provided? - new_battery.remaining_percent = bat_status.percent_remaining; // to test * 1e-2f; - - //bat_status also has: - //uint32_t current; /*< [mA] Battery current (through all cells/loads). UINT32_MAX: field not provided.*/ - // uint32_t current_consumed; /*< [mAh] Consumed charge (since vehicle powered on). UINT32_MAX: field not provided. Note: For power modules the expectation is that batteries are fully charged before turning on the vehicle.*/ - // uint32_t current_remaining; /*< [mAh] Remaining charge (until empty). UINT32_MAX: field not provided. Note: Power monitors should not set this value.*/ + new_battery.remaining_percent = (std::numeric_limits::max() == bat_status.percent_remaining) ? NAN : bat_status.percent_remaining; + + // To be added + // uint32_t current; /*< [mA] Battery current (through all cells/loads). UINT32_MAX: field not provided.*/ // uint32_t status_flags; /*< Fault, health, and readiness status indications.*/ - // int16_t temperature; /*< [cdegC] Temperature of the whole battery pack (not internal electronics). INT16_MAX field not provided.*/ - + + // Not exposed: temperature, capacity_remaining, capacity_consumed set_battery(new_battery);