ekf_hgt: call specific height reset function instead of generic one

- Also use the delayed current data instead of newest that might not be
available (gps buffer is sometimes empty if the dt between samples is
larger than the delayed horizon).

- Separate "baro fault" from "baro intermittent": intermittent is a
  temporary failure and should prevent from switching to baro right now,
  but "fault" means that it should never be used anymore

- In case of height timeout, check for metrics but not for consistency
  as the filter is likely to have diverged already.

src/modules/ekf2/EKF/control.cpp

@@ -87,7 +87,7 @@ void Ekf::controlFusionModes()
 
 	if (_baro_buffer) {
 		// check for intermittent data
-		_baro_hgt_faulty = !isRecent(_time_last_baro, 2 * BARO_MAX_INTERVAL);
+		_baro_hgt_intermittent = !isRecent(_time_last_baro, 2 * BARO_MAX_INTERVAL);
 
 		const uint64_t baro_time_prev = _baro_sample_delayed.time_us;
 		_baro_data_ready = _baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed);
@@ -626,25 +626,16 @@ void Ekf::controlHeightSensorTimeouts()
 
 	if (hgt_fusion_timeout || continuous_bad_accel_hgt) {
 
-		bool request_height_reset = false;
 		const char *failing_height_source = nullptr;
 		const char *new_height_source = nullptr;
 
 		if (_control_status.flags.baro_hgt) {
-			// check if GPS height is available
-			bool gps_hgt_accurate = false;
-
-			if (_gps_buffer) {
-				const gpsSample &gps_init = _gps_buffer->get_newest();
-				gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
-			}
-
-			// check for inertial sensing errors in the last BADACC_PROBATION seconds
-			const bool prev_bad_vert_accel = isRecent(_time_bad_vert_accel, BADACC_PROBATION);
+			bool reset_to_gps = false;
 
 			// reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data
-			const bool reset_to_gps = !_gps_intermittent &&
-						  ((gps_hgt_accurate && !prev_bad_vert_accel) || _baro_hgt_faulty);
+			if (!_gps_intermittent) {
+				reset_to_gps = (_gps_checks_passed && !_fault_status.flags.bad_acc_vertical) || _baro_hgt_faulty || _baro_hgt_intermittent;
+			}
 
 			if (reset_to_gps) {
 				// set height sensor health
@@ -652,47 +643,33 @@ void Ekf::controlHeightSensorTimeouts()
 
 				startGpsHgtFusion();
 
-				request_height_reset = true;
 				failing_height_source = "baro";
 				new_height_source = "gps";
 
-			} else if (!_baro_hgt_faulty) {
-				request_height_reset = true;
+			} else if (!_baro_hgt_faulty && !_baro_hgt_intermittent) {
+				resetHeightToBaro();
+
 				failing_height_source = "baro";
 				new_height_source = "baro";
 			}
 
 		} else if (_control_status.flags.gps_hgt) {
-			// check if GPS height is available
-			bool gps_hgt_accurate = false;
+			bool reset_to_baro = false;
 
-			if (_gps_buffer) {
-				const gpsSample &gps_init = _gps_buffer->get_newest();
-				gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
+			// if baro data is available and GPS data is inaccurate and the timeout cannot be blamed on IMU data, reset height to baro
+			if (!_baro_hgt_faulty && !_baro_hgt_intermittent) {
+				reset_to_baro = (!_fault_status.flags.bad_acc_vertical && !_gps_checks_passed) || _gps_intermittent;
 			}
 
-			// check the baro height source for consistency and freshness
-			bool baro_data_consistent = false;
-
-			if (_baro_buffer) {
-				const baroSample &baro_init = _baro_buffer->get_newest();
-				const float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
-				baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P(9, 9)) * sq(_params.baro_innov_gate);
-			}
-
-			// if baro data is acceptable and GPS data is inaccurate, reset height to baro
-			const bool reset_to_baro = !_baro_hgt_faulty &&
-						   ((baro_data_consistent && !gps_hgt_accurate) || _gps_intermittent);
-
 			if (reset_to_baro) {
 				startBaroHgtFusion();
 
-				request_height_reset = true;
 				failing_height_source = "gps";
 				new_height_source = "baro";
 
 			} else if (!_gps_intermittent) {
-				request_height_reset = true;
+				resetHeightToGps();
+
 				failing_height_source = "gps";
 				new_height_source = "gps";
 			}
@@ -700,14 +677,14 @@ void Ekf::controlHeightSensorTimeouts()
 		} else if (_control_status.flags.rng_hgt) {
 
 			if (_range_sensor.isHealthy()) {
-				request_height_reset = true;
+				resetHeightToRng();
+
 				failing_height_source = "rng";
 				new_height_source = "rng";
 
-			} else if (!_baro_hgt_faulty) {
+			} else if (!_baro_hgt_faulty && !_baro_hgt_intermittent) {
 				startBaroHgtFusion();
 
-				request_height_reset = true;
 				failing_height_source = "rng";
 				new_height_source = "baro";
 			}
@@ -722,21 +699,21 @@ void Ekf::controlHeightSensorTimeouts()
 			}
 
 			if (ev_data_available) {
-				request_height_reset = true;
+				resetHeightToEv();
+
 				failing_height_source = "ev";
 				new_height_source = "ev";
 
 			} else if (_range_sensor.isHealthy()) {
 				// Fallback to rangefinder data if available
 				startRngHgtFusion();
-				request_height_reset = true;
+
 				failing_height_source = "ev";
 				new_height_source = "rng";
 
-			} else if (!_baro_hgt_faulty) {
+			} else if (!_baro_hgt_faulty && !_baro_hgt_intermittent) {
 				startBaroHgtFusion();
 
-				request_height_reset = true;
 				failing_height_source = "ev";
 				new_height_source = "baro";
 			}
@@ -747,9 +724,12 @@ void Ekf::controlHeightSensorTimeouts()
 			ECL_WARN("%s hgt timeout - reset to %s", failing_height_source, new_height_source);
 		}
 
