ekf2: split gnss pos/vel flags

They can be selected independently in the control parameter, so there is
no reason why they should share the same flag.

Tools/ecl_ekf/analyse_logdata_ekf.py

@@ -111,13 +111,13 @@ def find_checks_that_apply(
         innov_fail_checks.append('yaw')
 
     # Velocity Sensor Checks
-    if (np.amax(estimator_status_flags['cs_gps']) > 0.5):
+    if (np.amax(estimator_status_flags['cs_gnss_vel']) > 0.5):
         sensor_checks.append('vel')
         innov_fail_checks.append('velh')
         innov_fail_checks.append('velv')
 
     # Position Sensor Checks
-    if (pos_checks_when_sensors_not_fused or (np.amax(estimator_status_flags['cs_gps']) > 0.5)
+    if (pos_checks_when_sensors_not_fused or (np.amax(estimator_status_flags['cs_gnss_pos']) > 0.5)
         or (np.amax(estimator_status_flags['cs_ev_pos']) > 0.5)):
         sensor_checks.append('pos')
         innov_fail_checks.append('posh')

Tools/ecl_ekf/plotting/pdf_report.py

@@ -185,7 +185,7 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
         # plot control mode summary A
         data_plot = ControlModeSummaryPlot(
             status_flags_time, estimator_status_flags, [['cs_tilt_align', 'cs_yaw_align'],
-            ['cs_gps', 'cs_opt_flow', 'cs_ev_pos'], ['cs_baro_hgt', 'cs_gps_hgt',
+            ['cs_gnss_pos', 'cs_opt_flow', 'cs_ev_pos'], ['cs_baro_hgt', 'cs_gps_hgt',
              'cs_rng_hgt', 'cs_ev_hgt'], ['cs_mag_hdg', 'cs_mag_3d', 'cs_mag_dec']],
             x_label='time (sec)', y_labels=['aligned', 'pos aiding', 'hgt aiding', 'mag aiding'],
             annotation_text=[['tilt alignment', 'yaw alignment'], ['GPS aiding', 'optical flow aiding',

msg/EstimatorStatus.msg

@@ -20,7 +20,7 @@ uint8 GPS_CHECK_FAIL_SPOOFED = 10		# 10 : GPS signal is spoofed
 uint64 control_mode_flags	# Bitmask to indicate EKF logic state
 uint8 CS_TILT_ALIGN = 0		# 0 - true if the filter tilt alignment is complete
 uint8 CS_YAW_ALIGN = 1		# 1 - true if the filter yaw alignment is complete
-uint8 CS_GPS = 2		# 2 - true if GPS measurements are being fused
+uint8 CS_GNSS_POS = 2		# 2 - true if GNSS position measurements are being fused
 uint8 CS_OPT_FLOW = 3		# 3 - true if optical flow measurements are being fused
 uint8 CS_MAG_HDG = 4		# 4 - true if a simple magnetic yaw heading is being fused
 uint8 CS_MAG_3D = 5		# 5 - true if 3-axis magnetometer measurement are being fused
@@ -47,6 +47,7 @@ uint8 CS_SYNTHETIC_MAG_Z = 25	# 25 - true when we are using a synthesized measur
 uint8 CS_VEHICLE_AT_REST = 26	# 26 - true when the vehicle is at rest
 uint8 CS_GPS_YAW_FAULT = 27	# 27 - true when the GNSS heading has been declared faulty and is no longer being used
 uint8 CS_RNG_FAULT = 28		# 28 - true when the range finder has been declared faulty and is no longer being used
+uint8 CS_GNSS_VEL = 44		# 44 - true if GNSS velocity measurements are being fused
 
 uint32 filter_fault_flags	# Bitmask to indicate EKF internal faults
 # 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error

msg/EstimatorStatusFlags.msg

@@ -6,7 +6,7 @@ uint64 timestamp_sample                   # the timestamp of the raw data (micro
 uint32 control_status_changes # number of filter control status (cs) changes
 bool cs_tilt_align            #  0 - true if the filter tilt alignment is complete
 bool cs_yaw_align             #  1 - true if the filter yaw alignment is complete
-bool cs_gps                   #  2 - true if GPS measurement fusion is intended
+bool cs_gnss_pos              #  2 - true if GNSS position measurement fusion is intended
 bool cs_opt_flow              #  3 - true if optical flow measurements fusion is intended
 bool cs_mag_hdg               #  4 - true if a simple magnetic yaw heading fusion is intended
 bool cs_mag_3d                #  5 - true if 3-axis magnetometer measurement fusion is intended
@@ -48,6 +48,7 @@ bool cs_opt_flow_terrain        # 40 - true if we are fusing flow data for terra
 bool cs_valid_fake_pos          # 41 - true if a valid constant position is being fused
 bool cs_constant_pos            # 42 - true if the vehicle is at a constant position
 bool cs_baro_fault	        # 43 - true when the current baro has been declared faulty and is no longer being used
+bool cs_gnss_vel                # 44 - true if GNSS velocity measurement fusion is intended
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes

src/modules/airspeed_selector/airspeed_selector_main.cpp

@@ -561,7 +561,8 @@ void AirspeedModule::poll_topics()
 	_gnss_lpos_valid = (_time_now_usec - _vehicle_local_position.timestamp < 1_s)
 			   && (_vehicle_local_position.timestamp > 0)
 			   && _vehicle_local_position.v_xy_valid
-			   && _estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
+			   && (_estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GNSS_POS)
+			       || _estimator_status.control_mode_flags & (static_cast<uint64_t>(1) << estimator_status_s::CS_GNSS_VEL));
 }
 
 void AirspeedModule::update_wind_estimator_sideslip()

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

@@ -224,7 +224,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 
 	// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
 	if (_param_sys_has_gps.get()) {
-		const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
+		const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GNSS_POS);
 		const bool ekf_gps_check_fail = estimator_status.gps_check_fail_flags > 0;
 
 		if (ekf_gps_fusion) {

src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp

@@ -70,7 +70,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
 			pos = ev_sample.pos - pos_offset_earth;
 			pos_cov = matrix::diag(ev_sample.position_var);
 
-			if (_control_status.flags.gps) {
+			if (_control_status.flags.gnss_pos) {
 				_ev_pos_b_est.setFusionActive();
 
 			} else {
@@ -109,7 +109,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
 				pos_cov(i, i) = math::max(pos_cov(i, i), orientation_var_max);
 			}
 
-			if (_control_status.flags.gps) {
+			if (_control_status.flags.gnss_pos) {
 				_ev_pos_b_est.setFusionActive();
 
 			} else {
@@ -128,8 +128,8 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
 
 #if defined(CONFIG_EKF2_GNSS)
 
-	// increase minimum variance if GPS active (position reference)
+	// increase minimum variance if GNSS is active (position reference)
-	if (_control_status.flags.gps) {
+	if (_control_status.flags.gnss_pos) {
 		for (int i = 0; i < 2; i++) {
 			pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.gps_pos_noise));
 		}
@@ -148,7 +148,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
 
 	const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite();
 
-	// bias fusion activated (GPS activated)
+	// bias fusion activated (GNSS position activated)
 	if (!bias_fusion_was_active && _ev_pos_b_est.fusionActive()) {
 		if (quality_sufficient) {
 			// reset the bias estimator
@@ -213,7 +213,7 @@ void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurem
 {
 	// activate fusion
 	// TODO:  (_params.position_sensor_ref == PositionSensor::EV)
-	if (_control_status.flags.gps) {
+	if (_control_status.flags.gnss_pos) {
 		ECL_INFO("starting %s fusion", EV_AID_SRC_NAME);
 		_ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement);
 		_ev_pos_b_est.setFusionActive();
@@ -240,7 +240,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
 
 		if (quality_sufficient) {
 
-			if (!_control_status.flags.gps) {
+			if (!_control_status.flags.gnss_pos) {
 				ECL_INFO("reset to %s", EV_AID_SRC_NAME);
 				_information_events.flags.reset_pos_to_vision = true;
 				resetHorizontalPositionTo(measurement, measurement_var);
@@ -275,14 +275,14 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
 			// Data seems good, attempt a reset
 			ECL_WARN("%s fusion failing, resetting", EV_AID_SRC_NAME);
 
-			if (_control_status.flags.gps && !pos_xy_fusion_failing) {
+			if (_control_status.flags.gnss_pos && !pos_xy_fusion_failing) {
 				// reset EV position bias
 				_ev_pos_b_est.setBias(-Vector2f(getLocalHorizontalPosition()) + measurement);
 
 			} else {
 				_information_events.flags.reset_pos_to_vision = true;
 
-				if (_control_status.flags.gps) {
+				if (_control_status.flags.gnss_pos) {
 					resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var + _ev_pos_b_est.getBiasVar());
 					_ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement);
 

src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp

@@ -72,8 +72,8 @@ void Ekf::controlEvYawFusion(const imuSample &imu_sample, const extVisionSample
 					     && PX4_ISFINITE(aid_src.observation)
 					     && PX4_ISFINITE(aid_src.observation_variance);
 
-	// if GPS enabled don't allow EV yaw if EV isn't NED
+	// if GNSS is enabled don't allow EV yaw if EV isn't NED
-	if (_control_status.flags.gps && _control_status.flags.yaw_align
+	if ((_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) && _control_status.flags.yaw_align
 	    && (ev_sample.pos_frame != PositionFrame::LOCAL_FRAME_NED)
 	   ) {
 		continuing_conditions_passing = false;

src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp

@@ -96,11 +96,10 @@ void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample)
 		} else {
 			if (starting_conditions_passing) {
 				// Try to activate GNSS yaw fusion
-				const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos;
 
 				if (!_control_status.flags.in_air
 				    || !_control_status.flags.yaw_align
-				    || not_using_ne_aiding) {
+				    || !isNorthEastAidingActive()) {
 
 					// Reset before starting the fusion
 					if (resetYawToGnss(gnss_sample.yaw, gnss_sample.yaw_offset)) {

src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp

@@ -42,7 +42,7 @@
 void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 {
 	if (!_gps_buffer || (_params.gnss_ctrl == 0)) {
-		stopGpsFusion();
+		stopGnssFusion();
 		return;
 	}
 
@@ -78,19 +78,20 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 			// Skip this sample
 			_gps_data_ready = false;
 
-			if (_control_status.flags.gps && isTimedOut(_last_gps_pass_us, _params.reset_timeout_max)) {
+			if ((_control_status.flags.gnss_vel || _control_status.flags.gnss_pos)
-				stopGpsFusion();
+			    && isTimedOut(_last_gps_pass_us, _params.reset_timeout_max)) {
-				ECL_WARN("GPS quality poor - stopping use");
+				stopGnssFusion();
+				ECL_WARN("GNSS quality poor - stopping use");
 			}
 		}
 
 		updateGnssPos(gnss_sample, _aid_src_gnss_pos);
 		updateGnssVel(imu_delayed, gnss_sample, _aid_src_gnss_vel);
 
-	} else if (_control_status.flags.gps) {
+	} else if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) {
 		if (!isNewestSampleRecent(_time_last_gps_buffer_push, _params.reset_timeout_max)) {
-			stopGpsFusion();
+			stopGnssFusion();
-			ECL_WARN("GPS data stopped");
+			ECL_WARN("GNSS data stopped");
 		}
 	}
 
@@ -102,81 +103,105 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 
 		controlGnssYawEstimator(_aid_src_gnss_vel);
 
-		const bool gnss_vel_enabled = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL));
+		bool do_vel_pos_reset = false;
-		const bool gnss_pos_enabled = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS));
 
-		const bool continuing_conditions_passing = (gnss_vel_enabled || gnss_pos_enabled)
+		if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) {
-				&& _control_status.flags.tilt_align
+			// Reset checks need to run together, before each control function runs because the result would change
-				&& _control_status.flags.yaw_align;
+			// after the first one runs
-		const bool starting_conditions_passing = continuing_conditions_passing && _gps_checks_passed;
+			do_vel_pos_reset = shouldResetGpsFusion();
-		const bool gpos_init_conditions_passing = gnss_pos_enabled && _gps_checks_passed;
 
-		if (_control_status.flags.gps) {
+			if (_control_status.flags.in_air
-			if (continuing_conditions_passing) {
+			    && isYawFailure()
-				if (gnss_vel_enabled) {
+			    && isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
-					fuseVelocity(_aid_src_gnss_vel);
+			    && (_time_last_hor_vel_fuse > _time_last_on_ground_us)) {
-				}
+				do_vel_pos_reset = tryYawEmergencyReset();
+			}
+		}
 
-				if (gnss_pos_enabled) {
+		controlGnssVelFusion(_aid_src_gnss_vel, do_vel_pos_reset);
-					fuseHorizontalPosition(_aid_src_gnss_pos);
+		controlGnssPosFusion(_aid_src_gnss_pos, do_vel_pos_reset);
-				}
+	}
+}
 
-				bool do_vel_pos_reset = shouldResetGpsFusion();
+void Ekf::controlGnssVelFusion(estimator_aid_source3d_s &aid_src, const bool force_reset)
+{
+	const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
+			&& _control_status.flags.tilt_align
+			&& _control_status.flags.yaw_align;
+	const bool starting_conditions_passing = continuing_conditions_passing && _gps_checks_passed;
 
-				if (_control_status.flags.in_air
+	if (_control_status.flags.gnss_vel) {
-				    && isYawFailure()
+		if (continuing_conditions_passing) {
-				    && isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
+			if (force_reset) {
-				    && (_time_last_hor_vel_fuse > _time_last_on_ground_us)) {
+				ECL_WARN("GNSS fusion timeout, resetting");
-					do_vel_pos_reset = tryYawEmergencyReset();
+				resetVelocityToGnss(aid_src);
-				}
-
-				if (do_vel_pos_reset) {
-					ECL_WARN("GPS fusion timeout, resetting");
-				}
-
-				if (gnss_vel_enabled) {
-					if (do_vel_pos_reset) {
-						resetVelocityToGnss(_aid_src_gnss_vel);
-
-					} else if (isHeightResetRequired()) {
-						// reset vertical velocity if height is failing
-						resetVerticalVelocityTo(_aid_src_gnss_vel.observation[2], _aid_src_gnss_vel.observation_variance[2]);
-					}
-				}
-
-				if (gnss_pos_enabled && do_vel_pos_reset) {
-					resetHorizontalPositionToGnss(_aid_src_gnss_pos);
-				}
 
 			} else {
-				stopGpsFusion();
+				fuseVelocity(aid_src);
+
+				if (isHeightResetRequired()) {
+					// reset vertical velocity if height is failing
+					resetVerticalVelocityTo(aid_src.observation[2], aid_src.observation_variance[2]);
+				}
 			}
 
 		} else {
-			if (starting_conditions_passing) {
+			stopGnssVelFusion();
-				ECL_INFO("starting GPS fusion");
+		}
-				_information_events.flags.starting_gps_fusion = true;
 
-				// when already using another velocity source velocity reset is not necessary
+	} else {
-				if (!isHorizontalAidingActive()
+		if (starting_conditions_passing) {
-				    || isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
+			ECL_INFO("starting GNSS velocity fusion");
-				    || !_control_status_prev.flags.yaw_align
+			_information_events.flags.starting_gps_fusion = true;
-				   ) {
-					// reset velocity
-					if (gnss_vel_enabled) {
-						resetVelocityToGnss(_aid_src_gnss_vel);
-					}
-				}
 
-				if (gnss_pos_enabled) {
+			// when already using another velocity source velocity reset is not necessary
-					resetHorizontalPositionToGnss(_aid_src_gnss_pos);
+			if (!isHorizontalAidingActive()
-				}
+			    || isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
-
+			    || !_control_status_prev.flags.yaw_align
-				_control_status.flags.gps = true;
+			   ) {
-
+				// reset velocity
-			} else if (gpos_init_conditions_passing && !_local_origin_lat_lon.isInitialized()) {
+				resetVelocityToGnss(aid_src);
-				resetHorizontalPositionToGnss(_aid_src_gnss_pos);
 			}
+
+			_control_status.flags.gnss_vel = true;
+		}
+	}
+}
+
+void Ekf::controlGnssPosFusion(estimator_aid_source2d_s &aid_src, const bool force_reset)
+{
+	const bool gnss_pos_enabled = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS));
+
+	const bool continuing_conditions_passing = gnss_pos_enabled
+			&& _control_status.flags.tilt_align
+			&& _control_status.flags.yaw_align;
+	const bool starting_conditions_passing = continuing_conditions_passing && _gps_checks_passed;
+	const bool gpos_init_conditions_passing = gnss_pos_enabled && _gps_checks_passed;
+
+	if (_control_status.flags.gnss_pos) {
+		if (continuing_conditions_passing) {
+			if (force_reset) {
+				ECL_WARN("GNSS fusion timeout, resetting");
+				resetHorizontalPositionToGnss(aid_src);
+
+			} else {
+				fuseHorizontalPosition(aid_src);
+			}
+
+		} else {
+			stopGnssPosFusion();
+		}
+
+	} else {
+		if (starting_conditions_passing) {
+			ECL_INFO("starting GNSS position fusion");
+			_information_events.flags.starting_gps_fusion = true;
+
+			resetHorizontalPositionToGnss(aid_src);
+			_control_status.flags.gnss_pos = true;
+
+		} else if (gpos_init_conditions_passing && !_local_origin_lat_lon.isInitialized()) {
+			resetHorizontalPositionToGnss(aid_src);
 		}
 	}
 }
@@ -231,8 +256,8 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s
 	// relax the upper observation noise limit which prevents bad GPS perturbing the position estimate
 	float pos_noise = math::max(gnss_sample.hacc, _params.gps_pos_noise);
 
-	if (!isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
+	if (!isOtherSourceOfHorizontalAidingThan(_control_status.flags.gnss_pos)) {
-		// if we are not using another source of aiding, then we are reliant on the GPS
+		// if we are not using another source of aiding, then we are reliant on the GNSS
 		// observations to constrain attitude errors and must limit the observation noise value.
 		if (pos_noise > _params.pos_noaid_noise) {
 			pos_noise = _params.pos_noaid_noise;
@@ -366,17 +391,15 @@ bool Ekf::shouldResetGpsFusion() const
 	return (is_reset_required || is_inflight_nav_failure);
 }
 
-void Ekf::stopGpsFusion()
+void Ekf::stopGnssFusion()
 {
-	if (_control_status.flags.gps) {
+	if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) {
-		ECL_INFO("stopping GPS position and velocity fusion");
-
 		_last_gps_fail_us = 0;
 		_last_gps_pass_us = 0;
-
-		_control_status.flags.gps = false;
 	}
 
+	stopGnssVelFusion();
+	stopGnssPosFusion();
 	stopGpsHgtFusion();
 #if defined(CONFIG_EKF2_GNSS_YAW)
 	stopGnssYawFusion();
@@ -385,6 +408,34 @@ void Ekf::stopGpsFusion()
 	_yawEstimator.reset();
 }
 
+void Ekf::stopGnssVelFusion()
+{
+	if (_control_status.flags.gnss_vel) {
+		ECL_INFO("stopping GNSS velocity fusion");
+		_control_status.flags.gnss_vel = false;
+
+		//TODO: what if gnss yaw or height is used?
+		if (!_control_status.flags.gnss_pos) {
+			_last_gps_fail_us = 0;
+			_last_gps_pass_us = 0;
+		}
+	}
+}
+
+void Ekf::stopGnssPosFusion()
+{
+	if (_control_status.flags.gnss_pos) {
+		ECL_INFO("stopping GNSS position fusion");
+		_control_status.flags.gnss_pos = false;
+
+		//TODO: what if gnss yaw or height is used?
+		if (!_control_status.flags.gnss_vel) {
+			_last_gps_fail_us = 0;
+			_last_gps_pass_us = 0;
+		}
+	}
+}
+
 bool Ekf::isYawEmergencyEstimateAvailable() const
 {
 	// don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp

@@ -167,11 +167,8 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 
 		checkMagHeadingConsistency(mag_sample);
 
-		const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos;
-
-
 		{
-			const bool mag_consistent_or_no_ne_aiding = _control_status.flags.mag_heading_consistent || !using_ne_aiding;
+			const bool mag_consistent_or_no_ne_aiding = _control_status.flags.mag_heading_consistent || !isNorthEastAidingActive();
 			const bool common_conditions_passing = _control_status.flags.mag
 							       && ((_control_status.flags.yaw_align && mag_consistent_or_no_ne_aiding)
 									       || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
@@ -200,7 +197,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 
 		// if we are using 3-axis magnetometer fusion, but without external NE aiding,
 		// then the declination must be fused as an observation to prevent long term heading drift
-		const bool no_ne_aiding_or_not_moving = !using_ne_aiding || _control_status.flags.vehicle_at_rest;
+		const bool no_ne_aiding_or_not_moving = !isNorthEastAidingActive() || _control_status.flags.vehicle_at_rest;
 		_control_status.flags.mag_dec = _control_status.flags.mag && no_ne_aiding_or_not_moving;
 
 		if (_control_status.flags.mag) {
@@ -321,10 +318,9 @@ void Ekf::stopMagFusion()
 		resetMagBiasCov();
 
 		if (_control_status.flags.yaw_align && (_control_status.flags.mag_3D || _control_status.flags.mag_hdg)) {
-			// reset yaw alignment from mag unless using GNSS aiding
+			// reset yaw alignment from mag unless yaw is observable through North-East aiding
-			const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos;
 
-			if (!using_ne_aiding) {
+			if (!isNorthEastAidingActive()) {
 				_control_status.flags.yaw_align = false;
 			}
 		}
@@ -480,9 +476,8 @@ void Ekf::checkMagHeadingConsistency(const magSample &mag_sample)
 
 	if (fabsf(_mag_heading_innov_lpf.getState()) < _params.mag_heading_noise) {
 		// Check if there has been enough change in horizontal velocity to make yaw observable
-		const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos;
 
-		if (using_ne_aiding && (_accel_horiz_lpf.getState().longerThan(_params.mag_acc_gate))) {
+		if (isNorthEastAidingActive() && (_accel_horiz_lpf.getState().longerThan(_params.mag_acc_gate))) {
 			// yaw angle must be observable to consider consistency
 			_control_status.flags.mag_heading_consistent = true;
 		}

src/modules/ekf2/EKF/common.h

@@ -566,7 +566,7 @@ union filter_control_status_u {
 	struct {
 		uint64_t tilt_align              : 1; ///< 0 - true if the filter tilt alignment is complete
 		uint64_t yaw_align               : 1; ///< 1 - true if the filter yaw alignment is complete
-		uint64_t gps                     : 1; ///< 2 - true if GPS measurement fusion is intended
+		uint64_t gnss_pos                : 1; ///< 2 - true if GNSS position measurement fusion is intended
 		uint64_t opt_flow                : 1; ///< 3 - true if optical flow measurements fusion is intended
 		uint64_t mag_hdg                 : 1; ///< 4 - true if a simple magnetic yaw heading fusion is intended
 		uint64_t mag_3D                  : 1; ///< 5 - true if 3-axis magnetometer measurement fusion is intended
@@ -620,8 +620,8 @@ uint64_t mag_heading_consistent  :
 		uint64_t opt_flow_terrain        : 1; ///< 40 - true if we are fusing flow data for terrain
 		uint64_t valid_fake_pos          : 1; ///< 41 - true if a valid constant position is being fused
 		uint64_t constant_pos            : 1; ///< 42 - true if the vehicle is at a constant position
-		uint64_t baro_fault	      : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used
+		uint64_t baro_fault              : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used
-
+		uint64_t gnss_vel                : 1; ///< 44 - true if GNSS velocity measurement fusion is intended
 	} flags;
 	uint64_t value;
 };

src/modules/ekf2/EKF/ekf.h

@@ -862,7 +862,11 @@ private:
 #if defined(CONFIG_EKF2_GNSS)
 	// control fusion of GPS observations
 	void controlGpsFusion(const imuSample &imu_delayed);
-	void stopGpsFusion();
+	void controlGnssVelFusion(estimator_aid_source3d_s &aid_src, bool force_reset);
+	void controlGnssPosFusion(estimator_aid_source2d_s &aid_src, const bool force_reset);
+	void stopGnssFusion();
+	void stopGnssVelFusion();
+	void stopGnssPosFusion();
 	void updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src);
 	void updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s &aid_src);
 	void controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel);

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -262,7 +262,7 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
 	if (_horizontal_deadreckon_time_exceeded) {
 #if defined(CONFIG_EKF2_GNSS)
 
-		if (_control_status.flags.gps) {
+		if (_control_status.flags.gnss_pos) {
 			hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
 		}
 
@@ -302,10 +302,14 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 
 #if defined(CONFIG_EKF2_GNSS)
 
-		if (_control_status.flags.gps) {
+		if (_control_status.flags.gnss_pos) {
 			vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm());
 		}
 
+		if (_control_status.flags.gnss_vel) {
+			vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_vel.innovation).norm());
+		}
+
 #endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
@@ -446,7 +450,7 @@ float Ekf::getHorizontalVelocityInnovationTestRatio() const
 
 #if defined(CONFIG_EKF2_GNSS)
 
-	if (_control_status.flags.gps) {
+	if (_control_status.flags.gnss_vel) {
 		for (int i = 0; i < 2; i++) { // only xy
 			test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_vel.test_ratio_filtered[i]));
 		}
@@ -488,7 +492,7 @@ float Ekf::getVerticalVelocityInnovationTestRatio() const
 
 #if defined(CONFIG_EKF2_GNSS)
 
-	if (_control_status.flags.gps) {
+	if (_control_status.flags.gnss_vel) {
 		test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_vel.test_ratio_filtered[2]));
 	}
 
@@ -516,7 +520,7 @@ float Ekf::getHorizontalPositionInnovationTestRatio() const
 
 #if defined(CONFIG_EKF2_GNSS)
 
-	if (_control_status.flags.gps) {
+	if (_control_status.flags.gnss_pos) {
 		for (auto &test_ratio_filtered : _aid_src_gnss_pos.test_ratio_filtered) {
 			test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered));
 		}
@@ -706,7 +710,7 @@ uint16_t Ekf::get_ekf_soln_status() const
 	soln_status.flags.pos_vert_agl = isTerrainEstimateValid();
 #endif // CONFIG_EKF2_TERRAIN
 
-	// 128	ESTIMATOR_CONST_POS_MODE	True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow)
+	// 128	ESTIMATOR_CONST_POS_MODE	True if the EKF is in a constant position mode and is not using external measurements (eg GNSS or optical flow)
 	soln_status.flags.const_pos_mode = _control_status.flags.fake_pos || _control_status.flags.valid_fake_pos
 					   || _control_status.flags.vehicle_at_rest;
 
@@ -714,13 +718,13 @@ uint16_t Ekf::get_ekf_soln_status() const
 	soln_status.flags.pred_pos_horiz_rel = isHorizontalAidingActive();
 
 	// 512	ESTIMATOR_PRED_POS_HORIZ_ABS	True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate
-	soln_status.flags.pred_pos_horiz_abs = _control_status.flags.gps || _control_status.flags.aux_gpos;
+	soln_status.flags.pred_pos_horiz_abs = _control_status.flags.gnss_pos || _control_status.flags.aux_gpos;
 
-	// 1024	ESTIMATOR_GPS_GLITCH	True if the EKF has detected a GPS glitch
+	// 1024	ESTIMATOR_GPS_GLITCH	True if the EKF has detected a GNSS glitch
 #if defined(CONFIG_EKF2_GNSS)
-	const bool gps_vel_innov_bad = Vector3f(_aid_src_gnss_vel.test_ratio).max() > 1.f;
+	const bool gnss_vel_innov_bad = Vector3f(_aid_src_gnss_vel.test_ratio).max() > 1.f;
-	const bool gps_pos_innov_bad = Vector2f(_aid_src_gnss_pos.test_ratio).max() > 1.f;
+	const bool gnss_pos_innov_bad = Vector2f(_aid_src_gnss_pos.test_ratio).max() > 1.f;
-	soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad);
+	soln_status.flags.gps_glitch = (gnss_vel_innov_bad || gnss_pos_innov_bad);
 #endif // CONFIG_EKF2_GNSS
 
 	// 2048	ESTIMATOR_ACCEL_ERROR	True if the EKF has detected bad accelerometer data
@@ -799,14 +803,14 @@ void Ekf::updateHorizontalDeadReckoningstatus()
 	bool aiding_expected_in_air = false;
 
 	// velocity aiding active
-	if ((_control_status.flags.gps || _control_status.flags.ev_vel)
+	if ((_control_status.flags.gnss_vel || _control_status.flags.ev_vel)
 	    && isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max)
 	   ) {
 		inertial_dead_reckoning = false;
 	}
 
 	// position aiding active
-	if ((_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.aux_gpos)
+	if ((_control_status.flags.gnss_pos || _control_status.flags.ev_pos || _control_status.flags.aux_gpos)
 	    && isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
 	   ) {
 		inertial_dead_reckoning = false;

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -617,7 +617,7 @@ bool EstimatorInterface::isOtherSourceOfHorizontalPositionAidingThan(const bool
 
 int EstimatorInterface::getNumberOfActiveHorizontalPositionAidingSources() const
 {
-	return int(_control_status.flags.gps)
+	return int(_control_status.flags.gnss_pos)
 	       + int(_control_status.flags.ev_pos)
 	       + int(_control_status.flags.aux_gpos);
 }
@@ -672,10 +672,17 @@ bool EstimatorInterface::isVerticalVelocityAidingActive() const
 
 int EstimatorInterface::getNumberOfActiveVerticalVelocityAidingSources() const
 {
-	return int(_control_status.flags.gps)
+	return int(_control_status.flags.gnss_vel)
 	       + int(_control_status.flags.ev_vel);
 }
 
+bool EstimatorInterface::isNorthEastAidingActive() const
+{
+	return _control_status.flags.gnss_pos
+	       || _control_status.flags.gnss_vel
+	       || _control_status.flags.aux_gpos;
+}
+
 void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name)
 {
 	if (buffer_name) {

src/modules/ekf2/EKF/estimator_interface.h

@@ -227,6 +227,7 @@ public:
 	// the flags considered are opt_flow, gps, ev_vel and ev_pos
 	bool isHorizontalAidingActive() const;
 	bool isVerticalAidingActive() const;
+	bool isNorthEastAidingActive() const;
 
 	int getNumberOfActiveHorizontalAidingSources() const;
 	int getNumberOfActiveHorizontalPositionAidingSources() const;

src/modules/ekf2/EKF/height_control.cpp

@@ -205,7 +205,7 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
 		checks[1] = {ReferenceType::GNSS, _aid_src_gnss_hgt.innovation, _aid_src_gnss_hgt.innovation_variance};
 	}
 
-	if (_control_status.flags.gps) {
+	if (_control_status.flags.gnss_vel) {
 		checks[2] = {ReferenceType::GNSS, _aid_src_gnss_vel.innovation[2], _aid_src_gnss_vel.innovation_variance[2]};
 	}
 

src/modules/ekf2/EKF2.cpp

@@ -533,7 +533,8 @@ void EKF2::Run()
 			} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE) {
 
 				if ((_ekf.control_status_flags().wind_dead_reckoning || _ekf.control_status_flags().inertial_dead_reckoning
-				     || (!_ekf.control_status_flags().in_air && !_ekf.control_status_flags().gps)) && PX4_ISFINITE(vehicle_command.param2)
+				     || (!_ekf.control_status_flags().in_air && !_ekf.control_status_flags().gnss_pos))
+				    && PX4_ISFINITE(vehicle_command.param2)
 				    && PX4_ISFINITE(vehicle_command.param5) && PX4_ISFINITE(vehicle_command.param6)
 				   ) {
 
@@ -1892,7 +1893,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.control_status_changes   = _filter_control_status_changes;
 		status_flags.cs_tilt_align            = _ekf.control_status_flags().tilt_align;
 		status_flags.cs_yaw_align             = _ekf.control_status_flags().yaw_align;
-		status_flags.cs_gps                   = _ekf.control_status_flags().gps;
+		status_flags.cs_gnss_pos              = _ekf.control_status_flags().gnss_pos;
 		status_flags.cs_opt_flow              = _ekf.control_status_flags().opt_flow;
 		status_flags.cs_mag_hdg               = _ekf.control_status_flags().mag_hdg;
 		status_flags.cs_mag_3d                = _ekf.control_status_flags().mag_3D;
@@ -1934,6 +1935,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_valid_fake_pos      = _ekf.control_status_flags().valid_fake_pos;
 		status_flags.cs_constant_pos        = _ekf.control_status_flags().constant_pos;
 		status_flags.cs_baro_fault	    = _ekf.control_status_flags().baro_fault;
+		status_flags.cs_gnss_vel            = _ekf.control_status_flags().gnss_vel;
 
 		status_flags.fault_status_changes     = _filter_fault_status_changes;
 		status_flags.fs_bad_mag_x             = _ekf.fault_status_flags().bad_mag_x;

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp

@@ -117,7 +117,7 @@ void EkfWrapper::disableGpsFusion()
 
 bool EkfWrapper::isIntendingGpsFusion() const
 {
-	return _ekf->control_status_flags().gps;
+	return _ekf->control_status_flags().gnss_vel || _ekf->control_status_flags().gnss_pos;
 }
 
 void EkfWrapper::enableGpsHeadingFusion()

src/modules/ekf2/test/test_EKF_basics.cpp

@@ -103,7 +103,8 @@ TEST_F(EkfBasicsTest, initialControlMode)
 	// THEN: EKF control status should be reasonable
 	EXPECT_EQ(1, (int) _ekf->control_status_flags().tilt_align);
 	EXPECT_EQ(1, (int) _ekf->control_status_flags().yaw_align);
-	EXPECT_EQ(0, (int) _ekf->control_status_flags().gps);
+	EXPECT_EQ(0, (int) _ekf->control_status_flags().gnss_pos);
+	EXPECT_EQ(0, (int) _ekf->control_status_flags().gnss_vel);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().opt_flow);
 	EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_hdg);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_3D);
@@ -158,7 +159,8 @@ TEST_F(EkfBasicsTest, gpsFusion)
 	// THEN: EKF should fuse GPS, but no other position sensor
 	EXPECT_EQ(1, (int) _ekf->control_status_flags().tilt_align);
 	EXPECT_EQ(1, (int) _ekf->control_status_flags().yaw_align);
-	EXPECT_EQ(1, (int) _ekf->control_status_flags().gps);
+	EXPECT_EQ(1, (int) _ekf->control_status_flags().gnss_pos);
+	EXPECT_EQ(1, (int) _ekf->control_status_flags().gnss_vel);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().opt_flow);
 	EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_hdg);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_3D);