make yaw_error a vec3

BossHobby · Sep 3, 2024 · 42cdc7e · 42cdc7e
1 parent a3af2c3
commit 42cdc7e
Showing 1 changed file with 13 additions and 13 deletions.
diff --git a/src/flight/control.c b/src/flight/control.c
@@ -110,10 +110,10 @@ static void control_flight_mode() {
 
     // apply yaw from the top of the quad
     // yaw rotation vector
-    const float yawerror[3] = {
-        state.GEstG.pitch * rates.yaw,
-        -state.GEstG.roll * rates.yaw,
-        state.GEstG.yaw * rates.yaw,
+    const vec3_t yaw_error = {
+        .roll = state.GEstG.pitch * rates.yaw,
+        .pitch = -state.GEstG.roll * rates.yaw,
+        .yaw = state.GEstG.yaw * rates.yaw,
     };
 
     if (rx_aux_on(AUX_RACEMODE) && !rx_aux_on(AUX_HORIZON)) { // racemode with angle behavior on roll ais
@@ -125,7 +125,7 @@ static void control_flight_mode() {
         state.error.pitch = rates.pitch - state.gyro.pitch;
       } else {
         // roll is leveled to max angle limit
-        state.setpoint.roll = angle_pid(0) + yawerror[0];
+        state.setpoint.roll = angle_pid(0) + yaw_error.axis[0];
         state.error.roll = state.setpoint.roll - state.gyro.roll;
 
         // pitch is acro
@@ -134,7 +134,7 @@ static void control_flight_mode() {
       }
       // yaw
       state.setpoint.yaw = rates.yaw;
-      state.error.yaw = yawerror[2] - state.gyro.yaw;
+      state.error.yaw = yaw_error.axis[2] - state.gyro.yaw;
 
     } else if (rx_aux_on(AUX_RACEMODE) && rx_aux_on(AUX_HORIZON)) { // racemode with horizon behavior on roll axis
       float inclinationRoll = state.attitude.roll;
@@ -169,16 +169,16 @@ static void control_flight_mode() {
       } else {
         // apply a transitioning mix of acro and level behavior inside of stick HORIZON_TRANSITION point and full acro beyond stick HORIZON_TRANSITION point
         // roll angle strength fades out as sticks approach HORIZON_TRANSITION while acro stength fades in according to value of acroFade factor
-        state.setpoint.roll = (angle_pid(0) + yawerror[0]) * (1.0f - fade) + fade * (rates.roll);
-        state.error.roll = ((angle_pid(0) + yawerror[0] - state.gyro.roll) * (1 - fade)) + (fade * (rates.roll - state.gyro.roll));
+        state.setpoint.roll = (angle_pid(0) + yaw_error.axis[0]) * (1.0f - fade) + fade * (rates.roll);
+        state.error.roll = ((angle_pid(0) + yaw_error.axis[0] - state.gyro.roll) * (1 - fade)) + (fade * (rates.roll - state.gyro.roll));
         // pitch is acro
         state.setpoint.pitch = rates.pitch;
         state.error.pitch = rates.pitch - state.gyro.pitch;
       }
 
       // yaw
       state.setpoint.yaw = rates.yaw;
-      state.error.yaw = yawerror[2] - state.gyro.yaw;
+      state.error.yaw = yaw_error.axis[2] - state.gyro.yaw;
 
     } else if (!rx_aux_on(AUX_RACEMODE) && rx_aux_on(AUX_HORIZON)) { // horizon overrites standard level behavior
       // pitch and roll
@@ -213,18 +213,18 @@ static void control_flight_mode() {
         } else {
           // apply a transitioning mix of acro and level behavior inside of stick HORIZON_TRANSITION point and full acro beyond stick HORIZON_TRANSITION point
           // angle strength fades out as sticks approach HORIZON_TRANSITION while acro stength fades in according to value of acroFade factor
-          state.setpoint.axis[i] = (angle_pid(i) + yawerror[i]) * (1.0f - fade) + fade * (rates.axis[i]);
-          state.error.axis[i] = ((angle_pid(i) + yawerror[i] - state.gyro.axis[i]) * (1 - fade)) + (fade * (rates.axis[i] - state.gyro.axis[i]));
+          state.setpoint.axis[i] = (angle_pid(i) + yaw_error.axis[i]) * (1.0f - fade) + fade * (rates.axis[i]);
+          state.error.axis[i] = ((angle_pid(i) + yaw_error.axis[i] - state.gyro.axis[i]) * (1 - fade)) + (fade * (rates.axis[i] - state.gyro.axis[i]));
         }
       }
       // yaw
       state.setpoint.yaw = rates.yaw;
-      state.error.yaw = yawerror[2] - state.gyro.yaw;
+      state.error.yaw = yaw_error.axis[2] - state.gyro.yaw;
 
     } else { // standard level mode
       // roll, pitch and yaw
       for (uint32_t i = 0; i < 3; i++) {
-        state.setpoint.axis[i] = angle_pid(i) + yawerror[i];
+        state.setpoint.axis[i] = angle_pid(i) + yaw_error.axis[i];
         state.error.axis[i] = state.setpoint.axis[i] - state.gyro.axis[i];
       }
     }