mc-auto: handle EKF heading reset

src/lib/motion_planning/HeadingSmoothing.cpp

@@ -42,7 +42,10 @@ void HeadingSmoothing::reset(const float heading, const float heading_rate)
 {
 	const float wrapped_heading = matrix::wrap_pi(heading);
 	_velocity_smoothing.setCurrentVelocity(wrapped_heading);
-	_velocity_smoothing.setCurrentAcceleration(heading_rate);
+
+	if (PX4_ISFINITE(heading_rate)) {
+		_velocity_smoothing.setCurrentAcceleration(heading_rate);
+	}
 }
 
 void HeadingSmoothing::update(const float heading_setpoint, const float time_elapsed)

src/lib/motion_planning/HeadingSmoothing.hpp

@@ -69,7 +69,7 @@ public:
 	 * @param heading [rad] [-pi,pi]
 	 * @param heading_rate [rad/s]
 	 */
-	void reset(const float heading, const float heading_rate);
+	void reset(const float heading, const float heading_rate = NAN);
 
 	/**
 	 * @brief updates the heading setpoint, re-calculates trajectory, and takes an integration step

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

@@ -707,19 +707,20 @@ void FlightTaskAuto::_ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vx
 	_position_smoothing.forceSetVelocity({_velocity(0), _velocity(1), NAN});
 }
 
-void FlightTaskAuto::_ekfResetHandlerPositionZ(float delta_z)
+void FlightTaskAuto::_ekfResetHandlerPositionZ(const float delta_z)
 {
 	_position_smoothing.forceSetPosition({NAN, NAN, _position(2)});
 }
 
-void FlightTaskAuto::_ekfResetHandlerVelocityZ(float delta_vz)
+void FlightTaskAuto::_ekfResetHandlerVelocityZ(const float delta_vz)
 {
 	_position_smoothing.forceSetVelocity({NAN, NAN, _velocity(2)});
 }
 
-void FlightTaskAuto::_ekfResetHandlerHeading(float delta_psi)
+void FlightTaskAuto::_ekfResetHandlerHeading(const float delta_psi)
 {
-	_yaw_setpoint_previous += delta_psi;
+	_yaw_setpoint_previous = wrap_pi(_yaw_setpoint_previous + delta_psi);
+	_heading_smoothing.reset(wrap_pi(_heading_smoothing.getSmoothedHeading() + delta_psi));
 }
 
 void FlightTaskAuto::_checkEmergencyBraking()