Better / cleaner initialization of the attitude estimator

3 files changed, 38 insertions, 22 deletions

diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp
index 95afbc181..5f4bf0729 100644
--- a/src/modules/ekf_att_pos_estimator/estimator.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator.cpp
@@ -4,7 +4,7 @@
 // Define EKF_DEBUG here to enable the debug print calls
 // if the macro is not set, these will be completely
 // optimized out by the compiler.
-//#define EKF_DEBUG
+#define EKF_DEBUG
 
 #ifdef EKF_DEBUG
 #include <stdio.h>
@@ -2263,21 +2263,29 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
             if (!isfinite(KH[i][j])) {
 
                 err_report->covarianceNaN = true;
-                ekf_debug("KH NaN");
                 err = true;
             } //  intermediate result used for covariance updates
+            if (err) {
+                ekf_debug("KH NaN");
+            }
+
             if (!isfinite(KHP[i][j])) {
 
                 err_report->covarianceNaN = true;
-                ekf_debug("KHP NaN");
                 err = true;
             } // intermediate result used for covariance updates
+            if (err) {
+                ekf_debug("KHP NaN");
+            }
+
             if (!isfinite(P[i][j])) {
 
                 err_report->covarianceNaN = true;
-                ekf_debug("P NaN");
                 err = true;
             } // covariance matrix
+            if (err) {
+                ekf_debug("P NaN");
+            }
         }
 
         if (!isfinite(Kfusion[i])) {
@@ -2461,12 +2469,12 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
     summedDelVel.z = 0.0f;
 }
 
-void AttPosEKF::InitialiseFilter(float (&initvelNED)[3])
+void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt)
 {
     //store initial lat,long and height
-    latRef = gpsLat;
-    lonRef = gpsLon;
-    hgtRef = gpsHgt;
+    latRef = referenceLat;
+    lonRef = referenceLon;
+    hgtRef = referenceHgt;
 
     memset(&last_ekf_error, 0, sizeof(last_ekf_error));
 
diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h
index 734ad0c05..5e60e506f 100644
--- a/src/modules/ekf_att_pos_estimator/estimator.h
+++ b/src/modules/ekf_att_pos_estimator/estimator.h
@@ -308,7 +308,7 @@ void OnGroundCheck();
 
 void CovarianceInit();
 
-void InitialiseFilter(float (&initvelNED)[3]);
+void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt);
 
 float ConstrainFloat(float val, float min, float max);
 
diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
index 828775719..c8005f3d3 100644
--- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -606,7 +606,6 @@ FixedwingEstimator::task_main()
 			/* guard against too large deltaT's */
 			if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
 				deltaT = 0.01f;
-				warnx("TS fail");
 			}
 
 
@@ -650,14 +649,14 @@ FixedwingEstimator::task_main()
 			IMUmsec = _sensor_combined.timestamp / 1e3f;
 
 			float deltaT = (_sensor_combined.timestamp - last_run) / 1e6f;
-			last_run = _sensor_combined.timestamp;
 
 			/* guard against too large deltaT's */
 			if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
 				deltaT = 0.01f;
-				warnx("TS fail");
 			}
 
+			last_run = _sensor_combined.timestamp;
+
 			// Always store data, independent of init status
 			/* fill in last data set */
 			_ekf->dtIMU = deltaT;
@@ -823,6 +822,8 @@ FixedwingEstimator::task_main()
 			 */
 			int check = _ekf->CheckAndBound();
 
+			const char* ekfname = "[ekf] ";
+
 			switch (check) {
 				case 0:
 					/* all ok */
@@ -830,27 +831,34 @@ FixedwingEstimator::task_main()
 				case 1:
 				{
 					const char* str = "NaN in states, resetting";
-					warnx(str);
-					mavlink_log_critical(_mavlink_fd, str);
+					warnx("%s%s", ekfname, str);
+					mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
 					break;
 				}
 				case 2:
 				{
 					const char* str = "stale IMU data, resetting";
-					warnx(str);
-					mavlink_log_critical(_mavlink_fd, str);
+					warnx("%s%s", ekfname, str);
+					mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
 					break;
 				}
 				case 3:
 				{
-					const char* str = "switching dynamic / static state";
-					warnx(str);
-					mavlink_log_critical(_mavlink_fd, str);
+					const char* str = "switching to dynamic state";
+					warnx("%s%s", ekfname, str);
+					mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
 					break;
 				}
+
+				default:
+				{
+					const char* str = "unknown reset condition";
+					warnx("%s%s", ekfname, str);
+					mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
+				}
 			}
 
-			// If non-zero, we got a problem
+			// If non-zero, we got a filter reset
 			if (check) {
 
 				struct ekf_status_report ekf_report;
@@ -912,7 +920,7 @@ FixedwingEstimator::task_main()
 					double lon = home.lon;
 					float alt = home.alt;
 
-					_ekf->InitialiseFilter(_ekf->velNED);
+					_ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, alt);
 
 					// Initialize projection
 					_local_pos.ref_lat = home.lat * 1e7;
@@ -942,7 +950,7 @@ FixedwingEstimator::task_main()
 
 					_ekf->posNE[0] = _ekf->posNED[0];
 					_ekf->posNE[1] = _ekf->posNED[1];
-					_ekf->InitialiseFilter(_ekf->velNED);
+					_ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f);
 				}
 			}