Ensure NaN check is executed before any parts of the filter are run. Fix GPS velocity reset for case of initialized GPS

1 files changed, 15 insertions, 11 deletions

diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 5d768b73d..bc5f709e5 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -1140,6 +1140,20 @@ FixedwingEstimator::task_main()
 
 				float initVelNED[3];
 
+				// Guard before running any parts of the filter
+				// of NaN / invalid values
+				if (_ekf->statesInitialised) {
+
+					// We're apparently initialized in this case now
+
+					int check = check_filter_state();
+
+					if (check) {
+						// Let the system re-initialize itself
+						continue;
+					}
+				}
+
 				/* Initialize the filter first */
 				if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) {
 
@@ -1204,16 +1218,6 @@ FixedwingEstimator::task_main()
 					_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
 				} else if (_ekf->statesInitialised) {
 
-					// We're apparently initialized in this case now
-
-					int check = check_filter_state();
-
-					if (check) {
-						// Let the system re-initialize itself
-						continue;
-					}
-
-
 					// Run the strapdown INS equations every IMU update
 					_ekf->UpdateStrapdownEquationsNED();
 	#if 0
@@ -1281,7 +1285,7 @@ FixedwingEstimator::task_main()
 						// run the fusion step
 						_ekf->FuseVelposNED();
 
-					} else if (_ekf->statesInitialised) {
+					} else if (!_gps_initialized && _ekf->statesInitialised) {
 						// Convert GPS measurements to Pos NE, hgt and Vel NED
 						_ekf->velNED[0] = 0.0f;
 						_ekf->velNED[1] = 0.0f;