diff --git a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp index 4ab5b606a4..b1004787be 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,36 @@ 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 = (std::numeric_limits::max() == bat_status.voltage) ? NAN : bat_status.voltage * 1e-3f; + 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.*/ + + // Not exposed: temperature, capacity_remaining, capacity_consumed + + 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); diff --git a/third_party/mavlink/CMakeLists.txt b/third_party/mavlink/CMakeLists.txt index 132aa3866d..a7e386364e 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 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/daabb9a54d1fc3b8cc6a4f3517168d381d278678 PREFIX mavlink CONFIGURE_COMMAND Python3::Interpreter -m pymavlink.tools.mavgen