From ba2cfee2fbe7a98a32ddad8dc8dc239ba5d3b62a Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 19 Feb 2012 12:40:51 -0800 Subject: [PATCH] Updated manual throttle control during throttle hold --- ArduCopter/ArduCopter.pde | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index c7534e4413..91c5ea52aa 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1602,9 +1602,12 @@ void update_throttle_mode(void) manual_boost, iterm); //*/ + // this lets us know we need to update the altitude after manual throttle control reset_throttle_flag = true; }else{ + // we are under automatic throttle control + // --------------------------------------- if(reset_throttle_flag) { set_new_altitude(max(current_loc.alt, 100)); reset_throttle_flag = false; @@ -1920,15 +1923,16 @@ static void update_altitude() static void adjust_altitude() { - if(g.rc_3.control_in <= 180){ + if(g.rc_3.control_in <= (MINIMUM_THROTTLE + 100)){ // we remove 0 to 100 PWM from hover - manual_boost = g.rc_3.control_in - 180; - manual_boost = max(-120, manual_boost); + manual_boost = (g.rc_3.control_in - MINIMUM_THROTTLE) -100; + manual_boost = max(-100, manual_boost); update_throttle_cruise(); - }else if (g.rc_3.control_in >= 650){ + }else if (g.rc_3.control_in >= (MAXIMUM_THROTTLE - 100)){ // we add 0 to 100 PWM to hover - manual_boost = g.rc_3.control_in - 650; + manual_boost = g.rc_3.control_in - (MAXIMUM_THROTTLE - 100); + manual_boost = min(100, manual_boost); update_throttle_cruise(); }else { manual_boost = 0;