Ardupilot-MAVlink battery monitor

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 <AP_HAL/AP_HAL.h>
 
@@ -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()

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;
 

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;
 

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 &params)
+    : 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

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 &params);
+
+    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

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

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: