APD F120 packet modification

.gitmodules

@@ -21,7 +21,7 @@
 	url = https://github.com/ArduPilot/gsoap
 [submodule "modules/DroneCAN/DSDL"]
 	path = modules/DroneCAN/DSDL
-	url = https://github.com/DroneCAN/DSDL.git
+	url = https://github.com/CarbonixUAV/DSDL.git
 [submodule "modules/CrashDebug"]
 	path = modules/CrashDebug
 	url = https://github.com/ardupilot/CrashDebug

Tools/AP_Periph/can.cpp

@@ -1660,6 +1660,22 @@ void AP_Periph_FW::esc_telem_update()
                          CANARD_TRANSFER_PRIORITY_LOW,
                          &buffer[0],
                          total_size);
+
+        uavcan_equipment_esc_StatusExtended pkt2{};
+        pkt2.esc_index = pkt.esc_index;
+        bool has_ext_data = esc_telem.get_input_duty(i, pkt2.input_pct)
+                | esc_telem.get_output_duty(i, pkt2.output_pct)
+                | esc_telem.get_flags(i, pkt2.status_flags);
+
+        if (has_ext_data) {
+            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

@@ -151,12 +151,23 @@ const AP_Param::GroupInfo AP_BLHeli::var_info[] = {
     // @RebootRequired: True
     AP_GROUPINFO("RVMASK",  12, AP_BLHeli, channel_reversed_mask, 0),
 
+#if HAL_WITH_ESC_TELEM
+    // @Param: EXTLM
+    // @DisplayName: Enable extended BLHeli telemetry
+    // @Description: When set to 1, this listens for 13-byte extended telemetry packets from BLHeli ESCs
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("EXTLM",  13, AP_BLHeli, extended_telemetry, 0),
+#endif
+
     AP_GROUPEND
 };
 
 #define RPM_SLEW_RATE 50
 
 AP_BLHeli *AP_BLHeli::_singleton;
+uint8_t AP_BLHeli::telem_packet_size;
 
 // constructor
 AP_BLHeli::AP_BLHeli(void)
@@ -1415,6 +1426,8 @@ void AP_BLHeli::init(void)
             telem_uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_ESCTelemetry,0);
         }
     }
