manual_yaw: compensate for yaw estimate convergence

When the yaw estimate is converging, the controller makes the drone yaw
in order to follow the current setpoint. This is unintuitive for the
pilot and it is preferable if the drone continues to fly towards the
same physical direction.

src/drivers/ins/vectornav/VectorNav.cpp

@@ -250,6 +250,7 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
 			local_position.evv = velocityUncertaintyEstimated;
 			local_position.xy_valid = true;
 			local_position.heading_good_for_control = mode_tracking;
+			local_position.unaided_heading = NAN;
 			local_position.timestamp = hrt_absolute_time();
 			_local_position_pub.publish(local_position);
 

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

@@ -221,8 +221,7 @@ void FlightTaskAuto::rcHelpModifyYaw(float &yaw_sp)
 {
 	// Only set a yawrate setpoint if weather vane is not active or the yaw stick is out of its dead-zone
 	if (!_weathervane.isActive() || fabsf(_sticks.getYawExpo()) > FLT_EPSILON) {
-		_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, yaw_sp, _sticks.getYawExpo(), _yaw, _is_yaw_good_for_control,
-					       _deltatime);
+		_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, yaw_sp, _sticks.getYawExpo(), _yaw, _deltatime);
 
 		// Hack to make sure the MPC_YAW_MODE 4 alignment doesn't stop the vehicle from descending when there's yaw input
 		_yaw_sp_aligned = true;

src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp

@@ -60,8 +60,7 @@ bool FlightTaskDescend::update()
 
 	// Nudging
 	if (_param_mpc_land_rc_help.get() && _sticks.checkAndUpdateStickInputs()) {
-		_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw,
-					       _is_yaw_good_for_control, _deltatime);
+		_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, _deltatime);
 		_acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(_sticks.getPitchRoll(), _deltatime, _yaw,
 					      _yaw_setpoint);
 

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

@@ -112,6 +112,7 @@ void FlightTask::_evaluateVehicleLocalPosition()
 
 		// yaw
 		_yaw = _sub_vehicle_local_position.get().heading;
+		_unaided_yaw = _sub_vehicle_local_position.get().unaided_heading;
 		_is_yaw_good_for_control = _sub_vehicle_local_position.get().heading_good_for_control;
 
 		// position

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

@@ -218,6 +218,7 @@ protected:
 	matrix::Vector3f _velocity; /**< current vehicle velocity */
 
 	float _yaw{}; /**< current vehicle yaw heading */
+	float _unaided_yaw{};
 	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 if dist_bottom is valid */
 	float _dist_to_ground{}; /**< equals _dist_to_bottom if available, height above home otherwise */

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

@@ -64,6 +64,7 @@ bool FlightTaskManualAltitude::activate(const trajectory_setpoint_s &last_setpoi
 	_acceleration_setpoint = Vector3f(0.f, 0.f, NAN); // altitude is controlled from position/velocity
 	_position_setpoint(2) = _position(2);
 	_velocity_setpoint(2) = 0.f;
+	_stick_yaw.reset(_yaw, _unaided_yaw);
 	_setDefaultConstraints();
 
 	_updateConstraintsFromEstimator();
@@ -273,14 +274,15 @@ void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi)
 {
 	// Only reset the yaw setpoint when the heading is locked
 	if (PX4_ISFINITE(_yaw_setpoint)) {
-		_yaw_setpoint += delta_psi;
+		_yaw_setpoint = wrap_pi(_yaw_setpoint + delta_psi);
 	}
+
+	_stick_yaw.ekfResetHandler(delta_psi);
 }
 
 void FlightTaskManualAltitude::_updateSetpoints()
 {
-	_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw,
-				       _is_yaw_good_for_control, _deltatime);
+	_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, _deltatime, _unaided_yaw);
 	_acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(_sticks.getPitchRoll(), _deltatime, _yaw,
 				      _yaw_setpoint);
 	_updateAltitudeLock();

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

@@ -40,7 +40,6 @@
 #pragma once
 
 #include "FlightTask.hpp"
-#include <lib/mathlib/math/filter/AlphaFilter.hpp>
 #include "Sticks.hpp"
 #include "StickTiltXY.hpp"
 #include "StickYaw.hpp"
@@ -115,6 +114,8 @@ private:
 
 	void setGearAccordingToSwitch();
 
+	bool _updateYawCorrection();
+
 	uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
 	bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */
 	bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */

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

@@ -35,23 +35,80 @@
 
 #include <px4_platform_common/defines.h>
 
+using matrix::wrap_pi;
+
 StickYaw::StickYaw(ModuleParams *parent) :
 	ModuleParams(parent)
 {}
 
