AP_ESC_Telem : APD f120 Extended packets with UAVCAN changes

Tools/AP_Periph/Parameters.cpp

@@ -384,6 +384,10 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     // @User: Advanced
     // @RebootRequired: True
     GSCALAR(esc_telem_port, "ESC_TELEM_PORT", AP_PERIPH_ESC_TELEM_PORT_DEFAULT),
+
+    // @Group: ESC_TLM
+    // @Path: ../libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
+    GOBJECT(esc_telem, "ESC_TLM", AP_ESC_Telem),
 #endif
 #endif
 

Tools/AP_Periph/Parameters.h

@@ -66,6 +66,7 @@ class Parameters {
         k_param_esc_number1,
         k_param_pole_count1,
         k_param_esc_serial_port1,
+        k_param_esc_telem,
     };
 
     AP_Int16 format_version;

Tools/AP_Periph/can.cpp

@@ -1660,6 +1660,50 @@ void AP_Periph_FW::esc_telem_update()
                          CANARD_TRANSFER_PRIORITY_LOW,
                          &buffer[0],
                          total_size);
+
+        if (esc_telem.is_extended_telem_enabled()) {
+            uavcan_equipment_esc_StatusExtended pkt2{};
+
+            if (channel == nullptr) {
+                pkt2.esc_index = i;
+            }
+            else {
+                const int8_t motor_num = channel->get_motor_num();
+                pkt2.esc_index = motor_num == -1 ? i : motor_num;
+            }
+
+            uint8_t input_duty;
+            if (esc_telem.get_input_duty(i, input_duty)) {
+                pkt2.input_pct = input_duty;
+            }
+            else {
+                pkt2.input_pct = 0;
+            }
+
+            uint8_t output_duty;
+            if (esc_telem.get_output_duty(i, output_duty)) {
+                pkt2.output_pct = output_duty;
+            }
+            else {
+                pkt2.output_pct = 0;
+            }
+
+            uint8_t flags;
+            if (esc_telem.get_flags(i, flags)) {
+                pkt2.status_flags = flags;
+            }
+            else {
+                pkt2.status_flags = 0;
+            }
+
+            uint8_t buffer_ext[UAVCAN_EQUIPMENT_ESC_STATUSEXTENDED_MAX_SIZE] {};
+            uint16_t total_size_ext = uavcan_equipment_esc_StatusExtended_encode(&pkt2, buffer_ext, !periph.canfdout());
+            canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUSEXTENDED_SIGNATURE,
+                            UAVCAN_EQUIPMENT_ESC_STATUSEXTENDED_ID,
+                            CANARD_TRANSFER_PRIORITY_LOW,
+                            &buffer_ext[0],
+                            total_size_ext);
+        }
     }
 }
 #endif // HAL_WITH_ESC_TELEM

libraries/AP_BLHeli/AP_BLHeli.cpp

@@ -1415,6 +1415,8 @@ void AP_BLHeli::init(void)
             telem_uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_ESCTelemetry,0);
         }
     }
