diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index d39c261b73..667fe557bb 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -121,8 +121,8 @@ get_nav_throttle(int32_t z_error) { int16_t rate_error; - float dt = (abs(z_error) < 400) ? .1 : 0.0; - //float dt = .1; + // XXX HACK, need a better way not to ramp this i term in large altitude changes. + float dt = (abs(z_error) < 400) ? .1 : 0.01; // limit error to prevent I term run up z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX); diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 0e38a00a6d..a0178b4c24 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -51,9 +51,6 @@ static void read_trim_switch() do_flip = true; } - #elif CH7_OPTION == CH7_SIMPLE_MODE - //Serial.println(g.rc_7.control_in, DEC); - #elif CH7_OPTION == CH7_SET_HOVER // switch is engaged if (g.rc_7.control_in > 800){