From 01ac335eb708f7e2154dcd352801eb1f1c920fc0 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Sun, 1 Oct 2023 23:15:36 +0200 Subject: [PATCH] [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 * Bound defaults to min in case of NaN --------- Co-authored-by: Christophe De Wagter * [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 Co-authored-by: Ewoud Smeur --- conf/airframes/tudelft/nederdrone4.xml | 1 + conf/airframes/tudelft/nederdrone6.xml | 1 + conf/airframes/tudelft/nederdrone7.xml | 1 + conf/airframes/tudelft/nederdrone8.xml | 1 + conf/modules/guidance_indi_hybrid.xml | 12 +- conf/modules/stabilization_indi.xml | 7 +- conf/modules/wls.xml | 27 ++++ .../guidance/guidance_indi_hybrid.c | 116 ++++++++++++++---- .../guidance/guidance_indi_hybrid.h | 1 + .../stabilization/stabilization_indi.c | 12 +- sw/airborne/math/pprz_algebra_float.c | 49 ++++++++ sw/airborne/math/pprz_algebra_float.h | 2 + .../stabilization => math}/wls/wls_alloc.c | 78 ++++++------ .../stabilization => math}/wls/wls_alloc.h | 19 +-- sw/airborne/test/Makefile | 4 +- sw/airborne/test/test_alloc.c | 2 +- 16 files changed, 244 insertions(+), 89 deletions(-) create mode 100644 conf/modules/wls.xml rename sw/airborne/{firmwares/rotorcraft/stabilization => math}/wls/wls_alloc.c (87%) rename sw/airborne/{firmwares/rotorcraft/stabilization => math}/wls/wls_alloc.h (83%) diff --git a/conf/airframes/tudelft/nederdrone4.xml b/conf/airframes/tudelft/nederdrone4.xml index 9f92cd994c..59d29adfac 100644 --- a/conf/airframes/tudelft/nederdrone4.xml +++ b/conf/airframes/tudelft/nederdrone4.xml @@ -110,6 +110,7 @@ + diff --git a/conf/airframes/tudelft/nederdrone6.xml b/conf/airframes/tudelft/nederdrone6.xml index 3c33f0e709..21ca167152 100644 --- a/conf/airframes/tudelft/nederdrone6.xml +++ b/conf/airframes/tudelft/nederdrone6.xml @@ -106,6 +106,7 @@ + diff --git a/conf/airframes/tudelft/nederdrone7.xml b/conf/airframes/tudelft/nederdrone7.xml index db12654be8..79d973c7fa 100644 --- a/conf/airframes/tudelft/nederdrone7.xml +++ b/conf/airframes/tudelft/nederdrone7.xml @@ -106,6 +106,7 @@ + diff --git a/conf/airframes/tudelft/nederdrone8.xml b/conf/airframes/tudelft/nederdrone8.xml index 24a0fec04a..87685104ee 100644 --- a/conf/airframes/tudelft/nederdrone8.xml +++ b/conf/airframes/tudelft/nederdrone8.xml @@ -106,6 +106,7 @@ + diff --git a/conf/modules/guidance_indi_hybrid.xml b/conf/modules/guidance_indi_hybrid.xml index ee95d95959..1535377fa5 100644 --- a/conf/modules/guidance_indi_hybrid.xml +++ b/conf/modules/guidance_indi_hybrid.xml @@ -5,6 +5,11 @@ Guidance controller for hybrids using INDI +
+ + + +
@@ -17,14 +22,15 @@ - - + + + - @navigation,guidance_rotorcraft + @navigation,guidance_rotorcraft,wls guidance,attitude_command
diff --git a/conf/modules/stabilization_indi.xml b/conf/modules/stabilization_indi.xml index b0fec1e2db..276718c1dd 100644 --- a/conf/modules/stabilization_indi.xml +++ b/conf/modules/stabilization_indi.xml @@ -55,7 +55,7 @@ - stabilization_rotorcraft,@attitude_command + stabilization_rotorcraft,@attitude_command,wls commands
@@ -65,15 +65,10 @@ - - - - - diff --git a/conf/modules/wls.xml b/conf/modules/wls.xml new file mode 100644 index 0000000000..ef0393529d --- /dev/null +++ b/conf/modules/wls.xml @@ -0,0 +1,27 @@ + + + + + + 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 + + + + +
+ +
+ + + + + + + + diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c index 07d0a5d0a3..e3b0f7f530 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c +++ b/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) { diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h index c68b9b4e67..c59e7ca8e8 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h +++ b/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 diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c index 114d26a555..cf596b4516 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c +++ b/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 // 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]; diff --git a/sw/airborne/math/pprz_algebra_float.c b/sw/airborne/math/pprz_algebra_float.c index ef0503f7ea..09300d066a 100644 --- a/sw/airborne/math/pprz_algebra_float.c +++ b/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 diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index 7c68805f52..b0b611d2c6 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/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); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.c b/sw/airborne/math/wls/wls_alloc.c similarity index 87% rename from sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.c rename to sw/airborne/math/wls/wls_alloc.c index bac94cced5..b099cb4041 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.c +++ b/sw/airborne/math/wls/wls_alloc.c @@ -36,7 +36,6 @@ */ #include "wls_alloc.h" -#include #include "std.h" #include @@ -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 +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"); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.h b/sw/airborne/math/wls/wls_alloc.h similarity index 83% rename from sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.h rename to sw/airborne/math/wls/wls_alloc.h index 1472913807..5d365fdcee 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.h +++ b/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); diff --git a/sw/airborne/test/Makefile b/sw/airborne/test/Makefile index 805215d199..be0abd38c6 100644 --- a/sw/airborne/test/Makefile +++ b/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) diff --git a/sw/airborne/test/test_alloc.c b/sw/airborne/test/test_alloc.c index 2f285dd0e2..51361a1af4 100644 --- a/sw/airborne/test/test_alloc.c +++ b/sw/airborne/test/test_alloc.c @@ -30,7 +30,7 @@ #include #include "std.h" -#include "firmwares/rotorcraft/stabilization/wls/wls_alloc.h" +#include "math/wls/wls_alloc.h" #define INDI_OUTPUTS 4