Terrain estimator: formatting and remove redundant comments

EKF/control.cpp

@@ -502,7 +502,7 @@ void Ekf::controlOpticalFlowFusion()
 			&& !_control_status.flags.opt_flow // we are not yet using flow data
 			&& _control_status.flags.tilt_align // we know our tilt attitude
 			&& !_inhibit_flow_use
-			&& get_terrain_valid()) // we have a valid distance to ground estimate
+			&& isTerrainEstimateValid())
 		{
 			// If the heading is not aligned, reset the yaw and magnetic field states
 			if (!_control_status.flags.yaw_align) {
@@ -1023,7 +1023,7 @@ void Ekf::controlHeightFusion()
 			// 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) {
-				if (get_terrain_valid()) {
+				if (isTerrainEstimateValid()) {
 					_hgt_sensor_offset = _terrain_vpos;
 
 				} else {
@@ -1070,7 +1070,7 @@ void Ekf::controlHeightFusion()
 		// measurement 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 && get_terrain_valid()) {
+			if (_control_status.flags.in_air && isTerrainEstimateValid()) {
 
 				_hgt_sensor_offset = _terrain_vpos;
 
@@ -1105,7 +1105,7 @@ void Ekf::controlHeightFusion()
 			// 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) {
-				if (get_terrain_valid()) {
+				if (isTerrainEstimateValid()) {
 					_hgt_sensor_offset = _terrain_vpos;
 
 				} else {
@@ -1186,7 +1186,7 @@ void Ekf::checkRangeAidSuitability()
 
 	if (_control_status.flags.in_air
 	    && !_rng_hgt_faulty
-	    && get_terrain_valid()
+	    && isTerrainEstimateValid()
 	    && horz_vel_valid) {
 		// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
 		// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
@@ -1417,7 +1417,7 @@ void Ekf::controlMagFusion()
 		if (!_control_status.flags.mag_align_complete && _control_status.flags.in_air) {
 			// Check if height has increased sufficiently to be away from ground magnetic anomalies
 			// and request a yaw reset if not already requested.
-			float terrain_vpos_estimate = get_terrain_valid() ? _terrain_vpos : _last_on_ground_posD;
+			float terrain_vpos_estimate = isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD;
 			_mag_yaw_reset_req |= (terrain_vpos_estimate - _state.pos(2)) > 1.5f;
 		}
 

EKF/ekf.h

@@ -104,11 +104,8 @@ public:
 	// gets the innovation of the drag specific force measurement
 	void get_drag_innov(float drag_innov[2]);
 
-	// gets the innovation variance of the HAGL measurement
+	void getHaglInnovVar(float *hagl_innov_var);
-	void get_hagl_innov_var(float *hagl_innov_var);
+	void getHaglInnov(float *hagl_innov);
-
-	// gets the innovation of the HAGL measurement
-	void get_hagl_innov(float *hagl_innov);
 
 	// get the state vector at the delayed time horizon
 	void get_state_delayed(float *state);
@@ -197,14 +194,12 @@ public:
 	// check if the EKF is dead reckoning horizontal velocity using inertial data only
 	void update_deadreckoning_status();
 
-	// return true if the terrain estimate is valid
+	bool isTerrainEstimateValid();
-	bool get_terrain_valid();
 
-	// update terrain validity status
+	void updateTerrainValidity();
-	void update_terrain_valid();
 
 	// get the estimated terrain vertical position relative to the NED origin
-	void get_terrain_vert_pos(float *ret);
+	void getTerrainVertPos(float *ret);
 
 	// get the terrain variance
 	float get_terrain_var() const { return _terrain_var; }

EKF/ekf_helper.cpp

@@ -1213,7 +1213,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;
-	soln_status.flags.pos_vert_agl = get_terrain_valid();
+	soln_status.flags.pos_vert_agl = isTerrainEstimateValid();
 	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_horiz_abs;

EKF/estimator_interface.h

@@ -122,11 +122,8 @@ public:
 	// gets the innovation of the drag specific force measurement
 	virtual void get_drag_innov(float drag_innov[2]) = 0;
 
-	// gets the innovation variance of the HAGL measurement
+	virtual void getHaglInnovVar(float *hagl_innov_var) = 0;
-	virtual void get_hagl_innov_var(float *hagl_innov_var) = 0;
+	virtual void getHaglInnov(float *hagl_innov) = 0;
-
-	// gets the innovation of the HAGL measurement
-	virtual void get_hagl_innov(float *hagl_innov) = 0;
 
 	// return an array containing the output predictor angular, velocity and position tracking
 	// error magnitudes (rad), (m/s), (m)
@@ -268,11 +265,10 @@ public:
 	// return true if the EKF is dead reckoning the position using inertial data only
 	bool inertial_dead_reckoning() {return _is_dead_reckoning;}
 
-	// return true if the terrain estimate is valid
+	virtual bool isTerrainEstimateValid() = 0;
-	virtual bool get_terrain_valid() = 0;
 
 	// get the estimated terrain vertical position relative to the NED origin
-	virtual void get_terrain_vert_pos(float *ret) = 0;
+	virtual void getTerrainVertPos(float *ret) = 0;
 
 	// return true if the local position estimate is valid
 	bool local_position_is_valid();

EKF/terrain_estimator.cpp

@@ -103,8 +103,8 @@ void Ekf::runTerrainEstimator()
 		_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise);
 
 		// process noise due to terrain gradient
-		_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel(
+		_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient)
-					1)));
+				* (sq(_state.vel(0)) + sq(_state.vel(1)));
 
 		// limit the variance to prevent it becoming badly conditioned
 		_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);
@@ -117,7 +117,6 @@ void Ekf::runTerrainEstimator()
 			// we do this here to avoid doing those calculations at a high rate
 			_sin_tilt_rng = sinf(_params.rng_sens_pitch);
 			_cos_tilt_rng = cosf(_params.rng_sens_pitch);
-
 		}
 
 		if (_flow_for_terrain_data_ready) {
@@ -131,8 +130,7 @@ void Ekf::runTerrainEstimator()
 		}
 	}
 
-	// Update terrain validity
+	updateTerrainValidity();
-	update_terrain_valid();
 }
 
 void Ekf::fuseHagl()
