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