fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 21:47:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw att: roll ctrl: robustify against non finite numbers

Browse Source
This commit is contained in:
Thomas Gubler
2014-05-18 00:15:50 +02:00
parent 7c165689ce
commit 52596be98c
1 changed files with 13 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+13 -2
View File
@@ -65,6 +65,10 @@ ECL_RollController::ECL_RollController() :
float ECL_RollController::control_attitude(float roll_setpoint, float roll)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(roll_setpoint) && isfinite(roll))) {
return _rate_setpoint;
}
/* Calculate error */
float roll_error = roll_setpoint - roll;
@@ -86,6 +90,13 @@ float ECL_RollController::control_bodyrate(float pitch,
float yaw_rate_setpoint,
float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(pitch) && isfinite(roll_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) &&
isfinite(airspeed_min) && isfinite(airspeed_max) &&
isfinite(scaler))) {
return math::constrain(_last_output, -1.0f, 1.0f);
}
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
@@ -122,8 +133,8 @@ float ECL_RollController::control_bodyrate(float pitch,
float id = _rate_error * dt;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);