make range sensor height offset computation more robust

- when switching to range finder use the current terrain estimate as
height sensor offset, otherwise spikes in the range measurements could lead
to a wrong offset

Signed-off-by: Roman <bapstroman@gmail.com>

EKF/control.cpp

@@ -713,8 +713,12 @@ void Ekf::controlHeightFusion()
 			// we have just switched to using range finder, calculate height sensor offset such that current
 			// measurment matches our current height estimate
 			if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
+				if (_terrain_initialised) {
+					_hgt_sensor_offset = _terrain_vpos;
+				} else {
 					_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
 				}
+			}
 
         } else if (_baro_data_ready && !_baro_hgt_faulty && !_in_range_aid_mode) {
 			setControlBaroHeight();
@@ -745,8 +749,12 @@ void Ekf::controlHeightFusion()
 			// we have just switched to using range finder, calculate height sensor offset such that current
 			// measurment matches our current height estimate
 			if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
+				if (_terrain_initialised) {
+					_hgt_sensor_offset = _terrain_vpos;
+				} else {
 					_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
 				}
+			}
 
         } else if (_gps_data_ready && !_gps_hgt_faulty && !_in_range_aid_mode) {
 			setControlGPSHeight();