dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fw att: yaw ctrl: robustify against non finite numbers

This commit is contained in:
Thomas Gubler 2014-05-18 00:16:10 +02:00
parent 52596be98c
commit 9a35bac2ad
1 changed files with 12 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
View File

@ -66,6 +66,12 @@ float ECL_YawController::control_attitude(float roll, float pitch,
float speed_body_u, float speed_body_v, float speed_body_w,
float roll_rate_setpoint, float pitch_rate_setpoint)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(roll) && isfinite(pitch) && isfinite(speed_body_u) && isfinite(speed_body_v) &&
isfinite(speed_body_w) && isfinite(roll_rate_setpoint) &&
isfinite(pitch_rate_setpoint))) {
return _rate_setpoint;
}
// static int counter = 0;
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
_rate_setpoint = 0.0f;
@ -103,6 +109,12 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
float pitch_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(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) &&
isfinite(pitch_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();