-void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float stick_yaw,
-				   const float yaw, const bool is_yaw_good_for_control, const float deltatime)
+void StickYaw::reset(const float yaw, const float unaided_yaw)
 {
+	if (PX4_ISFINITE(unaided_yaw)) {
+		_yaw_error_lpf.reset(wrap_pi(yaw - unaided_yaw));
+	}
+}
+
+void StickYaw::ekfResetHandler(const float delta_yaw)
+{
+	_yaw_error_lpf.reset(wrap_pi(_yaw_error_lpf.getState() + delta_yaw));
+	_yaw_error_ref = wrap_pi(_yaw_error_ref + delta_yaw);
+}
+
+void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float stick_yaw,
+				   const float yaw, const float deltatime, const float unaided_yaw)
+{
+	_yaw_error_lpf.setParameters(deltatime, _kYawErrorTimeConstant);
+	const float yaw_correction_prev = _yaw_correction;
+	const bool reset_setpoint = updateYawCorrection(yaw, unaided_yaw);
+
+	if (reset_setpoint) {
+		yaw_setpoint = NAN;
+	}
+
 	_yawspeed_filter.setParameters(deltatime, _param_mpc_man_y_tau.get());
 	yawspeed_setpoint = _yawspeed_filter.update(stick_yaw * math::radians(_param_mpc_man_y_max.get()));
-	yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint, is_yaw_good_for_control);
+	yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint, yaw_correction_prev);
+}
+
+bool StickYaw::updateYawCorrection(const float yaw, const float unaided_yaw)
+{
+	if (!PX4_ISFINITE(unaided_yaw)) {
+		_yaw_correction = 0.f;
+		return false;
+	}
+
+	// Detect the convergence phase of the yaw estimate by monitoring its relative
+	// distance from an unaided yaw source.
+	const float yaw_error = wrap_pi(yaw - unaided_yaw);
+
+	// Run it through a high-pass filter to detect transients
+	const float yaw_error_hpf = wrap_pi(yaw_error - _yaw_error_lpf.getState());
+	_yaw_error_lpf.update(yaw_error);
+
+	const bool was_converging = _yaw_estimate_converging;
+	_yaw_estimate_converging = fabsf(yaw_error_hpf) > _kYawErrorChangeThreshold;
+
+	bool reset_setpoint = false;
+
+	if (!_yaw_estimate_converging) {
+		_yaw_error_ref = yaw_error;
+
+		if (was_converging) {
+			// Force a reset of the locking mechanism
+			reset_setpoint = true;
+		}
+	}
+
+	_yaw_correction = wrap_pi(yaw_error - _yaw_error_ref);
+
+	return reset_setpoint;
 }
 
 float StickYaw::updateYawLock(const float yaw, const float yawspeed_setpoint, const float yaw_setpoint,
-			      const bool is_yaw_good_for_control)
+			      const float yaw_correction_prev) const
 {
 	// Yaw-lock depends on desired yawspeed input. If not locked, yaw_sp is set to NAN.
-	if ((fabsf(yawspeed_setpoint) > FLT_EPSILON) || !is_yaw_good_for_control) {
+	if (fabsf(yawspeed_setpoint) > FLT_EPSILON) {
 		// no fixed heading when rotating around yaw by stick
 		return NAN;
 
@@ -61,7 +118,7 @@ float StickYaw::updateYawLock(const float yaw, const float yawspeed_setpoint, co
 			return yaw;
 
 		} else {
-			return yaw_setpoint;
+			return wrap_pi(yaw_setpoint - yaw_correction_prev + _yaw_correction);
 		}
 	}
 }

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

@@ -48,12 +48,23 @@ public:
 	StickYaw(ModuleParams *parent);
 	~StickYaw() = default;
 
-	void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, float stick_yaw, float yaw,
-				 bool is_yaw_good_for_control, float deltatime);
+	void reset(float yaw, float unaided_yaw = NAN);
+	void ekfResetHandler(float delta_yaw);
+	void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, float stick_yaw, float yaw, float deltatime,
+				 float unaided_yaw = NAN);
 
 private:
 	AlphaFilter<float> _yawspeed_filter;
 
+	float _yaw_error_ref{0.f};
+	float _yaw_correction{0.f};
+	bool _yaw_estimate_converging{false};
+	AlphaFilter<float> _yaw_error_lpf{0.01f}; ///< used to create a high-pass filter
+	static constexpr float _kYawErrorTimeConstant{1.f}; ///< time constant of the high-pass filter used to detect yaw convergence
+	static constexpr float _kYawErrorChangeThreshold{radians(1.f)}; ///< we consider the yaw estimate as "converging" when above this threshold
+
+	bool updateYawCorrection(float yaw, float unaided_yaw);
+
 	/**
 	 * Lock yaw when not currently turning
 	 * When applying a yawspeed the vehicle is turning, when the speed is
@@ -65,7 +76,7 @@ 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(float yaw, float yawspeed_setpoint, float yaw_setpoint, bool is_yaw_good_for_control);
+	float updateYawLock(float yaw, float yawspeed_setpoint, float yaw_setpoint, float yaw_correction_prev) const;
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max, ///< Maximum yaw speed with full stick deflection

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -600,7 +600,10 @@ void BlockLocalPositionEstimator::publishLocalPos()
 			_pub_lpos.get().z = xLP(X_z);	// down
 		}
 
-		_pub_lpos.get().heading = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi();
+		const float heading = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi();
+		_pub_lpos.get().heading = heading;
+		_pub_lpos.get().heading_good_for_control = PX4_ISFINITE(heading);
+		_pub_lpos.get().unaided_heading = NAN;
 
 		_pub_lpos.get().vx = xLP(X_vx);		// north
 		_pub_lpos.get().vy = xLP(X_vy);		// east

src/modules/mavlink/mavlink_receiver.cpp

@@ -2713,6 +2713,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 
 		matrix::Eulerf euler{matrix::Quatf(hil_state.attitude_quaternion)};
 		hil_local_pos.heading = euler.psi();
+		hil_local_pos.heading_good_for_control = PX4_ISFINITE(euler.psi());
+		hil_local_pos.unaided_heading = NAN;
 		hil_local_pos.xy_global = true;
 		hil_local_pos.z_global = true;
 		hil_local_pos.vxy_max = INFINITY;

src/modules/simulation/simulator_sih/sih.cpp

@@ -546,6 +546,7 @@ void Sih::publish_ground_truth(const hrt_abstime &time_now_us)
 
 		local_position.heading = Eulerf(_q).psi();
 		local_position.heading_good_for_control = true;
+		local_position.unaided_heading = NAN;
 
 		local_position.timestamp = hrt_absolute_time();
 		_local_position_ground_truth_pub.publish(local_position);