Add XC-ESC Technology DRONECAN Support!

Tools/scripts/build_options.py

@@ -417,6 +417,7 @@ BUILD_OPTIONS = [
     Feature('Actuators', 'KDECAN', 'AP_KDECAN_ENABLED', 'KDE Direct KDECAN ESC', 0, None),
     Feature('Actuators', 'HimarkServo', 'AP_DRONECAN_HIMARK_SERVO_SUPPORT', 'Enable Himark DroneCAN servos', 0, "DroneCAN"),
     Feature('Actuators', 'HobbywingESC', 'AP_DRONECAN_HOBBYWING_ESC_SUPPORT', 'Enable Hobbywing DroneCAN ESCs', 0, "DroneCAN"),
+    Feature('Actuators', 'XiongCaiESC', 'AP_DRONECAN_XCKJ_ESC_SUPPORT','Enable XCKJ DroneCAN ESCs', 0,"DroneCAN"),
 
     Feature('Precision Landing', 'PrecLand', 'AC_PRECLAND_ENABLED', 'Enable Precision landing support', 0, None),
     Feature('Precision Landing', 'PrecLand - Companion', 'AC_PRECLAND_COMPANION_ENABLED', 'Enable Companion computer precision landing ', 0, "PrecLand"),  # noqa
@@ -452,3 +453,4 @@ for x in BUILD_OPTIONS:
 
 if sanity_check_failed:
     raise ValueError("Bad labels in Feature list")
+

libraries/AP_DroneCAN/AP_DroneCAN.cpp

@@ -129,7 +129,7 @@ const AP_Param::GroupInfo AP_DroneCAN::var_info[] = {
     // @Param: OPTION
     // @DisplayName: DroneCAN options
     // @Description: Option flags
-    // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy,4:SendServoAsPWM,5:SendGNSS,6:UseHimarkServo,7:HobbyWingESC,8:EnableStats,9:EnableFlexDebug
+    // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy,4:SendServoAsPWM,5:SendGNSS,6:UseHimarkServo,7:HobbyWingESC,8:EnableStats,9:EnableFlexDebug,10:XiongCaiESC
     // @User: Advanced
     AP_GROUPINFO("OPTION", 5, AP_DroneCAN, _options, 0),
     
@@ -425,6 +425,13 @@ void AP_DroneCAN::init(uint8_t driver_index, bool enable_filters)
     esc_hobbywing_raw.set_priority(CANARD_TRANSFER_PRIORITY_HIGH);
 #endif
 
+#if AP_DRONECAN_XCKJ_ESC_SUPPORT
+    esc_xckj_throtgroup1.set_timeout_ms(2);
+    esc_xckj_throtgroup1.set_priority(CANARD_TRANSFER_PRIORITY_HIGH - 1);
+    esc_xckj_throtgroup2.set_timeout_ms(2);
+    esc_xckj_throtgroup2.set_priority(CANARD_TRANSFER_PRIORITY_HIGH - 1);
+#endif
+
 #if AP_DRONECAN_HIMARK_SERVO_SUPPORT
     himark_out.set_timeout_ms(2);
     himark_out.set_priority(CANARD_TRANSFER_PRIORITY_HIGH);
@@ -552,6 +559,10 @@ void AP_DroneCAN::loop(void)
 #if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
         hobbywing_ESC_update();
 #endif
+
+#if AP_DRONECAN_XCKJ_ESC_SUPPORT
+        xckj_ESC_update();
+#endif
         if (_SRV_armed_mask != 0) {
             // we have active servos
             uint32_t now = AP_HAL::micros();
@@ -662,6 +673,91 @@ void AP_DroneCAN::handle_hobbywing_StatusMsg2(const CanardRxTransfer& transfer,
 }
 #endif // AP_DRONECAN_HOBBYWING_ESC_SUPPORT
 
+#if AP_DRONECAN_XCKJ_ESC_SUPPORT
+void AP_DroneCAN::xckj_ESC_update(void)
+{
+    if (hal.util->get_soft_armed()) {
+        // don't update ID database while disarmed, as it can cause some  ESCs to stutter
+        return;
+    }
+    uint32_t now = AP_HAL::millis();
+    if (now - xckj.last_GetId_Time_Ms >= 1000U) {
+        xckj.last_GetId_Time_Ms = now;
+        com_xckj_esc_OperateId msg;
+        msg.payload.len = 3;
+        msg.payload.data[0] = 2;
+        msg.payload.data[1] = 0;
+        msg.payload.data[2] = 0;
+        esc_xckj_OperateId.broadcast(msg);
+    }
+}
+
+/*
+  handle xckj OperateId reply. This gets us the mapping between CAN NodeID and throttle channel
+ */
+void AP_DroneCAN::handle_xckj_OperateId(const CanardRxTransfer& transfer, const com_xckj_esc_OperateId& msg)
+{
+    if ((msg.payload.len == 3) && (msg.payload.data[0] == 3) &&
+        (msg.payload.data[1] == transfer.source_node_id)) {
+        // throttle channel is 2nd payload byte
+        const uint8_t thr_channel = msg.payload.data[2];
+        if (thr_channel > 0 && thr_channel <= XCKJ_MAX_ESC) {
+            xckj.throt_chan[thr_channel - 1] = transfer.source_node_id;
+        }
+    }
+}
+
+/*
+  find the ESC index given a CAN node ID
+ */
+bool AP_DroneCAN::xckj_find_esc_index(uint8_t node_id, uint8_t& esc_index) const
+{
+    for (uint8_t i = 0; i < XCKJ_MAX_ESC; i++) {
+        if (xckj.throt_chan[i] == node_id) {
+            const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, DRONECAN_SRV_NUMBER);
+            esc_index = i + esc_offset;
+            return true;
+        }
+    }
+    return false;
+}
+
+/*
+  handle xckj AutoUpMsg1 reply
+ */
+void AP_DroneCAN::handle_xckj_AutoUpMsg1(const CanardRxTransfer& transfer, const com_xckj_esc_AutoUpMsg1& msg)
+{
+    uint8_t esc_index;
+    if (xckj_find_esc_index(transfer.source_node_id, esc_index)) {
+        update_rpm(esc_index, msg.rpm * 0.1f);
+        TelemetryData t{
+            .current = msg.current * 0.1f,
+        };
+        update_telem_data(esc_index, t, AP_ESC_Telem_Backend::TelemetryType::CURRENT);
+    }
+}
+
+/*
+  handle xckj StatusMsg2 reply
+ */
+void AP_DroneCAN::handle_xckj_AutoUpMsg2(const CanardRxTransfer& transfer, const com_xckj_esc_AutoUpMsg2& msg)
+{
+    uint8_t esc_index;
+    if (xckj_find_esc_index(transfer.source_node_id, esc_index)) {
+        TelemetryData t{
+            .temperature_cdeg = int16_t(msg.MOS_T - 40),
+            .voltage = msg.voltage * 0.1f,
+            .motor_temp_cdeg = int16_t(msg.Motor_T - 40),
+        };
+        update_telem_data(esc_index, t,
+            AP_ESC_Telem_Backend::TelemetryType::VOLTAGE |
+            AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE |
+            AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE);
+    }
+
+}
+#endif // AP_DRONECAN_XCKJ_ESC_SUPPORT
+
 void AP_DroneCAN::send_node_status(void)
 {
     const uint32_t now = AP_HAL::millis();
@@ -926,6 +1022,78 @@ void AP_DroneCAN::SRV_send_esc_hobbywing(void)
 }
 #endif // AP_DRONECAN_HOBBYWING_ESC_SUPPORT
 
+#if AP_DRONECAN_XCKJ_ESC_SUPPORT
+/*
+  support for xckj DroneCAN ESCs
+ */
+void AP_DroneCAN::SRV_send_esc_xckj(void)
+{
+    com_xckj_esc_ThrotGroup1 throtgroup1_msg;
+    com_xckj_esc_ThrotGroup2 throtgroup2_msg;
+
+    uint8_t active_esc_num = 0, max_esc_num = 0;
+    uint8_t k = 0;
+
+    // esc offset allows for efficient packing of higher ESC numbers in RawCommand
+    const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, DRONECAN_SRV_NUMBER);
+
+    // find out how many esc we have enabled and if they are active at all
+    for (uint8_t i = esc_offset; i < DRONECAN_SRV_NUMBER; i++) {
+        if ((((uint32_t)1) << i) & _ESC_armed_mask) {
+            max_esc_num = i + 1;
+            if (_SRV_conf[i].esc_pending) {
+                active_esc_num++;
+            }
+        }
+    }
+
+    // if at least one is active (update) we need to send to all
+    if (active_esc_num > 0) {
+        k = 0;
+        const bool armed = hal.util->get_soft_armed();
+        for (uint8_t i = esc_offset; k < 8; i++) {
+            if (k < 4) {
+                if (armed && ((((uint32_t)1U) << i) & _ESC_armed_mask)) {
+                    throtgroup1_msg.command.data[k] = scale_esc_output(i);
+                }
+                else {
+                    throtgroup1_msg.command.data[k] = static_cast<unsigned>(0);
+                }
+            }
+            else if (max_esc_num > esc_offset + 4) {
+                if (armed && ((((uint32_t)1U) << i) & _ESC_armed_mask)) {
+                    throtgroup2_msg.command.data[k - 4] = scale_esc_output(i);
+                }
+                else {
+                    throtgroup2_msg.command.data[k - 4] = static_cast<unsigned>(0);
+                }
+            }
+            k++;
+        }
+
+        throtgroup1_msg.command.len = 4;
+        if (esc_xckj_throtgroup1.broadcast(throtgroup1_msg)) {
+            _esc_send_count++;
+        }
+        else {
+            _fail_send_count++;
+        }
+        if (max_esc_num > esc_offset + 4) {
+            throtgroup2_msg.command.len = 4;
+            if (esc_xckj_throtgroup2.broadcast(throtgroup2_msg)) {
+                _esc_send_count++;
+            }
+            else {
+                _fail_send_count++;
+            }
+        }
+        // immediately push data to CAN bus
+        canard_iface.processTx(true);
+    }
+}
+#endif // AP_DRONECAN_XCKJ_ESC_SUPPORT
+
+
 void AP_DroneCAN::SRV_push_servos()
 {
     WITH_SEMAPHORE(SRV_sem);
@@ -958,6 +1126,12 @@ void AP_DroneCAN::SRV_push_servos()
 
     if (_ESC_armed_mask != 0) {
         // push ESCs as fast as we can
+#if AP_DRONECAN_XCKJ_ESC_SUPPORT
+        if (option_is_set(Options::USE_XCKJ_ESC)){
+            SRV_send_esc_xckj();
+        }
+        else{ 
+#endif               
 #if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
             if (option_is_set(Options::USE_HOBBYWING_ESC)) {
                 SRV_send_esc_hobbywing();
@@ -966,6 +1140,9 @@ void AP_DroneCAN::SRV_push_servos()
             {
                 SRV_send_esc();
             }
+#if AP_DRONECAN_XCKJ_ESC_SUPPORT
+        }
+#endif        
     }
 }
 
@@ -1993,3 +2170,4 @@ bool AP_DroneCAN::write_aux_frame(AP_HAL::CANFrame &out_frame, const uint32_t ti
 }
 
 #endif // HAL_NUM_CAN_IFACES
+

libraries/AP_DroneCAN/AP_DroneCAN.h

@@ -57,6 +57,10 @@
 #define AP_DRONECAN_HOBBYWING_ESC_SUPPORT (BOARD_FLASH_SIZE>1024)
 #endif
 
+#ifndef AP_DRONECAN_XCKJ_ESC_SUPPORT
+#define AP_DRONECAN_XCKJ_ESC_SUPPORT (BOARD_FLASH_SIZE>1024)
+#endif
+
 #ifndef AP_DRONECAN_HIMARK_SERVO_SUPPORT
 #define AP_DRONECAN_HIMARK_SERVO_SUPPORT (BOARD_FLASH_SIZE>1024)
 #endif
@@ -150,6 +154,7 @@ public:
         USE_HOBBYWING_ESC         = (1U<<7),
         ENABLE_STATS              = (1U<<8),
         ENABLE_FLEX_DEBUG         = (1U<<9),
+        USE_XCKJ_ESC              = (1U<<10),
     };
 
     // check if a option is set
@@ -408,6 +413,35 @@ private:
     void handle_hobbywing_StatusMsg2(const CanardRxTransfer& transfer, const com_hobbywing_esc_StatusMsg2& msg);
 #endif // AP_DRONECAN_HOBBYWING_ESC_SUPPORT
 
+#if AP_DRONECAN_XCKJ_ESC_SUPPORT
+#define XCKJ_MAX_ESC 8
+    struct {
+        uint32_t last_GetId_Time_Ms;
+        uint8_t throt_chan[XCKJ_MAX_ESC];
+    }xckj;
+    void xckj_ESC_update();
+
+
+    void SRV_send_esc_xckj();
+    Canard::Publisher<com_xckj_esc_ThrotGroup1> esc_xckj_throtgroup1{canard_iface};
+    Canard::Publisher<com_xckj_esc_ThrotGroup2> esc_xckj_throtgroup2{canard_iface};
+
+    Canard::Publisher<com_xckj_esc_OperateId> esc_xckj_OperateId{canard_iface};
+    Canard::ObjCallback<AP_DroneCAN, com_xckj_esc_OperateId> esc_xckj_OperateId_cb{this, &AP_DroneCAN::handle_xckj_OperateId};
+    Canard::Subscriber<com_xckj_esc_OperateId> esc_xckj_OperateId_listener{esc_xckj_OperateId_cb, _driver_index};
+
+    Canard::ObjCallback<AP_DroneCAN, com_xckj_esc_AutoUpMsg1> esc_xckj_AutoUpMsg1_cb{this, &AP_DroneCAN::handle_xckj_AutoUpMsg1};
+    Canard::Subscriber<com_xckj_esc_AutoUpMsg1> esc_xckj_AutoUpMsg1_listener{esc_xckj_AutoUpMsg1_cb, _driver_index};
+
+    Canard::ObjCallback<AP_DroneCAN, com_xckj_esc_AutoUpMsg2> esc_xckj_AutoUpMsg2_cb{this, &AP_DroneCAN::handle_xckj_AutoUpMsg2};
+    Canard::Subscriber<com_xckj_esc_AutoUpMsg2> esc_xckj_AutoUpMsg2_listener{esc_xckj_AutoUpMsg2_cb, _driver_index};
+
+    bool xckj_find_esc_index(uint8_t node_id, uint8_t& esc_index) const;
+    void handle_xckj_OperateId(const CanardRxTransfer& transfer, const com_xckj_esc_OperateId& msg);
+    void handle_xckj_AutoUpMsg1(const CanardRxTransfer& transfer, const com_xckj_esc_AutoUpMsg1& msg);
+    void handle_xckj_AutoUpMsg2(const CanardRxTransfer& transfer, const com_xckj_esc_AutoUpMsg2& msg);
+#endif // AP_DRONECAN_XCKJ_ESC_SUPPORT
+
 #if AP_DRONECAN_HIMARK_SERVO_SUPPORT && AP_SERVO_TELEM_ENABLED
     void handle_himark_servoinfo(const CanardRxTransfer& transfer, const com_himark_servo_ServoInfo &msg);
 #endif
@@ -443,3 +477,4 @@ private:
 };
 
 #endif // #if HAL_ENABLE_DRONECAN_DRIVERS
+