aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_AHRS: add sanity checks

Browse Source
This commit is contained in:
Jonathan Challinger 2015-03-22 17:03:54 -07:00 committed by Randy Mackay
parent c6f71ea2e0
commit 6a6ccb4f32
1 changed files with 16 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -256,6 +256,22 @@ void AP_AHRS::update_trig(void)
// sin_roll, sin_pitch // sin_roll, sin_pitch
_sin_pitch = -temp.c.x; _sin_pitch = -temp.c.x;
_sin_roll = temp.c.y / _cos_pitch; _sin_roll = temp.c.y / _cos_pitch;
// sanity checks
if (yaw_vector.is_inf() || yaw_vector.is_nan()) {
yaw_vector.x = 0.0f;
yaw_vector.y = 0.0f;
_sin_yaw = 1.0f;
_cos_yaw = 0.0f;
}
if (isinf(_cos_roll) || isnan(_cos_roll)) {
_cos_roll = cosf(roll);
}
if (isinf(_sin_roll) || isnan(_sin_roll)) {
_sin_roll = sinf(roll);
}
} }
/* /*