Robustified filter init / sequencing

3 files changed, 14 insertions, 6 deletions

diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp
index ac9abf5ca..5de22fdae 100644
--- a/src/modules/ekf_att_pos_estimator/estimator.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator.cpp
@@ -153,11 +153,12 @@ AttPosEKF::AttPosEKF() :
     useCompass(true),
     useRangeFinder(true),
     numericalProtection(true),
+    refSet(false),
     storeIndex(0),
     gpsHgt(0.0f),
     baroHgt(0.0f),
     GPSstatus(0),
-    VtasMeas(0.0f),
+    VtasMeas(0.0f)
 {
     velNED[0] = 0.0f;
     velNED[1] = 0.0f;
@@ -1977,7 +1978,7 @@ void AttPosEKF::OnGroundCheck()
 {
     onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f));
     if (staticMode) {
-        staticMode = !(GPSstatus > GPS_FIX_2D);
+        staticMode = (!refSet || (GPSstatus < GPS_FIX_3D));
     }
 }
 
@@ -2485,6 +2486,7 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do
     latRef = referenceLat;
     lonRef = referenceLon;
     hgtRef = referenceHgt;
+    refSet = true;
 
     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 5e60e506f..e118ae573 100644
--- a/src/modules/ekf_att_pos_estimator/estimator.h
+++ b/src/modules/ekf_att_pos_estimator/estimator.h
@@ -211,6 +211,7 @@ public:
     double latRef; // WGS-84 latitude of reference point (rad)
     double lonRef; // WGS-84 longitude of reference point (rad)
     float hgtRef; // WGS-84 height of reference point (m)
+    bool refSet;                ///< flag to indicate if the reference position has been set
     Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
     unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
 
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 ad7cb3687..19111a9b4 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
@@ -192,6 +192,7 @@ private:
 
 	bool						_initialized;
 	bool						_gps_initialized;
+	uint64_t					_gps_start_time;
 
 	int						_mavlink_fd;
 
@@ -720,6 +721,9 @@ FixedwingEstimator::task_main()
 
 				} else {
 
+					/* store time of valid GPS measurement */
+					_gps_start_time = hrt_absolute_time();
+
 					/* check if we had a GPS outage for a long time */
 					if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) {
 						_ekf->ResetPosition();
@@ -859,7 +863,7 @@ FixedwingEstimator::task_main()
 			}
 
 			// XXX trap for testing
-			if (check == 1 || check == 2) {
+			if (check == 1) {
 				errx(1, "NUMERIC ERROR IN FILTER");
 			}
 
@@ -907,7 +911,7 @@ FixedwingEstimator::task_main()
 			// XXX we rather want to check all updated
 
 
-			if (hrt_elapsed_time(&start_time) > 100000) {
+			if (hrt_elapsed_time(&_gps_start_time) > 50000) {
 
 				bool home_set;
 				orb_check(_home_sub, &home_set);
@@ -920,7 +924,6 @@ FixedwingEstimator::task_main()
 					struct home_position_s home;
 
 					orb_copy(ORB_ID(home_position), _home_sub, &home);
-					warnx("HOME SET");
 
 					double lat = home.lat;
 					double lon = home.lon;
@@ -942,7 +945,9 @@ FixedwingEstimator::task_main()
 
 					// XXX this is not multithreading safe
 					map_projection_init(lat, lon);
-					mavlink_log_info(_mavlink_fd, "[position estimator] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
+					mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)alt);
+					warnx("[ekf] HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)alt,
+						(double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
 
 					_gps_initialized = true;