diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 25eb2ed94a..a0c8d385f1 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -869,7 +869,7 @@ static void update_current_mode(void) // and throttle gives speed in proportion to cruise speed, up // to 50% throttle, then uses nudging above that. float target_speed = channel_throttle->pwm_to_angle() * 0.01 * 2 * g.speed_cruise; - in_reverse = (target_speed < 0); + set_reverse(target_speed < 0); if (in_reverse) { target_speed = constrain_float(target_speed, -g.speed_cruise, 0); } else { @@ -892,7 +892,7 @@ static void update_current_mode(void) // mark us as in_reverse when using a negative throttle to // stop AHRS getting off - in_reverse = (channel_throttle->servo_out < 0); + set_reverse(channel_throttle->servo_out < 0); break; case HOLD: diff --git a/APMrover2/radio.pde b/APMrover2/radio.pde index 1962249ed4..2de38d456d 100644 --- a/APMrover2/radio.pde +++ b/APMrover2/radio.pde @@ -53,8 +53,8 @@ static void read_radio() channel_throttle->servo_out = channel_throttle->control_in; - if (channel_throttle->servo_out > 50) { - throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((channel_throttle->norm_input()-0.5) / 0.5); + if (abs(channel_throttle->servo_out) > 50) { + throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((fabsf(channel_throttle->norm_input())-0.5) / 0.5); } else { throttle_nudge = 0; } diff --git a/APMrover2/system.pde b/APMrover2/system.pde index f49002a4d0..d1137906a8 100644 --- a/APMrover2/system.pde +++ b/APMrover2/system.pde @@ -242,6 +242,19 @@ static void startup_ground(void) gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to drive.")); } +/* + set the in_reverse flag + reset the throttle integrator if this changes in_reverse + */ +static void set_reverse(bool reverse) +{ + if (in_reverse == reverse) { + return; + } + g.pidSpeedThrottle.reset_I(); + in_reverse = reverse; +} + static void set_mode(enum mode mode) { @@ -252,7 +265,7 @@ static void set_mode(enum mode mode) control_mode = mode; throttle_last = 0; throttle = 500; - in_reverse = false; + set_reverse(false); g.pidSpeedThrottle.reset_I(); if (control_mode != AUTO) {