From b12b4dad0d442ec98e3cb9ad2aefaee914760796 Mon Sep 17 00:00:00 2001
From: Kevin Plaizier <plaizier1@gmail.com>
Date: Mon, 18 Dec 2023 15:54:22 -0700
Subject: [PATCH] Use yaw in angle PID to better follow a yaw around gravity
 convention. Also fixes issue where when at 90 deg pitch and setpoint is at 90
 deg roll angle PID will ask for no corrections.

---
 src/flight/control.c | 7 ++-----
 src/flight/control.h | 2 +-
 src/flight/input.c   | 2 +-
 3 files changed, 4 insertions(+), 7 deletions(-)

diff --git a/src/flight/control.c b/src/flight/control.c
index c57e6f353..0fd4cbde7 100644
--- a/src/flight/control.c
+++ b/src/flight/control.c
@@ -224,15 +224,12 @@ static void control_flight_mode() {
       state.error.axis[2] = yawerror[2] - state.gyro.axis[2];
 
     } else { // standard level mode
-      // pitch and roll
-      for (int i = 0; i <= 1; i++) {
+      // roll, pitch and yaw
+      for (int i = 0; i <= 2; i++) {
         state.angleerror[i] = errorvect.axis[i];
         state.setpoint.axis[i] = angle_pid(i) + yawerror[i];
         state.error.axis[i] = state.setpoint.axis[i] - state.gyro.axis[i];
       }
-      // yaw
-      state.setpoint.axis[2] = rates.axis[2];
-      state.error.axis[2] = yawerror[2] - state.gyro.axis[2];
     }
   } else { // rate mode
     state.setpoint.axis[0] = rates.axis[0];
diff --git a/src/flight/control.h b/src/flight/control.h
index 0f7c8b12e..79700be7c 100644
--- a/src/flight/control.h
+++ b/src/flight/control.h
@@ -8,7 +8,7 @@
 #include "rx/rx.h"
 #include "util/vector.h"
 
-#define ANGLE_PID_SIZE 2
+#define ANGLE_PID_SIZE 3
 
 #define RXMODE_BIND 0
 #define RXMODE_NORMAL 1
diff --git a/src/flight/input.c b/src/flight/input.c
index b0668c5bd..42eb75bca 100644
--- a/src/flight/input.c
+++ b/src/flight/input.c
@@ -38,7 +38,7 @@ vec3_t input_stick_vector(float rx_input[]) {
   return (vec3_t){
       .roll = constrain((state.GEstG.yaw * stickvector.roll) - (state.GEstG.roll * stickvector.yaw), -1.0f, 1.0f),
       .pitch = constrain(-((state.GEstG.pitch * stickvector.yaw) - (state.GEstG.yaw * stickvector.pitch)), -1.0f, 1.0f),
-      .yaw = 0,
+      .yaw = constrain(((state.GEstG.roll * stickvector.pitch) - (state.GEstG.roll * stickvector.pitch)), -1.0f, 1.0f),
   };
 }