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

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

libraries/AP_DroneCAN/AP_DroneCAN.h

@@ -326,6 +326,9 @@ 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};
 
+    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};
+
     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 +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);

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

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

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

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

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