From 8195e84c2ad02fcb1bdec5fe5dcf4a22f3d6ac59 Mon Sep 17 00:00:00 2001 From: Pradeep CK Date: Wed, 26 Jun 2024 15:59: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 | 28 +++++++ libraries/AP_DroneCAN/AP_DroneCAN.h | 4 + libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 73 +++++++++++++++++++ libraries/AP_ESC_Telem/AP_ESC_Telem.h | 11 +++ libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h | 10 +++ libraries/AP_ESC_Telem/LogStructure.h | 27 ++++++- libraries/AP_HAL/AP_HAL_Boards.h | 4 + 7 files changed, 153 insertions(+), 4 deletions(-) diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index 986f0f7600c1e8..027ce8e48453c8 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -1448,6 +1448,34 @@ void AP_DroneCAN::handle_ESC_status(const CanardRxTransfer& transfer, const uavc #endif } +/* + handle Extended ESC status message + */ +void AP_DroneCAN::handle_esc_ext_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_StatusExtended& msg) +{ +#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; + + if (!is_esc_data_index_valid(esc_index)) { + return; + } + + TelemetryData telemetryData { + .motor_temp_cdeg = (int16_t)(msg.motor_temperature_degC * 100), + .input_duty = msg.input_pct, + .output_duty = msg.output_pct, + .flags = (uint32_t)msg.status_flags, + }; + + update_telem_data(esc_index, telemetryData, + AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE + | AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY + | AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY + | AP_ESC_Telem_Backend::TelemetryType::FLAGS); +#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_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h index 8b37c2528b89b9..bf666238b0eeab 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.h +++ b/libraries/AP_DroneCAN/AP_DroneCAN.h @@ -326,6 +326,9 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { Canard::ObjCallback esc_status_cb{this, &AP_DroneCAN::handle_ESC_status}; Canard::Subscriber esc_status_listener{esc_status_cb, _driver_index}; + Canard::ObjCallback esc_status_extended_cb{this, &AP_DroneCAN::handle_esc_ext_status}; + Canard::Subscriber esc_status_extended_listener{esc_status_extended_cb, _driver_index}; + Canard::ObjCallback debug_cb{this, &AP_DroneCAN::handle_debug}; Canard::Subscriber debug_listener{debug_cb, _driver_index}; @@ -387,6 +390,7 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { void handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg); void handle_actuator_status_Volz(const CanardRxTransfer& transfer, const com_volz_servo_ActuatorStatus& msg); void handle_ESC_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_Status& msg); + void handle_esc_ext_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_StatusExtended& msg); static bool is_esc_data_index_valid(const uint8_t index); void handle_debug(const CanardRxTransfer& transfer, const uavcan_protocol_debug_LogMessage& msg); void handle_param_get_set_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_GetSetResponse& rsp); diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 9a3e34f84251ee..35fe812a77538d 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -305,6 +305,44 @@ 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 +{ + if (esc_index >= ESC_TELEM_MAX_ESCS + || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS + || !(_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 +{ + if (esc_index >= ESC_TELEM_MAX_ESCS + || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS + || !(_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 +{ + if (esc_index >= ESC_TELEM_MAX_ESCS + || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS + || !(_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_ENABLE + // send ESC telemetry messages over MAVLink void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan) { @@ -468,6 +506,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_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); @@ -607,6 +657,29 @@ void AP_ESC_Telem::update() error_rate : rpmdata.error_rate }; 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 = 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) { + const struct log_EscX pkt2{ + LOG_PACKET_HEADER_INIT(uint8_t(LOG_ESCX_MSG)), + time_us : AP_HAL::micros64(), + instance : i, + input_duty : telemdata.input_duty, + output_duty : telemdata.output_duty, + flags : telemdata.flags, + }; + AP::logger().WriteBlock(&pkt2, sizeof(pkt2)); + } +#endif //AP_EXTENDED_ESC_TELEM_ENABLE _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 4c5d0719915b8f..42c8f692aa7bb9 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_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; + + // 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_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 e46eb48ceb6b03..4eb81e115ea318 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_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; @@ -50,6 +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 c50ac47b95e97f..dea9c5ffdff40f 100644 --- a/libraries/AP_ESC_Telem/LogStructure.h +++ b/libraries/AP_ESC_Telem/LogStructure.h @@ -4,7 +4,8 @@ #define LOG_IDS_FROM_ESC_TELEM \ LOG_ESC_MSG, \ - LOG_EDT2_MSG + LOG_EDT2_MSG, \ + LOG_ESCX_MSG // @LoggerMessage: ESC // @Description: Feedback received from ESCs @@ -58,9 +59,27 @@ struct PACKED log_Edt2 { uint8_t status; }; -#define LOG_STRUCTURE_FROM_ESC_TELEM \ +// @LoggerMessage: ESCX +// @Description: Extended telemetry feedback received from ESCs +// @Field: TimeUS: microseconds since system startup +// @Field: Instance: ESC instance number +// @Field: inpct: input duty cycle +// @Field: outpct: output duty cycle +// @Field: flags: manufacturer-specific status flags +struct PACKED log_EscX { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t instance; + uint8_t input_duty; + uint8_t output_duty; + uint32_t flags; +}; + +#define LOG_STRUCTURE_FROM_ESC_TELEM \ { LOG_ESC_MSG, sizeof(log_Esc), \ - "ESC", "QBffffcfcf", "TimeUS,Instance,RPM,RawRPM,Volt,Curr,Temp,CTot,MotTemp,Err", "s#qqvAOaO%", "F-00--BCB-" , true }, \ + "ESC", "QBeeffcfcf", "TimeUS,Instance,RPM,RawRPM,Volt,Curr,Temp,CTot,MotTemp,Err", "s#qqvAOaO%", "F-BB--BCB-" , 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