position_estimator_inav: added NaN checks

src/modules/position_estimator_inav/inertial_filter.c

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