-		// Reset vertical position and velocity states to the last measurement
-		if (request_height_reset) {
-			resetHeight();
+		// Also reset the vertical velocity
+		if (_control_status.flags.gps && !_gps_intermittent && _gps_checks_passed) {
+			resetVerticalVelocityToGps();
+
+		} else {
+			resetVerticalVelocityToZero();
 		}
 	}
 }
@@ -835,8 +815,14 @@ void Ekf::controlHeightFusion()
 			}
 
 		} else {
-			if (!_control_status.flags.baro_hgt && !_baro_hgt_faulty) {
-				startBaroHgtFusion();
+			if (!_control_status.flags.baro_hgt) {
+				if (!_baro_hgt_faulty && !_baro_hgt_intermittent) {
+					startBaroHgtFusion();
+
+				} else if (!_control_status.flags.gps_hgt && !_gps_intermittent && _gps_checks_passed) {
+					// Use GPS as a fallback
+					startGpsHgtFusion();
+				}
 			}
 		}
 
@@ -880,7 +866,7 @@ void Ekf::controlHeightFusion()
 					// In fallback mode and GPS has recovered so start using it
 					startGpsHgtFusion();
 
-				} else if (!_control_status.flags.baro_hgt && !_baro_hgt_faulty) {
+				} else if (!_control_status.flags.baro_hgt && !_baro_hgt_faulty && !_baro_hgt_intermittent) {
 					// Use baro as a fallback
 					startBaroHgtFusion();
 				}

src/modules/ekf2/EKF/ekf.h

@@ -542,8 +542,9 @@ private:
 	terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground
 
 	// height sensor status
-	bool _baro_hgt_faulty{true};		///< true if valid baro data is unavailable for use
-	bool _gps_intermittent{true};           ///< true if into the buffer is intermittent
+	bool _baro_hgt_faulty{false};		///< true if baro data have been declared faulty TODO: move to fault flags
+	bool _baro_hgt_intermittent{true};	///< true if data into the buffer is intermittent
+	bool _gps_intermittent{true};           ///< true if data into the buffer is intermittent
 
 	// imu fault status
 	uint64_t _time_bad_vert_accel{0};	///< last time a bad vertical accel was detected (uSec)
@@ -656,7 +657,13 @@ private:
 
 	void resetVerticalPositionTo(float new_vert_pos);
 
-	void resetHeight();
+	void resetHeightToBaro();
+	void resetHeightToGps();
+	void resetHeightToRng();
+	void resetHeightToEv();
+
+	void resetVerticalVelocityToGps();
+	void resetVerticalVelocityToZero();
 
 	// fuse optical flow line of sight rate measurements
 	void fuseOptFlow();

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -157,9 +157,6 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel)
 
 void Ekf::resetHorizontalPosition()
 {
-	// let the next odometry update know that the previous value of states cannot be used to calculate the change in position
-	_hpos_prev_available = false;
-
 	if (_control_status.flags.gps) {
 		// this reset is only called if we have new gps data at the fusion time horizon
 		resetHorizontalPositionToGps();
@@ -212,6 +209,9 @@ void Ekf::resetHorizontalPositionToVision()
 
 	resetHorizontalPositionTo(Vector2f(_ev_pos));
 	P.uncorrelateCovarianceSetVariance<2>(7, _ev_sample_delayed.posVar.slice<2, 1>(0, 0));
+
+	// let the next odometry update know that the previous value of states cannot be used to calculate the change in position
+	_hpos_prev_available = false;
 }
 
 void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos)
