fix bug that gave zero effectiveness matrix (#3151)

paparazzi · Nov 1, 2023 · 9212392 · 9212392
1 parent 4ae4056
commit 9212392
Showing 1 changed file with 42 additions and 47 deletions.
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
@@ -385,7 +385,7 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
       du_gih, v_gih, du_min_gih, du_max_gih,
       Bwls_gih, 0, 0, Wv_gih, Wu_gih, du_pref_gih, 100000, 10,
       GUIDANCE_INDI_HYBRID_U, GUIDANCE_INDI_HYBRID_V);
-  
+
   euler_cmd.x = du_gih[0];
   euler_cmd.y = du_gih[1];
   euler_cmd.z = du_gih[2];
@@ -750,46 +750,7 @@ void guidance_indi_propagate_filters(void) {
  * @param a_diff acceleration errors in earth frame
  * @param body_v 3D vector to write the control objective v
  */
-#if GUIDANCE_INDI_HYBRID_USE_WLS
-void WEAK guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U], struct FloatVect3 a_diff, float v_gih[GUIDANCE_INDI_HYBRID_V]) {
-
-  /*Pre-calculate sines and cosines*/
-  float sphi = sinf(eulers_zxy.phi);
-  float cphi = cosf(eulers_zxy.phi);
-  float stheta = sinf(eulers_zxy.theta);
-  float ctheta = cosf(eulers_zxy.theta);
-  float spsi = sinf(eulers_zxy.psi);
-  float cpsi = cosf(eulers_zxy.psi);
-  //minus gravity is a guesstimate of the thrust force, thrust measurement would be better
-
-#ifndef GUIDANCE_INDI_PITCH_EFF_SCALING
-#define GUIDANCE_INDI_PITCH_EFF_SCALING 1.0
-#endif
-
-  /*Amount of lift produced by the wing*/
-  float pitch_lift = eulers_zxy.theta;
-  Bound(pitch_lift,-M_PI_2,0);
-  float lift = sinf(pitch_lift)*9.81;
-  float T = cosf(pitch_lift)*-9.81;
-
-  // get the derivative of the lift wrt to theta
-  float liftd = guidance_indi_get_liftd(stateGetAirspeed_f(), eulers_zxy.theta);
-
-  Gmat[0][0] =  cphi*ctheta*spsi*T + cphi*spsi*lift;
-  Gmat[1][0] = -cphi*ctheta*cpsi*T - cphi*cpsi*lift;
-  Gmat[2][0] = -sphi*ctheta*T -sphi*lift;
-  Gmat[0][1] = (ctheta*cpsi - sphi*stheta*spsi)*T*GUIDANCE_INDI_PITCH_EFF_SCALING + sphi*spsi*liftd;
-  Gmat[1][1] = (ctheta*spsi + sphi*stheta*cpsi)*T*GUIDANCE_INDI_PITCH_EFF_SCALING - sphi*cpsi*liftd;
-  Gmat[2][1] = -cphi*stheta*T*GUIDANCE_INDI_PITCH_EFF_SCALING + cphi*liftd;
-  Gmat[0][2] = stheta*cpsi + sphi*ctheta*spsi;
-  Gmat[1][2] = stheta*spsi - sphi*ctheta*cpsi;
-  Gmat[2][2] = cphi*ctheta;
-
-  v_gih[0] = a_diff.x;
-  v_gih[1] = a_diff.y;
-  v_gih[2] = a_diff.z;
-}
-#elif defined(GUIDANCE_INDI_QUADPLANE)
+#if defined(GUIDANCE_INDI_QUADPLANE)
 /**
  * Perform WLS
  *
@@ -825,7 +786,7 @@ void WEAK guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_I
   Gmat[2][1] = -cphi*stheta*lift_thrust_bz*GUIDANCE_INDI_PITCH_EFF_SCALING + cphi*liftd;
 
   Gmat[0][2] =  cphi*stheta;
-  Gmat[1][2] = -sphi; 
+  Gmat[1][2] = -sphi;
   Gmat[2][2] =  cphi*ctheta;
 
   Gmat[0][3] =  ctheta;
@@ -837,16 +798,50 @@ void WEAK guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_I
   body_v[1] = -spsi * a_diff.x + cpsi * a_diff.y;
   body_v[2] =  a_diff.z;
 }
-
 #else
+void WEAK guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U], struct FloatVect3 a_diff, float v_gih[GUIDANCE_INDI_HYBRID_V]) {
+
+  /*Pre-calculate sines and cosines*/
+  float sphi = sinf(eulers_zxy.phi);
+  float cphi = cosf(eulers_zxy.phi);
+  float stheta = sinf(eulers_zxy.theta);
+  float ctheta = cosf(eulers_zxy.theta);
+  float spsi = sinf(eulers_zxy.psi);
+  float cpsi = cosf(eulers_zxy.psi);
+  //minus gravity is a guesstimate of the thrust force, thrust measurement would be better
+
+#ifndef GUIDANCE_INDI_PITCH_EFF_SCALING
+#define GUIDANCE_INDI_PITCH_EFF_SCALING 1.0
+#endif
+
+  /*Amount of lift produced by the wing*/
+  float pitch_lift = eulers_zxy.theta;
+  Bound(pitch_lift,-M_PI_2,0);
+  float lift = sinf(pitch_lift)*9.81;
+  float T = cosf(pitch_lift)*-9.81;
+
+  // get the derivative of the lift wrt to theta
+  float liftd = guidance_indi_get_liftd(stateGetAirspeed_f(), eulers_zxy.theta);
 
-void WEAK guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U] UNUSED, struct FloatVect3 a_diff UNUSED, float body_v[GUIDANCE_INDI_HYBRID_V] UNUSED) {}
+  Gmat[0][0] =  cphi*ctheta*spsi*T + cphi*spsi*lift;
+  Gmat[1][0] = -cphi*ctheta*cpsi*T - cphi*cpsi*lift;
+  Gmat[2][0] = -sphi*ctheta*T -sphi*lift;
+  Gmat[0][1] = (ctheta*cpsi - sphi*stheta*spsi)*T*GUIDANCE_INDI_PITCH_EFF_SCALING + sphi*spsi*liftd;
+  Gmat[1][1] = (ctheta*spsi + sphi*stheta*cpsi)*T*GUIDANCE_INDI_PITCH_EFF_SCALING - sphi*cpsi*liftd;
+  Gmat[2][1] = -cphi*stheta*T*GUIDANCE_INDI_PITCH_EFF_SCALING + cphi*liftd;
+  Gmat[0][2] = stheta*cpsi + sphi*ctheta*spsi;
+  Gmat[1][2] = stheta*spsi - sphi*ctheta*cpsi;
+  Gmat[2][2] = cphi*ctheta;
 
+  v_gih[0] = a_diff.x;
+  v_gih[1] = a_diff.y;
+  v_gih[2] = a_diff.z;
+}
 #endif
 /**
- * 
- * @param body_v 
- * 
+ *
+ * @param body_v
+ *
  * WEAK function to set the quadplane wls settings
  */
 #if GUIDANCE_INDI_HYBRID_USE_WLS