@@ -149,7 +147,9 @@ void Ekf::fuseHagl()
 		_hagl_innov = pred_hagl - meas_hagl;
 
 		// calculate the observation variance adding the variance of the vehicles own height uncertainty
-		float obs_variance = fmaxf(P[9][9] * _params.vehicle_variance_scaler, 0.0f) + sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng);
+		float obs_variance = fmaxf(P[9][9] * _params.vehicle_variance_scaler, 0.0f)
+				     + sq(_params.range_noise)
+				     + sq(_params.range_noise_scaler * _range_sample_delayed.rng);
 
 		// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
 		_hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);
@@ -168,18 +168,20 @@ void Ekf::fuseHagl()
 			// record last successful fusion event
 			_time_last_hagl_fuse = _time_last_imu;
 			_innov_check_fail_status.flags.reject_hagl = false;
+
 		} else {
 			// If we have been rejecting range data for too long, reset to measurement
 			if ((_time_last_imu - _time_last_hagl_fuse) > (uint64_t)10E6) {
 				_terrain_vpos = _state.pos(2) + meas_hagl;
 				_terrain_var = obs_variance;
+
 			} else {
 				_innov_check_fail_status.flags.reject_hagl = true;
 			}
 		}
+
 	} else {
 		_innov_check_fail_status.flags.reject_hagl = true;
-		return;
 	}
 }
 
@@ -286,45 +288,37 @@ void Ekf::fuseFlowForTerrain()
 	}
 }
 
-// return true if the terrain height estimate is valid
+bool Ekf::isTerrainEstimateValid()
-bool Ekf::get_terrain_valid()
 {
 	return _hagl_valid;
 }
 
-// determine terrain validity
+void Ekf::updateTerrainValidity()
-void Ekf::update_terrain_valid()
 {
 	// we have been fusing range finder measurements in the last 5 seconds
-	bool recent_range_fusion = (_time_last_imu - _time_last_hagl_fuse) < 5*1000*1000;
+	bool recent_range_fusion = (_time_last_imu - _time_last_hagl_fuse) < (uint64_t)5e6;
 
 	// we have been fusing optical flow measurements for terrain estimation within the last 5 seconds
 	// this can only be the case if the main filter does not fuse optical flow
-	bool recent_flow_for_terrain_fusion = ((_time_last_imu - _time_last_of_fuse) < 5*1000*1000) && !_control_status.flags.opt_flow;
+	bool recent_flow_for_terrain_fusion = ((_time_last_imu - _time_last_of_fuse) < (uint64_t)5e6)
+					      && !_control_status.flags.opt_flow;
 
-
+	_hagl_valid = (_terrain_initialised && (recent_range_fusion || recent_flow_for_terrain_fusion));
-	if (_terrain_initialised && (recent_range_fusion || recent_flow_for_terrain_fusion)) {
-
-		_hagl_valid = true;
-
-	} else {
-		_hagl_valid = false;
-	}
 }
 
 // get the estimated vertical position of the terrain relative to the NED origin
-void Ekf::get_terrain_vert_pos(float *ret)
+void Ekf::getTerrainVertPos(float *ret)
 {
 	memcpy(ret, &_terrain_vpos, sizeof(float));
 }
 
-void Ekf::get_hagl_innov(float *hagl_innov)
+void Ekf::getHaglInnov(float *hagl_innov)
 {
 	memcpy(hagl_innov, &_hagl_innov, sizeof(_hagl_innov));
 }
 
 
-void Ekf::get_hagl_innov_var(float *hagl_innov_var)
+void Ekf::getHaglInnovVar(float *hagl_innov_var)
 {
 	memcpy(hagl_innov_var, &_hagl_innov_var, sizeof(_hagl_innov_var));
 }
@@ -342,10 +336,5 @@ void Ekf::checkRangeDataContinuity()
 
 	_dt_last_range_update_filt_us = fminf(_dt_last_range_update_filt_us, 4e6f);
 
-	if (_dt_last_range_update_filt_us < 2e6f) {
+	_range_data_continuous = (_dt_last_range_update_filt_us < 2e6f);
-		_range_data_continuous = true;
-
-	} else {
-		_range_data_continuous = false;
-	}
 }