refactor start of gps into separate function

PX4 · Aug 7, 2020 · 310b989 · 310b989
1 parent 7eb2b08
commit 310b989
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 12 deletions.
diff --git a/EKF/control.cpp b/EKF/control.cpp
@@ -514,16 +514,7 @@ void Ekf::controlGpsFusion()
 
 				// If the heading is valid start using gps aiding
 				if (_control_status.flags.yaw_align) {
-					resetHorizontalPositionToGps();
-
-					// when adding with optical flow,
-					// velocity reset is not necessary
-					if (!_control_status.flags.opt_flow) {
-						resetVelocityToGps();
-					}
-
-					ECL_INFO_TIMESTAMPED("starting GPS fusion");
-					_control_status.flags.gps = true;
+					startGpsFusion();
 				}
 			}
 

diff --git a/EKF/ekf.h b/EKF/ekf.h
@@ -847,10 +847,9 @@ class Ekf : public EstimatorInterface
 		return sensor_timestamp + acceptance_interval > _time_last_imu;
 	}
 
+	void startGpsFusion();
 	void stopGpsFusion();
-
 	void stopGpsPosFusion();
-
 	void stopGpsVelFusion();
 
 	void startGpsYawFusion();

diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
@@ -1530,6 +1530,20 @@ void Ekf::loadMagCovData()
 	P.slice<2, 2>(16, 16) = _saved_mag_ef_covmat;
 }
 
+void Ekf::startGpsFusion()
+{
+	resetHorizontalPositionToGps();
+
+	// when using optical flow,
+	// velocity reset is not necessary
+	if (!_control_status.flags.opt_flow) {
+		resetVelocityToGps();
+	}
+
+	ECL_INFO_TIMESTAMPED("starting GPS fusion");
+	_control_status.flags.gps = true;
+}
+
 void Ekf::stopGpsFusion()
 {
 	stopGpsPosFusion();