refactor input_stick_vector call

src/flight/control.c

@@ -126,7 +126,7 @@ static void control_flight_mode() {
   if (rx_aux_on(AUX_LEVELMODE)) {
 
     // calculate roll / pitch error
-    input_stick_vector(state.rx_filtered.axis, 0);
+    const vec3_t errorvect = input_stick_vector(state.rx_filtered.axis);
 
     // apply yaw from the top of the quad
     // yaw rotation vector
@@ -145,7 +145,7 @@ static void control_flight_mode() {
         state.error.axis[1] = rates.axis[1] - state.gyro.axis[1];
       } else {
         // roll is leveled to max angle limit
-        state.angleerror[0] = state.errorvect.axis[0];
+        state.angleerror[0] = errorvect.axis[0];
         state.setpoint.axis[0] = angle_pid(0) + yawerror[0];
         state.error.axis[0] = state.setpoint.axis[0] - state.gyro.axis[0];
 
@@ -188,7 +188,7 @@ static void control_flight_mode() {
         state.error.axis[0] = rates.axis[0] - state.gyro.axis[0];
         state.error.axis[1] = rates.axis[1] - state.gyro.axis[1];
       } else { // apply a transitioning mix of acro and level behavior inside of stick HORIZON_TRANSITION point and full acro beyond stick HORIZON_TRANSITION point
-        state.angleerror[0] = state.errorvect.axis[0];
+        state.angleerror[0] = errorvect.axis[0];
         // roll angle strength fades out as sticks approach HORIZON_TRANSITION while acro stength fades in according to value of acroFade factor
         state.setpoint.axis[0] = (angle_pid(0) + yawerror[0]) * (1.0f - fade) + fade * (rates.axis[0]);
         state.error.axis[0] = ((angle_pid(0) + yawerror[0] - state.gyro.axis[0]) * (1 - fade)) + (fade * (rates.axis[0] - state.gyro.axis[0]));
@@ -232,7 +232,7 @@ static void control_flight_mode() {
           state.setpoint.axis[i] = rates.axis[i];
           state.error.axis[i] = rates.axis[i] - state.gyro.axis[i];
         } else { // apply a transitioning mix of acro and level behavior inside of stick HORIZON_TRANSITION point and full acro beyond stick HORIZON_TRANSITION point
-          state.angleerror[i] = state.errorvect.axis[i];
+          state.angleerror[i] = errorvect.axis[i];
           //  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]));
@@ -245,7 +245,7 @@ static void control_flight_mode() {
     } else { // standard level mode
       // pitch and roll
       for (int i = 0; i <= 1; i++) {
-        state.angleerror[i] = state.errorvect.axis[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];
       }

src/flight/control.h

@@ -92,9 +92,8 @@ typedef struct {
   vec3_t GEstG; // gravity vector
   vec3_t attitude;
 
-  vec3_t setpoint;  // angular velocity setpoint from stick input
-  vec3_t error;     // setpoint - gyro = error in angular velocity
-  vec3_t errorvect; // error vector between stick vector and quad orientation
+  vec3_t setpoint; // angular velocity setpoint from stick input
+  vec3_t error;    // setpoint - gyro = error in angular velocity
 
   vec3_t pid_p_term;
   vec3_t pid_i_term;
@@ -144,7 +143,6 @@ typedef struct {
   MEMBER(attitude, vec3_t)                  \
   MEMBER(setpoint, vec3_t)                  \
   MEMBER(error, vec3_t)                     \
-  MEMBER(errorvect, vec3_t)                 \
   MEMBER(pid_p_term, vec3_t)                \
   MEMBER(pid_i_term, vec3_t)                \
   MEMBER(pid_d_term, vec3_t)                \

src/flight/input.c

@@ -12,7 +12,7 @@
 
 #define BF_RC_RATE_INCREMENTAL 14.54f
 
-void input_stick_vector(float rx_input[], float maxangle) {
+vec3_t input_stick_vector(float rx_input[]) {
   // rotate down vector to match stick position
   const float pitch = rx_input[1] * profile.rate.level_max_angle * DEGTORAD;
   const float roll = rx_input[0] * profile.rate.level_max_angle * DEGTORAD;
@@ -26,24 +26,19 @@ void input_stick_vector(float rx_input[], float maxangle) {
   float mag2 = (stickvector[0] * stickvector[0] + stickvector[1] * stickvector[1]);
   if (mag2 > 0.001f) {
     mag2 = Q_rsqrt(mag2 / (1 - stickvector[2] * stickvector[2]));
-  } else
+  } else {
     mag2 = 0.707f;
+  }
 
   stickvector[0] *= mag2;
   stickvector[1] *= mag2;
 
   // find error between stick vector and quad orientation
   // vector cross product
-  state.errorvect.axis[1] = -((state.GEstG.axis[1] * stickvector[2]) - (state.GEstG.axis[2] * stickvector[1]));
-  state.errorvect.axis[0] = (state.GEstG.axis[2] * stickvector[0]) - (state.GEstG.axis[0] * stickvector[2]);
-
-  // some limits just in case
-  state.errorvect.axis[0] = constrain(state.errorvect.axis[0], -1.0f, 1.0f);
-  state.errorvect.axis[1] = constrain(state.errorvect.axis[1], -1.0f, 1.0f);
-
-  // fix to recover if triggered inverted
-  // the vector cross product results in zero for opposite vectors, so it's bad at 180 error
-  // without this the quad will not invert if angle difference = 180
+  vec3_t errorvect = {.roll = 0, .pitch = 0, .yaw = 0};
+  errorvect.axis[0] = constrain((state.GEstG.axis[2] * stickvector[0]) - (state.GEstG.axis[0] * stickvector[2]), -1.0f, 1.0f);
+  errorvect.axis[1] = constrain(-((state.GEstG.axis[1] * stickvector[2]) - (state.GEstG.axis[2] * stickvector[1])), -1.0f, 1.0f);
+  return errorvect;
 }
 
 static vec3_t input_get_expo() {

src/flight/input.h

@@ -2,6 +2,6 @@
 
 #include "util/vector.h"
 
-void input_stick_vector(float rx_input[], float maxangle);
+vec3_t input_stick_vector(float rx_input[]);
 vec3_t input_rates_calc();
 float input_throttle_calc(float throttle);