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

Merge pull request #764 from PX4/inav_nan_checks 

position_estimator_inav: added NaN checks
This commit is contained in:
Thomas Gubler 2014-03-21 17:28:40 +01:00
parent 057bcf3172 b2f8fcb9d9
commit 0cc7270b83
1 changed files with 15 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/modules/position_estimator_inav/inertial_filter.c
View File

@ -5,24 +5,30 @@
* Author: Anton Babushkin <rk3dov@gmail.com> * Author: Anton Babushkin <rk3dov@gmail.com>
*/ */
#include <math.h>
#include "inertial_filter.h" #include "inertial_filter.h"
void inertial_filter_predict(float dt, float x[3]) void inertial_filter_predict(float dt, float x[3])
{ {
x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; if (isfinite(dt)) {
x[1] += x[2] * dt; x[0] += x[1] * dt + x[2] * dt * dt / 2.0f;
x[1] += x[2] * dt;
}
} }
void inertial_filter_correct(float e, float dt, float x[3], int i, float w) void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
{ {
float ewdt = e * w * dt; if (isfinite(e) && isfinite(w) && isfinite(dt)) {
x[i] += ewdt; float ewdt = e * w * dt;
x[i] += ewdt;
if (i == 0) { if (i == 0) {
x[1] += w * ewdt; x[1] += w * ewdt;
x[2] += w * w * ewdt / 3.0; x[2] += w * w * ewdt / 3.0;
} else if (i == 1) { } else if (i == 1) {
x[2] += w * ewdt; x[2] += w * ewdt;
}
} }
} }