Ekf: extract baro height offset computation

EKF/control.cpp

@@ -1050,16 +1050,7 @@ void Ekf::controlHeightFusion()
 		break;
 	}
 
-	// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference
+	updateBaroHgtOffset();
-	if (!_control_status.flags.baro_hgt && _baro_data_ready) {
-		float local_time_step = 1e-6f * _delta_time_baro_us;
-		local_time_step = math::constrain(local_time_step, 0.0f, 1.0f);
-
-		// apply a 10 second first order low pass filter to baro offset
-		float offset_rate_correction =  0.1f * (_baro_sample_delayed.hgt + _state.pos(
-				2) - _baro_hgt_offset);
-		_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
-	}
 
 	if (isTimedOut(_time_last_hgt_fuse, 2 * RNG_MAX_INTERVAL) && _control_status.flags.rng_hgt
 	    && (!_range_sensor.isDataHealthy())) {

EKF/ekf.h

@@ -761,6 +761,8 @@ private:
 	void startBaroHgtFusion();
 	void startGpsHgtFusion();
 
+	void updateBaroHgtOffset();
+
 	// calculate the measurement variance for the optical flow sensor
 	float calcOptFlowMeasVar();
 

EKF/ekf_helper.cpp

@@ -1498,6 +1498,21 @@ void Ekf::startGpsHgtFusion()
 	}
 }
 
+void Ekf::updateBaroHgtOffset()
+{
+	// calculate a filtered offset between the baro origin and local NED origin if we are not
+	// using the baro as a height reference
+	if (!_control_status.flags.baro_hgt && _baro_data_ready) {
+		float local_time_step = 1e-6f * _delta_time_baro_us;
+		local_time_step = math::constrain(local_time_step, 0.0f, 1.0f);
+
+		// apply a 10 second first order low pass filter to baro offset
+		float offset_rate_correction =  0.1f * (_baro_sample_delayed.hgt + _state.pos(
+				2) - _baro_hgt_offset);
+		_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
+	}
+}
+
 // update the rotation matrix which rotates EV measurements into the EKF's navigation frame
 void Ekf::calcExtVisRotMat()
 {