+
+    telem_packet_size = AP_ESC_Telem_Backend::is_extended_telem_enabled() ? 13 : 10;
 }
 
 /*
@@ -1423,7 +1425,7 @@ void AP_BLHeli::init(void)
 void AP_BLHeli::read_telemetry_packet(void)
 {
 #if HAL_WITH_ESC_TELEM
-    uint8_t buf[telem_packet_size];
+    uint8_t buf[telem_packet_array_size];
     if (telem_uart->read(buf, telem_packet_size) < telem_packet_size) {
         // short read, we should have 10 bytes ready when this function is called
         return;
@@ -1460,6 +1462,18 @@ void AP_BLHeli::read_telemetry_packet(void)
             | AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION
             | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
 
+    if (AP_ESC_Telem_Backend::is_extended_telem_enabled()) {
+        ExtendedTelemData e{
+            .input_duty = buf[9],
+            .output_duty = buf[10],
+            .flags = buf[11],
+        };
+        update_ext_telem_data(motor_idx - chan_offset, e,
+            AP_ESC_Telem_Backend::ExtTelemetryType::INPUT_DUTY
+            | AP_ESC_Telem_Backend::ExtTelemetryType::OUTPUT_DUTY
+            | AP_ESC_Telem_Backend::ExtTelemetryType::FLAGS);
+    }
+
     if (debug_level >= 2) {
         uint16_t trpm = new_rpm;
         if (has_bidir_dshot(last_telem_esc)) {

libraries/AP_BLHeli/AP_BLHeli.h

@@ -250,7 +250,8 @@ class AP_BLHeli : public AP_ESC_Telem_Backend {
     // when did we last request telemetry?
     uint32_t last_telem_request_us;
     uint8_t last_telem_esc;
-    static const uint8_t telem_packet_size = 10;
+    static const uint8_t telem_packet_array_size = 13;
+    uint8_t telem_packet_size;
     bool telem_uart_started;
     uint32_t last_telem_byte_read_us;
     int8_t last_control_port;

libraries/AP_ESC_Telem/AP_ESC_Telem.cpp

@@ -39,6 +39,15 @@ const AP_Param::GroupInfo AP_ESC_Telem::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("_MAV_OFS", 1, AP_ESC_Telem, mavlink_offset, 0),
 
+    // @Param: _EXT_TELEM
+    // @DisplayName: Extended ESC Telemetry 
+    // @Description: This enables extended telemetry for ESCs that support it.
+    // @Values: 0:None,1:Enable extended telemetry
+    // @Range: 0 1
+    // @User: Standard
+    // @RebootRequired: False
+    AP_GROUPINFO("_EXTEND", 2, AP_ESC_Telem, ext_esc_telem, 0),
+
     AP_GROUPEND
 };
 
@@ -153,6 +162,12 @@ bool AP_ESC_Telem::is_telemetry_active(uint32_t servo_channel_mask) const
     return true;
 }
 
+// checks if extended telemetry is enabled
+bool AP_ESC_Telem::is_extended_telem_enabled() const
+{
+    return (ext_esc_telem==1?true:false);
+}
+
 // get an individual ESC's slewed rpm if available, returns true on success
 bool AP_ESC_Telem::get_rpm(uint8_t esc_index, float& rpm) const
 {
@@ -290,6 +305,39 @@ bool AP_ESC_Telem::get_usage_seconds(uint8_t esc_index, uint32_t& usage_s) const
     return true;
 }
 
+// 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() - _ext_telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS)) {
+        return false;
+    }
+    input_duty = _ext_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() - _ext_telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS)) {
+        return false;
+    }
+    output_duty = _ext_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, uint8_t& flags) const
+{
+    if (esc_index >= ESC_TELEM_MAX_ESCS) {
+        // || AP_HAL::millis() - _ext_telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS)) {
+        return false;
+    }
+    flags = _ext_telem_data[esc_index].flags;
+    return true;
+}
+
 // send ESC telemetry messages over MAVLink
 void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan)
 {
@@ -441,6 +489,26 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem
     _telem_data[esc_index].last_update_ms = AP_HAL::millis();
 }
 
+// record an update to the extended telemetry data together with timestamp
+// this should be called by backends when new extended telemetry values are available
+void AP_ESC_Telem::update_ext_telem_data(const uint8_t esc_index, const AP_ESC_Telem_Backend::ExtendedTelemData& new_data, const uint16_t data_mask)
+{
+    if (esc_index >= ESC_TELEM_MAX_ESCS) {
+        return;
+    }
+    if (data_mask & AP_ESC_Telem_Backend::ExtTelemetryType::INPUT_DUTY) {
+        _ext_telem_data[esc_index].input_duty = new_data.input_duty;
+    }
+    if (data_mask & AP_ESC_Telem_Backend::ExtTelemetryType::OUTPUT_DUTY) {
+        _ext_telem_data[esc_index].output_duty = new_data.output_duty;
+    }
+    if (data_mask & AP_ESC_Telem_Backend::ExtTelemetryType::FLAGS) {
+        _ext_telem_data[esc_index].flags = new_data.flags;
+    }
+
+    _ext_telem_data[esc_index].last_update_ms = AP_HAL::millis();
+}
+
 // record an update to the RPM together with timestamp, this allows the notch values to be slewed
 // this should be called by backends when new telemetry values are available
 void AP_ESC_Telem::update_rpm(const uint8_t esc_index, const float new_rpm, const float error_rate)
@@ -507,6 +575,27 @@ void AP_ESC_Telem::update()
                     error_rate  : _rpm_data[i].error_rate
                 };
                 AP::logger().WriteBlock(&pkt, sizeof(pkt));
+
+                // Write ESC extended status messages
+                //   id starts from 0
+                //   input duty
+                //   output duty
+                //   status flags: These are specific to APD F120 series ESCs
+                //      - bit-0 : over temperature flag
+                //      - bit-1 : signal loss flag [200mS since last throttle packet]
+                //      - bit-2 : over voltage flag
+                //      - bit-3 : reboot flag [After every reboot or power cycle this bit will stay high for 30 seconds for cube to capture it]
+                const struct log_ExtEsc pkt2{
+                    LOG_PACKET_HEADER_INIT(uint8_t(LOG_EXTESC_MSG)),
+                    time_us     : AP_HAL::micros64(),
+                    instance    : i,
+                    input_duty  : _ext_telem_data[i].input_duty,
+                    output_duty : _ext_telem_data[i].output_duty,
+                    flags       : _ext_telem_data[i].flags,
+                };
+                AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
+
+                _last_ext_telem_log_ms[i] = _ext_telem_data[i].last_update_ms;
                 _last_telem_log_ms[i] = _telem_data[i].last_update_ms;
                 _last_rpm_log_us[i] = _rpm_data[i].last_update_us;
             }

libraries/AP_ESC_Telem/AP_ESC_Telem.h

@@ -66,6 +66,15 @@ 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;
 
+    // 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, uint8_t& flags) const;
+
     // 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); };
 
@@ -97,6 +106,9 @@ class AP_ESC_Telem {
     // is rpm telemetry configured for the provided channel mask
     bool is_telemetry_active(uint32_t servo_channel_mask) const;
 
+    // checks if extended telemetry is enabled
+    bool is_extended_telem_enabled() const;
+
     // callback to update the rpm in the frontend, should be called by the driver when new data is available
     // can also be called from scripting
     void update_rpm(const uint8_t esc_index, const float new_rpm, const float error_rate);
@@ -113,11 +125,17 @@ class AP_ESC_Telem {
     // callback to update the data in the frontend, should be called by the driver when new data is available
     void update_telem_data(const uint8_t esc_index, const AP_ESC_Telem_Backend::TelemetryData& new_data, const uint16_t data_mask);
 
+    // callback to update the data in the frontend, should be called by the driver when new data is available
+    void update_ext_telem_data(const uint8_t esc_index, const AP_ESC_Telem_Backend::ExtendedTelemData& new_data, const uint16_t data_mask);
+
     // rpm data
     volatile AP_ESC_Telem_Backend::RpmData _rpm_data[ESC_TELEM_MAX_ESCS];
     // telemetry data
     volatile AP_ESC_Telem_Backend::TelemetryData _telem_data[ESC_TELEM_MAX_ESCS];
+    // Extended telemetry data
+    volatile AP_ESC_Telem_Backend::ExtendedTelemData _ext_telem_data[ESC_TELEM_MAX_ESCS];
 
+    uint32_t _last_ext_telem_log_ms[ESC_TELEM_MAX_ESCS];
     uint32_t _last_telem_log_ms[ESC_TELEM_MAX_ESCS];
     uint32_t _last_rpm_log_us[ESC_TELEM_MAX_ESCS];
     uint8_t next_idx;
@@ -131,6 +149,7 @@ class AP_ESC_Telem {
     bool _have_data;
 
     AP_Int8 mavlink_offset;
+    AP_Int8 ext_esc_telem;
 
     static AP_ESC_Telem *_singleton;
 };

libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp

@@ -40,4 +40,14 @@ void AP_ESC_Telem_Backend::update_telem_data(const uint8_t esc_index, const Tele
     _frontend->update_telem_data(esc_index, new_data, data_present_mask);
 }
 
+// callback to update the data in the frontend, should be called by the driver when new data is available
+void AP_ESC_Telem_Backend::update_ext_telem_data(const uint8_t esc_index, const ExtendedTelemData& new_ext_data, const uint16_t data_mask) {
+    _frontend->update_ext_telem_data(esc_index, new_ext_data, data_mask);
+}
+
+// check if ESC extended telemetry is enabled
+bool AP_ESC_Telem_Backend::is_extended_telem_enabled() {
+    return _frontend->is_extended_telem_enabled();
+}
+
 #endif

libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h

@@ -27,6 +27,13 @@ class AP_ESC_Telem_Backend {
         uint16_t count;             // number of times updated
     };
 
+    struct ExtendedTelemData {
+        uint8_t input_duty;         // input duty cycle
+        uint8_t output_duty;        // output duty cycle
+        uint8_t flags;              // Status flags
+        uint32_t last_update_ms;    // last update time in milliseconds, determines whether active
+    };
+
     struct RpmData {
         float    rpm;               // rpm
         float    prev_rpm;          // previous rpm
@@ -44,6 +51,11 @@ class AP_ESC_Telem_Backend {
         USAGE       = 1 << 5
     };
 
+    enum ExtTelemetryType {
+        INPUT_DUTY = 1 << 0,
+        OUTPUT_DUTY = 1 << 1,
+        FLAGS = 1 << 2
+    };
 
     AP_ESC_Telem_Backend();
 
@@ -58,6 +70,12 @@ class AP_ESC_Telem_Backend {
     // callback to update the data in the frontend, should be called by the driver when new data is available
     void update_telem_data(const uint8_t esc_index, const TelemetryData& new_data, const uint16_t data_present_mask);
 
+    // callback to update the data in the frontend, should be called by the driver when new data is available
+    void update_ext_telem_data(const uint8_t esc_index, const ExtendedTelemData& new_ext_data, const uint16_t data_present_mask);
+
+    // check if ESC extended telemetry is enabled
+    bool is_extended_telem_enabled();
+
 private:
     AP_ESC_Telem* _frontend;
 };

libraries/AP_ESC_Telem/LogStructure.h

@@ -5,6 +5,9 @@
 #define LOG_IDS_FROM_ESC_TELEM                  \
     LOG_ESC_MSG
 
+#define LOG_IDS_FROM_EXTESC_TELEM               \
+    LOG_EXTESC_MSG
+
 // @LoggerMessage: ESC
 // @Description: Feedback received from ESCs
 // @Field: TimeUS: microseconds since system startup
@@ -34,3 +37,23 @@ struct PACKED log_Esc {
 #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 },
+
+// @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: flgs: flags
+struct PACKED log_ExtEsc {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t instance;
+    uint8_t input_duty;
+    uint8_t output_duty;
+    uint8_t flags;
+};
+
+#define LOG_STRUCTURE_FROM_EXTESC_TELEM \
+    { LOG_EXTESC_MSG, sizeof(log_ExtEsc), \
+      "ESCX", "QBBBB", "TimeUS,Instance,inpct,outpct,flags", "s#---%", "F----" , true },

libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/hwdef.dat

@@ -334,3 +334,4 @@ define AP_MOTORS_FRAME_OCTAQUAD_ENABLED 0
 define HAL_SMARTAUDIO_ENABLED 0
 define AP_TRAMP_ENABLED 0
 define AP_ICENGINE_TCA9554_STARTER_ENABLED 1
+define HAL_WITH_ESC_TELEM 1

libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat

@@ -120,4 +120,6 @@ define HAL_PERIPH_ENABLE_RC_OUT
 # enable ESC control
 define HAL_SUPPORT_RCOUT_SERIAL 1
 
-define HAL_PERIPH_ARM_MONITORING_ENABLE 1
+define HAL_PERIPH_ARM_MONITORING_ENABLE 1
+
+define HAL_WITH_ESC_TELEM 1