WLS test

paparazzi · Sep 30, 2023 · d527ac6 · d527ac6
1 parent 2468706
commit d527ac6
Show file tree

Hide file tree

Showing 2 changed files with 293 additions and 0 deletions.
diff --git a/tests/wls/Makefile b/tests/wls/Makefile
@@ -0,0 +1,10 @@
+
+
+FILES = test_wls.c ../../sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.c ../../sw/airborne/math/qr_solve/qr_solve.c ../../sw/airborne/math/qr_solve/r8lib_min.c
+
+CFLAGS = -g -I ../../sw/airborne -I ../../sw/include -D WLS_VERBOSE=1 -D CA_N_V_INNER=5 -D CA_N_U_INNER=9
+
+
+all:
+	gcc $(CFLAGS) $(FILES) -lm -o test.a
+	./test.a
diff --git a/tests/wls/test_wls.c b/tests/wls/test_wls.c
@@ -0,0 +1,283 @@
+#include <stdio.h>
+#include <math.h>
+
+#include "../../sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.h"
+
+#define INDI_NUM_ACT  9
+#define INDI_OUTPUTS  5
+
+
+void printmat(char* var, float* d, int size) {
+  printf("  ->  %s: %f",var, d[0]);
+  for (int i=1;i<size;i++) {
+    printf(",%f",d[i]);
+  }
+  printf("\n");
+}
+
+
+int checknan(float* d, int size) {
+  for (int i=0; i<size; i++) {
+    if (isnan(d[i])) {
+      return 1;
+    }
+  }
+  return 0;
+}
+
+
+int stabilization(int plot, int fix) {
+
+  ///////////////////////////////////
+  // Stabilization
+
+  float indi_u[INDI_NUM_ACT];
+  float indi_du[INDI_NUM_ACT];
+
+/* GOOD */
+/*
+  float indi_v[5] = {-0.331298,26.089878,11.914940,-6.518403,0.669281};
+  float du_min_stab_indi[INDI_NUM_ACT] = {-9597.776367,-9281.334961,-1247.192017,-6648.239258,-16160.971680, 2000, 0, 0, 0};
+  float du_max_stab_indi[INDI_NUM_ACT] = {2.223633,318.665039,8352.807617,2951.760742,3039.028320, 100, 100, 2000, 20};
+  float g1[INDI_NUM_ACT] = {0.000000,-0.014209,0.000000,0.011393,0.000000,0.000000,0.000000,0.000000,0.000000};
+  float g2[INDI_NUM_ACT] = {0.001500,0.000031,-0.001500,-0.000031,0.000000,0.001792,0.000000,0.000000,0.000000};
+  float g3[INDI_NUM_ACT] = {-0.000300,0.000300,-0.000300,0.000300,0.000220,0.000000,0.000000,0.000000,0.000000};
+  float g4[INDI_NUM_ACT] = {-0.000575,-0.000575,-0.000575,-0.000575,0.000000,0.000000,0.000000,0.000000,0.000000};
+  float g5[INDI_NUM_ACT] = {0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000797};
+  float Wv[INDI_OUTPUTS] = {1000.000000,1000.000000,1.000000,100.000000,100.000000};
+  float indi_Wu[INDI_NUM_ACT] = {1.000000,1.000000,1.000000,1.000000,1.000000,1.000000,1.000000,1.000000,1.000000};
+  float du_pref_stab_indi[INDI_NUM_ACT] = {-9597.776367,-9281.334961,-1247.192017,-6648.239258,-6560.971680,0.000000,-835.022156,-787.110168,-9598.166016};
+ */
+/* BAD: 
+p-indi_du:-nan,-nan,-nan,-nan,-nan,nan,-nan,-nan,-nan,
+-> indi_du:0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+-> indi_v:-0.448624,0.055364,-0.357490,0.056721,0.000000,
+-> du_min_stab_indi:-0.000000,-0.056307,-0.000014,-98.589226,-6783.355469,-4788.924316,-9600.000000,-88.000000,-0.000171,
+-> du_max_stab_indi:9600.000000,9599.943359,9600.000000,9501.411133,12416.644531,4811.075684,-87.514648,-94.110352,9600.000000,
+-> Bwls[]:0.000000,-0.013301,0.000000,0.005984,0.000000,0.000000,0.000027,0.000020,0.000000,
+-> Bwls[]:0.001500,0.000222,-0.001500,-0.000222,0.000000,0.003032,0.000000,0.000000,0.000000,
+-> Bwls[]:-0.000300,0.000300,-0.000300,0.000300,0.000050,0.000000,0.000000,0.000000,0.000000,
+-> Bwls[]:-0.000575,-0.000575,-0.000575,-0.000575,0.000000,0.000000,0.000000,0.000000,0.000000,
+-> Bwls[]:0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000300,
+-> Wv:1000.000000,1000.000000,1.000000,100.000000,100.000000,
+-> indi_Wu:1.000000,1.000000,1.000000,1.000000,1.000000,1.000000,1.000000,1.000000,1.000000,
+-> du_pref_stab_indi:-0.000000,-0.056307,-0.000014,-98.589226,2816.644287,0.000000,-9687.514648,-9694.110352,-0.000171,
+ */
+
+  float indi_v[5] = {-0.448624,0.055364,-0.357490,0.056721,0.000000};
+  float du_min_stab_indi[INDI_NUM_ACT] = {-0.000000,-0.056307,-0.000014,-98.589226,-6783.355469,-4788.924316,-9600.000000,-88.000000,-0.000171};
+  float du_max_stab_indi[INDI_NUM_ACT] = {9600.000000,9599.943359,9600.000000,9501.411133,12416.644531,4811.075684,-87.514648,-94.110352,9600.000000};
+  float g1[INDI_NUM_ACT] = {0.000000,-0.013301,0.000000,0.005984,0.000000,0.000000,0.000027,0.000020,0.000000};
+  float g2[INDI_NUM_ACT] = {0.001500,0.000222,-0.001500,-0.000222,0.000000,0.003032,0.000000,0.000000,0.000000};
+  float g3[INDI_NUM_ACT] = {-0.000300,0.000300,-0.000300,0.000300,0.000050,0.000000,0.000000,0.000000,0.000000};
+  float g4[INDI_NUM_ACT] = {-0.000575,-0.000575,-0.000575,-0.000575,0.000000,0.000000,0.000000,0.000000,0.000000};
+  float g5[INDI_NUM_ACT] = {0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000300};
+  float Wv[INDI_OUTPUTS] = {1000.000000,1000.000000,1.000000,100.000000,100.000000};
+  float indi_Wu[INDI_NUM_ACT] = {1.000000,1.000000,1.000000,1.000000,1.000000,1.000000,1.000000,1.000000,1.000000};
+  float du_pref_stab_indi[INDI_NUM_ACT] = {-0.000000,-0.056307,-0.000014,-98.589226,2816.644287,0.000000,-9687.514648,-9694.110352,-0.000171};
+
+
+if (fix > 0) {
+  for (int i=0; i<INDI_NUM_ACT; i++) {
+    if (du_min_stab_indi[i] >= du_max_stab_indi[i]) {
+      if (plot > 0)
+        printf("MIN >= MAX: %d: %f | %f\n",i, du_min_stab_indi[i], du_max_stab_indi[i]);
+      //du_min_stab_indi[i] = du_max_stab_indi[i];
+    }
+    if (du_pref_stab_indi[i] < du_min_stab_indi[i]) {
+      if (plot > 0)
+        printf("PREF < MIN: %d\n",i);
+      du_pref_stab_indi[i] = du_min_stab_indi[i];
+    }
+    if (du_pref_stab_indi[i] > du_max_stab_indi[i]) {
+      if (plot > 0)
+        printf("PREF > MAX: %d\n",i);
+      du_pref_stab_indi[i] = du_max_stab_indi[i];
+    }
+  }
+}
+
+float* Bwls[INDI_OUTPUTS] = {g1,g2,g3,g4,g5};
+
+  if (plot > 0) {
+
+    printmat("indi_v", indi_v, INDI_OUTPUTS);
+    printmat("du_min_stab_indi", du_min_stab_indi, INDI_NUM_ACT);
+    printmat("du_max_stab_indi", du_max_stab_indi, INDI_NUM_ACT);
+    for (int i=0;i<INDI_OUTPUTS;i++) {
+      printmat("Bwls[]", Bwls[i], INDI_NUM_ACT);
+    }
+    printmat("Wv", Wv, INDI_OUTPUTS);
+    printmat("indi_Wu", indi_Wu, INDI_NUM_ACT);
+    printmat("du_pref_stab_indi", du_pref_stab_indi, INDI_NUM_ACT);
+  }
+
+  int n_u = INDI_NUM_ACT;
+  int n_v = INDI_OUTPUTS;
+
+
+  // WLS Control Allocator
+  int 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, n_u, n_v);
+
+  if (plot > 0) {
+    printf("Iterations=%i\n", num_iter);
+    printmat("indi_du", indi_du, INDI_NUM_ACT);
+  }
+
+  int test = checknan(indi_du, INDI_NUM_ACT);
+
+  return test;
+}
+
+
+
+
+
+
+int guidance(int plot, int fix) {
+  ///////////////////////////////////
+  // Stabilization
+
+  #define GUIDANCE_ACT 4
+  #define GUIDANCE_OUT 3
+
+
+
+  float indi_u[GUIDANCE_ACT];
+  float indi_du[GUIDANCE_ACT];
+
+  /* BAD GUIDANCE 1 
+GUID-p-guidance_indi_du:-nan,nan,nan,-nan,
+/*
+  float indi_v[GUIDANCE_OUT] = {-0.151431,-0.054205,-0.024687};
+  float du_min_stab_indi[GUIDANCE_ACT] = {-0.719067,-0.406864,-0.000000,-0.139885};
+  float du_max_stab_indi[GUIDANCE_ACT] = {0.328130,0.151642,0.001961,9.000000};
+  float g1[GUIDANCE_ACT] = {0.116995,-10.132722,0.057489,0.998282};
+  float g2[GUIDANCE_ACT] = {10.150160,10.734044,-0.193029,0.000000};
+  float g3[GUIDANCE_ACT] = {1.993394,-54.562763,0.979508,-0.058591};
+  float Wv[GUIDANCE_OUT] = {170.000000,10.000000,10.000000};
+  float indi_Wu[GUIDANCE_ACT] = {104.139999,302.829987,4.382000,30.000000};
+  float du_pref_stab_indi[GUIDANCE_ACT] = {0.328130,0.151642,0.001961,9.000000};
+*/
+
+/* BAD GUIDANCE 2
+GUID-p-guidance_indi_du:-nan,nan,nan,-nan,
+*/
+  /* BAD 2 */
+  /*
+  float indi_v[GUIDANCE_OUT] = {-0.166235,-0.076728,-0.002801};
+  float du_min_stab_indi[GUIDANCE_ACT] = {-0.729811,-0.417911,-0.000000,-0.164344};
+  float du_max_stab_indi[GUIDANCE_ACT] = {0.317387,0.140594,0.000849,9.000000};
+  float g1[GUIDANCE_ACT] = {0.145615,-10.088613,0.068091,0.997579};
+  float g2[GUIDANCE_ACT] = {10.113091,11.552245,-0.202768,0.000000};
+  float g3[GUIDANCE_ACT] = {2.089039,-55.789337,0.976857,-0.069536};
+  float Wv[GUIDANCE_OUT] = {170.000000,10.000000,10.000000};
+  float indi_Wu[GUIDANCE_ACT] = {104.139999,302.829987,4.382000,30.000000};
+  float du_pref_stab_indi[GUIDANCE_ACT] = {0.317387,0.140594,0.000849,9.000000};
+  */
+
+/* BAD GUIDANCE 3: SEGFAULT
+GUID-p-guidance_indi_du:-nan,nan,nan,-nan,
+*/
+  float indi_v[GUIDANCE_OUT] = {-0.127563,-0.211419,-0.270193};
+  float du_min_stab_indi[GUIDANCE_ACT] = {-0.520696,-0.399549,-0.000000,-0.069723};
+  float du_max_stab_indi[GUIDANCE_ACT] = {0.526502,0.158956,0.000090,9.000000};
+  float g1[GUIDANCE_ACT] = {-0.003253,-9.998791,0.051375,0.998679};
+  float g2[GUIDANCE_ACT] = {10.012013,-0.359303,0.006325,0.000000};
+  float g3[GUIDANCE_ACT] = {-0.063240,-56.808598,0.998659,-0.051376};
+  float Wv[GUIDANCE_OUT] = {170.000000,10.000000,10.000000};
+  float indi_Wu[GUIDANCE_ACT] = {104.139999,302.829987,4.382000,30.000000};
+  float du_pref_stab_indi[GUIDANCE_ACT] = {0.326502,0.158956,0.000090,9.000000};
+
+
+/*
+Dennis:
+
+  float indi_v[GUIDANCE_OUT] = {0.005557,0.423254,0.088190};
+  float du_min_stab_indi[GUIDANCE_ACT] = {-0.502626,-0.435115,-0.000000,-0.008926};
+  float du_max_stab_indi[GUIDANCE_ACT] = {0.544572,0.123391,0.000011,9.000000};
+  float g1[GUIDANCE_ACT] = {-0.011988,-9.948572,0.086566,0.996245};
+  float g2[GUIDANCE_ACT] = {9.986066,-0.787544,0.013865,0.000000};
+  float g3[GUIDANCE_ACT] = {-0.137950,-56.795391,0.996150,-0.086574};
+  float Wv[GUIDANCE_OUT] = {170.000000,10.000000,10.000000};
+  float indi_Wu[GUIDANCE_ACT] = {104.139999,302.829987,4.382000,30.000000};
+  float du_pref_stab_indi[GUIDANCE_ACT] = {0.000000,-0.033691,0.000011,0.005557};
+*/
+
+
+if (fix > 0) {
+  for (int i=0; i<GUIDANCE_ACT; i++) {
+    if (du_min_stab_indi[i] >= du_max_stab_indi[i]) {
+      if (plot > 0)
+        printf("MIN >= MAX: %d: %f | %f\n",i, du_min_stab_indi[i], du_max_stab_indi[i]);
+      du_min_stab_indi[i] = du_max_stab_indi[i];
+    }
+    if (du_pref_stab_indi[i] < du_min_stab_indi[i]) {
+      if (plot > 0)
+        printf("PREF < MIN: %d\n",i);
+      du_pref_stab_indi[i] = du_min_stab_indi[i];
+    }
+    if (du_pref_stab_indi[i] > du_max_stab_indi[i]) {
+      if (plot > 0)
+        printf("PREF > MAX: %d\n",i);
+      du_pref_stab_indi[i] = du_max_stab_indi[i];
+    }
+  }
+}
+
+float* Bwls[GUIDANCE_OUT] = {g1,g2,g3};
+
+  if (plot > 0) {
+
+    printmat("indi_v", indi_v, GUIDANCE_OUT);
+    printmat("du_min_stab_indi", du_min_stab_indi, GUIDANCE_ACT);
+    printmat("du_max_stab_indi", du_max_stab_indi, GUIDANCE_ACT);
+    for (int i=0;i<GUIDANCE_OUT;i++) {
+      printmat("Bwls[]", Bwls[i], GUIDANCE_ACT);
+    }
+    printmat("Wv", Wv, GUIDANCE_OUT);
+    printmat("indi_Wu", indi_Wu, GUIDANCE_ACT);
+    printmat("du_pref_stab_indi", du_pref_stab_indi, GUIDANCE_ACT);
+  }
+
+  int n_u = GUIDANCE_ACT;
+  int n_v = GUIDANCE_OUT;
+
+
+  // WLS Control Allocator
+  int 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, n_u, n_v);
+
+  if (plot > 0) {
+    printf("Iterations=%i\n", num_iter);
+    printmat("indi_du", indi_du, GUIDANCE_ACT);
+  }
+
+  int test = checknan(indi_du, GUIDANCE_ACT);
+
+  return test;
+}
+
+int main(void) {
+  printf(" ==== TEST ====\n");
+
+
+  printf(" ----------------------- STABILIZAION -----------------\n");
+  stabilization(1, 0);
+
+  printf(" ----------------------- GUIDANCE -----------------\n");
+  guidance(1, 0);
+
+  return 0;
+
+
+  int good = 0;
+  int total = 0;
+
+
+  for (;total<10;total++) {
+    good += stabilization(0,1);
+  }
+
+  printf("Failed: %d out of %d times \n", good, total);
+  return 0;
+}