From 4fab6057a7996a4f15263d340b5316733d9b046f Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 24 Jan 2020 19:35:10 -0500
Subject: [PATCH] EKF control height fusion add switch statement for all dist
 sensor types

---
 EKF/control.cpp | 59 +++++++++++++++++++++++++++----------------------
 1 file changed, 32 insertions(+), 27 deletions(-)

diff --git a/EKF/control.cpp b/EKF/control.cpp
index 4e75e23eec..80a9ba03ac 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -932,8 +932,12 @@ void Ekf::controlHeightFusion()
 	checkRangeAidSuitability();
 	_range_aid_mode_selected = (_params.range_aid == 1) && isRangeAidSuitable();
 
-	if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) {
+	switch (_params.vdist_sensor_type) {
+	default:
+		ECL_ERR("Invalid height mode: %d", _params.vdist_sensor_type);
 
+	// FALLTHROUGH
+	case VDIST_SENSOR_BARO:
 		if (_range_aid_mode_selected && _range_data_ready && _rng_hgt_valid) {
 			setControlRangeHeight();
 			_fuse_height = true;
@@ -977,45 +981,44 @@ void Ekf::controlHeightFusion()
 				_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
 			}
 		}
-	}
 
-	// set the height data source to range if requested
-	if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && _rng_hgt_valid) {
-		setControlRangeHeight();
-		_fuse_height = _range_data_ready;
+		break;
 
-		// we have just switched to using range finder, calculate height sensor offset such that current
-		// measurement matches our current height estimate
-		if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
+	case VDIST_SENSOR_RANGE:
+		if (_range_data_ready && _rng_hgt_valid) {
+			setControlRangeHeight();
+			_fuse_height = _range_data_ready;
+
+		} else if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
+			// we have just switched to using range finder, calculate height sensor offset such that current
+			// measurement matches our current height estimate
 			// use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
 			if (_control_status.flags.in_air && isTerrainEstimateValid()) {
-
 				_hgt_sensor_offset = _terrain_vpos;
 
 			} else if (_control_status.flags.in_air) {
-
 				_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
 
 			} else {
-
 				_hgt_sensor_offset = _params.rng_gnd_clearance;
 			}
+
+		} else if (_baro_data_ready && !_baro_hgt_faulty) {
+			setControlBaroHeight();
+			_fuse_height = true;
+
+			// we have just switched to using baro height, we don't need to set a height sensor offset
+			// since we track a separate _baro_hgt_offset
+			if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) {
+				_hgt_sensor_offset = 0.0f;
+			}
 		}
 
-	} else if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && _baro_data_ready && !_baro_hgt_faulty) {
-		setControlBaroHeight();
-		_fuse_height = true;
+		break;
 
-		// we have just switched to using baro height, we don't need to set a height sensor offset
-		// since we track a separate _baro_hgt_offset
-		if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) {
-			_hgt_sensor_offset = 0.0f;
-		}
-	}
-
-	// Determine if GPS should be used as the height source
-	if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) {
+	case VDIST_SENSOR_GPS:
 
+		// Determine if GPS should be used as the height source
 		if (_range_aid_mode_selected && _range_data_ready && _rng_hgt_valid) {
 			setControlRangeHeight();
 			_fuse_height = true;
@@ -1051,10 +1054,10 @@ void Ekf::controlHeightFusion()
 				_hgt_sensor_offset = 0.0f;
 			}
 		}
-	}
 
-	// Determine if we rely on EV height but switched to baro
-	if (_params.vdist_sensor_type == VDIST_SENSOR_EV) {
+		break;
+
+	case VDIST_SENSOR_EV:
 
 		// don't start using EV data unless data is arriving frequently
 		if (!_control_status.flags.ev_hgt && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
@@ -1078,6 +1081,8 @@ void Ekf::controlHeightFusion()
 		if (_control_status.flags.ev_hgt) {
 			_fuse_height = true;
 		}
+
+		break;
 	}
 
 	// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference