test+++++

frtos/config/magnetometer_config.h

@@ -75,7 +75,6 @@ typedef struct
     float               roll;
     float               pitch;
     float               yaw;
-    float               target_yaw;
     compass_direction_t orientation;
     angle_t             roll_angle;
     angle_t             pitch_angle;

frtos/magnetometer/magnetometer_init.h

@@ -282,7 +282,6 @@ magnetometer_init(car_struct_t *p_car_struct)
     p_car_struct->p_direction->roll        = 0;
     p_car_struct->p_direction->pitch       = 0;
     p_car_struct->p_direction->yaw         = 0;
-    p_car_struct->p_direction->target_yaw  = 0;
     p_car_struct->p_direction->orientation = NORTH;
     p_car_struct->p_direction->roll_angle  = LEFT;
     p_car_struct->p_direction->pitch_angle = UP;

frtos/motor/motor_direction.h

@@ -27,13 +27,6 @@ set_wheel_direction(uint32_t direction)
     gpio_set_mask(direction);
 }
 
-void
-update_target_yaw(volatile direction_t * g_direction)
-{
-//    updateDirection(g_direction);
-    g_direction->target_yaw = g_direction->yaw;
-}
-
 /*!
  * @brief Set the direction of the wheel to opposite direction using bit mask
  */

frtos/motor/motor_pid.h

@@ -11,25 +11,6 @@
 #include "magnetometer_init.h"
 #include "magnetometer_direction.h"
 
-float calculate_yaw_difference(float current_yaw, float set_yaw) {
-    // Normalize yaws to the range [0.0, 359.0]
-    current_yaw = fmod(current_yaw, 360.0);
-    set_yaw = fmod(set_yaw, 360.0);
-
-    // Calculate the direct difference
-    float diff = current_yaw - set_yaw;
-
-    // Adjust the difference to consider the circular nature of yaw values
-    if (diff > 180.0) {
-        diff -= 360.0;
-    } else if (diff < -180.0) {
-        diff += 360.0;
-    }
-
-    return diff;
-}
-
-
 /*!
  * @brief Compute the control signal using PID controller
  * @param integral The integral term of the PID controller
@@ -57,27 +38,6 @@ compute_pid(float *integral, float *prev_error, car_struct_t *car_struct)
     return control_signal;
 }
 
-float
-compute_pid_yaw(float *integral, float *prev_error, car_struct_t *car_struct)
-{
-    updateDirection(car_struct->p_direction);
-
-    float error = calculate_yaw_difference(car_struct->p_direction->yaw,
-                                           car_struct->p_direction->target_yaw);
-
-    *integral += error;
-
-    float derivative = error - *prev_error;
-
-    float control_signal
-        = car_struct->p_pid->kp_value * error
-          + car_struct->p_pid->ki_value * (*integral)
-          + car_struct->p_pid->kd_value * derivative;
-
-    *prev_error = error;
-
-    return control_signal;
-}
 
 /*!
  * @brief Repeating timer handler for the PID controller
@@ -97,7 +57,7 @@ repeating_pid_handler(struct repeating_timer *ppp_timer)
         return true;
     }
 
-    float control_signal = compute_pid_yaw(&integral, &prev_error, car_strut);
+    float control_signal = compute_pid(&integral, &prev_error, car_strut);
 
     float temp
         = (float)car_strut->p_right_motor->pwm.level + control_signal * 0.05f;