+
+    telem_packet_size = extended_telemetry ? 13 : 10;
 }
 
 /*
@@ -1423,9 +1436,9 @@ 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[13];
     if (telem_uart->read(buf, telem_packet_size) < telem_packet_size) {
-        // short read, we should have 10 bytes ready when this function is called
+        // short read, we should have all bytes ready when this function is called
         return;
     }
 
@@ -1447,18 +1460,28 @@ void AP_BLHeli::read_telemetry_packet(void)
     hal.rcout->set_active_escs_mask(1<<motor_idx);
     update_rpm(motor_idx - chan_offset, new_rpm);
 
-    TelemetryData t {
+    TelemetryData telemetryData {
         .temperature_cdeg = int16_t(buf[0] * 100),
         .voltage = float(uint16_t((buf[1]<<8) | buf[2])) * 0.01,
         .current = float(uint16_t((buf[3]<<8) | buf[4])) * 0.01,
         .consumption_mah = float(uint16_t((buf[5]<<8) | buf[6])),
     };
 
-    update_telem_data(motor_idx - chan_offset, t,
-        AP_ESC_Telem_Backend::TelemetryType::CURRENT
+    uint16_t types = AP_ESC_Telem_Backend::TelemetryType::CURRENT
             | AP_ESC_Telem_Backend::TelemetryType::VOLTAGE
             | AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION
-            | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
+            | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE;
+
+    if (extended_telemetry) {
+        telemetryData.input_duty = buf[9];
+        telemetryData.output_duty = buf[10];
+        telemetryData.flags = buf[11];
+        types |= AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY
+                | AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY
+                | AP_ESC_Telem_Backend::TelemetryType::FLAGS;
+    }
+
+    update_telem_data(motor_idx - chan_offset, telemetryData, types);
 
     if (debug_level >= 2) {
         uint16_t trpm = new_rpm;
@@ -1470,10 +1493,10 @@ void AP_BLHeli::read_telemetry_packet(void)
         }
         DEV_PRINTF("ESC[%u] T=%u V=%f C=%f con=%f RPM=%u e=%.1f t=%u\n",
                             last_telem_esc,
-                            t.temperature_cdeg,
-                            t.voltage,
-                            t.current,
-                            t.consumption_mah,
+                            telemetryData.temperature_cdeg,
+                            telemetryData.voltage,
+                            telemetryData.current,
+                            telemetryData.consumption_mah,
                             trpm, hal.rcout->get_erpm_error_rate(motor_idx), (unsigned)AP_HAL::millis());
     }
 #endif // HAL_WITH_ESC_TELEM

libraries/AP_BLHeli/AP_BLHeli.h

@@ -76,6 +76,7 @@ class AP_BLHeli : public AP_ESC_Telem_Backend {
     AP_Int8 motor_poles;
     // mask of channels with bi-directional dshot enabled
     AP_Int32 channel_bidir_dshot_mask;
+    AP_Int8 extended_telemetry;
 
     enum mspState {
         MSP_IDLE=0,
@@ -250,7 +251,7 @@ 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 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

@@ -290,6 +290,42 @@ 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() - _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;
+}
+
 // send ESC telemetry messages over MAVLink
 void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan)
 {
@@ -435,6 +471,15 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem
     if (data_mask & AP_ESC_Telem_Backend::TelemetryType::USAGE) {
         _telem_data[esc_index].usage_s = new_data.usage_s;
     }
+    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;
+    }
 
     _telem_data[esc_index].count++;
     _telem_data[esc_index].types |= data_mask;
@@ -507,6 +552,28 @@ 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: 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 = _telem_data[i].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  : _telem_data[i].input_duty,
+                        output_duty : _telem_data[i].output_duty,
+                        flags       : _telem_data[i].flags,
+                    };
+                    AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
+                }
+
                 _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, uint32_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); };
 

libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h

@@ -25,6 +25,9 @@ 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
+        uint8_t input_duty;         // input duty cycle
+        uint8_t output_duty;        // output duty cycle
+        uint32_t flags;              // Status flags
     };
 
     struct RpmData {
@@ -41,7 +44,13 @@ class AP_ESC_Telem_Backend {
         VOLTAGE     = 1 << 2,
         CURRENT     = 1 << 3,
         CONSUMPTION = 1 << 4,
-        USAGE       = 1 << 5
+        USAGE       = 1 << 5,
+        // 6 reserved for temperature external
+        // 7 reserved for motor temperature external
+        // 8 and 9 are reserved for recent dshot extensions
+        INPUT_DUTY  = 1 << 10,
+        OUTPUT_DUTY = 1 << 11,
+        FLAGS       = 1 << 12
     };
 
 

libraries/AP_ESC_Telem/LogStructure.h

@@ -3,7 +3,8 @@
 #include <AP_Logger/LogStructure.h>
 
 #define LOG_IDS_FROM_ESC_TELEM                  \
-    LOG_ESC_MSG
+    LOG_ESC_MSG,                                \
+    LOG_ESCX_MSG
 
 // @LoggerMessage: ESC
 // @Description: Feedback received from ESCs
@@ -31,6 +32,24 @@ struct PACKED log_Esc {
     float error_rate;
 };
 
+// @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",  "QBeeffcfcf", "TimeUS,Instance,RPM,RawRPM,Volt,Curr,Temp,CTot,MotTemp,Err", "s#qqvAOaO%", "F-BB--BCB-" , true }, \
+    { LOG_ESCX_MSG, sizeof(log_EscX), \
+      "ESCX", "QBBBI", "TimeUS,Instance,inpct,outpct,flags", "s#%%-", "F----" , true },

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 NUM_SERVO_CHANNELS 2

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -35,6 +35,7 @@
 
 #include <uavcan/equipment/esc/RawCommand.hpp>
 #include <uavcan/equipment/esc/Status.hpp>
+#include <uavcan/equipment/esc/StatusExtended.hpp>
 #include <uavcan/equipment/indication/LightsCommand.hpp>
 #include <uavcan/equipment/indication/SingleLightCommand.hpp>
 #include <uavcan/equipment/indication/BeepCommand.hpp>
@@ -204,6 +205,10 @@ static uavcan::Subscriber<uavcan::equipment::device::Temperature, DeviceTemperat
 UC_REGISTRY_BINDER(ESCStatusCb, uavcan::equipment::esc::Status);
 static uavcan::Subscriber<uavcan::equipment::esc::Status, ESCStatusCb> *esc_status_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
 
+// handler Extended ESC status
+UC_REGISTRY_BINDER(ESCXStatusCb, uavcan::equipment::esc::StatusExtended);
+static uavcan::Subscriber<uavcan::equipment::esc::StatusExtended, ESCXStatusCb> *extesc_status_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
+
 // handler DEBUG
 UC_REGISTRY_BINDER(DebugCb, uavcan::protocol::debug::LogMessage);
 static uavcan::Subscriber<uavcan::protocol::debug::LogMessage, DebugCb> *debug_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
@@ -432,6 +437,11 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
         esc_status_listener[driver_index]->start(ESCStatusCb(this, &handle_ESC_status));
     }
 
+    extesc_status_listener[driver_index] = new uavcan::Subscriber<uavcan::equipment::esc::StatusExtended, ESCXStatusCb>(*_node);
+    if (extesc_status_listener[driver_index]) {
+        extesc_status_listener[driver_index]->start(ESCXStatusCb(this, &handle_esc_ext_status));
+    }
+
     debug_listener[driver_index] = new uavcan::Subscriber<uavcan::protocol::debug::LogMessage, DebugCb>(*_node);
     if (debug_listener[driver_index]) {
         debug_listener[driver_index]->start(DebugCb(this, &handle_debug));
@@ -1073,6 +1083,34 @@ void AP_UAVCAN::handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const E
 #endif
 }
 
+/*
+  handle ESC status extended message
+ */
+void AP_UAVCAN::handle_esc_ext_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCXStatusCb &cb)
+{
+#if HAL_WITH_ESC_TELEM
+    const uint8_t esc_offset = constrain_int16(ap_uavcan->_esc_offset.get(), 0, UAVCAN_SRV_NUMBER);
+    const uint8_t esc_index = cb.msg->esc_index + esc_offset;
+
+    if (!is_esc_data_index_valid(esc_index)) {
+        return;
+    }
+
+    TelemetryData telemetryData {
+        .motor_temp_cdeg = (int16_t)(cb.msg->motor_temperature_degC * 100),
+        .input_duty = cb.msg->input_pct,
+        .output_duty = cb.msg->output_pct,
+        .flags = (uint32_t)cb.msg->status_flags,
+    };
+
+    ap_uavcan->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_UAVCAN::is_esc_data_index_valid(const uint8_t index) {
     if (index > UAVCAN_SRV_NUMBER) {
         // printf("UAVCAN: invalid esc index: %d. max index allowed: %d\n\r", index, UAVCAN_SRV_NUMBER);

libraries/AP_UAVCAN/AP_UAVCAN.h

@@ -51,6 +51,7 @@ class ActuatorStatusCb;
 class PowerCktStatusCb;
 class DeviceTemperatureCb;
 class ESCStatusCb;
+class ESCXStatusCb;
 class DebugCb;
 class ParamGetSetCb;
 class ParamExecuteOpcodeCb;
@@ -346,6 +347,7 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     static void handle_power_cktstatus(AP_UAVCAN* ap_uavcan, uint8_t node_id, const PowerCktStatusCb &cb);
     static void handle_device_temperature(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DeviceTemperatureCb &cb);
     static void handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb);
+    static void handle_esc_ext_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCXStatusCb &cb);
     static bool is_esc_data_index_valid(const uint8_t index);
     static void handle_debug(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DebugCb &cb);
     static void handle_param_get_set_response(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ParamGetSetCb &cb);