Fixed re-initialization of estimator, re-initializes in air now reliably. Does give useful HIL results.

2 files changed, 97 insertions, 82 deletions

diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp
index 86b7efafb..a6d6a9db5 100644
--- a/src/modules/ekf_att_pos_estimator/estimator.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator.cpp
@@ -138,35 +138,15 @@ void swap_var(float &d1, float &d2)
     d2 = tmp;
 }
 
-AttPosEKF::AttPosEKF() :
-    fusionModeGPS(0),
-    covSkipCount(0),
-    statesInitialised(false),
-    fuseVelData(false),
-    fusePosData(false),
-    fuseHgtData(false),
-    fuseMagData(false),
-    fuseVtasData(false),
-    onGround(true),
-    staticMode(true),
-    useAirspeed(true),
-    useCompass(true),
-    useRangeFinder(true),
-    numericalProtection(true),
-    refSet(false),
-    storeIndex(0),
-    gpsHgt(0.0f),
-    baroHgt(0.0f),
-    GPSstatus(0),
-    VtasMeas(0.0f),
-    magDeclination(0.0f)
+AttPosEKF::AttPosEKF()
+
+    /* NOTE: DO NOT initialize class members here. Use ZeroVariables()
+     * instead to allow clean in-air re-initialization.
+     */
 {
-    velNED[0] = 0.0f;
-    velNED[1] = 0.0f;
-    velNED[2] = 0.0f;
 
-    InitialiseParameters();
     ZeroVariables();
+    InitialiseParameters();
 }
 
 AttPosEKF::~AttPosEKF()
@@ -2348,7 +2328,7 @@ int AttPosEKF::CheckAndBound()
     }
 
     // Reset the filter if the IMU data is too old
