diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index ae83948aef536..95d1d8984279d 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -417,6 +417,7 @@ def config_option(self): 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 @@ def config_option(self): if sanity_check_failed: raise ValueError("Bad labels in Feature list") + diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index 58cdc7e1b6a0c..be6e8edfb0dcd 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/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(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(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,14 +1126,23 @@ 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(); - } else + if (option_is_set(Options::USE_HOBBYWING_ESC)) { + SRV_send_esc_hobbywing(); + } else #endif - { - SRV_send_esc(); + { + 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 + diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h index 3e594bac079c2..600f74a946a58 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.h +++ b/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 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { 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 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { 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 esc_xckj_throtgroup1{canard_iface}; + Canard::Publisher esc_xckj_throtgroup2{canard_iface}; + + Canard::Publisher esc_xckj_OperateId{canard_iface}; + Canard::ObjCallback esc_xckj_OperateId_cb{this, &AP_DroneCAN::handle_xckj_OperateId}; + Canard::Subscriber esc_xckj_OperateId_listener{esc_xckj_OperateId_cb, _driver_index}; + + Canard::ObjCallback esc_xckj_AutoUpMsg1_cb{this, &AP_DroneCAN::handle_xckj_AutoUpMsg1}; + Canard::Subscriber esc_xckj_AutoUpMsg1_listener{esc_xckj_AutoUpMsg1_cb, _driver_index}; + + Canard::ObjCallback esc_xckj_AutoUpMsg2_cb{this, &AP_DroneCAN::handle_xckj_AutoUpMsg2}; + Canard::Subscriber 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 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { }; #endif // #if HAL_ENABLE_DRONECAN_DRIVERS +