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.
BossHobby · Dec 20, 2023 · b12b4da · b12b4da
1 parent 381a554
commit b12b4da
Show file tree

Hide file tree

Showing 3 changed files with 4 additions and 7 deletions.
diff --git 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
@@ -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
@@ -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),
   };
 }