From 8106291bfe278fdddf4bae7c12e3cb8ba0249357 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 18 May 2022 16:35:28 +1000 Subject: [PATCH] AP_UAVCAN: added CAN_Dx_UC_ESC_OF parameter this allows for an offset in ESC numbering for much more efficient CAN bandwidth usage. For example, on a coaxial OctoQuad quadplane the ESCs are typically setup as outputs 5 to 12. An ideal setup is to split these over 2 CAN buses, with one CAN bus for the top layer and the one bus for the bottom layer (allowing for VTOL flight with one bus failed). Without this offset parameter you would be sending RawCommand messages like this: bus1: [ 0, 0, 0, 0, ESC1, ESC2, ESC3, ESC4 ] bus2: [ 0, 0, 0, 0, 0, 0, 0, 0, ESC1, ESC2, ESC3, ESC4 ] this is very wasteful of bus bandwidth, with bus1 using 3x the bandwidth it should and bus2 using 4x the bandwidth it should (the above will take 3 can frames for bus1, and 4 can frames for bus 2) With this patch you can set: CAN_D1_UC_ESC_OF = 4 CAN_D2_UC_ESC_OF = 8 and you will get this on the bus: bus1: [ ESC1, ESC2, ESC3, ESC4 ] bus2: [ ESC1, ESC2, ESC3, ESC4 ] that takes just 1 can frame per send on each bus --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 21 ++++++++++++++++----- libraries/AP_UAVCAN/AP_UAVCAN.h | 1 + 2 files changed, 17 insertions(+), 5 deletions(-) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index de77e3f300..37f991d8d3 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -103,14 +103,14 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = { AP_GROUPINFO("NODE", 1, AP_UAVCAN, _uavcan_node, 10), // @Param: SRV_BM - // @DisplayName: RC Out channels to be transmitted as servo over UAVCAN + // @DisplayName: Output channels to be transmitted as servo over UAVCAN // @Description: Bitmask with one set for channel to be transmitted as a servo command over UAVCAN // @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8, 8: Servo 9, 9: Servo 10, 10: Servo 11, 11: Servo 12, 12: Servo 13, 13: Servo 14, 14: Servo 15 // @User: Advanced AP_GROUPINFO("SRV_BM", 2, AP_UAVCAN, _servo_bm, 0), // @Param: ESC_BM - // @DisplayName: RC Out channels to be transmitted as ESC over UAVCAN + // @DisplayName: Output channels to be transmitted as ESC over UAVCAN // @Description: Bitmask with one set for channel to be transmitted as a ESC command over UAVCAN // @Bitmask: 0: ESC 1, 1: ESC 2, 2: ESC 3, 3: ESC 4, 4: ESC 5, 5: ESC 6, 6: ESC 7, 7: ESC 8, 8: ESC 9, 9: ESC 10, 10: ESC 11, 11: ESC 12, 12: ESC 13, 13: ESC 14, 14: ESC 15, 15: ESC 16 // @User: Advanced @@ -139,6 +139,13 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = { // @User: Advanced AP_GROUPINFO("NTF_RT", 6, AP_UAVCAN, _notify_state_hz, 20), + // @Param: ESC_OF + // @DisplayName: ESC Output channels offset + // @Description: Offset for ESC numbering in DroneCAN ESC RawCommand messages. This allows for more efficient packing of ESC command messages. If your ESCs are on servo functions 5 to 8 and you set this parameter to 4 then the ESC RawCommand will be sent with the first 4 slots filled. This can be used for more efficint usage of CAN bandwidth + // @Range: 0 18 + // @User: Advanced + AP_GROUPINFO("ESC_OF", 7, AP_UAVCAN, _esc_offset, 0), + AP_GROUPEND }; @@ -530,8 +537,11 @@ void AP_UAVCAN::SRV_send_esc(void) WITH_SEMAPHORE(SRV_sem); + // esc offset allows for efficient packing of higher ESC numbers in RawCommand + const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, UAVCAN_SRV_NUMBER); + // find out how many esc we have enabled and if they are active at all - for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) { + for (uint8_t i = esc_offset; i < UAVCAN_SRV_NUMBER; i++) { if ((((uint32_t) 1) << i) & _esc_bm) { max_esc_num = i + 1; if (_SRV_conf[i].esc_pending) { @@ -544,7 +554,7 @@ void AP_UAVCAN::SRV_send_esc(void) if (active_esc_num > 0) { k = 0; - for (uint8_t i = 0; i < max_esc_num && k < 20; i++) { + for (uint8_t i = esc_offset; i < max_esc_num && k < 20; i++) { if ((((uint32_t) 1) << i) & _esc_bm) { // TODO: ESC negative scaling for reverse thrust and reverse rotation float scaled = cmd_max * (hal.rcout->scale_esc_to_unity(_SRV_conf[i].pulse) + 1.0) / 2.0; @@ -949,7 +959,8 @@ void AP_UAVCAN::handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, co void AP_UAVCAN::handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb) { #if HAL_WITH_ESC_TELEM - const uint8_t esc_index = cb.msg->esc_index; + 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; diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index a285afa92f..7048225fbe 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -260,6 +260,7 @@ private: AP_Int8 _uavcan_node; AP_Int32 _servo_bm; AP_Int32 _esc_bm; + AP_Int8 _esc_offset; AP_Int16 _servo_rate_hz; AP_Int16 _options; AP_Int16 _notify_state_hz;