ekf2: support heading external update from MAV_CMD_EXTERNAL_ATTITUDE_ESTIMATE

docs/zh/msg_docs/EstimatorStatusFlags.md

@@ -54,6 +54,7 @@ bool cs_valid_fake_pos          # 41 - true if a valid constant position is bein
 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
+bool cs_yaw_manual              # 45 - true if yaw has been set manually
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes

msg/EstimatorStatus.msg

@@ -49,6 +49,7 @@ uint8 CS_GPS_YAW_FAULT = 27	# 27 - true when the GNSS heading has been declared
 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 measurement fusion is intended
 uint8 CS_GNSS_FAULT = 45        # 45 - true if GNSS measurements have been declared faulty and are no longer used
+uint8 CS_YAW_MANUAL = 46        # 46 - true if yaw has been set manually
 
 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

@@ -50,6 +50,7 @@ bool cs_constant_pos            # 42 - true if the vehicle is at a constant posi
 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
 bool cs_gnss_fault              # 45 - true if GNSS measurements have been declared faulty and are no longer used
+bool cs_yaw_manual              # 46 - true if yaw has been set manually
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes

msg/versioned/VehicleCommand.msg

@@ -88,6 +88,7 @@ uint16 VEHICLE_CMD_REQUEST_CAMERA_INFORMATION = 521 # Request camera information
 uint16 VEHICLE_CMD_SET_CAMERA_MODE = 530 # Set camera capture mode (photo, video, etc.).
 uint16 VEHICLE_CMD_SET_CAMERA_ZOOM = 531 # Set camera zoom.
 uint16 VEHICLE_CMD_SET_CAMERA_FOCUS = 532
+uint16 VEHICLE_CMD_EXTERNAL_ATTITUDE_ESTIMATE = 620 # Set an external estimate of vehicle attitude in degrees.
 uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000 # Setpoint to be sent to a gimbal manager to set a gimbal pitch and yaw.
 uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE = 1001 # Gimbal configuration to set which sysid/compid is in primary and secondary control.
 uint16 VEHICLE_CMD_IMAGE_START_CAPTURE = 2000 # Start image capture sequence.

src/modules/commander/Commander.cpp

@@ -462,6 +462,22 @@ int Commander::custom_command(int argc, char *argv[])
 		}
 	}
 
