diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index a71c545846adec..02679c96601bc4 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -121,6 +121,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 659b2a1a9468bd..2c713ffaa673fe 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', 'AP_Extended_Telem::get_input_duty',), + ('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 027ce8e48453c8..86153bd94134ca 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -1448,12 +1448,13 @@ 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) { -#if HAL_WITH_ESC_TELEM && AP_EXTENDED_ESC_TELEM_ENABLE +#if HAL_WITH_ESC_TELEM const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, DRONECAN_SRV_NUMBER); const uint8_t esc_index = msg.esc_index + esc_offset; @@ -1473,8 +1474,9 @@ void AP_DroneCAN::handle_esc_ext_status(const CanardRxTransfer& transfer, const | AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY | AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY | AP_ESC_Telem_Backend::TelemetryType::FLAGS); -#endif +#endif // HAL_WITH_ESC_TELEM } +#endif // AP_EXTENDED_ESC_TELEM_ENABLED bool AP_DroneCAN::is_esc_data_index_valid(const uint8_t index) { if (index > DRONECAN_SRV_NUMBER) { diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h index bf666238b0eeab..b5527fb5947cc6 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.h +++ b/libraries/AP_DroneCAN/AP_DroneCAN.h @@ -326,8 +326,10 @@ 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}; @@ -390,7 +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 35fe812a77538d..9f15e0c9d2df46 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -305,12 +305,13 @@ bool AP_ESC_Telem::get_usage_seconds(uint8_t esc_index, uint32_t& usage_s) const return true; } -#if AP_EXTENDED_ESC_TELEM_ENABLE +#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 - || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS + || telemdata.stale() || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY)) { return false; } @@ -321,8 +322,9 @@ bool AP_ESC_Telem::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 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 - || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS + || telemdata.stale() || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY)) { return false; } @@ -333,15 +335,16 @@ bool AP_ESC_Telem::get_output_duty(uint8_t esc_index, uint8_t& output_duty) cons // 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 - || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS + || 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_ENABLE +#endif // AP_EXTENDED_ESC_TELEM_ENABLED // send ESC telemetry messages over MAVLink void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan) @@ -506,7 +509,7 @@ 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 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; } @@ -516,7 +519,7 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem if (data_mask & AP_ESC_Telem_Backend::TelemetryType::FLAGS) { _telem_data[esc_index].flags = new_data.flags; } -#endif //AP_EXTENDED_ESC_TELEM_ENABLE +#endif //AP_EXTENDED_ESC_TELEM_ENABLED #if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED if (data_mask & AP_ESC_Telem_Backend::TelemetryType::EDT2_STATUS) { @@ -658,7 +661,7 @@ void AP_ESC_Telem::update() }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); -#if AP_EXTENDED_ESC_TELEM_ENABLE +#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 @@ -669,17 +672,25 @@ void AP_ESC_Telem::update() 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)); + // @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_ENABLE +#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 42c8f692aa7bb9..a91c9259af432c 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h @@ -67,7 +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 +#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; @@ -76,7 +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 +#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 4eb81e115ea318..a93f70d852408b 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h @@ -23,11 +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 +#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_ENABLE +#endif // AP_EXTENDED_ESC_TELEM_ENABLED // return true if the data is stale bool stale(uint32_t now_ms=0) const volatile; @@ -55,11 +55,11 @@ class AP_ESC_Telem_Backend { EDT2_STATUS = 1 << 8, EDT2_STRESS = 1 << 9, #endif -#if AP_EXTENDED_ESC_TELEM_ENABLE +#if AP_EXTENDED_ESC_TELEM_ENABLED INPUT_DUTY = 1 << 10, OUTPUT_DUTY = 1 << 11, FLAGS = 1 << 12 -#endif // AP_EXTENDED_ESC_TELEM_ENABLE +#endif // AP_EXTENDED_ESC_TELEM_ENABLED }; diff --git a/libraries/AP_ESC_Telem/LogStructure.h b/libraries/AP_ESC_Telem/LogStructure.h index dea9c5ffdff40f..81e8a7def31f73 100644 --- a/libraries/AP_ESC_Telem/LogStructure.h +++ b/libraries/AP_ESC_Telem/LogStructure.h @@ -4,8 +4,7 @@ #define LOG_IDS_FROM_ESC_TELEM \ LOG_ESC_MSG, \ - LOG_EDT2_MSG, \ - LOG_ESCX_MSG + LOG_EDT2_MSG // @LoggerMessage: ESC // @Description: Feedback received from ESCs @@ -59,27 +58,8 @@ struct PACKED log_Edt2 { uint8_t status; }; -// @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", "QBeeffcfcf", "TimeUS,Instance,RPM,RawRPM,Volt,Curr,Temp,CTot,MotTemp,Err", "s#qqvAOaO%", "F-BB--BCB-" , true }, \ + "ESC", "QBffffcfcf", "TimeUS,Instance,RPM,RawRPM,Volt,Curr,Temp,CTot,MotTemp,Err", "s#qqvAOaO%", "F-00--BCB-" , true }, \ { LOG_EDT2_MSG, sizeof(log_Edt2), \ - "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 }, - + "EDT2", "QBBBB", "TimeUS,Instance,Stress,MaxStress,Status", "s#---", "F----" , true }, diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index c59f44885d9590..42b5b594d5f565 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -188,8 +188,8 @@ #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 +#ifndef AP_EXTENDED_ESC_TELEM_ENABLED +#define AP_EXTENDED_ESC_TELEM_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif // this is used as a general mechanism to make a 'small' build by