diff --git a/libraries/APM_Control/AP_SteerController.cpp b/libraries/APM_Control/AP_SteerController.cpp index 6cbd0ecabebd5e..b1a8e987c26607 100644 --- a/libraries/APM_Control/AP_SteerController.cpp +++ b/libraries/APM_Control/AP_SteerController.cpp @@ -83,6 +83,34 @@ const AP_Param::GroupInfo AP_SteerController::var_info[] = { // @User: User AP_GROUPINFO("FF", 7, AP_SteerController, _K_FF, 0), + + // @Param: DRTSPD + // @DisplayName: Derating speed + // @Description: Speed after that the maximum degree of steering will start to derate. Set this speed to a maximum speed that a plane can do controlled turn at maximum angle of steering wheel without rolling to wing. If 0 then no derating is used. + // @Range: 0.0 30.0 + // @Increment: 0.1 + // @Units: m/s + // @User: Advanced + AP_GROUPINFO("DRTSPD", 8, AP_SteerController, _deratespeed, 0), + + // @Param: DRTFCT + // @DisplayName: Derating factor + // @Description: Degrees of steering wheel to derate at each additional m/s of speed above "Derating speed". Should be set so that at higher speeds the plane does not roll to the wing in turns. + // @Range: 0.0 50.0 + // @Increment: 0.1 + // @Units: degree/(m/s) + // @User: Advanced + AP_GROUPINFO("DRTFCT", 9, AP_SteerController, _deratefactor, 10), + + // @Param: DRTMIN + // @DisplayName: Minimum angle of wheel + // @Description: The angle that limits smallest angle of steering wheel at maximum speed. Even if it should derate below, it will stop derating at this angle. + // @Range: 0.0 4500.0 + // @Increment: 0.1 + // @Units: Centidegrees + // @User: Advanced + AP_GROUPINFO("DRTMIN", 10, AP_SteerController, _mindegree, 4500), + AP_GROUPEND }; @@ -102,7 +130,7 @@ int32_t AP_SteerController::get_steering_out_rate(float desired_rate) float speed = _ahrs.groundspeed(); if (speed < _minspeed) { - // assume a minimum speed. This stops osciallations when first starting to move + // assume a minimum speed. This stops oscillations when first starting to move speed = _minspeed; } @@ -127,8 +155,8 @@ int32_t AP_SteerController::get_steering_out_rate(float desired_rate) float k_ff = _K_FF * 45.0f; float delta_time = (float)dt * 0.001f; - // Multiply roll rate error by _ki_rate and integrate - // Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs + // Multiply yaw rate error by _ki_rate and integrate + // Don't integrate if in stabilize mode as the integrator will wind up against the pilots inputs if (ki_rate > 0 && speed >= _minspeed) { // only integrate if gain and time step are positive. if (dt > 0) { @@ -156,11 +184,21 @@ int32_t AP_SteerController::get_steering_out_rate(float desired_rate) _pid_info.P = (ToRad(desired_rate) * kp_ff) * scaler; _pid_info.FF = (ToRad(desired_rate) * k_ff) * scaler; - // Calculate the demanded control surface deflection - _last_out = _pid_info.D + _pid_info.FF + _pid_info.P + _pid_info.I; + // Calculate the demanded control surface deflection + _last_out = _pid_info.D + _pid_info.FF + _pid_info.P + _pid_info.I; - // Convert to centi-degrees and constrain - return constrain_float(_last_out * 100, -4500, 4500); + float derate_constraint = 4500; + + // Calculate required constrain based on speed + if (!is_zero(_deratespeed) && speed > _deratespeed) { + derate_constraint = 4500 - (speed - _deratespeed) * _deratefactor * 100; + if (derate_constraint < _mindegree) { + derate_constraint = _mindegree; + } + } + + // Convert to centi-degrees and constrain + return constrain_float(_last_out * 100, -derate_constraint, derate_constraint); } diff --git a/libraries/APM_Control/AP_SteerController.h b/libraries/APM_Control/AP_SteerController.h index 44d06f456503a7..93a85a8d432d66 100644 --- a/libraries/APM_Control/AP_SteerController.h +++ b/libraries/APM_Control/AP_SteerController.h @@ -59,6 +59,10 @@ class AP_SteerController { uint32_t _last_t; float _last_out; + AP_Float _deratespeed; + AP_Float _deratefactor; + AP_Float _mindegree; + DataFlash_Class::PID_Info _pid_info {}; AP_AHRS &_ahrs;