From 131026a356a9653a3a66d534714ad2b8ad6e338c 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 | 9 +++------
 src/flight/control.h | 2 +-
 src/flight/input.c   | 2 +-
 3 files changed, 5 insertions(+), 8 deletions(-)

diff --git a/src/flight/control.c b/src/flight/control.c
index b8bd22c36..1d9841198 100644
--- a/src/flight/control.c
+++ b/src/flight/control.c
@@ -118,7 +118,7 @@ static void control_flight_mode() {
     };
 
     if (rx_aux_on(AUX_RACEMODE) && !rx_aux_on(AUX_HORIZON)) { // racemode with angle behavior on roll ais
-      if (state.GEstG.yaw < 0) {                          // acro on roll and pitch when inverted
+      if (state.GEstG.yaw < 0) {                              // acro on roll and pitch when inverted
         state.setpoint.roll = rates.roll;
         state.setpoint.pitch = rates.pitch;
 
@@ -224,15 +224,12 @@ static void control_flight_mode() {
       state.error.yaw = yawerror[2] - state.gyro.yaw;
 
     } else { // standard level mode
-      // pitch and roll
-      for (int i = 0; i <= 1; i++) {
+      // roll, pitch and yaw
+      for (uint32_t i = 0; i < 3; 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.yaw = rates.yaw;
-      state.error.yaw = yawerror[2] - state.gyro.yaw;
     }
   } else { // rate mode
     state.setpoint.roll = rates.roll;
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 fb2dbcbdd..1d2c6184b 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.pitch * stickvector.roll)), -1.0f, 1.0f),
   };
 }