[wls] make WLS lib, add support for guidance_indi_hybrid (#3115)

* [wls] make WLS lib, add support for guidance_indi_hybrid

* [wls] update test program

* [wls] restore static matrix size for WLS

* [wls] update test prog

* [indi] ABI thrust message to 3D vector (#3116)

* [indi] ABI thrust message to 3D vector

Common WLS for innerloop and outerloop

Co-authored-by: Christophe De Wagter <dewagter@gmail.com>

* Bound defaults to min in case of NaN

---------

Co-authored-by: Christophe De Wagter <dewagter@gmail.com>

* [guidance_indi] lift effectiveness and set stabilization limits in WEAK (#3117)

* do not allow code to run with insufficient matrix sizes

* Prefer not to roll when able.

* [wls] only check size if WLS is used

---------

Co-authored-by: Christophe De Wagter <dewagter@gmail.com>
Co-authored-by: Ewoud Smeur <e.j.j.smeur@tudelft.nl>

conf/airframes/tudelft/nederdrone4.xml

@@ -110,6 +110,7 @@
     </module>
     <module name="stabilization" type="indi">
       <configure name="INDI_NUM_ACT" value="8"/>
+      <define name="WLS_N_U" value="8"/>
       <define name="TILT_TWIST_CTRL" value="TRUE"/>
     </module>
     <module name="stabilization" type="rate_indi"/>

conf/airframes/tudelft/nederdrone6.xml

@@ -106,6 +106,7 @@
     </module>
     <module name="stabilization" type="indi">
       <configure name="INDI_NUM_ACT" value="8"/>
+      <define name="WLS_N_U" value="8"/>
       <define name="TILT_TWIST_CTRL" value="TRUE"/>
     </module>
     <module name="stabilization" type="rate_indi"/>

conf/airframes/tudelft/nederdrone7.xml

@@ -106,6 +106,7 @@
     </module>
     <module name="stabilization" type="indi">
       <configure name="INDI_NUM_ACT" value="8"/>
+      <define name="WLS_N_U" value="8"/>
       <define name="TILT_TWIST_CTRL" value="TRUE"/>
     </module>
     <module name="stabilization" type="rate_indi"/>

conf/airframes/tudelft/nederdrone8.xml

@@ -106,6 +106,7 @@
     </module>
     <module name="stabilization" type="indi">
       <configure name="INDI_NUM_ACT" value="8"/>
+      <define name="WLS_N_U" value="8"/>
       <define name="TILT_TWIST_CTRL" value="TRUE"/>
     </module>
     <module name="stabilization" type="rate_indi"/>

conf/modules/guidance_indi_hybrid.xml

@@ -5,6 +5,11 @@
     <description>
       Guidance controller for hybrids using INDI
     </description>
+    <section name="GUIDANCE_INDI_HYBRID" prefix="GUIDANCE_INDI_HYBRID_">
+      <define name="USE_WLS" value="FALSE|TRUE" description="use WLS allocation instead of matrix inversion (default: FALSE)"/>
+      <define name="WLS_PRIORITIES" value="{100., 100., 1.}" description="WLS priorities on control objectives"/>
+      <define name="WLS_WU" value="{1., 1., 1.}" description="WLS weighting on outputs"/>
+    </section>
   </doc>
   <settings>
     <dl_settings>
@@ -17,14 +22,15 @@
         <dl_setting var="gih_params.speed_gain" min="0" step="0.1" max="10.0" shortname="kd" param="GUIDANCE_INDI_SPEED_GAIN" persistent="true"/>
         <dl_setting var="gih_params.speed_gainz" min="0" step="0.1" max="10.0" shortname="kd_z" param="GUIDANCE_INDI_SPEED_GAINZ" persistent="true"/>
         <dl_setting var="gih_params.heading_bank_gain" min="0" step="0.1" max="30.0" shortname="bank gain" param="GUIDANCE_INDI_HEADING_BANK_GAIN" persistent="true"/>
-        <dl_setting var="guidance_indi_max_airspeed" min="12.0" step="1.0" max="30.0" shortname="cruise_airspeed"/>
-        <dl_setting var="guidance_indi_max_bank" min="10.0" step="1.0" max="50.0" shortname="max_bank"/>
+        <dl_setting var="guidance_indi_max_airspeed" min="12.0" step="1.0" max="30.0" shortname="cruise_airspeed" param="GUIDANCE_INDI_MAX_AIRSPEED"/>
+        <dl_setting var="guidance_indi_max_bank" min="10.0" step="1.0" max="50.0" shortname="max_bank" param="GUIDANCE_H_MAX_BANK"/>
+        <dl_setting var="guidance_indi_min_pitch" min="-130.0" step="1.0" max="-30.0" shortname="min_pitch" param="GUIDANCE_INDI_MIN_PITCH"/>
         <dl_setting var="take_heading_control" min="0" step="1" max="1" values="OFF|ON" shortname="take_heading"/>
       </dl_settings>
     </dl_settings>
   </settings>
   <dep>
-    <depends>@navigation,guidance_rotorcraft</depends>
+    <depends>@navigation,guidance_rotorcraft,wls</depends>
     <provides>guidance,attitude_command</provides>
   </dep>
   <header>

conf/modules/stabilization_indi.xml

@@ -55,7 +55,7 @@
     </dl_settings>
   </settings>
   <dep>
-    <depends>stabilization_rotorcraft,@attitude_command</depends>
+    <depends>stabilization_rotorcraft,@attitude_command,wls</depends>
     <provides>commands</provides>
   </dep>
   <header>
@@ -65,15 +65,10 @@
   <makefile target="ap|nps" firmware="rotorcraft">
     <file name="stabilization_indi.c" dir="$(SRC_FIRMWARE)/stabilization"/>
     <file name="stabilization_attitude_quat_indi.c" dir="$(SRC_FIRMWARE)/stabilization"/>
-    <file name="wls_alloc.c" dir="$(SRC_FIRMWARE)/stabilization/wls"/>
-    <file name="qr_solve.c" dir="math/qr_solve"/>
-    <file name="r8lib_min.c" dir="math/qr_solve"/>
     <configure name="INDI_OUTPUTS" default="4"/>
     <configure name="INDI_NUM_ACT" default="4"/>
     <define name="INDI_OUTPUTS" value="$(INDI_OUTPUTS)"/>
     <define name="INDI_NUM_ACT" value="$(INDI_NUM_ACT)"/>
-    <define name="CA_N_V" value="$(INDI_OUTPUTS)"/>
-    <define name="CA_N_U" value="$(INDI_NUM_ACT)"/>
     <define name="STABILIZATION_ATTITUDE_TYPE_INT"/>
     <define name="STABILIZATION_ATTITUDE_TYPE_H" value="stabilization/stabilization_attitude_quat_indi.h" type="string"/>
     <define name="STABILIZATION_ATTITUDE_INDI_FULL" value="true"/>

conf/modules/wls.xml

@@ -0,0 +1,27 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="wls" task="core">
+  <doc>
+    <description>
+      Weighted Least Square optimization algorithm.
+
+      Used for dynamic allocation of actuators, in particular
+      with INDI-based control algorithms.
+
+      The size of the matrix (output vs. objectives) must be defined large
+      enough for all the controllers
+    </description>
+    <define name="WLS_N_U" value="4" description="size of the control output vector (default: 6)"/>
+    <define name="WLS_N_V" value="4" description="size of the control objectives vector (default: 4)"/>
+  </doc>
+  <header>
+    <file name="wls_alloc.h" dir="math/wls"/>
+  </header>
+  <makefile>
+    <file name="wls_alloc.c" dir="math/wls"/>
+    <file name="qr_solve.c" dir="math/qr_solve"/>
+    <file name="r8lib_min.c" dir="math/qr_solve"/>
+    <test/>
+  </makefile>
+</module>
+

sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c

@@ -91,6 +91,18 @@ struct guidance_indi_hybrid_params gih_params = {
 #endif
 float guidance_indi_max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED;
 
+// If using WLS, check that the matrix size is sufficient
+#if GUIDANCE_INDI_HYBRID_USE_WLS
+#if 3 > WLS_N_U
+#error Matrix-WLS_N_U too small: increase WLS_N_U in airframe file
+#endif
+
+#if 3 > WLS_N_V
+#error Matrix-WLS_N_V too small: increase WLS_N_V in airframe file
+#endif
+#endif
+
+
 // Tell the guidance that the airspeed needs to be zeroed.
 // Recomended to also put GUIDANCE_INDI_NAV_SPEED_MARGIN low in this case.
 #ifndef GUIDANCE_INDI_ZERO_AIRSPEED
@@ -145,6 +157,7 @@ float inv_eff[4];
 
 // Max bank angle in radians
 float guidance_indi_max_bank = GUIDANCE_H_MAX_BANK;
+float guidance_indi_min_pitch = GUIDANCE_INDI_MIN_PITCH;
 
 /** state eulers in zxy order */
 struct FloatEulers eulers_zxy;
@@ -158,10 +171,27 @@ Butterworth2LowPass accely_filt;
 
 struct FloatVect2 desired_airspeed;
 
-struct FloatMat33 Ga;
-struct FloatMat33 Ga_inv;
+float Ga[3][3];
 struct FloatVect3 euler_cmd;
 
+#if GUIDANCE_INDI_HYBRID_USE_WLS
+#include "math/wls/wls_alloc.h"
+float du_min_gih[3];
+float du_max_gih[3];
+float du_pref_gih[3];
+float *Bwls_gih[3];
+#ifdef GUIDANCE_INDI_HYBRID_WLS_PRIORITIES
+float Wv_gih[3] = GUIDANCE_INDI_HYBRID_WLS_PRIORITIES;
+#else
+float Wv_gih[3] = { 100.f, 100.f, 1.f }; // X,Y accel, Z accel
+#endif
+#ifdef GUIDANCE_INDI_HYBRID_WLS_WU
+float Wu_gih[3] = GUIDANCE_INDI_HYBRID_WLS_WU;
+#else
+float Wu_gih[3] = { 1.f, 1.f, 1.f };
+#endif
+#endif
+
 float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF;
 
 float guidance_indi_hybrid_heading_sp = 0.f;
@@ -179,7 +209,7 @@ struct FloatVect3 indi_vel_sp = {0.0, 0.0, 0.0};
 float time_of_vel_sp = 0.0;
 
 void guidance_indi_propagate_filters(void);
-static void guidance_indi_calcg_wing(struct FloatMat33 *Gmat);
+static void guidance_indi_calcg_wing(float Gmat[3][3]);
 
 #if PERIODIC_TELEMETRY
 #include "modules/datalink/telemetry.h"
@@ -219,6 +249,12 @@ void guidance_indi_init(void)
   init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, 0.0);
   init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0);
 
+#if GUIDANCE_INDI_HYBRID_USE_WLS
+  for (int8_t i = 0; i < 3; i++) {
+    Bwls_gih[i] = Ga[i];
+  }
+#endif
+
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE_INDI_HYBRID, send_guidance_indi_hybrid);
 #endif
@@ -291,11 +327,6 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
   sp_accel.z = -(radio_control.values[RADIO_THROTTLE]-4500)*8.0/9600.0;
 #endif
 
-  // Calculate matrix of partial derivatives
-  guidance_indi_calcg_wing(&Ga);
-  // Invert this matrix
-  MAT33_INV(Ga_inv, Ga);
-
   struct FloatVect3 accel_filt;
   accel_filt.x = filt_accel_ned[0].o[0];
   accel_filt.y = filt_accel_ned[1].o[0];
@@ -319,8 +350,41 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
 #endif
 #endif
 
+  // Calculate matrix of partial derivatives
+  guidance_indi_calcg_wing(Ga);
+
+#if GUIDANCE_INDI_HYBRID_USE_WLS
+  // Set lower limits
+  du_min_gih[0] = -guidance_indi_max_bank - roll_filt.o[0]; // roll
+  du_min_gih[1] = RadOfDeg(guidance_indi_min_pitch) - pitch_filt.o[0]; // pitch
+  du_min_gih[2] = (MAX_PPRZ - actuator_state_filt_vect[0]) * g1g2[3][0] + (MAX_PPRZ - actuator_state_filt_vect[1]) * g1g2[3][1] + (MAX_PPRZ - actuator_state_filt_vect[2]) * g1g2[3][2] + (MAX_PPRZ - actuator_state_filt_vect[3]) * g1g2[3][3];
+
+  // Set upper limits limits
+  du_max_gih[0] = guidance_indi_max_bank - roll_filt.o[0]; //roll
+  du_max_gih[1] = RadOfDeg(GUIDANCE_INDI_MAX_PITCH) - pitch_filt.o[0]; // pitch
+  du_max_gih[2] = -(actuator_state_filt_vect[0]*g1g2[3][0] + actuator_state_filt_vect[1]*g1g2[3][1] + actuator_state_filt_vect[2]*g1g2[3][2] + actuator_state_filt_vect[3]*g1g2[3][3]);
+
+  // Set prefered states
+  du_pref_gih[0] = -roll_filt.o[0]; // prefered delta roll angle
+  du_pref_gih[1] = -pitch_filt.o[0]; // prefered delta pitch angle
+  du_pref_gih[2] = du_max_gih[2];
+
+  float v_gih[3] = { a_diff.x, a_diff.y, a_diff.z };
+  float du_gih[3];
+  int num_iter UNUSED = wls_alloc(
+      du_gih, v_gih, du_min_gih, du_max_gih,
+      Bwls_gih, 0, 0, Wv_gih, Wu_gih, du_pref_gih, 100000, 10);
+  euler_cmd.x = du_gih[0];
+  euler_cmd.y = du_gih[1];
+  euler_cmd.z = du_gih[2];
+
+#else
+  // compute inverse matrix of Ga
+  float Ga_inv[3][3] = {};
+  float_mat_inv_3d(Ga_inv, Ga);
   // Calculate roll,pitch and thrust command
-  MAT33_VECT3_MUL(euler_cmd, Ga_inv, a_diff);
+  float_mat3_mult(&euler_cmd, Ga_inv, a_diff);
+#endif
 
   struct FloatVect3 thrust_vect;
   thrust_vect.x = 0.0;  // Fill for quadplanes
@@ -345,7 +409,7 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
 
   //Bound euler angles to prevent flipping
   Bound(guidance_euler_cmd.phi, -guidance_indi_max_bank, guidance_indi_max_bank);
-  Bound(guidance_euler_cmd.theta, RadOfDeg(GUIDANCE_INDI_MIN_PITCH), RadOfDeg(GUIDANCE_INDI_MAX_PITCH));
+  Bound(guidance_euler_cmd.theta, RadOfDeg(guidance_indi_min_pitch), RadOfDeg(GUIDANCE_INDI_MAX_PITCH));
 
   // Use the current roll angle to determine the corresponding heading rate of change.
   float coordinated_turn_roll = eulers_zxy.phi;
@@ -663,7 +727,7 @@ void guidance_indi_propagate_filters(void) {
  *
  * @param Gmat array to write the matrix to [3x3]
  */
-void guidance_indi_calcg_wing(struct FloatMat33 *Gmat) {
+void guidance_indi_calcg_wing(float Gmat[3][3]) {
 
   /*Pre-calculate sines and cosines*/
   float sphi = sinf(eulers_zxy.phi);
@@ -687,15 +751,15 @@ void guidance_indi_calcg_wing(struct FloatMat33 *Gmat) {
   // get the derivative of the lift wrt to theta
   float liftd = guidance_indi_get_liftd(stateGetAirspeed_f(), eulers_zxy.theta);
 
-  RMAT_ELMT(*Gmat, 0, 0) =  cphi*ctheta*spsi*T + cphi*spsi*lift;
-  RMAT_ELMT(*Gmat, 1, 0) = -cphi*ctheta*cpsi*T - cphi*cpsi*lift;
-  RMAT_ELMT(*Gmat, 2, 0) = -sphi*ctheta*T -sphi*lift;
-  RMAT_ELMT(*Gmat, 0, 1) = (ctheta*cpsi - sphi*stheta*spsi)*T*GUIDANCE_INDI_PITCH_EFF_SCALING + sphi*spsi*liftd;
-  RMAT_ELMT(*Gmat, 1, 1) = (ctheta*spsi + sphi*stheta*cpsi)*T*GUIDANCE_INDI_PITCH_EFF_SCALING - sphi*cpsi*liftd;
-  RMAT_ELMT(*Gmat, 2, 1) = -cphi*stheta*T*GUIDANCE_INDI_PITCH_EFF_SCALING + cphi*liftd;
-  RMAT_ELMT(*Gmat, 0, 2) = stheta*cpsi + sphi*ctheta*spsi;
-  RMAT_ELMT(*Gmat, 1, 2) = stheta*spsi - sphi*ctheta*cpsi;
-  RMAT_ELMT(*Gmat, 2, 2) = cphi*ctheta;
+  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;
 }
 
 /**
@@ -706,17 +770,17 @@ void guidance_indi_calcg_wing(struct FloatMat33 *Gmat) {
  * @return The derivative of lift w.r.t. pitch
  */
 float WEAK guidance_indi_get_liftd(float airspeed, float theta) {
-  float liftd = 0.0;
+  float liftd = 0.0f;
 
-  if(airspeed < 12) {
+  if (airspeed < 12.f) {
   /* Assume the airspeed is too low to be measured accurately
     * Use scheduling based on pitch angle instead.
     * You can define two interpolation segments
     */
     float pitch_interp = DegOfRad(theta);
-    const float min_pitch = -80.0;
-    const float middle_pitch = -50.0;
-    const float max_pitch = -20.0;
+    const float min_pitch = -80.0f;
+    const float middle_pitch = -50.0f;
+    const float max_pitch = -20.0f;
 
     Bound(pitch_interp, min_pitch, max_pitch);
     if (pitch_interp > middle_pitch) {

sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h

@@ -77,6 +77,7 @@ extern float guidance_indi_specific_force_gain;
 extern float guidance_indi_max_airspeed;
 extern bool take_heading_control;
 extern float guidance_indi_max_bank;
+extern float guidance_indi_min_pitch;
 extern bool force_forward;       ///< forward flight for hybrid nav
 
 

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c

@@ -42,7 +42,7 @@
 #include "modules/actuators/actuators.h"
 #include "modules/core/abi.h"
 #include "filters/low_pass_filter.h"
-#include "wls/wls_alloc.h"
+#include "math/wls/wls_alloc.h"
 #include <stdio.h>
 
 // Factor that the estimated G matrix is allowed to deviate from initial one
@@ -85,6 +85,16 @@
 #warning SetAutoCommandsFromRC not used: STAB_INDI overwrites actuators
 #endif
 
+#if !STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
+#if INDI_NUM_ACT > WLS_N_U
+#error Matrix-WLS_N_U too small: increase WLS_N_U in airframe file
+#endif
+
+#if INDI_OUTPUTS > WLS_N_V
+#error Matrix-WLS_N_V too small: increase WLS_N_V in airframe file
+#endif
+#endif
+
 float du_min_stab_indi[INDI_NUM_ACT];
 float du_max_stab_indi[INDI_NUM_ACT];
 float du_pref_stab_indi[INDI_NUM_ACT];

sw/airborne/math/pprz_algebra_float.c

@@ -813,6 +813,55 @@ void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct FloatVect
   vect_out->y = mat[2] * vect_in.x + mat[3] * vect_in.y;
 }
 
+/**
+ * @brief 3x3 matrix inverse
+ *
+ * @param inv_out[3][3] inverted matrix output
+ * @param mat_in[3][3] matrix to be inverted
+ *
+ * @return success (0) or not invertible (1)
+ */
+bool float_mat_inv_3d(float inv_out[3][3], float mat_in[3][3])
+{
+  const float m00 = mat_in[1][1]*mat_in[2][2] - mat_in[1][2]*mat_in[2][1];
+  const float m10 = mat_in[0][1]*mat_in[2][2] - mat_in[0][2]*mat_in[2][1];
+  const float m20 = mat_in[0][1]*mat_in[1][2] - mat_in[0][2]*mat_in[1][1];
+  const float m01 = mat_in[1][0]*mat_in[2][2] - mat_in[1][2]*mat_in[2][0];
+  const float m11 = mat_in[0][0]*mat_in[2][2] - mat_in[0][2]*mat_in[2][0];
+  const float m21 = mat_in[0][0]*mat_in[1][2] - mat_in[0][2]*mat_in[1][0];
+  const float m02 = mat_in[1][0]*mat_in[2][1] - mat_in[1][1]*mat_in[2][0];
+  const float m12 = mat_in[0][0]*mat_in[2][1] - mat_in[0][1]*mat_in[2][0];
+  const float m22 = mat_in[0][0]*mat_in[1][1] - mat_in[0][1]*mat_in[1][0];
+  const float det = mat_in[0][0]*m00 - mat_in[1][0]*m10 + mat_in[2][0]*m20;
+  if (fabs(det) > FLT_EPSILON) {
+    inv_out[0][0] =  m00 / det;
+    inv_out[1][0] = -m01 / det;
+    inv_out[2][0] =  m02 / det;
+    inv_out[0][1] = -m10 / det;
+    inv_out[1][1] =  m11 / det;
+    inv_out[2][1] = -m12 / det;
+    inv_out[0][2] =  m20 / det;
+    inv_out[1][2] = -m21 / det;
+    inv_out[2][2] =  m22 / det;
+    return 0;
+  }
+  return 1;
+}
+
+/**
+ * @brief Multiply 3D matrix with vector
+ *
+ * @param vect_out output vector
+ * @param mat[3][3] Matrix input
+ * @param vect_in Vector input
+ */
+void float_mat3_mult(struct FloatVect3 *vect_out, float mat[3][3], struct FloatVect3 vect_in)
+{
+  vect_out->x = mat[0][0] * vect_in.x + mat[0][1] * vect_in.y + mat[0][2] * vect_in.z;
+  vect_out->y = mat[1][0] * vect_in.x + mat[1][1] * vect_in.y + mat[1][2] * vect_in.z;
+  vect_out->z = mat[2][0] * vect_in.x + mat[2][1] * vect_in.y + mat[2][2] * vect_in.z;
+}
+
 /*
  * 4x4 Matrix inverse.
  * obtained from: http://rodolphe-vaillant.fr/?e=7

sw/airborne/math/pprz_algebra_float.h

@@ -908,6 +908,8 @@ static inline void float_mat_mul_scalar(float **o, float **a, float scalar, int
 
 extern bool float_mat_inv_2d(float inv_out[4], float mat_in[4]);
 extern void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct FloatVect2 vect_in);
+extern bool float_mat_inv_3d(float inv_out[3][3], float mat_in[3][3]);
+extern void float_mat3_mult(struct FloatVect3 *vect_out, float mat[3][3], struct FloatVect3 vect_in);
 extern bool float_mat_inv_4d(float invOut[4][4], float mat_in[4][4]);
 
 extern void float_vect3_bound_in_2d(struct FloatVect3 *vect3, float bound);

sw/airborne/math/wls/wls_alloc.c

@@ -36,7 +36,6 @@
  */
 
 #include "wls_alloc.h"
-#include <stdio.h>
 #include "std.h"
 
 #include <string.h>
@@ -45,22 +44,26 @@
 #include "math/qr_solve/qr_solve.h"
 #include "math/qr_solve/r8lib_min.h"
 
-void print_final_values(int n_u, int n_v, float* u, float** B, float* v, float* umin, float* umax);
-void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, float* p_free);
-
 // provide loop feedback
+#ifndef WLS_VERBOSE
 #define WLS_VERBOSE FALSE
-
-// Problem size needs to be predefined to avoid having to use VLAs
-#ifndef CA_N_V
-#error CA_N_V needs to be defined!
 #endif
 
-#ifndef CA_N_U
-#error CA_N_U needs to be defined!
+#if WLS_VERBOSE
+#include <stdio.h>
+static void print_final_values(int n_u, int n_v, float* u, float** B, float* v, float* umin, float* umax);
+static void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, float* p_free);
 #endif
 
-#define CA_N_C  (CA_N_U+CA_N_V)
+#ifndef WLS_N_U
+#define WLS_N_U 6
+#endif
+
+#ifndef WLS_N_V
+#define WLS_N_V 4
+#endif
+
+#define WLS_N_C ((WLS_N_U)+(WLS_N_V))
 
 /**
  * @brief Wrapper for qr solve
@@ -71,7 +74,7 @@ void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, flo
  * @param m number of rows
  * @param n number of columns
  */
-void qr_solve_wrapper(int m, int n, float** A, float* b, float* x) {
+static void qr_solve_wrapper(int m, int n, float** A, float* b, float* x) {
   float in[m * n];
   // convert A to 1d array
   int k = 0;
@@ -92,7 +95,7 @@ void qr_solve_wrapper(int m, int n, float** A, float* b, float* x) {
  * weighting matrices Wv and Wu
  *
  * @param u The control output vector
- * @param v The control objective
+ * @param v The control objective vector
  * @param umin The minimum u vector
  * @param umax The maximum u vector
  * @param B The control effectiveness matrix
@@ -107,44 +110,46 @@ void qr_solve_wrapper(int m, int n, float** A, float* b, float* x) {
  * control vector (sqare root of gamma)
  * @param imax Max number of iterations
  *
- * @return Number of iterations
+ * @return Number of iterations, -1 upon failure
  */
-int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
-    float* u_guess, float* W_init, float* Wv, float* Wu, float* up,
-    float gamma_sq, int imax) {
+int wls_alloc(float* u, float* v,
+    float* umin, float* umax, float** B,
+    float* u_guess, float* W_init, float* Wv, float* Wu,
+    float* up, float gamma_sq, int imax)
+{
   // allocate variables, use defaults where parameters are set to 0
   if(!gamma_sq) gamma_sq = 100000;
   if(!imax) imax = 100;
 
-  int n_c = CA_N_C;
-  int n_u = CA_N_U;
-  int n_v = CA_N_V;
+  int n_u = WLS_N_U;
+  int n_v = WLS_N_V;
+  int n_c = n_u + n_v;
 
-  float A[CA_N_C][CA_N_U];
-  float A_free[CA_N_C][CA_N_U];
+  float A[WLS_N_C][WLS_N_U];
+  float A_free[WLS_N_C][WLS_N_U];
 
   // Create a pointer array to the rows of A_free
   // such that we can pass it to a function
-  float * A_free_ptr[CA_N_C];
+  float * A_free_ptr[WLS_N_C];
   for(int i = 0; i < n_c; i++)
     A_free_ptr[i] = A_free[i];
 
-  float b[CA_N_C];
-  float d[CA_N_C];
+  float b[WLS_N_C];
+  float d[WLS_N_C];
 
-  int free_index[CA_N_U];
-  int free_index_lookup[CA_N_U];
+  int free_index[WLS_N_U];
+  int free_index_lookup[WLS_N_U];
   int n_free = 0;
   int free_chk = -1;
 
   int iter = 0;
-  float p_free[CA_N_U];
-  float p[CA_N_U];
-  float u_opt[CA_N_U];
-  int infeasible_index[CA_N_U] UNUSED;
+  float p_free[WLS_N_U];
+  float p[WLS_N_U];
+  float u_opt[WLS_N_U];
+  int infeasible_index[WLS_N_U] UNUSED;
   int n_infeasible = 0;
-  float lambda[CA_N_U];
-  float W[CA_N_U];
+  float lambda[WLS_N_U];
+  float W[WLS_N_U];
 
   // Initialize u and the working set, if provided from input
   if (!u_guess) {
@@ -161,7 +166,6 @@ int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
 
   memset(free_index_lookup, -1, n_u * sizeof(float));
 
-
   // find free indices
   for (int i = 0; i < n_u; i++) {
     if (W[i] == 0) {
@@ -311,11 +315,11 @@ int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
     }
   }
   // solution failed, return negative one to indicate failure
-  return iter;
+  return -1;
 }
 
 #if WLS_VERBOSE
-void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, float* p_free) {
+static void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, float* p_free) {
 
   printf("n_c = %d n_free = %d\n", n_c, n_free);
 
@@ -339,7 +343,7 @@ void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, flo
   printf("\n\n");
 }
 
-void print_final_values(int n_u, int n_v, float* u, float** B, float* v, float* umin, float* umax) {
+static void print_final_values(int n_u, int n_v, float* u, float** B, float* v, float* umin, float* umax) {
   printf("n_u = %d n_v = %d\n", n_u, n_v);
 
   printf("B =\n");

sw/airborne/math/wls/wls_alloc.h

@@ -20,17 +20,6 @@
  * Boston, MA 02111-1307, USA.
  */
 
-/**
- * @brief Wrapper for qr solve
- *
- * Possible to use a different solver if needed.
- * Solves a system of the form Ax = b for x.
- *
- * @param m number of rows
- * @param n number of columns
- */
-void qr_solve_wrapper(int m, int n, float** A, float* b, float* x);
-
 /**
  * @brief active set algorithm for control allocation
  *
@@ -38,8 +27,11 @@ void qr_solve_wrapper(int m, int n, float** A, float* b, float* x);
  * the inputs that will satisfy most of the control objective, subject to the
  * weighting matrices Wv and Wu
  *
+ * The dimension of the input vectors u and v are defined at compilation time
+ * and must be large enough for all the considered cases.
+ *
  * @param u The control output vector
- * @param v The control objective
+ * @param v The control objective vector
  * @param umin The minimum u vector
  * @param umax The maximum u vector
  * @param B The control effectiveness matrix
@@ -56,6 +48,7 @@ void qr_solve_wrapper(int m, int n, float** A, float* b, float* x);
  *
  * @return Number of iterations, -1 upon failure
  */
-int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
+int wls_alloc(float* u, float* v,
+              float* umin, float* umax, float** B,
               float* u_guess, float* W_init, float* Wv, float* Wu,
               float* ud, float gamma, int imax);

sw/airborne/test/Makefile

@@ -26,8 +26,8 @@ test_bla: test_bla.c ../math/pprz_trig_int.c ../math/pprz_algebra_int.c ../math/
 test_geo: test_geo_conversions.c ../math/pprz_trig_int.c ../math/pprz_algebra_int.c ../math/pprz_algebra_float.c ../math/pprz_algebra_double.c ../math/pprz_geodetic_int.c ../math/pprz_geodetic_float.c ../math/pprz_geodetic_double.c
 	$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
 
-test_alloc: test_alloc.c ../firmwares/rotorcraft/stabilization/wls/wls_alloc.c ../math/qr_solve/r8lib_min.c ../math/qr_solve/qr_solve.c
-	$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
+test_alloc: test_alloc.c ../math/wls/wls_alloc.c ../math/qr_solve/r8lib_min.c ../math/qr_solve/qr_solve.c
+	$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS) -DWLS_N_U=8 -DWLS_N_V=4
 
 test_tt: test_tilt_twist.c ../math/pprz_algebra_float.c
 	$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)

sw/airborne/test/test_alloc.c

@@ -30,7 +30,7 @@
 
 #include <stdio.h>
 #include "std.h"
-#include "firmwares/rotorcraft/stabilization/wls/wls_alloc.h"
+#include "math/wls/wls_alloc.h"
 
 #define INDI_OUTPUTS 4