From c539d5f9c7ab535b76bc0ca3baf1961ea115bdd3 Mon Sep 17 00:00:00 2001 From: Pradeep CK Date: Tue, 30 Jul 2024 11:00:40 +1000 Subject: [PATCH] AP_ESC_Telem: addition of ESC extended status message - ESCX structure definition - Backend function to update Extended telemetry data (ESCX) - Logging ESCX data - ESCX conditional compilation directives --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 84 +++++++++++++++++++ libraries/AP_ESC_Telem/AP_ESC_Telem.h | 11 +++ libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h | 10 +++ libraries/AP_ESC_Telem/AP_ESC_Telem_config.h | 8 ++ 4 files changed, 113 insertions(+) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 9a3e34f84251e..9f15e0c9d2df4 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -305,6 +305,47 @@ bool AP_ESC_Telem::get_usage_seconds(uint8_t esc_index, uint32_t& usage_s) const return true; } +#if AP_EXTENDED_ESC_TELEM_ENABLED +// get an individual ESC's input duty if available, returns true on success +bool AP_ESC_Telem::get_input_duty(uint8_t esc_index, uint8_t& input_duty) const +{ + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; + if (esc_index >= ESC_TELEM_MAX_ESCS + || telemdata.stale() + || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY)) { + return false; + } + input_duty = _telem_data[esc_index].input_duty; + return true; +} + +// get an individual ESC's output duty if available, returns true on success +bool AP_ESC_Telem::get_output_duty(uint8_t esc_index, uint8_t& output_duty) const +{ + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; + if (esc_index >= ESC_TELEM_MAX_ESCS + || telemdata.stale() + || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY)) { + return false; + } + output_duty = _telem_data[esc_index].output_duty; + return true; +} + +// get an individual ESC's status flags if available, returns true on success +bool AP_ESC_Telem::get_flags(uint8_t esc_index, uint32_t& flags) const +{ + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; + if (esc_index >= ESC_TELEM_MAX_ESCS + || telemdata.stale() + || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::FLAGS)) { + return false; + } + flags = _telem_data[esc_index].flags; + return true; +} +#endif // AP_EXTENDED_ESC_TELEM_ENABLED + // send ESC telemetry messages over MAVLink void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan) { @@ -468,6 +509,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); @@ -607,6 +660,37 @@ void AP_ESC_Telem::update() error_rate : rpmdata.error_rate }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); + +#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 + 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----", + "QBBBI", + AP_HAL::micros64(), + uint8_t(i), + uint8_t(telemdata.input_duty), + uint8_t(telemdata.output_duty), + uint32_t(telemdata.flags)); + } +#endif //AP_EXTENDED_ESC_TELEM_ENABLED _last_telem_log_ms[i] = telemdata.last_update_ms; _last_rpm_log_us[i] = rpmdata.last_update_us; } diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h index 4c5d0719915b8..a91c9259af432 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h @@ -67,6 +67,17 @@ class AP_ESC_Telem { // get an individual ESC's consumption in milli-Ampere.hour if available, returns true on success bool get_consumption_mah(uint8_t esc_index, float& consumption_mah) const; +#if AP_EXTENDED_ESC_TELEM_ENABLED + // get an individual ESC's input duty if available, returns true on success + bool get_input_duty(uint8_t esc_index, uint8_t& input_duty) const; + + // get an individual ESC's output duty if available, returns true on success + bool get_output_duty(uint8_t esc_index, uint8_t& output_duty) const; + + // get an individual ESC's status flags if available, returns true on success + bool get_flags(uint8_t esc_index, uint32_t& flags) const; +#endif // AP_EXTENDED_ESC_TELEM_ENABLED + // return the average motor frequency in Hz for dynamic filtering float get_average_motor_frequency_hz(uint32_t servo_channel_mask) const { return get_average_motor_rpm(servo_channel_mask) * (1.0f / 60.0f); }; diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h index e46eb48ceb6b0..a93f70d852408 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 @@ class AP_ESC_Telem_Backend { 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 @@ class AP_ESC_Telem_Backend { 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 fdd9cea2feba1..79ca012b1d45a 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 \ No newline at end of file