From 0e53eb74df45b3b8a7d8f4cd0e98a492ed252825 Mon Sep 17 00:00:00 2001 From: Pradeep CK Date: Sat, 27 Jul 2024 20:44:13 +0100 Subject: [PATCH] AP_ESC_Telem: addition of ESC extended status message - Conditional compilation definition : AP_EXTENDED_ESC_TELEM_ENABLE - ESCX log structure - Update functionalities for ESCX status message - ESCX DroneCAN callback --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 49 ++++++++++++++++++- libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h | 10 ++++ libraries/AP_ESC_Telem/AP_ESC_Telem_config.h | 8 +++ 3 files changed, 65 insertions(+), 2 deletions(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 87e77e70c7..1e4240f667 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -492,6 +492,18 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem telemdata.usage_s = new_data.usage_s; } +#if AP_EXTENDED_ESC_TELEM_ENABLED + if (data_mask & AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY) { + _telem_data[esc_index].input_duty = new_data.input_duty; + } + if (data_mask & AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY) { + _telem_data[esc_index].output_duty = new_data.output_duty; + } + if (data_mask & AP_ESC_Telem_Backend::TelemetryType::FLAGS) { + _telem_data[esc_index].flags = new_data.flags; + } +#endif //AP_EXTENDED_ESC_TELEM_ENABLED + #if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED if (data_mask & AP_ESC_Telem_Backend::TelemetryType::EDT2_STATUS) { telemdata.edt2_status = merge_edt2_status(telemdata.edt2_status, new_data.edt2_status); @@ -603,6 +615,10 @@ void AP_ESC_Telem::update() if (telemdata.last_update_ms != _last_telem_log_ms[i] || rpmdata.last_update_us != _last_rpm_log_us[i]) { + // Update last log timestamps + _last_telem_log_ms[i] = telemdata.last_update_ms; + _last_rpm_log_us[i] = rpmdata.last_update_us; + float rpm = AP::logger().quiet_nanf(); get_rpm(i, rpm); float raw_rpm = AP::logger().quiet_nanf(); @@ -631,8 +647,37 @@ void AP_ESC_Telem::update() error_rate : rpmdata.error_rate }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); - _last_telem_log_ms[i] = telemdata.last_update_ms; - _last_rpm_log_us[i] = rpmdata.last_update_us; + +#if AP_EXTENDED_ESC_TELEM_ENABLED + // Write ESC extended status messages + // id: starts from 0 + // input duty: duty cycle input to the ESC in percent + // output duty: duty cycle output to the motor in percent + // status flags: manufacurer-specific status flags + const bool has_ext_data = telemdata.types & + (AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY | + AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY | + AP_ESC_Telem_Backend::TelemetryType::FLAGS); + if (has_ext_data) { + // @LoggerMessage: ESCX + // @Description: ESC extended telemetry data + // @Field: TimeUS: Time since system startup + // @Field: Instance: starts from 0 + // @Field: inpct: input duty cycle in percent + // @Field: outpct: output duty cycle in percent + // @Field: flags: manufacturer-specific status flags + AP::logger().WriteStreaming("ESCX", + "TimeUS,Instance,inpct,outpct,flags", + "s" "#" "%" "%" "-", + "F" "-" "-" "-" "-", + "Q" "B" "B" "B" "I", + AP_HAL::micros64(), + i, + telemdata.input_duty, + telemdata.output_duty, + telemdata.flags); + } +#endif //AP_EXTENDED_ESC_TELEM_ENABLED } #if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h index e46eb48ceb..0fa2e3daba 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h @@ -23,6 +23,11 @@ public: uint16_t edt2_status; // status reported by Extended DShot Telemetry v2 uint16_t edt2_stress; // stress reported in dedicated messages by Extended DShot Telemetry v2 #endif +#if AP_EXTENDED_ESC_TELEM_ENABLED + uint8_t input_duty; // input duty cycle + uint8_t output_duty; // output duty cycle + uint32_t flags; // Status flags +#endif // AP_EXTENDED_ESC_TELEM_ENABLED // return true if the data is stale bool stale(uint32_t now_ms=0) const volatile; @@ -50,6 +55,11 @@ public: EDT2_STATUS = 1 << 8, EDT2_STRESS = 1 << 9, #endif +#if AP_EXTENDED_ESC_TELEM_ENABLED + INPUT_DUTY = 1 << 10, + OUTPUT_DUTY = 1 << 11, + FLAGS = 1 << 12 +#endif // AP_EXTENDED_ESC_TELEM_ENABLED }; diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_config.h b/libraries/AP_ESC_Telem/AP_ESC_Telem_config.h index fdd9cea2fe..f3fb6fac7b 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem_config.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_config.h @@ -6,3 +6,11 @@ #ifndef HAL_WITH_ESC_TELEM #define HAL_WITH_ESC_TELEM ((NUM_SERVO_CHANNELS > 0) && ((HAL_SUPPORT_RCOUT_SERIAL || HAL_MAX_CAN_PROTOCOL_DRIVERS))) #endif + +#ifndef AP_EXTENDED_ESC_TELEM_ENABLED +#define AP_EXTENDED_ESC_TELEM_ENABLED HAL_ENABLE_DRONECAN_DRIVERS +#endif + +#if AP_EXTENDED_ESC_TELEM_ENABLED && !HAL_WITH_ESC_TELEM + #error "AP_EXTENDED_ESC_TELEM_ENABLED requires HAL_WITH_ESC_TELEM" +#endif