From eaa2c1d4ad1ff8fb7efbfe72ad880d8ecb1b8fee Mon Sep 17 00:00:00 2001
From: franchioping <franchioping@galard.uk>
Date: Tue, 14 Apr 2026 16:14:04 +0100
Subject: [PATCH] only update yaw if velocity delta has similar norm.

---
 main/main.cpp   | 18 +++++++++---------
 main/sens_fus.h | 16 +++++++++-------
 2 files changed, 18 insertions(+), 16 deletions(-)

diff --git a/main/main.cpp b/main/main.cpp
index 7b3636a..b51290e 100644
--- a/main/main.cpp
+++ b/main/main.cpp
@@ -6,7 +6,7 @@
 #include "freertos/FreeRTOS.h"
 #include "freertos/idf_additions.h"
 #include "freertos/projdefs.h"
-#include "freertos/task.h" #include < cstddef>
+#include "freertos/task.h"
 #include <cstdint>
 #include <optional>
 
@@ -54,14 +54,14 @@ extern "C" void app_main(void) {
   //                         1     // Core ID
   // );
 
-  xTaskCreatePinnedToCore(motor_throttles_task,   // Function name
-                          "motor_throttles_task", // Name for debugging
-                          1024 * 4,               // Stack size in bytes
-                          NULL,                   // Parameters
-                          24,   // Priority (higher = more urgent)
-                          NULL, // Task handle
-                          1     // Core ID
-  );
+  // xTaskCreatePinnedToCore(motor_throttles_task,   // Function name
+  //                         "motor_throttles_task", // Name for debugging
+  //                         1024 * 4,               // Stack size in bytes
+  //                         NULL,                   // Parameters
+  //                         24,   // Priority (higher = more urgent)
+  //                         NULL, // Task handle
+  //                         1     // Core ID
+  // );
 
   // setup_imu();
   ESP_LOGI("MAIN", "All tasks spawned. Main loop free.");
diff --git a/main/sens_fus.h b/main/sens_fus.h
index 64ca11d..514030b 100644
--- a/main/sens_fus.h
+++ b/main/sens_fus.h
@@ -1,6 +1,7 @@
 #pragma once
 #include "Eigen/Core"
 #include <cmath>
+#include <cstdlib>
 
 #ifdef PS
 #undef PS
@@ -42,7 +43,7 @@ struct sens_fus_compl {
   Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 15.0f};
   Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 30.0f};
 
-  float tau_yaw = 20.0f; // Yaw remains a scalar
+  float tau_yaw = 20.0f;
 
   Eigen::Matrix3f yaw_rotation_matrix = Eigen::Matrix3f::Identity().eval();
 
@@ -81,14 +82,15 @@ struct sens_fus_compl {
           this->velocity - this->velocity_last_gps_measurment;
       Eigen::Vector3f delta_v_gps = this->velocity - gps_vel;
 
-      float yaw_delta = getYawDifference(delta_v_gps, delta_v_imu);
+      if (abs(delta_v_gps.norm() - delta_v_imu.norm()) < 1.0 * dt) {
+        float yaw_delta = getYawDifference(delta_v_gps, delta_v_imu);
 
-      this->yaw_offset += yaw_delta * alpha_yaw;
-
-      this->yaw_offset =
-          std::atan2(std::sin(this->yaw_offset), std::cos(this->yaw_offset));
-      this->update_yaw_matrix();
+        this->yaw_offset += yaw_delta * alpha_yaw;
 
+        this->yaw_offset =
+            std::atan2(std::sin(this->yaw_offset), std::cos(this->yaw_offset));
+        this->update_yaw_matrix();
+      }
       this->velocity = (Eigen::Vector3f::Ones() - alpha_vel).array() *
                            this->velocity.array() +
                        alpha_vel.array() * gps_vel.array();