From 7c6551af9b8f106f0cf8ce950cf7fb39e2bf4f41 Mon Sep 17 00:00:00 2001 From: Fred Kummer Date: Wed, 19 Jul 2023 15:08:39 -0400 Subject: [PATCH] AP_DroneCAN: allow sending negative RawCommands to ESCs --- libraries/AP_DroneCAN/AP_DroneCAN.cpp | 37 +++++++++++++++++---------- libraries/AP_DroneCAN/AP_DroneCAN.h | 4 +++ 2 files changed, 27 insertions(+), 14 deletions(-) diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index c9e99cb07d..a886707e8f 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -141,6 +141,13 @@ const AP_Param::GroupInfo AP_DroneCAN::var_info[] = { // @Range: 1024 16384 // @User: Advanced AP_GROUPINFO("POOL", 8, AP_DroneCAN, _pool_size, DRONECAN_NODE_POOL_SIZE), + + // @Param: ESC_RV + // @DisplayName: Bitmask for output channels for reversible ESCs over DroneCAN. + // @Description: Bitmask with one set for each output channel that uses a reversible ESC over DroneCAN. Reversible ESCs use both positive and negative values in RawCommands, with positive commanding the forward direction and negative commanding the reverse direction. + // @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, 16: ESC 17, 17: ESC 18, 18: ESC 19, 19: ESC 20, 20: ESC 21, 21: ESC 22, 22: ESC 23, 23: ESC 24, 24: ESC 25, 25: ESC 26, 26: ESC 27, 27: ESC 28, 28: ESC 29, 29: ESC 30, 30: ESC 31, 31: ESC 32 + // @User: Advanced + AP_GROUPINFO("ESC_RV", 9, AP_DroneCAN, _esc_rv, 0), AP_GROUPEND }; @@ -540,6 +547,20 @@ void AP_DroneCAN::handle_node_info_request(const CanardRxTransfer& transfer, con node_info_server.respond(transfer, node_info_rsp); } +int16_t AP_DroneCAN::scale_esc_output(uint8_t idx){ + static const int16_t cmd_max = ((1<<13)-1); + float scaled = 0; + + //Check if this channel has a reversible ESC. If it does, we can send negative commands. + if ((((uint32_t) 1) << idx) & _esc_rv) { + scaled = cmd_max * (hal.rcout->scale_esc_to_unity(_SRV_conf[idx].pulse)); + } else { + scaled = cmd_max * (hal.rcout->scale_esc_to_unity(_SRV_conf[idx].pulse) + 1.0) / 2.0; + scaled = constrain_float(scaled, 0, cmd_max); + } + + return static_cast(scaled); +} ///// SRV output ///// @@ -633,7 +654,6 @@ void AP_DroneCAN::SRV_send_himark(void) void AP_DroneCAN::SRV_send_esc(void) { - static const int cmd_max = ((1<<13)-1); uavcan_equipment_esc_RawCommand esc_msg; uint8_t active_esc_num = 0, max_esc_num = 0; @@ -658,12 +678,7 @@ void AP_DroneCAN::SRV_send_esc(void) for (uint8_t i = esc_offset; i < max_esc_num && k < 20; i++) { if ((((uint32_t) 1) << i) & _ESC_armed_mask) { - // 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; - - scaled = constrain_float(scaled, 0, cmd_max); - - esc_msg.cmd.data[k] = static_cast(scaled); + esc_msg.cmd.data[k] = scale_esc_output(i); } else { esc_msg.cmd.data[k] = static_cast(0); } @@ -692,7 +707,6 @@ void AP_DroneCAN::SRV_send_esc(void) */ void AP_DroneCAN::SRV_send_esc_hobbywing(void) { - static const int cmd_max = ((1<<13)-1); com_hobbywing_esc_RawCommand esc_msg; uint8_t active_esc_num = 0, max_esc_num = 0; @@ -717,12 +731,7 @@ void AP_DroneCAN::SRV_send_esc_hobbywing(void) for (uint8_t i = esc_offset; i < max_esc_num && k < 20; i++) { if ((((uint32_t) 1) << i) & _ESC_armed_mask) { - // 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; - - scaled = constrain_float(scaled, 0, cmd_max); - - esc_msg.command.data[k] = static_cast(scaled); + esc_msg.command.data[k] = scale_esc_output(i); } else { esc_msg.command.data[k] = static_cast(0); } diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h index a1dbaeca8c..eb3b26a824 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.h +++ b/libraries/AP_DroneCAN/AP_DroneCAN.h @@ -147,6 +147,9 @@ private: void SRV_send_esc(); void SRV_send_himark(); + //scale servo output appropriately before sending + int16_t scale_esc_output(uint8_t idx); + // SafetyState void safety_state_send(); @@ -188,6 +191,7 @@ private: AP_Int16 _options; AP_Int16 _notify_state_hz; AP_Int16 _pool_size; + AP_Int32 _esc_rv; uint32_t *mem_pool;