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

@@ -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;

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

@@ -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),
   };
 }