FlightTask: set yaw_setpoint to NAN when yaw should not be controlled

msg/vehicle_local_position.msg

@@ -41,6 +41,7 @@ float32 az        # Down velocity derivative in NED earth-fixed frame, (metres/s
 float32 heading				# Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI,  (radians)
 float32 delta_heading
 uint8 heading_reset_counter
+bool heading_good_for_control
 
 # Position of reference point (local NED frame origin) in global (GPS / WGS84) frame
 bool xy_global				# true if position (x, y) has a valid global reference (ref_lat, ref_lon)

src/modules/ekf2/EKF2.cpp

@@ -888,6 +888,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 
 	lpos.heading = Eulerf(_ekf.getQuaternion()).psi();
 	lpos.delta_heading = Eulerf(delta_q_reset).psi();
+	lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete();
 
 	// Distance to bottom surface (ground) in meters
 	// constrain the distance to ground to _rng_gnd_clearance

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -401,20 +401,24 @@ bool FlightTaskAuto::_evaluateTriplets()
 		_yaw_setpoint = NAN;
 
 	} else {
-		if ((_type != WaypointType::takeoff || _sub_triplet_setpoint.get().current.disable_weather_vane)
+		if (!_is_yaw_good_for_control) {
-		    && _sub_triplet_setpoint.get().current.yaw_valid) {
+			_yaw_lock = false;
+			_yaw_setpoint = NAN;
+			_yawspeed_setpoint = 0.f;
+
+		} else if ((_type != WaypointType::takeoff || _sub_triplet_setpoint.get().current.disable_weather_vane)
+			   && _sub_triplet_setpoint.get().current.yaw_valid) {
 			// Use the yaw computed in Navigator except during takeoff because
 			// Navigator is not handling the yaw reset properly.
 			// But: use if from Navigator during takeoff if disable_weather_vane is true,
 			// because we're then aligning to the transition waypoint.
 			// TODO: fix in navigator
 			_yaw_setpoint = _sub_triplet_setpoint.get().current.yaw;
+			_yawspeed_setpoint = NAN;
 
 		} else {
 			_set_heading_from_mode();
 		}
-
-		_yawspeed_setpoint = NAN;
 	}
 
 	return true;
@@ -471,6 +475,8 @@ void FlightTaskAuto::_set_heading_from_mode()
 		_yaw_lock = false;
 		_yaw_setpoint = NAN;
 	}
+
+	_yawspeed_setpoint = NAN;
 }
 
 bool FlightTaskAuto::_isFinite(const position_setpoint_s &sp)
@@ -809,7 +815,7 @@ void FlightTaskAuto::_prepareLandSetpoints()
 		land_speed *= (1 + _sticks.getPositionExpo()(2));
 
 		_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading,
-					       _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _deltatime);
+					       _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _is_yaw_good_for_control, _deltatime);
 		_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _land_heading, _position,
 				_velocity_setpoint_feedback.xy(), _deltatime);
 		_stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint);

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp

@@ -124,6 +124,7 @@ void FlightTask::_evaluateVehicleLocalPosition()
 
 		// yaw
 		_yaw = _sub_vehicle_local_position.get().heading;
+		_is_yaw_good_for_control = _sub_vehicle_local_position.get().heading_good_for_control;
 
 		// position
 		if (_sub_vehicle_local_position.get().xy_valid) {

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp

@@ -231,6 +231,7 @@ protected:
 	matrix::Vector3f _velocity; /**< current vehicle velocity */
 
 	float _yaw{}; /**< current vehicle yaw heading */
+	bool _is_yaw_good_for_control{}; /**< true if the yaw estimate can be used for yaw control */
 	float _dist_to_bottom{}; /**< current height above ground level */
 	float _dist_to_ground{}; /**< equals _dist_to_bottom if valid, height above home otherwise */
 

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp

@@ -68,8 +68,10 @@ bool FlightTaskManualAcceleration::update()
 	bool ret = FlightTaskManualAltitudeSmoothVel::update();
 
 	_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint,
-				       _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _deltatime);
+				       _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _is_yaw_good_for_control, _deltatime);
-	_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _yaw_setpoint, _position,
+
+	_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _yaw_setpoint,
+			_position,
 			_velocity_setpoint_feedback.xy(), _deltatime);
 	_stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint);
 

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -288,7 +288,7 @@ void FlightTaskManualAltitude::_respectGroundSlowdown()
 
 void FlightTaskManualAltitude::_rotateIntoHeadingFrame(Vector2f &v)
 {
-	float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw;
+	const float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw;
 	Vector3f v_r = Vector3f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * Vector3f(v(0), v(1), 0.0f));
 	v(0) = v_r(0);
 	v(1) = v_r(1);
@@ -296,7 +296,7 @@ void FlightTaskManualAltitude::_rotateIntoHeadingFrame(Vector2f &v)
 
 void FlightTaskManualAltitude::_updateHeadingSetpoints()
 {
-	if (_isYawInput()) {
+	if (_isYawInput() || !_is_yaw_good_for_control) {
 		_unlockYaw();
 
 	} else {

src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp

@@ -45,16 +45,17 @@ StickYaw::StickYaw()
 }
 
 void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float desired_yawspeed,
-				   const float yaw, const float deltatime)
+				   const float yaw, const bool is_yaw_good_for_control, const float deltatime)
 {
 	yawspeed_setpoint = _yawspeed_slew_rate.update(desired_yawspeed, deltatime);
-	yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint);
+	yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint, is_yaw_good_for_control);
 }
 
-float StickYaw::updateYawLock(const float yaw, const float yawspeed_setpoint, const float yaw_setpoint)
+float StickYaw::updateYawLock(const float yaw, const float yawspeed_setpoint, const float yaw_setpoint,
+			      const bool is_yaw_good_for_control)
 {
 	// Yaw-lock depends on desired yawspeed input. If not locked, yaw_sp is set to NAN.
-	if (fabsf(yawspeed_setpoint) > FLT_EPSILON) {
+	if ((fabsf(yawspeed_setpoint) > FLT_EPSILON) || !is_yaw_good_for_control) {
 		// no fixed heading when rotating around yaw by stick
 		return NAN;
 

src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp

@@ -47,8 +47,8 @@ public:
 	StickYaw();
 	~StickYaw() = default;
 
-	void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float desired_yawspeed, const float yaw,
+	void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, float desired_yawspeed, float yaw,
-				 const float deltatime);
+				 bool is_yaw_good_for_control, float deltatime);
 
 private:
 	SlewRate<float> _yawspeed_slew_rate;
@@ -64,5 +64,5 @@ private:
 	 * @param yaw current yaw setpoint which then will be overwritten by the return value
 	 * @return yaw setpoint to execute to have a yaw lock at the correct moment in time
 	 */
-	static float updateYawLock(const float yaw, const float yawspeed_setpoint, const float yaw_setpoint);
+	static float updateYawLock(float yaw, float yawspeed_setpoint, float yaw_setpoint, bool is_yaw_good_for_control);
 };