diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 8c766b4e362f9..08e765f510016 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -27,6 +27,7 @@ #include "AP_BattMonitor_Synthetic_Current.h" #include "AP_BattMonitor_AD7091R5.h" #include "AP_BattMonitor_Scripting.h" +#include "AP_BattMonitor_MAVLink.h" #include @@ -604,6 +605,11 @@ AP_BattMonitor::init() drivers[instance] = NEW_NOTHROW AP_BattMonitor_Scripting(*this, state[instance], _params[instance]); break; #endif // AP_BATTERY_SCRIPTING_ENABLED +#if AP_BATTERY_MAVLINK_ENABLED + case Type::MAVLink: + drivers[instance] = new AP_BattMonitor_MAVLink(*this, state[instance], _params[instance]); + break; +#endif // AP_BATTERY_MAVLINK_ENABLED case Type::NONE: default: break; @@ -1164,6 +1170,20 @@ bool AP_BattMonitor::handle_scripting(uint8_t idx, const BattMonitorScript_State } #endif +/* + pass along a mavlink message (for MAV type) + */ +void AP_BattMonitor::handle_msg(const mavlink_message_t &msg) +{ + + uint8_t i; + for (i=0; i<_num_instances; i++) { + if ((drivers[i] != nullptr) && ((Type)_params[i]._type.get() != Type::NONE)) { + drivers[i]->handle_msg(msg); + } + } +} + namespace AP { AP_BattMonitor &battery() diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index f03e9976320be..166c24e3adec8 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -115,6 +115,7 @@ class AP_BattMonitor EFI = 27, AD7091R5 = 28, Scripting = 29, + MAVLink = 30, }; FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t); @@ -175,6 +176,9 @@ class AP_BattMonitor /// Read the battery voltage and current for all batteries. Should be called at 10hz void read(); + // Pass mavlink data to message handler + void handle_msg(const mavlink_message_t &msg); + // healthy - returns true if monitor is functioning bool healthy(uint8_t instance) const; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h index a33c316d70d7b..2855cb8c81767 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h @@ -35,6 +35,9 @@ class AP_BattMonitor_Backend // initialise virtual void init() {}; + //read the latest battery informations from MAVLink messages + virtual void handle_msg(const mavlink_message_t &msg){} + // read the latest battery voltage virtual void read() = 0; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_MAVLink.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_MAVLink.cpp new file mode 100644 index 0000000000000..d08e4b1ea7241 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_MAVLink.cpp @@ -0,0 +1,69 @@ +#include "AP_BattMonitor_config.h" + +#if AP_BATTERY_MAVLINK_ENABLED + +#include "AP_BattMonitor_MAVLink.h" + + +// Constructor +AP_BattMonitor_MAVLink::AP_BattMonitor_MAVLink(AP_BattMonitor &mon, + AP_BattMonitor::BattMonitor_State &mon_state, + AP_BattMonitor_Params ¶ms) + : AP_BattMonitor_Backend(mon, mon_state, params) +{ + _state.healthy = true; +} + +void AP_BattMonitor_MAVLink::handle_msg(const mavlink_message_t &msg) +{ + // decode MAVlink message + mavlink_battery_status_t packet; + mavlink_msg_battery_status_decode(&msg, &packet); + + _state.voltage = 0.0; + // copy independent cells voltage from MAVLink msg + for (uint8_t i = 0; i < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN; i++) + { + _state.cell_voltages.cells[i] = packet.voltages[i]; + _state.voltage += packet.voltages[i] * 0.001; + } + + // copy battery readings from the MAVLink message + _state.current_amps = packet.current_battery; + _state.consumed_mah = packet.current_consumed; + _state.temperature = packet.temperature * 0.1; + _state.time_remaining = packet.time_remaining; + _state.last_time_micros = AP_HAL::micros(); + _remaining_pct = packet.battery_remaining; +} + +void AP_BattMonitor_MAVLink::read() +{ + update_health(); +} + +// update battery health flag +void AP_BattMonitor_MAVLink::update_health() +{ + uint32_t current_time_micros = AP_HAL::micros(); + if (current_time_micros - _state.last_time_micros > AP_BATTMONITOR_MAVLINK_TIMEOUT_MICROS) + { + _state.healthy = false; + _have_info = false; + return; + } + _state.healthy = true; + _have_info = true; +} + +// capacity_remaining_pct - returns true if the battery % is available and writes to the percentage argument +bool AP_BattMonitor_MAVLink::capacity_remaining_pct(uint8_t &percentage) const +{ + if (_have_info) + { + percentage = _remaining_pct; + } + return _have_info; +} + +#endif // AP_BATTERY_MAVLINK_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_MAVLink.h b/libraries/AP_BattMonitor/AP_BattMonitor_MAVLink.h new file mode 100644 index 0000000000000..91c245aa45aac --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_MAVLink.h @@ -0,0 +1,39 @@ +#pragma once +#if AP_BATTERY_MAVLINK_ENABLED +#include "AP_BattMonitor.h" +#include "AP_BattMonitor_Backend.h" +#define AP_BATTMONITOR_MAVLINK_TIMEOUT_MICROS 5000000 // sensor becomes unhealthy if no successful readings for 5 seconds + +// Base SUI class +class AP_BattMonitor_MAVLink : public AP_BattMonitor_Backend +{ +public: + // Constructor + AP_BattMonitor_MAVLink(AP_BattMonitor &mon, + AP_BattMonitor::BattMonitor_State &mon_state, + AP_BattMonitor_Params ¶ms); + + void handle_msg(const mavlink_message_t &msg) override; + + // read the battery voltage and current. Should be called at 10hz + void read() override; + + // returns true if battery monitor provides current info + bool has_current() const override { return _have_info; } + + // returns true if battery monitor provides individual cell voltage + bool has_cell_voltages() const override { return _have_info; } + + // returns true if battery monitor provides temperature + bool has_temperature() const override { return _have_info; }; + + // capacity_remaining_pct - returns true if the battery % is available and writes to the percentage argument + bool capacity_remaining_pct(uint8_t &percentage) const override WARN_IF_UNUSED; + +private: + bool _have_info; // flag is true if battery MAVlink message is received + uint8_t _remaining_pct; // battery remaining capacity in percentage + void update_health(); // function to update battery health status based on the last message received from the battery +}; + +#endif // AP_BATTERY_MAVLINK_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_config.h b/libraries/AP_BattMonitor/AP_BattMonitor_config.h index e15909c143579..be8c58a81b934 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_config.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_config.h @@ -119,3 +119,7 @@ #ifndef AP_BATTERY_SCRIPTING_ENABLED #define AP_BATTERY_SCRIPTING_ENABLED (AP_SCRIPTING_ENABLED && AP_BATTERY_BACKEND_DEFAULT_ENABLED) #endif + +#ifndef AP_BATTERY_MAVLINK_ENABLED +#define AP_BATTERY_MAVLINK_ENABLED AP_BATTERY_BACKEND_DEFAULT_ENABLED +#endif \ No newline at end of file diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 77369ba55b7cb..54307f4985d89 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4203,6 +4203,12 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) break; #endif +#if AP_BATTERY_MAVLINK_ENABLED + case MAVLINK_MSG_ID_BATTERY_STATUS: + AP::battery().handle_msg(msg); + break; +#endif + #if AP_GPS_ENABLED #if AP_MAVLINK_MSG_HIL_GPS_ENABLED case MAVLINK_MSG_ID_HIL_GPS: