AP_Battery: add param _ESC_ID to write to ESC_Telem

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -496,6 +496,10 @@ void AP_BattMonitor::read()
             drivers[i]->read();
             drivers[i]->update_resistance_estimate();
 
+#if AP_BATTERY_ESC_TELEM_OUTBOUND_ENABLED
+            drivers[i]->update_esc_telem_outbound();
+#endif
+
 #if HAL_LOGGING_ENABLED
             if (logger != nullptr && logger->should_log(_log_battery_bit)) {
                 const uint64_t time_us = AP_HAL::micros64();

libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp

@@ -18,6 +18,10 @@
 #include "AP_BattMonitor.h"
 #include "AP_BattMonitor_Backend.h"
 
+#if AP_BATTERY_ESC_TELEM_OUTBOUND_ENABLED
+#include "AP_ESC_Telem/AP_ESC_Telem.h"
+#endif
+
 /*
   base class constructor.
   This incorporates initialisation as well.
@@ -233,6 +237,42 @@ void AP_BattMonitor_Backend::check_failsafe_types(bool &low_voltage, bool &low_c
     }
 }
 
+#if AP_BATTERY_ESC_TELEM_OUTBOUND_ENABLED
+void AP_BattMonitor_Backend::update_esc_telem_outbound()
+{
+    const uint8_t esc_index = _params._esc_telem_outbound_index;
+    if (esc_index == 0 || !_state.healthy) {
+        // Disabled if there's no ESC identified to route the data to or if the battery is unhealthy
+        return;
+    }
+
+    AP_ESC_Telem_Backend::TelemetryData telem {};
+
+    uint16_t type = AP_ESC_Telem_Backend::TelemetryType::VOLTAGE;
+    telem.voltage = _state.voltage; // all battery backends have voltage
+
+    if (has_current()) {
+        telem.current = _state.current_amps;
+        type |= AP_ESC_Telem_Backend::TelemetryType::CURRENT;
+    }
+
+    if (has_consumed_energy()) {
+        telem.consumption_mah = _state.consumed_mah;
+        type |= AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION;
+    }
+
+    float temperature_c;
+    if (_mon.get_temperature(temperature_c, _state.instance)) {
+        // get the temperature from the frontend so we check for external temperature
+        telem.temperature_cdeg = temperature_c * 100;
+        type |= AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE;
+    }
+
+    AP::esc_telem().update_telem_data(esc_index-1, telem, type);
+}
+#endif
+
+
 /*
   default implementation for reset_remaining(). This sets consumed_wh
   and consumed_mah based on the given percentage. Use percentage=100

libraries/AP_BattMonitor/AP_BattMonitor_Backend.h

@@ -81,6 +81,9 @@ public:
     // set desired MPPT powered state (enabled/disabled)
     virtual void mppt_set_powered_state(bool power_on) {};
 
+    // Update an ESC telemetry channel's power information
+    void update_esc_telem_outbound();
+
     // amps: current (A)
     // dt_us: time between samples (micro-seconds)
     static float calculate_mah(float amps, float dt_us) { return (float) (amps * dt_us * AUS_TO_MAH); }

libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp

@@ -151,6 +151,16 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
     AP_GROUPINFO("OPTIONS", 21, AP_BattMonitor_Params, _options, 0),
 #endif // HAL_BUILD_AP_PERIPH
 
+#if AP_BATTERY_ESC_TELEM_OUTBOUND_ENABLED
+    // @Param: ESC_INDEX
+    // @DisplayName: ESC Telemetry Index to write to
+    // @Description: ESC Telemetry Index to write voltage, current, consumption and temperature data to. Use 0 to disable.
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("ESC_INDEX", 22, AP_BattMonitor_Params, _esc_telem_outbound_index, 0),
+#endif
+
     AP_GROUPEND
 
 };

libraries/AP_BattMonitor/AP_BattMonitor_Params.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <AP_Param/AP_Param.h>
+#include "AP_BattMonitor_config.h"
 
 class AP_BattMonitor_Params {
 public:
@@ -43,4 +44,7 @@ public:
     AP_Int8  _failsafe_voltage_source;  /// voltage type used for detection of low voltage event
     AP_Int8  _failsafe_low_action;      /// action to preform on a low battery failsafe
     AP_Int8  _failsafe_critical_action; /// action to preform on a critical battery failsafe
+#if AP_BATTERY_ESC_TELEM_OUTBOUND_ENABLED
+    AP_Int8  _esc_telem_outbound_index; /// bitmask of ESCs to forward voltage, current, consumption and temperature to.
+#endif
 };

libraries/AP_BattMonitor/AP_BattMonitor_config.h

@@ -30,6 +30,10 @@
 #define AP_BATTERY_ESC_ENABLED AP_BATTERY_BACKEND_DEFAULT_ENABLED && HAL_WITH_ESC_TELEM
 #endif
 
+#ifndef AP_BATTERY_ESC_TELEM_OUTBOUND_ENABLED
+#define AP_BATTERY_ESC_TELEM_OUTBOUND_ENABLED 0
+#endif
+
 #ifndef AP_BATTERY_SMBUS_ENABLED
 #define AP_BATTERY_SMBUS_ENABLED AP_BATTERY_BACKEND_DEFAULT_ENABLED
 #endif