diff --git a/libraries/APM_Control/AR_AttitudeControl.cpp b/libraries/APM_Control/AR_AttitudeControl.cpp index 54a9729f4d..f3f9974d06 100644 --- a/libraries/APM_Control/AR_AttitudeControl.cpp +++ b/libraries/APM_Control/AR_AttitudeControl.cpp @@ -159,6 +159,15 @@ const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = { // @User: Standard AP_GROUPINFO("_STR_RAT_MAX", 8, AR_AttitudeControl, _steer_rate_max, AR_ATTCONTROL_STEER_RATE_MAX), + // @Param: _DECEL_MAX + // @DisplayName: Speed control deceleration maximum in m/s/s + // @Description: Speed control and deceleration maximum in m/s/s. 0 to disable deceleration limiting + // @Range: 0.0 10.0 + // @Increment: 0.1 + // @Units: m/s/s + // @User: Standard + AP_GROUPINFO("_DECEL_MAX", 9, AR_AttitudeControl, _throttle_decel_max, 0.00f), + AP_GROUPEND }; @@ -446,6 +455,15 @@ bool AR_AttitudeControl::get_forward_speed(float &speed) const return true; } +float AR_AttitudeControl::get_decel_max() +{ + if (is_positive(_throttle_decel_max)) { + return _throttle_decel_max; + } else { + return _throttle_accel_max; + } +} + // get latest desired speed recorded during call to get_throttle_out_speed. For reporting purposes only float AR_AttitudeControl::get_desired_speed() const { @@ -473,7 +491,12 @@ float AR_AttitudeControl::get_desired_speed_accel_limited(float desired_speed) c } // acceleration limit desired speed - const float speed_change_max = _throttle_accel_max * dt; + float speed_change_max; + if (fabsf(desired_speed) < fabsf(_desired_speed) && is_positive(_throttle_decel_max)) { + speed_change_max = _throttle_decel_max * dt; + } else { + speed_change_max = _throttle_accel_max * dt; + } return constrain_float(desired_speed, speed_prev - speed_change_max, speed_prev + speed_change_max); } diff --git a/libraries/APM_Control/AR_AttitudeControl.h b/libraries/APM_Control/AR_AttitudeControl.h index 335e6f55c0..d07301d229 100644 --- a/libraries/APM_Control/AR_AttitudeControl.h +++ b/libraries/APM_Control/AR_AttitudeControl.h @@ -92,6 +92,9 @@ public: // get throttle/speed controller maximum acceleration (also used for deceleration) float get_accel_max() const { return MAX(_throttle_accel_max, 0.0f); } + // get throttle/speed controller maximum deceleration + float get_decel_max(); + // get latest desired speed recorded during call to get_throttle_out_speed. For reporting purposes only float get_desired_speed() const; @@ -114,6 +117,7 @@ private: AC_PID _steer_rate_pid; // steering rate controller AC_PID _throttle_speed_pid; // throttle speed controller AP_Float _throttle_accel_max; // speed/throttle control acceleration (and deceleration) maximum in m/s/s. 0 to disable limits + AP_Float _throttle_decel_max; // speed/throttle control deceleration maximum in m/s/s. 0 to disable limits AP_Int8 _brake_enable; // speed control brake enable/disable. if set to 1 a reversed output to the motors to slow the vehicle. AP_Float _stop_speed; // speed control stop speed. Motor outputs to zero once vehicle speed falls below this value AP_Float _steer_accel_max; // steering angle acceleration max in deg/s/s