From 996b36531b400b8981f5f3d6bf8fc1f88235b37c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 14 Oct 2024 12:10:26 +1100 Subject: [PATCH] AP_DroneCAN: force DroneCAN zero throttle when disarmed if a user has set CAN_D1_UC_ESC_RV which is the mask of ESCs that are reversible we were sending -8191 when disarmed, which is full reverse throttle. This is the correct output when armed as it is treated as full reverse at "PWM" 1000 and stopped at 1500, but when disarmed we should always send zero or the user may find all ESCs spin up at full reverse when disarmed if the ESC supports reverse throttle (which is rare in DroneCAN ESCs) --- libraries/AP_DroneCAN/AP_DroneCAN.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index 59174a778b..49aace0952 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -844,9 +844,9 @@ void AP_DroneCAN::SRV_send_esc(void) // 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; i < max_esc_num && k < 20; i++) { - if ((((uint32_t) 1) << i) & _ESC_armed_mask) { + if (armed && ((((uint32_t) 1U) << i) & _ESC_armed_mask)) { esc_msg.cmd.data[k] = scale_esc_output(i); } else { esc_msg.cmd.data[k] = static_cast(0); @@ -897,9 +897,9 @@ void AP_DroneCAN::SRV_send_esc_hobbywing(void) // 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; i < max_esc_num && k < 20; i++) { - if ((((uint32_t) 1) << i) & _ESC_armed_mask) { + if (armed && ((((uint32_t) 1U) << i) & _ESC_armed_mask)) { esc_msg.command.data[k] = scale_esc_output(i); } else { esc_msg.command.data[k] = static_cast(0);