SensorRangeFinder: add distBottom function to get the vertical distance

EKF/control.cpp

@@ -1001,7 +1001,7 @@ void Ekf::controlHeightFusion()
 					_hgt_sensor_offset = _terrain_vpos;
 
 				} else if (_control_status.flags.in_air) {
-					_hgt_sensor_offset = _range_sensor.getCosTilt() * _range_sensor.getRange() + _state.pos(2);
+					_hgt_sensor_offset = _range_sensor.getDistBottom() + _state.pos(2);
 
 				} else {
 					_hgt_sensor_offset = _params.rng_gnd_clearance;
@@ -1161,10 +1161,11 @@ void Ekf::controlHeightFusion()
 			Vector2f rng_hgt_innov_gate;
 			Vector3f rng_hgt_obs_var;
 			// use range finder with tilt correction
-			_rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sensor.getRange() * _range_sensor.getCosTilt(),
+			_rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(),
 							 _params.rng_gnd_clearance)) - _hgt_sensor_offset;
 			// observation variance - user parameter defined
-			rng_hgt_obs_var(2) = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getRange())) * sq(_range_sensor.getCosTilt()), 0.01f);
+			rng_hgt_obs_var(2) = fmaxf(sq(_params.range_noise)
+						   + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
 			// innovation gate size
 			rng_hgt_innov_gate(1) = fmaxf(_params.range_innov_gate, 1.0f);
 			// fuse height information

EKF/ekf_helper.cpp

@@ -227,7 +227,7 @@ void Ekf::resetHeight()
 
 	// reset the vertical position
 	if (_control_status.flags.rng_hgt) {
-		const float new_pos_down = _hgt_sensor_offset - _range_sensor.getRange() * _range_sensor.getCosTilt();
+		const float new_pos_down = _hgt_sensor_offset - _range_sensor.getDistBottom();
 
 		// update the state and associated variance
 		_state.pos(2) = new_pos_down;

EKF/sensor_range_finder.hpp

@@ -88,6 +88,8 @@ public:
 	void setRange(float rng) { _sample.rng = rng; }
 	float getRange() const { return _sample.rng; }
 
+	float getDistBottom() const { return _sample.rng * _cos_tilt_rng_to_earth; }
+
 	void setDataReadiness(bool is_ready) { _is_sample_ready = is_ready; }
 	void setValidity(bool is_valid) { _is_sample_valid = is_valid; }
 

EKF/terrain_estimator.cpp

@@ -57,7 +57,7 @@ bool Ekf::initHagl()
 	} else if ((_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder)
 		   && _range_sensor.isDataHealthy()) {
 		// if we have a fresh measurement, use it to initialise the terrain estimator
-		_terrain_vpos = _state.pos(2) + _range_sensor.getRange() * _range_sensor.getCosTilt();
+		_terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom();
 		// initialise state variance to variance of measurement
 		_terrain_var = sq(_params.range_noise);
 		// success
@@ -130,7 +130,7 @@ void Ekf::runTerrainEstimator()
 void Ekf::fuseHagl()
 {
 	// get a height above ground measurement from the range finder assuming a flat earth
-	const float meas_hagl = _range_sensor.getRange() * _range_sensor.getCosTilt();
+	const float meas_hagl = _range_sensor.getDistBottom();
 
 	// predict the hagl from the vehicle position and terrain height
 	const float pred_hagl = _terrain_vpos - _state.pos(2);

test/test_SensorRangeFinder.cpp

@@ -297,3 +297,20 @@ TEST_F(SensorRangeFinderTest, continuity)
 	EXPECT_TRUE(_range_finder.isDataHealthy());
 	EXPECT_TRUE(_range_finder.isHealthy());
 }
+
+TEST_F(SensorRangeFinderTest, distBottom)
+{
+	const Dcmf attitude{Eulerf(0.f, 0.f, 0.f)};
+	rangeSample sample{};
+	sample.rng = 1.f;
+	sample.time_us = 1e6;
+	sample.quality = 9;
+
+	_range_finder.setSample(sample);
+	_range_finder.runChecks(sample.time_us, attitude);
+	EXPECT_FLOAT_EQ(_range_finder.getDistBottom(), sample.rng);
+
+	const Dcmf attitude20{Eulerf(-0.35f, 0.f, 0.f)};
+	_range_finder.runChecks(sample.time_us, attitude20);
+	EXPECT_FLOAT_EQ(_range_finder.getDistBottom(), sample.rng * cosf(-0.35));
+}