From d014d76ca7594e8f6b5df75c6c3210f326cea6cd Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Mon, 27 Nov 2023 16:25:05 +0100
Subject: [PATCH] HeadingSmoothing: set correct maximum heading

The velocity smoothing library constrains the maximum vellocity.
I set the default constraint to 0 to find these exact issues.
The heading smoothing did not initialize the maximum velocity
which in this use case is the maximum heading.
So heading was always constrained to 0 -> north until this change.
---
 src/lib/motion_planning/HeadingSmoothing.cpp | 5 +++++
 src/lib/motion_planning/HeadingSmoothing.hpp | 2 +-
 2 files changed, 6 insertions(+), 1 deletion(-)

diff --git a/src/lib/motion_planning/HeadingSmoothing.cpp b/src/lib/motion_planning/HeadingSmoothing.cpp
index 50671f6f0f..037eeb701b 100644
--- a/src/lib/motion_planning/HeadingSmoothing.cpp
+++ b/src/lib/motion_planning/HeadingSmoothing.cpp
@@ -33,6 +33,11 @@
 
 #include "HeadingSmoothing.hpp"
 
+HeadingSmoothing::HeadingSmoothing()
+{
+	_velocity_smoothing.setMaxVel(M_PI_F); // smoothed "velocity" is heading [-pi, pi]
+}
+
 void HeadingSmoothing::reset(const float heading, const float heading_rate)
 {
 	const float wrapped_heading = matrix::wrap_pi(heading);
diff --git a/src/lib/motion_planning/HeadingSmoothing.hpp b/src/lib/motion_planning/HeadingSmoothing.hpp
index 30ef7c6805..5fdb4dde36 100644
--- a/src/lib/motion_planning/HeadingSmoothing.hpp
+++ b/src/lib/motion_planning/HeadingSmoothing.hpp
@@ -54,7 +54,7 @@
 class HeadingSmoothing
 {
 public:
-	HeadingSmoothing() = default;
+	HeadingSmoothing();
 	~HeadingSmoothing() = default;
 
 	/** @param max_heading_rate [rad/s] */