[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 roll,pitch and thrust command
-  MAT33_VECT3_MUL(euler_cmd, Ga_inv, a_diff);
+  // 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
+  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];