EKF: Enable use of range finder for primary height source

EKF/control.cpp

@@ -247,10 +247,22 @@ void Ekf::controlFusionModes()
 	}
 
 	// Control the soure of height measurements for the main filter
+	if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) {
 	_control_status.flags.baro_hgt = true;
 	_control_status.flags.rng_hgt = false;
 	_control_status.flags.gps_hgt = false;
 
+	} else if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) {
+		_control_status.flags.baro_hgt = false;
+		_control_status.flags.rng_hgt = true;
+		_control_status.flags.gps_hgt = false;
+
+	} else {
+		// TODO functionality to fuse GPS height
+		_control_status.flags.baro_hgt = false;
+		_control_status.flags.rng_hgt = false;
+		_control_status.flags.gps_hgt = false;
+	}
 
 	// Placeholder for control of wind velocity states estimation
 	// TODO add methods for true airspeed and/or sidelsip fusion or some type of drag force measurement

EKF/ekf.cpp

@@ -70,8 +70,10 @@ Ekf::Ekf():
 	_last_gps_origin_time_us(0),
 	_gps_alt_ref(0.0f),
 	_hgt_counter(0),
+	_time_last_hgt(0),
 	_hgt_sum(0.0f),
 	_mag_counter(0),
+	_time_last_mag(0),
 	_hgt_at_alignment(0.0f),
 	_terrain_vpos(0.0f),
 	_hagl_innov(0.0f),
@@ -212,15 +214,22 @@ bool Ekf::update()
 			_fuse_hagl_data = true;
 
 			// only use range finder as a height observation in the main filter if specifically enabled
-			if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) {
+			if (_control_status.flags.rng_hgt) {
 				_fuse_height = true;
 			}
+		}
 
+		// If we are supposed to be using range finder data as the primary height sensor and haven't received it for a while
+		// and are on the ground, then set the measurement to the expected on ground value
+		if ((_time_last_imu - _time_last_hgt_fuse) > 5e5 && _control_status.flags.rng_hgt && !_in_air) {
+			_range_sample_delayed.rng = _params.rng_gnd_clearance;
+			_range_sample_delayed.time_us = _imu_sample_delayed.time_us;
+			_fuse_height = true;
 		}
 
 		// determine if baro data has fallen behind the fuson time horizon and fuse it in the main filter if enabled
 		if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)
-			   && _params.vdist_sensor_type == VDIST_SENSOR_BARO) {
+		    && _control_status.flags.baro_hgt) {
 			_fuse_height = true;
 		}
 
@@ -303,22 +312,27 @@ bool Ekf::initialiseFilter(void)
 	if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) {
 		rangeSample range_init = _range_buffer.get_newest();
 
-		if (range_init.time_us != 0) {
+		if (range_init.time_us != _time_last_hgt) {
 			_hgt_counter ++;
 			_hgt_sum += range_init.rng;
+			_time_last_hgt = range_init.time_us;
 		}
 
-	} else {
+	} else if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) {
 		// initialize vertical position with newest baro measurement
 		baroSample baro_init = _baro_buffer.get_newest();
 
-		if (baro_init.time_us != 0) {
+		if (baro_init.time_us != _time_last_hgt) {
 			_hgt_counter ++;
 			_hgt_sum += baro_init.hgt;
+			_time_last_hgt = baro_init.time_us;
 		}
+
+	} else {
+		return false;
 	}
 
-	// check to see if we have enough measruements and return false if not
+	// check to see if we have enough measurements and return false if not
 	if (_hgt_counter < 10 || _mag_counter < 10) {
 		return false;
 
@@ -361,7 +375,7 @@ bool Ekf::initialiseFilter(void)
 		// calculate the initial magnetic field and yaw alignment
 		resetMagHeading(mag_init);
 
-		// calculate the averaged barometer reading
+		// calculate the averaged height reading
 		_hgt_at_alignment = _hgt_sum / (float)_hgt_counter;
 
 		// set the velocity to the GPS measurement (by definition, the initial position and height is at the origin)

EKF/ekf.h

@@ -197,9 +197,11 @@ private:
 	float _gps_alt_ref;		// WGS-84 height (m)
 
 	// Variables used to initialise the filter states
-	uint8_t _hgt_counter;		// number of baro samples averaged
-	float _hgt_sum;			// summed baro measurement
+	uint8_t _hgt_counter;		// number of height samples averaged
+	uint64_t _time_last_hgt;	// measurement time of last height sample
+	float _hgt_sum;			// summed height measurement
 	uint8_t _mag_counter;		// number of magnetometer samples averaged
+	uint64_t _time_last_mag;	// measurement time of last magnetomter sample
 	Vector3f _mag_sum;		// summed magnetometer measurement
 	Vector3f _delVel_sum;		// summed delta velocity
 	float _hgt_at_alignment;	// baro offset relative to alignment position

EKF/ekf_helper.cpp

@@ -87,7 +87,7 @@ bool Ekf::resetPosition()
 // Reset height state using the last height measurement
 void Ekf::resetHeight()
 {
-	if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) {
+	if (_control_status.flags.rng_hgt) {
 		rangeSample range_newest = _range_buffer.get_newest();
 
 		if (_time_last_imu - range_newest.time_us < 200000) {
@@ -96,7 +96,7 @@ void Ekf::resetHeight()
 		} else {
 			// TODO: reset to last known range based estimate
 		}
-	} else {
+	} else if (_control_status.flags.baro_hgt) {
 		// initialize vertical position with newest baro measurement
 		baroSample baro_newest = _baro_buffer.get_newest();
 
@@ -106,6 +106,8 @@ void Ekf::resetHeight()
 		} else {
 			// TODO: reset to last known baro based estimate
 		}
+	} else {
+		// TODO: reset to GPS height
 	}
 
 }