From 54b320a9a0e7e71965fd2e915929252b7883819e Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Tue, 31 Jul 2012 17:47:25 -0400 Subject: [PATCH] Fixing Trad Heli Ext ESC Controller -Added ramp-down rate instead of instantly setting ramp to zero when throttle is dropped to bottom. This is to allow "warm-restart" if shutdown was unintentional. -Actual ESC still goes to zero while throttle is dropped to the bottom, only the ramp counter winds down slowly behind the scenes. --- libraries/AP_Motors/AP_MotorsHeli.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 4c30ebbd5a..43d74f7a69 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -487,7 +487,7 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll void AP_MotorsHeli::ext_esc_control() { - switch ( AP_MOTORS_ESC_MODE_PASSTHROUGH ) { + switch ( AP_MOTORS_EXT_ESC_ACTIVE ) { case AP_MOTORS_ESC_MODE_PASSTHROUGH: if( armed() && _rc_8->control_in > 10 ){ @@ -513,7 +513,10 @@ void AP_MotorsHeli::ext_esc_control() ext_esc_output = ext_gov_setpoint; } } else { - ext_esc_ramp = 0; //Return ESC Ramp to 0 + ext_esc_ramp--; //Return ESC Ramp to 0 + if (ext_esc_ramp < 0){ + ext_esc_ramp = 0; + } ext_esc_output = 1000; //Just to be sure ESC output is 0 } _rc->OutputCh(AP_MOTORS_HELI_EXT_ESC, ext_esc_output);