+	if (!strcmp(argv[0], "set_heading")) {
+		if (argc > 1) {
+			const float heading = atof(argv[1]);
+			const float heading_accuracy = NAN;
+
+			bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_EXTERNAL_ATTITUDE_ESTIMATE,
+							0.f, 0.f, heading, 0.f, 0.f, 0.f, heading_accuracy);
+			return (ret ? 0 : 1);
+
+		} else {
+			PX4_ERR("missing argument");
+			return 0;
+		}
+	}
+
+
 	if (!strcmp(argv[0], "poweroff")) {
 
 		bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN,
@@ -1521,6 +1537,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 	case vehicle_command_s::VEHICLE_CMD_DO_GRIPPER:
 	case vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE:
 	case vehicle_command_s::VEHICLE_CMD_REQUEST_CAMERA_INFORMATION:
+	case vehicle_command_s::VEHICLE_CMD_EXTERNAL_ATTITUDE_ESTIMATE:
 		/* ignore commands that are handled by other parts of the system */
 		break;
 
@@ -3040,6 +3057,8 @@ The commander module contains the state machine for mode switching and failsafe
 	PRINT_MODULE_USAGE_COMMAND("set_ekf_origin");
 	PRINT_MODULE_USAGE_ARG("lat, lon, alt", "Origin Latitude, Longitude, Altitude", false);
 	PRINT_MODULE_USAGE_COMMAND_DESCR("lat|lon|alt", "Origin latitude longitude altitude");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("set_heading", "Set current heading");
+	PRINT_MODULE_USAGE_ARG("heading", "degrees from True North [0 360]", false);
 	PRINT_MODULE_USAGE_COMMAND_DESCR("poweroff", "Power off board (if supported)");
 #endif
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

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

@@ -181,7 +181,8 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 							       && !_control_status.flags.mag_fault
 							       && !_control_status.flags.mag_field_disturbed
 							       && !_control_status.flags.ev_yaw
-							       && !_control_status.flags.gnss_yaw;
+							       && !_control_status.flags.gnss_yaw
+							       && (!_control_status.flags.yaw_manual || _control_status.flags.mag_aligned_in_flight);
 
 			_control_status.flags.mag_3D = common_conditions_passing
 						       && (_params.ekf2_mag_type == MagFuseType::AUTO)
@@ -210,7 +211,8 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 
 			if (continuing_conditions_passing && _control_status.flags.yaw_align) {
 
-				if ((checkHaglYawResetReq() && (_control_status.flags.mag_hdg || _control_status.flags.mag_3D))
+				if ((checkHaglYawResetReq() && (_control_status.flags.mag_hdg || _control_status.flags.mag_3D
+								|| _control_status.flags.yaw_manual))
 				    || (wmm_updated && no_ne_aiding_or_not_moving)) {
 					ECL_INFO("reset to %s", AID_SRC_NAME);
 					const bool reset_heading = _control_status.flags.mag_hdg || _control_status.flags.mag_3D;
@@ -448,7 +450,7 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
 	}
 
 	// record the start time for the magnetic field alignment
-	if (_control_status.flags.in_air && reset_heading) {
+	if (_control_status.flags.in_air && (reset_heading || _control_status.flags.yaw_manual)) {
 		_control_status.flags.mag_aligned_in_flight = true;
 		_flt_mag_align_start_time = _time_delayed_us;
 	}

src/modules/ekf2/EKF/common.h

@@ -608,6 +608,7 @@ uint64_t mag_heading_consistent  :
 		uint64_t gnss_vel                : 1; ///< 44 - true if GNSS velocity measurement fusion is intended
 uint64_t gnss_fault              :
 		1; ///< 45 - true if GNSS measurements have been declared faulty and are no longer used
+		uint64_t yaw_manual              : 1; ///< 46 - true if yaw has been reset manually
 	} flags;
 	uint64_t value;
 };

src/modules/ekf2/EKF/ekf.h

@@ -416,6 +416,22 @@ public:
 	bool resetGlobalPosToExternalObservation(double latitude, double longitude, float altitude, float eph, float epv,
 			uint64_t timestamp_observation);
 
+	void resetHeadingToExternalObservation(float heading, float heading_accuracy)
+	{
+		if (_control_status.flags.yaw_align) {
+			resetYawByFusion(heading, heading_accuracy);
+
+		} else {
+			resetQuatStateYaw(heading, heading_accuracy);
+			_control_status.flags.yaw_align = true;
+		}
+
+		// Force the mag consistency check to pass again since an external heading reset is often done to
+		// counter mag disturbances.
+		_control_status.flags.mag_heading_consistent = false;
+		_control_status.flags.yaw_manual = true;
+	}
+
 	void updateParameters();
 
 	friend class AuxGlobalPosition;
@@ -647,8 +663,8 @@ private:
 	bool initialiseAltitudeTo(float altitude, float vpos_var = NAN);
 
 	// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
-	bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW);
+	bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW, const bool reset = false);
-	void computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const;
+	void computeYawInnovVarAndH(float observation_variance, float &innovation_variance, VectorState &H_YAW) const;
 
 	void updateIMUBiasInhibit(const imuSample &imu_delayed);
 
@@ -999,6 +1015,8 @@ private:
 	// yaw : Euler yaw angle (rad)
 	// yaw_variance : yaw error variance (rad^2)
 	void resetQuatStateYaw(float yaw, float yaw_variance);
+	void propagateQuatReset(const Quatf &quat_before_reset);
+	void resetYawByFusion(float yaw, float yaw_variance);
 
 	HeightSensor _height_sensor_ref{HeightSensor::UNKNOWN};
 	PositionSensor _position_sensor_ref{PositionSensor::GNSS};

src/modules/ekf2/EKF/yaw_fusion.cpp

@@ -37,7 +37,7 @@
 
 #include <mathlib/mathlib.h>
 
-bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW)
+bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW, bool reset)
 {
 	// check if the innovation variance calculation is badly conditioned
 	if (aid_src_status.innovation_variance >= aid_src_status.observation_variance) {
@@ -68,6 +68,11 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H
 		Kfusion(row) *= heading_innov_var_inv;
 	}
 
+	if (reset && fabsf(H_YAW(State::quat_nominal.idx + 2)) > FLT_EPSILON) {
+		// Reset the yaw estimate by forcing the measurement into the state
+		Kfusion(State::quat_nominal.idx + 2) = 1.f / H_YAW(State::quat_nominal.idx + 2);
+	}
+
 	// set the heading unhealthy if the test fails
 	if (aid_src_status.innovation_rejected) {
 		_innov_check_fail_status.flags.reject_yaw = true;
@@ -110,12 +115,12 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H
 	return true;
 }
 
-void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const
+void Ekf::computeYawInnovVarAndH(float observation_variance, float &innovation_variance, VectorState &H_YAW) const
 {
-	sym::ComputeYawInnovVarAndH(_state.vector(), P, variance, &innovation_variance, &H_YAW);
+	sym::ComputeYawInnovVarAndH(_state.vector(), P, observation_variance, &innovation_variance, &H_YAW);
 }
 
-void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
+void Ekf::resetQuatStateYaw(const float yaw, const float yaw_variance)
 {
 	// save a copy of the quaternion state for later use in calculating the amount of reset change
 	const Quatf quat_before_reset = _state.quat_nominal;
@@ -129,12 +134,17 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
 	// update the rotation matrix using the new yaw value
 	_R_to_earth = updateYawInRotMat(yaw, Dcmf(_state.quat_nominal));
 
-	// calculate the amount that the quaternion has changed by
-	const Quatf quat_after_reset(_R_to_earth);
-	const Quatf q_error((quat_after_reset * quat_before_reset.inversed()).normalized());
-
 	// update quaternion states
-	_state.quat_nominal = quat_after_reset;
+	_state.quat_nominal = Quatf(_R_to_earth);
+
+	_time_last_heading_fuse = _time_delayed_us;
+
+	propagateQuatReset(quat_before_reset);
+}
+
+void Ekf::propagateQuatReset(const Quatf &quat_before_reset)
+{
+	const Quatf q_error((_state.quat_nominal * quat_before_reset.inversed()).normalized());
 
 	// add the reset amount to the output observer buffered data
 	_output_predictor.resetQuaternion(q_error);
@@ -160,6 +170,23 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
 	}
 
 	_state_reset_status.reset_count.quat++;
-
+}
-	_time_last_heading_fuse = _time_delayed_us;
+
+void Ekf::resetYawByFusion(const float yaw, const float yaw_variance)
+{
+	const Quatf quat_before_reset = _state.quat_nominal;
+
+	estimator_aid_source1d_s aid_src_status{};
+	aid_src_status.observation = yaw;
+	aid_src_status.observation_variance = yaw_variance;
+	aid_src_status.innovation = wrap_pi(getEulerYaw(_state.quat_nominal) - yaw);
+
+	VectorState H_YAW;
+
+	computeYawInnovVarAndH(aid_src_status.observation_variance, aid_src_status.innovation_variance, H_YAW);
+
+	const bool reset_yaw = true;
+	fuseYaw(aid_src_status, H_YAW, reset_yaw);
+
+	propagateQuatReset(quat_before_reset);
 }

src/modules/ekf2/EKF2.cpp

@@ -580,6 +580,24 @@ void EKF2::Run()
 				command_ack.timestamp = hrt_absolute_time();
 				_vehicle_command_ack_pub.publish(command_ack);
 			}
+
+			if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_ATTITUDE_ESTIMATE) {
+				if (PX4_ISFINITE(vehicle_command.param3)) {
+					const float heading = wrap_pi(math::radians(vehicle_command.param3));
+					static constexpr float kDefaultHeadingAccuracyDeg = 20.f;
+					const float heading_accuracy = math::radians(PX4_ISFINITE(vehicle_command.param7)
+								       ? vehicle_command.param7
+								       : kDefaultHeadingAccuracyDeg);
+					_ekf.resetHeadingToExternalObservation(heading, heading_accuracy);
+					command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
+				} else {
+					command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+				}
+
+				command_ack.timestamp = hrt_absolute_time();
+				_vehicle_command_ack_pub.publish(command_ack);
+			}
 		}
 	}
 
@@ -1936,6 +1954,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_baro_fault	    = _ekf.control_status_flags().baro_fault;
 		status_flags.cs_gnss_vel            = _ekf.control_status_flags().gnss_vel;
 		status_flags.cs_gnss_fault          = _ekf.control_status_flags().gnss_fault;
+		status_flags.cs_yaw_manual          = _ekf.control_status_flags().yaw_manual;
 
 		status_flags.fault_status_changes     = _filter_fault_status_changes;
 		status_flags.fs_bad_mag_x             = _ekf.fault_status_flags().bad_mag_x;