diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index e57eab2127..f634fa101f 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -64,4 +64,10 @@ #define USERHOOK_VARIABLES "UserVariables.h" +// to enable, set to 1 +// to disable, set to 0 +#define AUTO_THROTTLE_HOLD 1 + + + //# define LOGGING_ENABLED DISABLED \ No newline at end of file diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 0ce2deace4..9d86beab8b 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1494,6 +1494,10 @@ void update_throttle_mode(void) { int16_t throttle_out; + #if AUTO_THROTTLE_HOLD != 0 + static float throttle_avg = THROTTLE_CRUISE; + #endif + switch(throttle_mode){ case THROTTLE_MANUAL: if (g.rc_3.control_in > 0){ @@ -1508,11 +1512,13 @@ void update_throttle_mode(void) } #endif + #if AUTO_THROTTLE_HOLD != 0 // calc average throttle - if ((g.rc_3.control_in > MINIMUM_THROTTLE)){ - //throttle_avg = throttle_avg * .98 + rc_3.control_in * .02; - //g.throttle_cruise = throttle_avg; + if ((g.rc_3.control_in > MINIMUM_THROTTLE) && abs(climb_rate) < 60){ + throttle_avg = throttle_avg * .98 + (float)g.rc_3.control_in * .02; + g.throttle_cruise = throttle_avg; } + #endif // Code to manage the Copter state if ((millis() - takeoff_timer) > 5000){ diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 7c05c0b400..7b81974192 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -121,6 +121,8 @@ static void init_disarm_motors() motor_armed = false; compass.save_offsets(); + g.throttle_cruise.save(); + // we are not in the air takeoff_complete = false; diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index f5ffa7241a..d783c5b5d7 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -616,12 +616,14 @@ static void update_throttle_cruise() static void init_throttle_cruise() { +#if AUTO_THROTTLE_HOLD == 0 // are we moving from manual throttle to auto_throttle? if((old_control_mode <= STABILIZE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){ g.pi_throttle.reset_I(); g.pi_alt_hold.reset_I(); g.throttle_cruise.set_and_save(g.rc_3.control_in); } +#endif } #if CLI_SLIDER_ENABLED == ENABLED && CLI_ENABLED == ENABLED