-    if (dtIMU > 0.2f) {
+    if (dtIMU > 0.3f) {
 
         ResetVelocity();
         ResetPosition();
@@ -2424,24 +2404,10 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f
 void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
 {
 
-    // Clear the init flag
-    statesInitialised = false;
-
-    // Clear other flags, waiting for new data
-    fusionModeGPS = 0;
-    fuseVelData = false;
-    fusePosData = false;
-    fuseHgtData = false;
-    fuseMagData = false;
-    fuseVtasData = false;
-    // onGround(true),
-    // staticMode(true),
-    useAirspeed = true;
-    useCompass = true;
-    useRangeFinder = true;
-
-    ZeroVariables();
-
+    // Fill variables with valid data
+    velNED[0] = initvelNED[0];
+    velNED[1] = initvelNED[1];
+    velNED[2] = initvelNED[2];
     magDeclination = declination;
 
     // Calculate initial filter quaternion states from raw measurements
@@ -2513,6 +2479,30 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do
 
 void AttPosEKF::ZeroVariables()
 {
+
+    // Initialize on-init initialized variables
+    fusionModeGPS = 0;
+    covSkipCount = 0;
+    statesInitialised = false;
+    fuseVelData = false;
+    fusePosData = false;
+    fuseHgtData = false;
+    fuseMagData = false;
+    fuseVtasData = false;
+    onGround = true;
+    staticMode = true;
+    useAirspeed = true;
+    useCompass = true;
+    useRangeFinder = true;
+    numericalProtection = true;
+    refSet = false;
+    storeIndex = 0;
+    gpsHgt = 0.0f;
+    baroHgt = 0.0f;
+    GPSstatus = 0;
+    VtasMeas = 0.0f;
+    magDeclination = 0.0f;
+
     // Do the data structure init
     for (unsigned i = 0; i < n_states; i++) {
         for (unsigned j = 0; j < n_states; j++) {
@@ -2529,6 +2519,9 @@ void AttPosEKF::ZeroVariables()
     summedDelAng.zero();
     summedDelVel.zero();
 
+    dAngIMU.zero();
+    dVelIMU.zero();
+
     for (unsigned i = 0; i < data_buffer_size; i++) {
 
         for (unsigned j = 0; j < n_states; j++) {
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 05cc39f11..07dfdbd68 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
@@ -546,24 +546,8 @@ FixedwingEstimator::task_main()
 	/* sets also parameters in the EKF object */
 	parameters_update();
 
-	/* set initial filter state */
-	_ekf->fuseVelData = false;
-	_ekf->fusePosData = false;
-	_ekf->fuseHgtData = false;
-	_ekf->fuseMagData = false;
-	_ekf->fuseVtasData = false;
-	_ekf->statesInitialised = false;
-
-	/* initialize measurement data */
-	_ekf->VtasMeas = 0.0f;
 	Vector3f lastAngRate = {0.0f, 0.0f, 0.0f};
-	Vector3f lastAccel = {0.0f, 0.0f, -9.81f};
-	_ekf->dVelIMU.x = 0.0f;
-	_ekf->dVelIMU.y = 0.0f;
-	_ekf->dVelIMU.z = 0.0f;
-	_ekf->dAngIMU.x = 0.0f;
-	_ekf->dAngIMU.y = 0.0f;
-	_ekf->dAngIMU.z = 0.0f;
+	Vector3f lastAccel = {0.0f, 0.0f, 0.0f};
 
 	/* wakeup source(s) */
 	struct pollfd fds[2];
@@ -621,12 +605,14 @@ FixedwingEstimator::task_main()
 			bool accel_updated;
 			bool mag_updated;
 
+			hrt_abstime last_sensor_timestamp;
+
 			perf_count(_perf_gyro);
 
 			/* Reset baro reference if switching to HIL, reset sensor states */
 			if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) {
 				/* system is in HIL now, wait for measurements to come in one last round */
-				usleep(65000);
+				usleep(60000);
 
 #ifndef SENSOR_COMBINED_SUB
 				orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
@@ -644,8 +630,13 @@ FixedwingEstimator::task_main()
 
 				_baro_init = false;
 				_gps_initialized = false;
+				last_sensor_timestamp = hrt_absolute_time();
+				last_run = last_sensor_timestamp;
+
+				_ekf->ZeroVariables();
+				_ekf->dtIMU = 0.01f;
 
-				/* now skip this loop and get data on the next one */
+				/* now skip this loop and get data on the next one, which will also re-init the filter */
 				continue;
 			}
 
@@ -653,8 +644,6 @@ FixedwingEstimator::task_main()
 			 *    PART ONE: COLLECT ALL DATA
 			 **/
 
-			hrt_abstime last_sensor_timestamp;
-
 			/* load local copies */
 #ifndef SENSOR_COMBINED_SUB
 			orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
@@ -668,7 +657,7 @@ FixedwingEstimator::task_main()
 			}
 
 			last_sensor_timestamp = _gyro.timestamp;
-			_ekf.IMUmsec = _gyro.timestamp / 1e3f;
+			IMUmsec = _gyro.timestamp / 1e3f;
 
 			float deltaT = (_gyro.timestamp - last_run) / 1e6f;
 			last_run = _gyro.timestamp;
@@ -689,6 +678,11 @@ FixedwingEstimator::task_main()
 				_ekf->angRate.x = _gyro.x;
 				_ekf->angRate.y = _gyro.y;
 				_ekf->angRate.z = _gyro.z;
+
+				if (!_gyro_valid) {
+					lastAngRate = _ekf->angRate;
+				}
+
 				_gyro_valid = true;
 			}
 
@@ -696,6 +690,11 @@ FixedwingEstimator::task_main()
 				_ekf->accel.x = _accel.x;
 				_ekf->accel.y = _accel.y;
 				_ekf->accel.z = _accel.z;
+
+				if (!_accel_valid) {
+					lastAccel = _ekf->accel;
+				}
+
 				_accel_valid = true;
 			}
 
@@ -741,6 +740,11 @@ FixedwingEstimator::task_main()
 				_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
 				_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
 				_ekf->angRate.z = _sensor_combined.gyro_rad_s[2];
+
+				if (!_gyro_valid) {
+					lastAngRate = _ekf->angRate;
+				}
+
 				_gyro_valid = true;
 			}
 
@@ -748,6 +752,11 @@ FixedwingEstimator::task_main()
 				_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
 				_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
 				_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
+
+				if (!_accel_valid) {
+					lastAccel = _ekf->accel;
+				}
+
 				_accel_valid = true;
 			}
 
@@ -986,6 +995,22 @@ FixedwingEstimator::task_main()
 					_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep);
 				}
 