@@ -250,106 +250,84 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos)
 
 	// add the reset amount to the output observer vertical position state
 	_output_vert_new.vert_vel_integ = _state.pos(2);
-}
-
-// Reset height state using the last height measurement
-void Ekf::resetHeight()
-{
-	// reset the vertical position
-	if (_control_status.flags.rng_hgt) {
-		float dist_bottom;
-
-		if (_control_status.flags.in_air) {
-			dist_bottom = _range_sensor.getDistBottom();
-
-		} else {
-			// use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
-			dist_bottom = _params.rng_gnd_clearance;
-		}
-
-		// update the state and associated variance
-		resetVerticalPositionTo(-dist_bottom + _hgt_sensor_offset);
-
-		// the state variance is the same as the observation
-		P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.range_noise));
-
-		// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
-		if (_baro_buffer) {
-			const baroSample &baro_newest = _baro_buffer->get_newest();
-			_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
-		}
-
-	} else if (_control_status.flags.baro_hgt) {
-		// initialize vertical position with newest baro measurement
-		if (!_baro_hgt_faulty) {
-			if (_baro_buffer) {
-				const baroSample &baro_newest = _baro_buffer->get_newest();
-				resetVerticalPositionTo(-baro_newest.hgt + _baro_hgt_offset);
-			}
-
-			// the state variance is the same as the observation
-			P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.baro_noise));
-
-		} else {
-			// TODO: reset to last known baro based estimate
-		}
-
-	} else if (_control_status.flags.gps_hgt) {
-		// initialize vertical position and velocity with newest gps measurement
-		if (!_gps_intermittent && _gps_buffer) {
-			const gpsSample &gps_newest = _gps_buffer->get_newest();
-			resetVerticalPositionTo(_hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref);
-
-			// the state variance is the same as the observation
-			P.uncorrelateCovarianceSetVariance<1>(9, sq(gps_newest.vacc));
-
-			// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
-			if (_baro_buffer) {
-				const baroSample &baro_newest = _baro_buffer->get_newest();
-				_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
-			}
-
-		} else {
-			// TODO: reset to last known gps based estimate
-		}
-
-	} else if (_control_status.flags.ev_hgt) {
-		// initialize vertical position with newest measurement
-		if (_ext_vision_buffer) {
-			const extVisionSample &ev_newest = _ext_vision_buffer->get_newest();
-
-			// use the most recent data if it's time offset from the fusion time horizon is smaller
-			if (ev_newest.time_us >= _ev_sample_delayed.time_us) {
-				resetVerticalPositionTo(ev_newest.pos(2));
-
-			} else {
-				resetVerticalPositionTo(_ev_sample_delayed.pos(2));
-			}
-		}
-	}
-
-	// reset the vertical velocity state
-	if (_control_status.flags.gps && !_gps_intermittent && _gps_buffer) {
-		// If we are using GPS, then use it to reset the vertical velocity
-		const gpsSample &gps_newest = _gps_buffer->get_newest();
-		resetVerticalVelocityTo(gps_newest.vel(2));
-
-		// the state variance is the same as the observation
-		P.uncorrelateCovarianceSetVariance<1>(6, sq(1.5f * gps_newest.sacc));
-
-	} else {
-		// we don't know what the vertical velocity is, so set it to zero
-		resetVerticalVelocityTo(0.0f);
-
-		// Set the variance to a value large enough to allow the state to converge quickly
-		// that does not destabilise the filter
-		P.uncorrelateCovarianceSetVariance<1>(6, 10.0f);
-	}
 
 	// Reset the timout timer
 	_time_last_hgt_fuse = _time_last_imu;
 }
 
