From 8e707ef4e2e4857f19dffd2dcb8c93fcebe8bf74 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Mon, 22 Jul 2024 23:49:22 +1000 Subject: [PATCH] AP_DroneCAN: prevent invalid numbers spinning ESCs This prevents bugs and misconfigurations from causing DroneCAN ESCs to spin at full speed while the vehicle is disarmed. --- libraries/AP_DroneCAN/AP_DroneCAN.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index 5e5637fee1..bc751e899e 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -709,14 +709,17 @@ void AP_DroneCAN::handle_node_info_request(const CanardRxTransfer& transfer, con int16_t AP_DroneCAN::scale_esc_output(uint8_t idx){ static const int16_t cmd_max = ((1<<13)-1); - float scaled = 0; - + float scaled = hal.rcout->scale_esc_to_unity(_SRV_conf[idx].pulse); + // Prevent invalid values (from misconfigured scaling parameters) from sending non-zero commands + if (!isfinite(scaled)) { + return 0; + } + scaled = constrain_float(scaled, -1, 1); //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)); + scaled *= cmd_max; } 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); + scaled = cmd_max * (scaled + 1.0) / 2.0; } return static_cast(scaled);