Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

APM_Control: Added derating of steering wheel

This commit is contained in:
Andrew Tridgell 2016-12-14 12:47:22 +11:00 committed by Grant Morphett
parent e497dedf35
commit f6cc506791
2 changed files with 49 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
libraries/APM_Control/AP_SteerController.cpp
View File

@ -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);
}

4
libraries/APM_Control/AP_SteerController.h
View File

@ -59,6 +59,10 @@ private:
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;