Cherry-pick extended ESC

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),

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<type>.*)::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<type>.*)::update',),
 

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);

libraries/AP_DroneCAN/AP_DroneCAN.h

@@ -326,6 +326,11 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     Canard::ObjCallback<AP_DroneCAN, uavcan_equipment_esc_Status> esc_status_cb{this, &AP_DroneCAN::handle_ESC_status};
     Canard::Subscriber<uavcan_equipment_esc_Status> esc_status_listener{esc_status_cb, _driver_index};
 
+#if AP_EXTENDED_ESC_TELEM_ENABLED
+    Canard::ObjCallback<AP_DroneCAN, uavcan_equipment_esc_StatusExtended> esc_status_extended_cb{this, &AP_DroneCAN::handle_esc_ext_status};
+    Canard::Subscriber<uavcan_equipment_esc_StatusExtended> esc_status_extended_listener{esc_status_extended_cb, _driver_index};
+#endif
+
     Canard::ObjCallback<AP_DroneCAN, uavcan_protocol_debug_LogMessage> debug_cb{this, &AP_DroneCAN::handle_debug};
     Canard::Subscriber<uavcan_protocol_debug_LogMessage> 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);

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); };
 

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
     };
 
 

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

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 = {}

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