Ekf: centralize GPS height fusion startup

PX4 · Apr 16, 2020 · b77988e · b77988e
1 parent 65e7c85
commit b77988e
Show file tree

Hide file tree

Showing 4 changed files with 15 additions and 11 deletions.
diff --git a/EKF/control.cpp b/EKF/control.cpp
@@ -822,7 +822,7 @@ void Ekf::controlHeightSensorTimeouts()
 				// set height sensor health
 				_baro_hgt_faulty = true;
 
-				setControlGPSHeight();
+				startGpsHgtFusion();
 
 				request_height_reset = true;
 				ECL_WARN_TIMESTAMPED("baro hgt timeout - reset to GPS");
@@ -1018,14 +1018,8 @@ void Ekf::controlHeightFusion()
 			}
 
 		} else if (!do_range_aid && _gps_data_ready && !_gps_hgt_intermittent && _gps_checks_passed) {
-			setControlGPSHeight();
 			fuse_height = true;
-
-			// we have just switched to using gps height, calculate height sensor offset such that current
-			// measurement matches our current height estimate
-			if (_control_status_prev.flags.gps_hgt != _control_status.flags.gps_hgt) {
-				_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
-			}
+			startGpsHgtFusion();
 
 		} else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
 			// switch to baro if there was a reset to baro

diff --git a/EKF/ekf.h b/EKF/ekf.h
@@ -759,6 +759,7 @@ class Ekf : public EstimatorInterface
 	void startMag3DFusion();
 
 	void startBaroHgtFusion();
+	void startGpsHgtFusion();
 
 	// calculate the measurement variance for the optical flow sensor
 	float calcOptFlowMeasVar();

diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
@@ -1487,6 +1487,17 @@ void Ekf::startBaroHgtFusion()
 	}
 }
 
+void Ekf::startGpsHgtFusion()
+{
+	setControlGPSHeight();
+
+	// we have just switched to using gps height, calculate height sensor offset such that current
+	// measurement matches our current height estimate
+	if (_control_status_prev.flags.gps_hgt != _control_status.flags.gps_hgt) {
+		_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
+	}
+}
+
 // update the rotation matrix which rotates EV measurements into the EKF's navigation frame
 void Ekf::calcExtVisRotMat()
 {

diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp
@@ -100,9 +100,7 @@ bool Ekf::collect_gps(const gps_message &gps)
 
 		if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) {
 			ECL_INFO_TIMESTAMPED("GPS checks passed (WGS-84 origin set, using GPS height)");
-			setControlGPSHeight();
-			// zero the sensor offset
-			_hgt_sensor_offset = 0.0f;
+			startGpsHgtFusion();
 		} else {
 			ECL_INFO_TIMESTAMPED("GPS checks passed (WGS-84 origin set)");
 		}