+				/* set sensors to de-initialized state */
+				_gyro_valid = false;
+				_accel_valid = false;
+				_mag_valid = false;
+
+				_baro_init = false;
+				_gps_initialized = false;
+				last_sensor_timestamp = hrt_absolute_time();
+				last_run = last_sensor_timestamp;
+
+				_ekf->ZeroVariables();
+				_ekf->dtIMU = 0.01f;
+
+				// Let the system re-initialize itself
+				continue;
+
 			}
 
 
@@ -995,15 +1020,13 @@ FixedwingEstimator::task_main()
 
 			if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) {
 
-				// bool home_set;
-				// orb_check(_home_sub, &home_set);
-				// struct home_position_s home;
-				// orb_copy(ORB_ID(home_position), _home_sub, &home);
+				float initVelNED[3];
 
 				if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) {
-					_ekf->velNED[0] = _gps.vel_n_m_s;
-					_ekf->velNED[1] = _gps.vel_e_m_s;
-					_ekf->velNED[2] = _gps.vel_d_m_s;
+
+					initVelNED[0] = _gps.vel_n_m_s;
+					initVelNED[1] = _gps.vel_e_m_s;
+					initVelNED[2] = _gps.vel_d_m_s;
 
 					// GPS is in scaled integers, convert
 					double lat = _gps.lat / 1.0e7;
@@ -1018,9 +1041,6 @@ FixedwingEstimator::task_main()
 
 					// Set up position variables correctly
 					_ekf->GPSstatus = _gps.fix_type;
-					_ekf->velNED[0] = _gps.vel_n_m_s;
-					_ekf->velNED[1] = _gps.vel_e_m_s;
-					_ekf->velNED[2] = _gps.vel_d_m_s;
 
 					_ekf->gpsLat = math::radians(lat);
 					_ekf->gpsLon = math::radians(lon) - M_PI;
@@ -1029,7 +1049,7 @@ FixedwingEstimator::task_main()
 					// Look up mag declination based on current position
 					float declination = math::radians(get_mag_declination(lat, lon));
 
-					_ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);
+					_ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);
 
 					// Initialize projection
 					_local_pos.ref_lat = lat;
@@ -1041,21 +1061,23 @@ FixedwingEstimator::task_main()
 					mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
 					warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
 						(double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
+					warnx("BARO: %8.4f m / ref: %8.4f m", _ekf->baroHgt, _ekf->hgtMea);
 					warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination));
 
 					_gps_initialized = true;
 
 				} else if (!_ekf->statesInitialised) {
-					_ekf->velNED[0] = 0.0f;
-					_ekf->velNED[1] = 0.0f;
-					_ekf->velNED[2] = 0.0f;
+
+					initVelNED[0] = 0.0f;
+					initVelNED[1] = 0.0f;
+					initVelNED[2] = 0.0f;
 					_ekf->posNED[0] = 0.0f;
 					_ekf->posNED[1] = 0.0f;
 					_ekf->posNED[2] = 0.0f;
 
 					_ekf->posNE[0] = _ekf->posNED[0];
 					_ekf->posNE[1] = _ekf->posNED[1];
-					_ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f, 0.0f);
+					_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
 				}
 			}