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

@@ -116,7 +116,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;
 
@@ -222,15 +222,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;

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

src/flight/input.c

@@ -23,22 +23,22 @@ vec3_t input_stick_vector(float rx_input[]) {
       .yaw = fastcos(roll) * fastcos(pitch),
   };
 
-  float mag2 = (stickvector.roll * stickvector.roll + stickvector.pitch * stickvector.pitch);
-  if (mag2 > 0.001f) {
-    mag2 = 1.0f / sqrtf(mag2 / (1 - stickvector.yaw * stickvector.yaw));
+  const float length = (stickvector.roll * stickvector.roll + stickvector.pitch * stickvector.pitch);
+  if (length > 0) {
+    const float mag = 1.0f / sqrtf(length / (1 - stickvector.yaw * stickvector.yaw));
+    stickvector.roll *= mag;
+    stickvector.pitch *= mag;
   } else {
-    mag2 = 0.707f;
+    stickvector.roll = 0.0f;
+    stickvector.pitch = 0.0f;
   }
 
-  stickvector.roll *= mag2;
-  stickvector.pitch *= mag2;
-
   // find error between stick vector and quad orientation
   // vector cross product
   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),
   };
 }