diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index c6cd5267d1a8b..4c410efcf6d8f 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -122,6 +122,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 49e8ed719ce20..174d4108b6f9b 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -62,6 +62,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',), diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index e8692bd8a5be2..d8b533a7913bd 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -1457,6 +1457,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 8b37c2528b89b..b5527fb5947cc 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); diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 87e77e70c7d1f..1e4240f667be0 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 e46eb48ceb6b0..0fa2e3dabaa65 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..f3fb6fac7bb89 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