+void Ekf::resetHeightToBaro()
+{
+	resetVerticalPositionTo(-_baro_sample_delayed.hgt + _baro_hgt_offset);
+
+	// the state variance is the same as the observation
+	P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.baro_noise));
+}
+
+void Ekf::resetHeightToGps()
+{
+	const float z_pos_before_reset = _state.pos(2);
+	resetVerticalPositionTo(-_gps_sample_delayed.hgt + _gps_alt_ref);
+
+	// the state variance is the same as the observation
+	P.uncorrelateCovarianceSetVariance<1>(9, getGpsHeightVariance());
+
+	// adjust the baro offset
+	_baro_hgt_offset += _state.pos(2) - z_pos_before_reset;
+}
+
+void Ekf::resetHeightToRng()
+{
+	float dist_bottom;
+
+	if (_control_status.flags.in_air) {
+		dist_bottom = _range_sensor.getDistBottom();
+
+	} else {
+		// use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
+		dist_bottom = _params.rng_gnd_clearance;
+	}
+
+	// update the state and associated variance
+	const float z_pos_before_reset = _state.pos(2);
+	resetVerticalPositionTo(-dist_bottom + _hgt_sensor_offset);
+
+	// the state variance is the same as the observation
+	P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.range_noise));
+
+	// adjust the baro offset
+	_baro_hgt_offset += _state.pos(2) - z_pos_before_reset;
+}
+
+void Ekf::resetHeightToEv()
+{
+	const float z_pos_before_reset = _state.pos(2);
+	resetVerticalPositionTo(_ev_sample_delayed.pos(2));
+
+	// the state variance is the same as the observation
+	P.uncorrelateCovarianceSetVariance<1>(9, fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f)));
+
+	// adjust the baro offset
+	_baro_hgt_offset += _state.pos(2) - z_pos_before_reset;
+}
+
+void Ekf::resetVerticalVelocityToGps()
+{
+	resetVerticalVelocityTo(_gps_sample_delayed.vel(2));
+
+	// the state variance is the same as the observation
+	P.uncorrelateCovarianceSetVariance<1>(6, sq(1.5f * _gps_sample_delayed.sacc));
+}
+
+void Ekf::resetVerticalVelocityToZero()
+{
+	// we don't know what the vertical velocity is, so set it to zero
+	resetVerticalVelocityTo(0.0f);
+
+	// Set the variance to a value large enough to allow the state to converge quickly
+	// that does not destabilise the filter
+	P.uncorrelateCovarianceSetVariance<1>(6, 10.0f);
+}
+
 // align output filter states to match EKF states at the fusion time horizon
 void Ekf::alignOutputFilter()
 {
@@ -1271,21 +1249,34 @@ void Ekf::startMag3DFusion()
 
 void Ekf::startBaroHgtFusion()
 {
-	setControlBaroHeight();
+	if (!_control_status.flags.baro_hgt) {
+		if (!_control_status.flags.rng_hgt) {
+			resetHeightToBaro();
+		}
 
-	// We don't need to set a height sensor offset
-	// since we track a separate _baro_hgt_offset
-	_hgt_sensor_offset = 0.0f;
+		setControlBaroHeight();
+
+		// We don't need to set a height sensor offset
+		// since we track a separate _baro_hgt_offset
+		_hgt_sensor_offset = 0.0f;
+	}
 }
 
 void Ekf::startGpsHgtFusion()
 {
 	if (!_control_status.flags.gps_hgt) {
-		setControlGPSHeight();
+		if (_control_status.flags.rng_hgt) {
+			// swith out of range aid
+			// calculate height sensor offset such that current
+			// measurement matches our current height estimate
+			_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
 
-		// calculate height sensor offset such that current
-		// measurement matches our current height estimate
-		_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
+		} else {
+			_hgt_sensor_offset = 0.f;
+			resetHeightToGps();
+		}
+
+		setControlGPSHeight();
 	}
 }
 
@@ -1300,7 +1291,7 @@ void Ekf::startRngHgtFusion()
 
 		if (!_control_status_prev.flags.ev_hgt) {
 			// EV and range finders are using the same height datum
-			resetHeight();
+			resetHeightToRng();
 		}
 	}
 }
@@ -1323,7 +1314,7 @@ void Ekf::startEvHgtFusion()
 
 		if (!_control_status_prev.flags.rng_hgt) {
 			// EV and range finders are using the same height datum
-			resetHeight();
+			resetHeightToEv();
 		}
 	}
 }

src/modules/ekf2/EKF/gps_checks.cpp

@@ -101,11 +101,6 @@ bool Ekf::collect_gps(const gps_message &gps)
 		_gps_origin_eph = gps.eph;
 		_gps_origin_epv = gps.epv;
 
-		// if the user has selected GPS as the primary height source, switch across to using it
-		if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) {
-			startGpsHgtFusion();
-		}
-
 		_information_events.flags.gps_checks_passed = true;
 		ECL_INFO("GPS checks passed");