Revert "Revert "Split get_terrain_vert_pos() into valid and get_vpos""

EKF/control.cpp

@@ -734,7 +734,7 @@ 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) {
+				if (get_terrain_valid()) {
 					_hgt_sensor_offset = _terrain_vpos;
 				} else {
 					_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
@@ -774,7 +774,7 @@ void Ekf::controlHeightFusion()
 		// measurment matches our current height estimate
 		if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
 			// use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
-			if (_control_status.flags.in_air && _terrain_initialised) {
+			if (_control_status.flags.in_air && get_terrain_valid()) {
 
 				_hgt_sensor_offset = _terrain_vpos;
 			} else if (_control_status.flags.in_air) {
@@ -807,7 +807,7 @@ 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) {
+				if (get_terrain_valid()) {
 					_hgt_sensor_offset = _terrain_vpos;
 				} else {
 					_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
@@ -875,12 +875,12 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode)
 		// check if we should use range finder measurements to estimate height, use hysteresis to avoid rapid switching
 		bool use_range_finder;
 		if (in_range_aid_mode) {
-			use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && _terrain_initialised;
+			use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && get_terrain_valid();
 
 		} else {
 			// if we were not using range aid in the previous iteration then require the current height above terrain to be
 			// smaller than 70 % of the maximum allowed ground distance for range aid
-			use_range_finder = (_terrain_vpos - _state.pos(2) < 0.7f * _params.max_hagl_for_range_aid) && _terrain_initialised;
+			use_range_finder = (_terrain_vpos - _state.pos(2) < 0.7f * _params.max_hagl_for_range_aid) && get_terrain_valid();
 		}
 
 		bool horz_vel_valid = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow)

EKF/ekf.h

@@ -154,9 +154,11 @@ public:
 	// return true if the EKF is dead reckoning the position using inertial data only
 	bool inertial_dead_reckoning();
 
-	// return true if the etimate is valid
+	// return true if the terrain estimate is valid
-	// return the estimated terrain vertical position relative to the NED origin
+	bool get_terrain_valid();
-	bool get_terrain_vert_pos(float *ret);
+
+	// get the estimated terrain vertical position relative to the NED origin
+	void get_terrain_vert_pos(float *ret);
 
 	// get the accerometer bias in m/s/s
 	void get_accel_bias(float bias[3]);

EKF/ekf_helper.cpp

@@ -1040,8 +1040,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status)
 	soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
 	soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0);
 	soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert;
-	float dummy_var;
+	soln_status.flags.pos_vert_agl = get_terrain_valid();
-	soln_status.flags.pos_vert_agl = get_terrain_vert_pos(&dummy_var);
 	soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz;
 	soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel;
 	soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_vert_abs;

EKF/estimator_interface.h

@@ -199,9 +199,11 @@ public:
 	// return true if the EKF is dead reckoning the position using inertial data only
 	virtual bool inertial_dead_reckoning() = 0;
 
-	// return true if the estimate is valid
+	// return true if the terrain estimate is valid
-	// return the estimated terrain vertical position relative to the NED origin
+	virtual bool get_terrain_valid() = 0;
-	virtual bool get_terrain_vert_pos(float *ret) = 0;
+
+	// get the estimated terrain vertical position relative to the NED origin
+	virtual void get_terrain_vert_pos(float *ret) = 0;
 
 	// return true if the local position estimate is valid
 	bool local_position_is_valid();

EKF/terrain_estimator.cpp

@@ -150,12 +150,9 @@ void Ekf::fuseHagl()
 	}
 }
 
-// return true if the estimate is fresh
+// return true if the terrain estimate is valid
-// return the estimated vertical position of the terrain relative to the NED origin
+bool Ekf::get_terrain_valid()
-bool Ekf::get_terrain_vert_pos(float *ret)
 {
-	memcpy(ret, &_terrain_vpos, sizeof(float));
-
 	if (_terrain_initialised && _range_data_continuous && !_innov_check_fail_status.flags.reject_hagl) {
 		return true;
 
@@ -164,6 +161,12 @@ bool Ekf::get_terrain_vert_pos(float *ret)
 	}
 }
 
+// get the estimated vertical position of the terrain relative to the NED origin
+void Ekf::get_terrain_vert_pos(float *ret)
+{
+	memcpy(ret, &_terrain_vpos, sizeof(float));
+}
+
 void Ekf::get_hagl_innov(float *hagl_innov)
 {
 	memcpy(hagl_innov, &_hagl_innov, sizeof(_hagl_innov));