From 9c353d1075d6c50e848cfc51c027d473f25746a3 Mon Sep 17 00:00:00 2001 From: Pradeep CK Date: Thu, 15 Aug 2024 16:12:23 +1000 Subject: [PATCH 01/11] module : DSDL for Extended ESC --- modules/DroneCAN/DSDL | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/DroneCAN/DSDL b/modules/DroneCAN/DSDL index de93d9c8bb..993be80a62 160000 --- a/modules/DroneCAN/DSDL +++ b/modules/DroneCAN/DSDL @@ -1 +1 @@ -Subproject commit de93d9c8bb76de1b093050e3a31e4d4ce539c577 +Subproject commit 993be80a62ec957c01fb41115b83663959a49f46 From ea8cb898822068935b26ea5eaf2c6421a0757e1b Mon Sep 17 00:00:00 2001 From: Pradeep CK Date: Sat, 27 Jul 2024 20:44:13 +0100 Subject: [PATCH 02/11] AP_DroneCAN: 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 | 8 ++++++++ 2 files changed, 36 insertions(+) diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index 810e1ba074..978e0d494e 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -1438,6 +1438,34 @@ void AP_DroneCAN::handle_ESC_status(const CanardRxTransfer& transfer, const uavc #endif } +#if AP_EXTENDED_ESC_TELEM_ENABLED +/* + handle Extended ESC status message + */ +void AP_DroneCAN::handle_esc_ext_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_StatusExtended& msg) +{ + 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 = 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 // AP_EXTENDED_ESC_TELEM_ENABLED + 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 8b37c2528b..b5527fb594 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.h +++ b/libraries/AP_DroneCAN/AP_DroneCAN.h @@ -326,6 +326,11 @@ 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}; +#if AP_EXTENDED_ESC_TELEM_ENABLED + 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}; +#endif + Canard::ObjCallback debug_cb{this, &AP_DroneCAN::handle_debug}; Canard::Subscriber debug_listener{debug_cb, _driver_index}; @@ -387,6 +392,9 @@ 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); +#if AP_EXTENDED_ESC_TELEM_ENABLED + void handle_esc_ext_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_StatusExtended& msg); +#endif 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); From 8733cbcbac161579bc80673758de69c23015e92e Mon Sep 17 00:00:00 2001 From: Pradeep CK Date: Sat, 27 Jul 2024 20:44:13 +0100 Subject: [PATCH 03/11] 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 | 51 +++++++++++++++++-- libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h | 10 ++++ libraries/AP_ESC_Telem/AP_ESC_Telem_config.h | 8 +++ 3 files changed, 66 insertions(+), 3 deletions(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 1f82ee3e8d..daf8f752b3 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -481,6 +481,18 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem _telem_data[esc_index].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 + _telem_data[esc_index].count++; _telem_data[esc_index].types |= data_mask; _telem_data[esc_index].last_update_ms = AP_HAL::millis(); @@ -527,7 +539,11 @@ void AP_ESC_Telem::update() if (_telem_data[i].last_update_ms != _last_telem_log_ms[i] || _rpm_data[i].last_update_us != _last_rpm_log_us[i]) { - float rpm = 0.0f; + // 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 = 0.0f; get_raw_rpm(i, raw_rpm); @@ -555,8 +571,37 @@ void AP_ESC_Telem::update() error_rate : _rpm_data[i].error_rate }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); - _last_telem_log_ms[i] = _telem_data[i].last_update_ms; - _last_rpm_log_us[i] = _rpm_data[i].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 } } #endif // HAL_LOGGING_ENABLED diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h index b6d6725a55..4501cf7394 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h @@ -19,6 +19,11 @@ class AP_ESC_Telem_Backend { uint32_t last_update_ms; // last update time in milliseconds, determines whether active uint16_t types; // telemetry types present uint16_t count; // number of times updated +#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; @@ -42,6 +47,11 @@ class AP_ESC_Telem_Backend { USAGE = 1 << 5, TEMPERATURE_EXTERNAL = 1 << 6, MOTOR_TEMPERATURE_EXTERNAL = 1 << 7, +#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 616b6c99bc..60d7eb8171 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) && !defined(HAL_BUILD_AP_PERIPH))) #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 From 557fdae8f53acb31c2923b4826a0fdb79a517da4 Mon Sep 17 00:00:00 2001 From: Pradeep CK Date: Sat, 27 Jul 2024 20:44:13 +0100 Subject: [PATCH 04/11] Tools: 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 --- Tools/scripts/build_options.py | 2 ++ Tools/scripts/extract_features.py | 2 ++ 2 files changed, 4 insertions(+) diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index ebb2248eaf..1a49c670f9 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -119,6 +119,8 @@ def __init__(self, Feature('ESC', 'PICCOLOCAN', 'HAL_PICCOLO_CAN_ENABLE', 'Enable PiccoloCAN', 0, None), Feature('ESC', 'TORQEEDO', 'HAL_TORQEEDO_ENABLED', 'Enable Torqeedo Motors', 0, None), + Feature('ESC', 'ESC_EXTENDED_TELM', 'AP_EXTENDED_ESC_TELEM_ENABLED', 'Enable Extended ESC Telem', 0, 'DroneCAN'), + Feature('AP_Periph', 'LONG_TEXT', 'HAL_PERIPH_SUPPORT_LONG_CAN_PRINTF', 'Enable extended length text strings', 0, None), Feature('Camera', 'Camera', 'AP_CAMERA_ENABLED', 'Enable Camera Trigger support', 0, None), diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index 5c54076fa2..01db41fdf7 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -61,6 +61,8 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('HAL_EFI_ENABLED', 'AP_EFI::AP_EFI',), ('AP_EFI_{type}_ENABLED', 'AP_EFI_(?P.*)::update',), + ('AP_EXTENDED_ESC_TELEM_ENABLED', r'AP_DroneCAN::handle_esc_ext_status\b',), + ('AP_TEMPERATURE_SENSOR_ENABLED', 'AP_TemperatureSensor::AP_TemperatureSensor',), ('AP_TEMPERATURE_SENSOR_{type}_ENABLED', 'AP_TemperatureSensor_(?P.*)::update',), From ccdf7ab3e2c5c46a186c84c7e354579bd6c1b51f Mon Sep 17 00:00:00 2001 From: Pradeep CK Date: Fri, 16 Aug 2024 11:39:28 +1000 Subject: [PATCH 05/11] AP_ESC: add extended telemetry data fields and log message - additional data fields for ESC backends - add extended telemetry log message - SW-187 --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 38 +++++++++++++++++++++++++ libraries/AP_ESC_Telem/AP_ESC_Telem.h | 11 +++++++ 2 files changed, 49 insertions(+) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index daf8f752b3..2b46e571f6 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -323,6 +323,44 @@ 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 +{ + 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_ENABLED + // send ESC telemetry messages over MAVLink void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan) { diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h index fa8a855729..3e6b8e5d53 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); }; From 428a80507a8c6b40e7e4dd952e8e6506a07483a0 Mon Sep 17 00:00:00 2001 From: Pradeep CK Date: Fri, 16 Aug 2024 11:30:27 +1000 Subject: [PATCH 06/11] LUA : added bindings for extended ESC telem --- libraries/AP_Scripting/docs/docs.lua | 19 +++++++++++++++++++ .../generator/description/bindings.desc | 4 ++++ 2 files changed, 23 insertions(+) diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 497a60b2d8..b02c937a33 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -1936,6 +1936,25 @@ function esc_telem:update_rpm(esc_index, rpm, error_rate) end ---@param scale_factor number -- factor function esc_telem:set_rpm_scale(esc_index, scale_factor) end +-- get the status flags of the last telemetry data for an ESC +---@param instance integer -- (0 is first motor) +---@return uint32_t_ud|nil +function esc_telem:get_flags(instance) end + +-- get the output_duty of last telemetry data for an ESC +---@param instance integer -- (0 is first motor) +---@return integer|nil +function esc_telem:get_output_duty(instance) end + +-- get the input_duty of last telemetry data for an ESC +---@param instance integer -- (0 is first motor) +---@return integer|nil +function esc_telem:get_input_duty(instance) end +-- get the timestamp of last telemetry data for an ESC +---@param instance integer -- (0 is first motor) +---@return uint32_t_ud +function esc_telem:get_last_telem_data_ms(instance) end + -- desc ---@class optical_flow optical_flow = {} diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 92ce1c8025..ce78b90185 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -437,6 +437,10 @@ singleton AP_ESC_Telem method get_usage_seconds boolean uint8_t 0 NUM_SERVO_CHAN singleton AP_ESC_Telem method update_rpm void uint8_t 0 NUM_SERVO_CHANNELS uint16_t'skip_check float'skip_check singleton AP_ESC_Telem method update_telem_data void uint8_t 0 NUM_SERVO_CHANNELS AP_ESC_Telem_Backend::TelemetryData uint16_t'skip_check singleton AP_ESC_Telem method set_rpm_scale void uint8_t 0 NUM_SERVO_CHANNELS float'skip_check +singleton AP_ESC_Telem method get_last_telem_data_ms uint32_t uint8_t 0 NUM_SERVO_CHANNELS +singleton AP_ESC_Telem method get_input_duty boolean uint8_t 0 NUM_SERVO_CHANNELS uint8_t'Null +singleton AP_ESC_Telem method get_output_duty boolean uint8_t 0 NUM_SERVO_CHANNELS uint8_t'Null +singleton AP_ESC_Telem method get_flags boolean uint8_t 0 NUM_SERVO_CHANNELS uint32_t'Null include AP_Param/AP_Param.h singleton AP_Param rename param From 682e33d381276baadd8ec2c17ce4276eebef94a2 Mon Sep 17 00:00:00 2001 From: Maxim Buzdalov Date: Tue, 20 Feb 2024 12:26:24 +0000 Subject: [PATCH 07/11] AP_ESC_Telem: cleanup whitespace --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 2b46e571f6..190f472b69 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -374,8 +374,8 @@ void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan) const uint32_t now_us = AP_HAL::micros(); // loop through groups of 4 ESCs - const uint8_t esc_offset = constrain_int16(mavlink_offset, 0, ESC_TELEM_MAX_ESCS-1); - const uint8_t num_idx = ESC_TELEM_MAX_ESCS/4; + const uint8_t esc_offset = constrain_int16(mavlink_offset, 0, ESC_TELEM_MAX_ESCS - 1); + const uint8_t num_idx = ESC_TELEM_MAX_ESCS / 4; for (uint8_t idx = 0; idx < num_idx; idx++) { const uint8_t i = (next_idx + idx) % num_idx; @@ -650,7 +650,7 @@ void AP_ESC_Telem::update() } } -bool AP_ESC_Telem::rpm_data_within_timeout (const volatile AP_ESC_Telem_Backend::RpmData &instance, const uint32_t now_us, const uint32_t timeout_us) +bool AP_ESC_Telem::rpm_data_within_timeout(const volatile AP_ESC_Telem_Backend::RpmData &instance, const uint32_t now_us, const uint32_t timeout_us) { // easy case, has the time window been crossed so it's invalid if ((now_us - instance.last_update_us) > timeout_us) { @@ -664,7 +664,7 @@ bool AP_ESC_Telem::rpm_data_within_timeout (const volatile AP_ESC_Telem_Backend: return instance.data_valid; } -bool AP_ESC_Telem::was_rpm_data_ever_reported (const volatile AP_ESC_Telem_Backend::RpmData &instance) +bool AP_ESC_Telem::was_rpm_data_ever_reported(const volatile AP_ESC_Telem_Backend::RpmData &instance) { return instance.last_update_us > 0; } From f675df700f6a511144ccafda0e4a486fb6eb39ff Mon Sep 17 00:00:00 2001 From: Maxim Buzdalov Date: Tue, 20 Feb 2024 12:27:09 +0000 Subject: [PATCH 08/11] AP_ESC_Telem: remove redundant initialization --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 190f472b69..ed129ee0f0 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -416,7 +416,7 @@ void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan) s.voltage[j] = constrain_float(_telem_data[esc_id].voltage * 100.0f, 0, UINT16_MAX); s.current[j] = constrain_float(_telem_data[esc_id].current * 100.0f, 0, UINT16_MAX); s.totalcurrent[j] = constrain_float(_telem_data[esc_id].consumption_mah, 0, UINT16_MAX); - float rpmf = 0.0f; + float rpmf; if (get_rpm(esc_id, rpmf)) { s.rpm[j] = constrain_float(rpmf, 0, UINT16_MAX); } From 53417d1232f9cfef80159ee4ae644cc7a2623bd4 Mon Sep 17 00:00:00 2001 From: Maxim Buzdalov Date: Tue, 20 Feb 2024 12:41:03 +0000 Subject: [PATCH 09/11] AP_ESC_Telem: split logging and invalidation, deduplicate micros64() --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index ed129ee0f0..0ca1fb7878 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -566,12 +566,9 @@ void AP_ESC_Telem::update() { #if HAL_LOGGING_ENABLED AP_Logger *logger = AP_Logger::get_singleton(); -#endif - - const uint32_t now_us = AP_HAL::micros(); + const uint64_t now_us64 = AP_HAL::micros64(); for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { -#if HAL_LOGGING_ENABLED // Push received telemetry data into the logging system if (logger && logger->logging_enabled()) { if (_telem_data[i].last_update_ms != _last_telem_log_ms[i] @@ -597,7 +594,7 @@ void AP_ESC_Telem::update() // error_rate is in percentage const struct log_Esc pkt{ LOG_PACKET_HEADER_INIT(uint8_t(LOG_ESC_MSG)), - time_us : AP_HAL::micros64(), + time_us : now_us64, instance : i, rpm : rpm, raw_rpm : raw_rpm, @@ -642,8 +639,12 @@ void AP_ESC_Telem::update() #endif //AP_EXTENDED_ESC_TELEM_ENABLED } } + } #endif // HAL_LOGGING_ENABLED + const uint32_t now_us = AP_HAL::micros(); + for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { + // Invalidate RPM data if not received for too long if ((now_us - _rpm_data[i].last_update_us) > ESC_RPM_DATA_TIMEOUT_US) { _rpm_data[i].data_valid = false; } From 8ad0aec0d0f0b98b73244682f7cbd846985fa81a Mon Sep 17 00:00:00 2001 From: Maxim Buzdalov Date: Tue, 20 Feb 2024 12:45:41 +0000 Subject: [PATCH 10/11] AP_ESC_Telem: replace selected repeated indexing with references --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 88 ++++++++++++++----------- 1 file changed, 49 insertions(+), 39 deletions(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 0ca1fb7878..fdf642f944 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -238,24 +238,26 @@ bool AP_ESC_Telem::get_raw_rpm(uint8_t esc_index, float& rpm) const // get an individual ESC's temperature in centi-degrees if available, returns true on success bool AP_ESC_Telem::get_temperature(uint8_t esc_index, int16_t& temp) const { + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; if (esc_index >= ESC_TELEM_MAX_ESCS - || _telem_data[esc_index].stale() - || !(_telem_data[esc_index].types & (AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL))) { + || telemdata.stale() + || !(telemdata.types & (AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL))) { return false; } - temp = _telem_data[esc_index].temperature_cdeg; + temp = telemdata.temperature_cdeg; return true; } // get an individual motor's temperature in centi-degrees if available, returns true on success bool AP_ESC_Telem::get_motor_temperature(uint8_t esc_index, int16_t& temp) const { + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; if (esc_index >= ESC_TELEM_MAX_ESCS - || _telem_data[esc_index].stale() - || !(_telem_data[esc_index].types & (AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL))) { + || telemdata.stale() + || !(telemdata.types & (AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL))) { return false; } - temp = _telem_data[esc_index].motor_temp_cdeg; + temp = telemdata.motor_temp_cdeg; return true; } @@ -278,48 +280,52 @@ bool AP_ESC_Telem::get_highest_motor_temperature(int16_t& temp) const // get an individual ESC's current in Ampere if available, returns true on success bool AP_ESC_Telem::get_current(uint8_t esc_index, float& amps) const { + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; if (esc_index >= ESC_TELEM_MAX_ESCS - || _telem_data[esc_index].stale() - || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::CURRENT)) { + || telemdata.stale() + || !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::CURRENT)) { return false; } - amps = _telem_data[esc_index].current; + amps = telemdata.current; return true; } // get an individual ESC's voltage in Volt if available, returns true on success bool AP_ESC_Telem::get_voltage(uint8_t esc_index, float& volts) const { + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; if (esc_index >= ESC_TELEM_MAX_ESCS - || _telem_data[esc_index].stale() - || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::VOLTAGE)) { + || telemdata.stale() + || !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::VOLTAGE)) { return false; } - volts = _telem_data[esc_index].voltage; + volts = telemdata.voltage; return true; } // get an individual ESC's energy consumption in milli-Ampere.hour if available, returns true on success bool AP_ESC_Telem::get_consumption_mah(uint8_t esc_index, float& consumption_mah) const { + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; if (esc_index >= ESC_TELEM_MAX_ESCS - || _telem_data[esc_index].stale() - || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION)) { + || telemdata.stale() + || !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION)) { return false; } - consumption_mah = _telem_data[esc_index].consumption_mah; + consumption_mah = telemdata.consumption_mah; return true; } // get an individual ESC's usage time in seconds if available, returns true on success bool AP_ESC_Telem::get_usage_seconds(uint8_t esc_index, uint32_t& usage_s) const { + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; if (esc_index >= ESC_TELEM_MAX_ESCS - || _telem_data[esc_index].stale() - || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::USAGE)) { + || telemdata.stale() + || !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::USAGE)) { return false; } - usage_s = _telem_data[esc_index].usage_s; + usage_s = telemdata.usage_s; return true; } @@ -411,16 +417,17 @@ void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan) if (esc_id >= ESC_TELEM_MAX_ESCS) { continue; } + volatile AP_ESC_Telem_Backend::TelemetryData const &telemdata = _telem_data[esc_id]; - s.temperature[j] = _telem_data[esc_id].temperature_cdeg / 100; - s.voltage[j] = constrain_float(_telem_data[esc_id].voltage * 100.0f, 0, UINT16_MAX); - s.current[j] = constrain_float(_telem_data[esc_id].current * 100.0f, 0, UINT16_MAX); - s.totalcurrent[j] = constrain_float(_telem_data[esc_id].consumption_mah, 0, UINT16_MAX); + s.temperature[j] = telemdata.temperature_cdeg / 100; + s.voltage[j] = constrain_float(telemdata.voltage * 100.0f, 0, UINT16_MAX); + s.current[j] = constrain_float(telemdata.current * 100.0f, 0, UINT16_MAX); + s.totalcurrent[j] = constrain_float(telemdata.consumption_mah, 0, UINT16_MAX); float rpmf; if (get_rpm(esc_id, rpmf)) { s.rpm[j] = constrain_float(rpmf, 0, UINT16_MAX); } - s.count[j] = _telem_data[esc_id].count; + s.count[j] = telemdata.count; } // make sure a msg hasn't been extended @@ -487,36 +494,37 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem } _have_data = true; + volatile AP_ESC_Telem_Backend::TelemetryData &telemdata = _telem_data[esc_index]; #if AP_TEMPERATURE_SENSOR_ENABLED // always allow external data. Block "internal" if external has ever its ever been set externally then ignore normal "internal" updates const bool has_temperature = (data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL) || - ((data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE) && !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL)); + ((data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE) && !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL)); const bool has_motor_temperature = (data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL) || - ((data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE) && !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL)); + ((data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE) && !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL)); #else const bool has_temperature = (data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE); const bool has_motor_temperature = (data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE); #endif if (has_temperature) { - _telem_data[esc_index].temperature_cdeg = new_data.temperature_cdeg; + telemdata.temperature_cdeg = new_data.temperature_cdeg; } if (has_motor_temperature) { - _telem_data[esc_index].motor_temp_cdeg = new_data.motor_temp_cdeg; + telemdata.motor_temp_cdeg = new_data.motor_temp_cdeg; } if (data_mask & AP_ESC_Telem_Backend::TelemetryType::VOLTAGE) { - _telem_data[esc_index].voltage = new_data.voltage; + telemdata.voltage = new_data.voltage; } if (data_mask & AP_ESC_Telem_Backend::TelemetryType::CURRENT) { - _telem_data[esc_index].current = new_data.current; + telemdata.current = new_data.current; } if (data_mask & AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION) { - _telem_data[esc_index].consumption_mah = new_data.consumption_mah; + telemdata.consumption_mah = new_data.consumption_mah; } if (data_mask & AP_ESC_Telem_Backend::TelemetryType::USAGE) { - _telem_data[esc_index].usage_s = new_data.usage_s; + telemdata.usage_s = new_data.usage_s; } #if AP_EXTENDED_ESC_TELEM_ENABLED @@ -569,10 +577,12 @@ void AP_ESC_Telem::update() const uint64_t now_us64 = AP_HAL::micros64(); for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { + const volatile AP_ESC_Telem_Backend::RpmData &rpmdata = _rpm_data[i]; + const volatile AP_ESC_Telem_Backend::TelemetryData &telemdata = _telem_data[i]; // Push received telemetry data into the logging system if (logger && logger->logging_enabled()) { - if (_telem_data[i].last_update_ms != _last_telem_log_ms[i] - || _rpm_data[i].last_update_us != _last_rpm_log_us[i]) { + 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; @@ -598,12 +608,12 @@ void AP_ESC_Telem::update() instance : i, rpm : rpm, raw_rpm : raw_rpm, - voltage : _telem_data[i].voltage, - current : _telem_data[i].current, - esc_temp : _telem_data[i].temperature_cdeg, - current_tot : _telem_data[i].consumption_mah, - motor_temp : _telem_data[i].motor_temp_cdeg, - error_rate : _rpm_data[i].error_rate + voltage : telemdata.voltage, + current : telemdata.current, + esc_temp : telemdata.temperature_cdeg, + current_tot : telemdata.consumption_mah, + motor_temp : telemdata.motor_temp_cdeg, + error_rate : rpmdata.error_rate }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); From fa18baec44d51d20fa79cb2711bc3e4fa8f20e34 Mon Sep 17 00:00:00 2001 From: Maxim Buzdalov Date: Sun, 18 Feb 2024 12:15:33 +0000 Subject: [PATCH 11/11] AP_ESC_Telem: for RPM, log NaN instead of 0 when there are no measurements --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index fdf642f944..4678c9ae9c 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -590,7 +590,7 @@ void AP_ESC_Telem::update() float rpm = AP::logger().quiet_nanf(); get_rpm(i, rpm); - float raw_rpm = 0.0f; + float raw_rpm = AP::logger().quiet_nanf(); get_raw_rpm(i, raw_rpm); // Write ESC status messages