WLS update NaN (#3120)

* wls_alloc update
paparazzi · Oct 2, 2023 · 46ea0a5 · 46ea0a5
1 parent 01ac335
commit 46ea0a5
Show file tree

Hide file tree

Showing 5 changed files with 38 additions and 40 deletions.
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
@@ -373,7 +373,7 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
   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);
+      Bwls_gih, 0, 0, Wv_gih, Wu_gih, du_pref_gih, 100000, 10, 3, 3);
   euler_cmd.x = du_gih[0];
   euler_cmd.y = du_gih[1];
   euler_cmd.z = du_gih[2];

diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
@@ -596,7 +596,7 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
 
   // WLS Control Allocator
   num_iter =
-    wls_alloc(indi_du, indi_v, du_min_stab_indi, du_max_stab_indi, Bwls, 0, 0, Wv, indi_Wu, du_pref_stab_indi, 10000, 10);
+    wls_alloc(indi_du, indi_v, du_min_stab_indi, du_max_stab_indi, Bwls, 0, 0, Wv, indi_Wu, du_pref_stab_indi, 10000, 10, INDI_NUM_ACT, INDI_OUTPUTS);
 #endif
 
   if (in_flight) {

diff --git a/sw/airborne/math/wls/wls_alloc.c b/sw/airborne/math/wls/wls_alloc.c
@@ -99,8 +99,6 @@ static void qr_solve_wrapper(int m, int n, float** A, float* b, float* x) {
  * @param umin The minimum u vector
  * @param umax The maximum u vector
  * @param B The control effectiveness matrix
- * @param n_u Length of u
- * @param n_v Lenght of v
  * @param u_guess Initial value for u
  * @param W_init Initial working set, if known
  * @param Wv Weighting on different control objectives
@@ -109,20 +107,18 @@ static void qr_solve_wrapper(int m, int n, float** A, float* b, float* x) {
  * @param gamma_sq Preference of satisfying control objective over desired
  * control vector (sqare root of gamma)
  * @param imax Max number of iterations
+ * @param n_u Length of u (the number of actuators)
+ * @param n_v Lenght of v (the number of control objectives)
  *
- * @return Number of iterations, -1 upon failure
+ * @return Number of iterations which is (imax+1) if it ran out of iterations
  */
-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,  int n_u, int n_v) {
   // allocate variables, use defaults where parameters are set to 0
   if(!gamma_sq) gamma_sq = 100000;
   if(!imax) imax = 100;
 
-  int n_u = WLS_N_U;
-  int n_v = WLS_N_V;
   int n_c = n_u + n_v;
 
   float A[WLS_N_C][WLS_N_U];
@@ -209,7 +205,10 @@ int wls_alloc(float* u, float* v,
     }
 
 
-    if (n_free) {
+    // Count the infeasible free actuators
+    n_infeasible = 0;
+
+    if (n_free > 0) {
       // Still free variables left, calculate corresponding solution
 
       // use a solver to find the solution to A_free*p_free = d
@@ -220,18 +219,15 @@ int wls_alloc(float* u, float* v,
       print_in_and_outputs(n_c, n_free, A_free_ptr, d, p_free);
 #endif
 
-    }
+      // Set the nonzero values of p and add to u_opt
+      for (int i = 0; i < n_free; i++) {
+        p[free_index[i]] = p_free[i];
+        u_opt[free_index[i]] += p_free[i];
 
-    // Set the nonzero values of p and add to u_opt
-    for (int i = 0; i < n_free; i++) {
-      p[free_index[i]] = p_free[i];
-      u_opt[free_index[i]] += p_free[i];
-    }
-    // check limits
-    n_infeasible = 0;
-    for (int i = 0; i < n_u; i++) {
-      if (u_opt[i] >= (umax[i] + 0.01) || u_opt[i] <= (umin[i] - 0.01)) {
-        infeasible_index[n_infeasible++] = i;
+        // check limits
+        if ( (u_opt[free_index[i]] > umax[free_index[i]] || u_opt[free_index[i]] < umin[free_index[i]])) {
+          infeasible_index[n_infeasible++] = free_index[i];
+        }
       }
     }
 
@@ -276,18 +272,20 @@ int wls_alloc(float* u, float* v,
         return iter;
       }
     } else {
-      float alpha = INFINITY;
+      // scaling back actuator command (0-1)
+      float alpha = 1.0;
       float alpha_tmp;
-      int id_alpha = 0;
+      int id_alpha = free_index[0];
 
       // find the lowest distance from the limit among the free variables
       for (int i = 0; i < n_free; i++) {
         int id = free_index[i];
-        if(fabs(p[id]) > FLT_EPSILON) {
-          alpha_tmp = (p[id] < 0) ? (umin[id] - u[id]) / p[id]
-            : (umax[id] - u[id]) / p[id];
-        } else {
-          alpha_tmp = INFINITY;
+
+        alpha_tmp = (p[id] < 0) ? (umin[id] - u[id]) / p[id]
+          : (umax[id] - u[id]) / p[id];
+
+        if (isnan(alpha_tmp) || alpha_tmp < 0.f) {
+          alpha_tmp = 1.0f;
         }
         if (alpha_tmp < alpha) {
           alpha = alpha_tmp;
@@ -298,6 +296,7 @@ int wls_alloc(float* u, float* v,
       // update input u = u + alpha*p
       for (int i = 0; i < n_u; i++) {
         u[i] += alpha * p[i];
+        Bound(u[i],umin[i],umax[i]);
       }
       // update d = d-alpha*A*p_free
       for (int i = 0; i < n_c; i++) {
@@ -314,8 +313,7 @@ int wls_alloc(float* u, float* v,
       free_index_lookup[id_alpha] = -1;
     }
   }
-  // solution failed, return negative one to indicate failure
-  return -1;
+  return iter;
 }
 
 #if WLS_VERBOSE

diff --git a/sw/airborne/math/wls/wls_alloc.h b/sw/airborne/math/wls/wls_alloc.h
@@ -32,8 +32,6 @@
  *
  * @param u The control output vector
  * @param v The control objective vector
- * @param umin The minimum u vector
- * @param umax The maximum u vector
  * @param B The control effectiveness matrix
  * @param n_u Length of u
  * @param n_v Lenght of v
@@ -45,10 +43,12 @@
  * @param gamma_sq Preference of satisfying control objective over desired
  * control vector (sqare root of gamma)
  * @param imax Max number of iterations
+ * @param n_u Length of u (the number of actuators)
+ * @param n_v Lenght of v (the number of control objectives)
  *
- * @return Number of iterations, -1 upon failure
+ * @return Number of iterations: (imax+1) means it ran out of iterations
  */
 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);
+              float* ud, float gamma, int imax, int n_u, int n_v);
diff --git a/sw/airborne/test/test_alloc.c b/sw/airborne/test/test_alloc.c
@@ -83,7 +83,7 @@ void test_four_by_four(void)
 
   // WLS Control Allocator
   int num_iter =
-    wls_alloc(indi_du, indi_v, u_min, u_max, Bwls, 0, 0, Wv, 0, 0, 10000, 10);
+    wls_alloc(indi_du, indi_v, u_min, u_max, Bwls, 0, 0, Wv, 0, 0, 10000, 10, INDI_NUM_ACT, INDI_OUTPUTS);
 
   printf("finished in %d iterations\n", num_iter);
   printf("du = %f, %f, %f, %f\n", indi_du[0], indi_du[1], indi_du[2], indi_du[3]);
@@ -144,7 +144,7 @@ void test_overdetermined(void)
 
   // WLS Control Allocator
   int num_iter =
-    wls_alloc(indi_du, indi_v, du_min, du_max, Bwls, 0, 0, Wv, 0, u_p, 0, 10);
+    wls_alloc(indi_du, indi_v, du_min, du_max, Bwls, 0, 0, Wv, 0, u_p, 0, 10, INDI_NUM_ACT, INDI_OUTPUTS);
 
   printf("finished in %d iterations\n", num_iter);
 
@@ -212,7 +212,7 @@ for (i=0; i< INDI_OUTPUTS;i++) {
 
   // WLS Control Allocator
   int num_iter =
-    wls_alloc(indi_du, indi_v, du_min, du_max, Bwls, 0, 0, Wv, indi_Wu, u_p, 0, 15);
+    wls_alloc(indi_du, indi_v, du_min, du_max, Bwls, 0, 0, Wv, indi_Wu, u_p, 0, 15, INDI_NUM_ACT, INDI_OUTPUTS);
 
   printf("finished in %d iterations\n", num_iter);