diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index f6044e8ea3..4389510893 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/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(); diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp index 6eccda0078..729d5d0247 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp +++ b/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 diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h index 1506e4d6a2..84ec427fe8 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h +++ b/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); } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index 60c9a8ac67..00a5de0c80 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/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 }; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h index 7b3223f3ee..9366965328 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h @@ -1,6 +1,7 @@ #pragma once #include +#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 }; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_config.h b/libraries/AP_BattMonitor/AP_BattMonitor_config.h index 8a6e370673..7b14fea17f 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_config.h +++ b/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