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

Merge pull request #2615 from Zefz/nan-protect

Browse Source 
attitude_estimator_q: Fix NaN protection
This commit is contained in:
Lorenz Meier 2015-07-28 18:41:12 +02:00
commit f226e808b2
1 changed files with 3 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
View File

@ -268,7 +268,7 @@ void AttitudeEstimatorQ::task_main() {
}
}
if (_acc_comp && _gpos.timestamp != 0 && hrt_absolute_time() < _gpos.timestamp + 20000 && _gpos.eph < 5.0f) {
if (_acc_comp && _gpos.timestamp != 0 && hrt_absolute_time() < _gpos.timestamp + 20000 && _gpos.eph < 5.0f && _inited) {
/* position data is actual */
if (gpos_updated) {
Vector<3> vel(_gpos.vel_n, _gpos.vel_e, _gpos.vel_d);
@ -450,6 +450,8 @@ bool AttitudeEstimatorQ::update(float dt) {
PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) {
// Reset quaternion to last good state
_q = q_last;
_rates.zero();
_gyro_bias.zero();
return false;
}