From ab24e7e489a8010068079d5a34e7cc6646da458d Mon Sep 17 00:00:00 2001 From: Bob Long Date: Mon, 22 Jul 2024 23:49:22 +1000 Subject: [PATCH 1/3] 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 5e5637fee1779..bc751e899e78b 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); From 97ee83552a90f5531fbda8520bd448ea071c605e Mon Sep 17 00:00:00 2001 From: Bob Long Date: Tue, 23 Jul 2024 00:02:27 +1000 Subject: [PATCH 2/3] Plane: try left/right throttle to init ESC scaling Without this, twin motor planes with DroneCAN ESCs need to set a dummy throttle channel for scale_esc_to_unity to work. --- ArduPlane/radio.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 3862b390b7245..cb2048be6d42f 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -58,6 +58,8 @@ void Plane::set_control_channels(void) // setup correct scaling for ESCs like the UAVCAN ESCs which // take a proportion of speed. For quadplanes we use AP_Motors // scaling + g2.servo_channels.set_esc_scaling_for(SRV_Channel::k_throttleLeft); + g2.servo_channels.set_esc_scaling_for(SRV_Channel::k_throttleRight); g2.servo_channels.set_esc_scaling_for(SRV_Channel::k_throttle); } } From 3534d3a58b7ca70880499a2ab6128e766bbfb183 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Tue, 6 Aug 2024 09:21:18 +1000 Subject: [PATCH 3/3] RCOutput: initialize _esc_pwm_min and _esc_pwm_max Further protection from potential dangerous behavior when these do not get initialized for some reason. --- libraries/AP_HAL/RCOutput.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL/RCOutput.h b/libraries/AP_HAL/RCOutput.h index 456cab154c6f6..68a84b1a10fcd 100644 --- a/libraries/AP_HAL/RCOutput.h +++ b/libraries/AP_HAL/RCOutput.h @@ -422,6 +422,6 @@ class AP_HAL::RCOutput { void append_to_banner(char banner_msg[], uint8_t banner_msg_len, output_mode out_mode, uint8_t low_ch, uint8_t high_ch) const; const char* get_output_mode_string(enum output_mode out_mode) const; - uint16_t _esc_pwm_min; - uint16_t _esc_pwm_max; + uint16_t _esc_pwm_min = 1000; + uint16_t _esc_pwm_max = 2000; };