fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 07:28:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

attitude_fw: constrain integrator properly to prevent building it up over the specified maximum

Browse Source
This commit is contained in:
Andreas Antener
2018-01-13 16:17:01 +01:00
committed by Lorenz Meier
parent c8ab806120
commit 9e16e51d3a
4 changed files with 12 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
attitude_fw/ecl_pitch_controller.cpp
+3 -6
View File
@@ -130,17 +130,14 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
id = math::min(id, 0.0f);
}
_integrator += id * _k_i;
/* add and constrain */
_integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max);
}
/* integrator limit */
//xxx: until start detection is available: integral part in control signal is limited here
float integrator_constrained = math::constrain(_integrator, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
_rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
+ integrator_constrained; //scaler is proportional to 1/airspeed
+ _integrator; //scaler is proportional to 1/airspeed
return math::constrain(_last_output, -1.0f, 1.0f);
}
attitude_fw/ecl_roll_controller.cpp
+3 -6
View File
@@ -119,17 +119,14 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
id = math::min(id, 0.0f);
}
_integrator += id * _k_i;
/* add and constrain */
_integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max);
}
/* integrator limit */
//xxx: until start detection is available: integral part in control signal is limited here
float integrator_constrained = math::constrain(_integrator, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
_rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
+ integrator_constrained; //scaler is proportional to 1/airspeed
+ _integrator; //scaler is proportional to 1/airspeed
return math::constrain(_last_output, -1.0f, 1.0f);
}
attitude_fw/ecl_wheel_controller.cpp
+3 -6
View File
@@ -95,16 +95,13 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da
id = math::min(id, 0.0f);
}
_integrator += id * _k_i;
/* add and constrain */
_integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max);
}
/* integrator limit */
//xxx: until start detection is available: integral part in control signal is limited here
float integrator_constrained = math::constrain(_integrator, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler +
ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler * (_rate_error * _k_p + integrator_constrained);
ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler * (_rate_error * _k_p + _integrator);
return math::constrain(_last_output, -1.0f, 1.0f);
}
attitude_fw/ecl_yaw_controller.cpp
+3 -6
View File
@@ -191,15 +191,12 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data
id = math::min(id, 0.0f);
}
_integrator += id * _k_i;
/* add and constrain */
_integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max);
}
/* integrator limit */
//xxx: until start detection is available: integral part in control signal is limited here
float integrator_constrained = math::constrain(_integrator, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler *
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + _integrator) * ctl_data.scaler *
ctl_data.scaler; //scaler is proportional to 1/airspeed