From 9da04c79f4c5af46fbbce25633060c592758ed21 Mon Sep 17 00:00:00 2001 From: Pradeep CK Date: Tue, 25 Jun 2024 15:41:14 +1000 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_DroneCAN/AP_DroneCAN.cpp | 3 +- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 29 ++++++++++++++----- libraries/AP_ESC_Telem/AP_ESC_Telem.h | 2 ++ libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h | 4 +++ libraries/AP_ESC_Telem/LogStructure.h | 6 ++-- libraries/AP_HAL/AP_HAL_Boards.h | 4 +++ 6 files changed, 36 insertions(+), 12 deletions(-) diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index 958eacf3f3aa78..027ce8e48453c8 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -1453,7 +1453,7 @@ void AP_DroneCAN::handle_ESC_status(const CanardRxTransfer& transfer, const uavc */ void AP_DroneCAN::handle_esc_ext_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_StatusExtended& msg) { -#if HAL_WITH_ESC_TELEM +#if HAL_WITH_ESC_TELEM && AP_EXTENDED_ESC_TELEM_ENABLE const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, DRONECAN_SRV_NUMBER); const uint8_t esc_index = msg.esc_index + esc_offset; @@ -1476,7 +1476,6 @@ void AP_DroneCAN::handle_esc_ext_status(const CanardRxTransfer& transfer, const #endif } - bool AP_DroneCAN::is_esc_data_index_valid(const uint8_t index) { if (index > DRONECAN_SRV_NUMBER) { // printf("DroneCAN: invalid esc index: %d. max index allowed: %d\n\r", index, DRONECAN_SRV_NUMBER); diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 10dbd00aa1e61c..821d17ff391cfd 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -305,6 +305,7 @@ bool AP_ESC_Telem::get_usage_seconds(uint8_t esc_index, uint32_t& usage_s) const return true; } +#if AP_EXTENDED_ESC_TELEM_ENABLE // 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 { @@ -340,6 +341,7 @@ bool AP_ESC_Telem::get_flags(uint8_t esc_index, uint32_t& flags) const flags = _telem_data[esc_index].flags; return true; } +#endif // AP_EXTENDED_ESC_TELEM_ENABLE // send ESC telemetry messages over MAVLink void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan) @@ -513,6 +515,18 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem _telem_data[esc_index].flags = new_data.flags; } +#if AP_EXTENDED_ESC_TELEM_ENABLE + 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_ENABLE + #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); @@ -653,12 +667,13 @@ void AP_ESC_Telem::update() }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); +#if AP_EXTENDED_ESC_TELEM_ENABLE // 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 = _telem_data[i].types & + 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); @@ -667,15 +682,15 @@ void AP_ESC_Telem::update() LOG_PACKET_HEADER_INIT(uint8_t(LOG_ESCX_MSG)), time_us : AP_HAL::micros64(), instance : i, - input_duty : _telem_data[i].input_duty, - output_duty : _telem_data[i].output_duty, - flags : _telem_data[i].flags, + input_duty : telemdata.input_duty, + output_duty : telemdata.output_duty, + flags : telemdata.flags, }; AP::logger().WriteBlock(&pkt2, sizeof(pkt2)); } - - _last_telem_log_ms[i] = _telem_data[i].last_update_ms; - _last_rpm_log_us[i] = _rpm_data[i].last_update_us; +#endif //AP_EXTENDED_ESC_TELEM_ENABLE + _last_telem_log_ms[i] = telemdata.last_update_ms; + _last_rpm_log_us[i] = rpmdata.last_update_us; } #if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h index bffb16343cb45b..42c8f692aa7bb9 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h @@ -67,6 +67,7 @@ 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_ENABLE // 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; @@ -75,6 +76,7 @@ class AP_ESC_Telem { // 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_ENABLE // 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 00a61f75111db5..4eb81e115ea318 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h @@ -23,9 +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_ENABLE uint8_t input_duty; // input duty cycle uint8_t output_duty; // output duty cycle uint32_t flags; // Status flags +#endif // AP_EXTENDED_ESC_TELEM_ENABLE // return true if the data is stale bool stale(uint32_t now_ms=0) const volatile; @@ -53,9 +55,11 @@ class AP_ESC_Telem_Backend { EDT2_STATUS = 1 << 8, EDT2_STRESS = 1 << 9, #endif +#if AP_EXTENDED_ESC_TELEM_ENABLE INPUT_DUTY = 1 << 10, OUTPUT_DUTY = 1 << 11, FLAGS = 1 << 12 +#endif // AP_EXTENDED_ESC_TELEM_ENABLE }; diff --git a/libraries/AP_ESC_Telem/LogStructure.h b/libraries/AP_ESC_Telem/LogStructure.h index 92c74ee88db9e8..dea9c5ffdff40f 100644 --- a/libraries/AP_ESC_Telem/LogStructure.h +++ b/libraries/AP_ESC_Telem/LogStructure.h @@ -78,8 +78,8 @@ struct PACKED log_EscX { #define LOG_STRUCTURE_FROM_ESC_TELEM \ { LOG_ESC_MSG, sizeof(log_Esc), \ "ESC", "QBeeffcfcf", "TimeUS,Instance,RPM,RawRPM,Volt,Curr,Temp,CTot,MotTemp,Err", "s#qqvAOaO%", "F-BB--BCB-" , true }, \ - { LOG_ESCX_MSG, sizeof(log_EscX), \ - "ESCX", "QBBBI", "TimeUS,Instance,inpct,outpct,flags", "s#%%-", "F----" , true },\ { LOG_EDT2_MSG, sizeof(log_Edt2), \ - "EDT2", "QBBBB", "TimeUS,Instance,Stress,MaxStress,Status", "s#---", "F----" , true }, + "EDT2", "QBBBB", "TimeUS,Instance,Stress,MaxStress,Status", "s#---", "F----" , true }, \ + { LOG_ESCX_MSG, sizeof(log_EscX), \ + "ESCX", "QBBBI", "TimeUS,Instance,inpct,outpct,flags", "s#%%-", "F----" , true }, diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index cd9dab25655eb2..c59f44885d9590 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -188,6 +188,10 @@ #define AP_EXTENDED_DSHOT_TELEM_V2_ENABLED HAL_REQUIRES_BDSHOT_SUPPORT #endif +#ifndef AP_EXTENDED_ESC_TELEM_ENABLE +#define AP_EXTENDED_ESC_TELEM_ENABLE 0 +#endif + // this is used as a general mechanism to make a 'small' build by // dropping little used features. We use this to allow us to keep // FMUv2 going for as long as possible