diff --git a/libraries/AP_RPM/AP_RPM.cpp b/libraries/AP_RPM/AP_RPM.cpp index 11c1bd89e8..de1dc27c96 100644 --- a/libraries/AP_RPM/AP_RPM.cpp +++ b/libraries/AP_RPM/AP_RPM.cpp @@ -199,6 +199,10 @@ void AP_RPM::update(void) } drivers[i]->update(); + +#if AP_RPM_ESC_TELEM_OUTBOUND_ENABLED + drivers[i]->update_esc_telem_outbound(); +#endif } } diff --git a/libraries/AP_RPM/AP_RPM_Params.cpp b/libraries/AP_RPM/AP_RPM_Params.cpp index 8a3419594e..1d58d41eb6 100644 --- a/libraries/AP_RPM/AP_RPM_Params.cpp +++ b/libraries/AP_RPM/AP_RPM_Params.cpp @@ -68,6 +68,16 @@ const AP_Param::GroupInfo AP_RPM_Params::var_info[] = { // @User: Advanced AP_GROUPINFO("ESC_MASK", 7, AP_RPM_Params, esc_mask, 0), +#if AP_RPM_ESC_TELEM_OUTBOUND_ENABLED + // @Param: ESC_INDEX + // @DisplayName: ESC Telemetry Index to write RPM to + // @Description: ESC Telemetry Index to write RPM to. Use 0 to disable. + // @Range: 0 10 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("ESC_INDEX", 8, AP_RPM_Params, esc_telem_outbound_index, 0), +#endif + AP_GROUPEND }; diff --git a/libraries/AP_RPM/AP_RPM_Params.h b/libraries/AP_RPM/AP_RPM_Params.h index c6f5d77adb..591bd76e7f 100644 --- a/libraries/AP_RPM/AP_RPM_Params.h +++ b/libraries/AP_RPM/AP_RPM_Params.h @@ -14,6 +14,7 @@ */ #pragma once #include +#include "AP_RPM_config.h" class AP_RPM_Params { @@ -29,6 +30,9 @@ public: AP_Float minimum; AP_Float quality_min; AP_Int32 esc_mask; +#if AP_RPM_ESC_TELEM_OUTBOUND_ENABLED + AP_Int8 esc_telem_outbound_index; +#endif static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AP_RPM/AP_RPM_config.h b/libraries/AP_RPM/AP_RPM_config.h index e38968c274..fd035381df 100644 --- a/libraries/AP_RPM/AP_RPM_config.h +++ b/libraries/AP_RPM/AP_RPM_config.h @@ -5,6 +5,7 @@ #include #include #include +#include #ifndef AP_RPM_ENABLED #define AP_RPM_ENABLED 1 @@ -41,3 +42,7 @@ #ifndef AP_RPM_GENERATOR_ENABLED #define AP_RPM_GENERATOR_ENABLED AP_RPM_BACKEND_DEFAULT_ENABLED && HAL_GENERATOR_ENABLED #endif + +#ifndef AP_RPM_ESC_TELEM_OUTBOUND_ENABLED +#define AP_RPM_ESC_TELEM_OUTBOUND_ENABLED AP_RPM_BACKEND_DEFAULT_ENABLED && HAL_WITH_ESC_TELEM +#endif diff --git a/libraries/AP_RPM/RPM_Backend.cpp b/libraries/AP_RPM/RPM_Backend.cpp index 9c90fb56a5..a04778fd37 100644 --- a/libraries/AP_RPM/RPM_Backend.cpp +++ b/libraries/AP_RPM/RPM_Backend.cpp @@ -19,6 +19,10 @@ #include "AP_RPM.h" +#if HAL_WITH_ESC_TELEM +#include "AP_ESC_Telem/AP_ESC_Telem.h" +#endif + /* base class constructor. */ @@ -29,4 +33,22 @@ AP_RPM_Backend::AP_RPM_Backend(AP_RPM &_ap_rpm, uint8_t instance, AP_RPM::RPM_St state.instance = instance; } +#if AP_RPM_ESC_TELEM_OUTBOUND_ENABLED +void AP_RPM_Backend::update_esc_telem_outbound() +{ + const uint8_t esc_index = ap_rpm._params[state.instance].esc_telem_outbound_index; + if (esc_index == 0) { + // Disabled if there's no ESC identified to route the data to + return; + } + if (!ap_rpm.healthy(state.instance)) { + // If we're unhealthy don't update the telemetry. Let it + // timeout on it's own instead of showing potentially wrong data + return; + } + + AP::esc_telem().update_rpm(esc_index-1, state.rate_rpm, 0); +} +#endif + #endif // AP_RPM_ENABLED diff --git a/libraries/AP_RPM/RPM_Backend.h b/libraries/AP_RPM/RPM_Backend.h index 7c2726c7ed..1ba4625da5 100644 --- a/libraries/AP_RPM/RPM_Backend.h +++ b/libraries/AP_RPM/RPM_Backend.h @@ -38,6 +38,8 @@ public: return ap_rpm._params[state.instance].pin.get(); } + void update_esc_telem_outbound(); + protected: AP_RPM &ap_rpm;