diff --git a/conf/airframes/ENAC/hybrid/cyfoam.xml b/conf/airframes/ENAC/hybrid/cyfoam.xml index a9635ae625..b11125cf58 100644 --- a/conf/airframes/ENAC/hybrid/cyfoam.xml +++ b/conf/airframes/ENAC/hybrid/cyfoam.xml @@ -75,8 +75,8 @@ - - + + diff --git a/conf/airframes/ENAC/quadrotor/anton_indi_aruco.xml b/conf/airframes/ENAC/quadrotor/anton_indi_aruco.xml index 7c2b17d61f..81a696112b 100644 --- a/conf/airframes/ENAC/quadrotor/anton_indi_aruco.xml +++ b/conf/airframes/ENAC/quadrotor/anton_indi_aruco.xml @@ -43,8 +43,8 @@ - - + + diff --git a/conf/airframes/ENAC/quadrotor/robobee.xml b/conf/airframes/ENAC/quadrotor/robobee.xml index a4c6e9347f..2a3ca18a78 100644 --- a/conf/airframes/ENAC/quadrotor/robobee.xml +++ b/conf/airframes/ENAC/quadrotor/robobee.xml @@ -47,8 +47,8 @@ - - + + diff --git a/conf/airframes/tudelft/bebop_indi_actuators.xml b/conf/airframes/tudelft/bebop_indi_actuators.xml index 8dbeaa5ea6..b46d7e3775 100644 --- a/conf/airframes/tudelft/bebop_indi_actuators.xml +++ b/conf/airframes/tudelft/bebop_indi_actuators.xml @@ -34,8 +34,8 @@ - - + + diff --git a/conf/airframes/tudelft/bebop_one_indi_generated_ekf2.xml b/conf/airframes/tudelft/bebop_one_indi_generated_ekf2.xml index 2cb4e949a7..6e5dc52a99 100644 --- a/conf/airframes/tudelft/bebop_one_indi_generated_ekf2.xml +++ b/conf/airframes/tudelft/bebop_one_indi_generated_ekf2.xml @@ -49,8 +49,8 @@ - - + + diff --git a/conf/airframes/tudelft/bebop_oneloop_ekf2.xml b/conf/airframes/tudelft/bebop_oneloop_ekf2.xml index 76f62c77e1..73f55a3e6e 100644 --- a/conf/airframes/tudelft/bebop_oneloop_ekf2.xml +++ b/conf/airframes/tudelft/bebop_oneloop_ekf2.xml @@ -43,8 +43,8 @@ - - + + diff --git a/conf/airframes/tudelft/bebop_oneloop_generated_ekf2.xml b/conf/airframes/tudelft/bebop_oneloop_generated_ekf2.xml index c1a575dc75..abcf00b8e6 100644 --- a/conf/airframes/tudelft/bebop_oneloop_generated_ekf2.xml +++ b/conf/airframes/tudelft/bebop_oneloop_generated_ekf2.xml @@ -44,8 +44,8 @@ - - + + diff --git a/conf/airframes/tudelft/bebop_opticflow_indoor_2x_30hz.xml b/conf/airframes/tudelft/bebop_opticflow_indoor_2x_30hz.xml index 9a88117bfc..5ad0bc64d5 100644 --- a/conf/airframes/tudelft/bebop_opticflow_indoor_2x_30hz.xml +++ b/conf/airframes/tudelft/bebop_opticflow_indoor_2x_30hz.xml @@ -27,8 +27,8 @@ pyramid level 2: 21 fps average, min=11fps - - + + diff --git a/conf/airframes/tudelft/bebop_optitrack.xml b/conf/airframes/tudelft/bebop_optitrack.xml index 2d26323920..61f900c4d2 100644 --- a/conf/airframes/tudelft/bebop_optitrack.xml +++ b/conf/airframes/tudelft/bebop_optitrack.xml @@ -18,8 +18,8 @@ - - + + diff --git a/conf/airframes/tudelft/cyfoam.xml b/conf/airframes/tudelft/cyfoam.xml index 6ef939c78d..15f3c14ac8 100644 --- a/conf/airframes/tudelft/cyfoam.xml +++ b/conf/airframes/tudelft/cyfoam.xml @@ -289,8 +289,8 @@ - - + + diff --git a/conf/airframes/tudelft/disco_rotorcraft_indi.xml b/conf/airframes/tudelft/disco_rotorcraft_indi.xml index 57e0d822a4..4487821971 100644 --- a/conf/airframes/tudelft/disco_rotorcraft_indi.xml +++ b/conf/airframes/tudelft/disco_rotorcraft_indi.xml @@ -56,8 +56,8 @@ - - + + diff --git a/conf/airframes/tudelft/fan_demo.xml b/conf/airframes/tudelft/fan_demo.xml index 95f12fb160..9d853afdbd 100644 --- a/conf/airframes/tudelft/fan_demo.xml +++ b/conf/airframes/tudelft/fan_demo.xml @@ -20,8 +20,8 @@ - - + + diff --git a/conf/airframes/tudelft/nederdrone4.xml b/conf/airframes/tudelft/nederdrone4.xml index 06c4731c45..c6b3ecabb3 100644 --- a/conf/airframes/tudelft/nederdrone4.xml +++ b/conf/airframes/tudelft/nederdrone4.xml @@ -111,8 +111,8 @@ - - + + diff --git a/conf/airframes/tudelft/nederdrone6.xml b/conf/airframes/tudelft/nederdrone6.xml index 09937aafa1..2b70fec5c8 100644 --- a/conf/airframes/tudelft/nederdrone6.xml +++ b/conf/airframes/tudelft/nederdrone6.xml @@ -106,8 +106,8 @@ - - + + diff --git a/conf/airframes/tudelft/nederdrone7.xml b/conf/airframes/tudelft/nederdrone7.xml index b915e910c0..04578a905e 100644 --- a/conf/airframes/tudelft/nederdrone7.xml +++ b/conf/airframes/tudelft/nederdrone7.xml @@ -106,8 +106,8 @@ - - + + diff --git a/conf/airframes/tudelft/nederdrone8.xml b/conf/airframes/tudelft/nederdrone8.xml index f8709d5b52..6cba175f6f 100644 --- a/conf/airframes/tudelft/nederdrone8.xml +++ b/conf/airframes/tudelft/nederdrone8.xml @@ -106,8 +106,8 @@ - - + + diff --git a/conf/airframes/tudelft/rot_wing_25kg.xml b/conf/airframes/tudelft/rot_wing_25kg.xml index 398774817c..6a7a911fe6 100644 --- a/conf/airframes/tudelft/rot_wing_25kg.xml +++ b/conf/airframes/tudelft/rot_wing_25kg.xml @@ -118,8 +118,8 @@ - - + + diff --git a/conf/airframes/tudelft/rot_wing_v3b.xml b/conf/airframes/tudelft/rot_wing_v3b.xml index be10dabece..d67704e30c 100644 --- a/conf/airframes/tudelft/rot_wing_v3b.xml +++ b/conf/airframes/tudelft/rot_wing_v3b.xml @@ -121,8 +121,8 @@ - - + + diff --git a/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml b/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml index 6576ba46d6..ca2e691818 100644 --- a/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml +++ b/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml @@ -131,8 +131,8 @@ - - + + diff --git a/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack.xml b/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack.xml index d3407db036..ad4294202a 100644 --- a/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack.xml +++ b/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack.xml @@ -132,8 +132,8 @@ - - + + diff --git a/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml b/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml index c44df9c52b..48c7a223c5 100644 --- a/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml +++ b/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml @@ -124,8 +124,8 @@ - - + + diff --git a/conf/airframes/tudelft/rot_wing_v3c_oneloop_simulation.xml b/conf/airframes/tudelft/rot_wing_v3c_oneloop_simulation.xml index 8e92a1a627..2d884eb6b5 100644 --- a/conf/airframes/tudelft/rot_wing_v3c_oneloop_simulation.xml +++ b/conf/airframes/tudelft/rot_wing_v3c_oneloop_simulation.xml @@ -92,8 +92,8 @@ - - + + diff --git a/conf/airframes/tudelft/rot_wing_v3d.xml b/conf/airframes/tudelft/rot_wing_v3d.xml index d88f0a6700..3444bdf03c 100644 --- a/conf/airframes/tudelft/rot_wing_v3d.xml +++ b/conf/airframes/tudelft/rot_wing_v3d.xml @@ -108,8 +108,8 @@ - - + + diff --git a/conf/airframes/tudelft/rot_wing_v3e.xml b/conf/airframes/tudelft/rot_wing_v3e.xml index 9d582c4fa1..f04184fe83 100644 --- a/conf/airframes/tudelft/rot_wing_v3e.xml +++ b/conf/airframes/tudelft/rot_wing_v3e.xml @@ -108,8 +108,8 @@ - - + + diff --git a/conf/airframes/tudelft/rot_wing_v3f.xml b/conf/airframes/tudelft/rot_wing_v3f.xml index 93799faa67..f9561dc418 100644 --- a/conf/airframes/tudelft/rot_wing_v3f.xml +++ b/conf/airframes/tudelft/rot_wing_v3f.xml @@ -108,8 +108,8 @@ - - + + diff --git a/conf/airframes/tudelft/rot_wing_v3g.xml b/conf/airframes/tudelft/rot_wing_v3g.xml index 7200db5d17..3477c55b33 100644 --- a/conf/airframes/tudelft/rot_wing_v3g.xml +++ b/conf/airframes/tudelft/rot_wing_v3g.xml @@ -113,8 +113,8 @@ - - + + diff --git a/conf/airframes/tudelft/rot_wing_v3h.xml b/conf/airframes/tudelft/rot_wing_v3h.xml index 26243b110f..0518e6fab2 100644 --- a/conf/airframes/tudelft/rot_wing_v3h.xml +++ b/conf/airframes/tudelft/rot_wing_v3h.xml @@ -113,8 +113,8 @@ - - + + diff --git a/conf/airframes/tudelft/splash4.xml b/conf/airframes/tudelft/splash4.xml index a6027c8ae4..e9e4ce87ad 100644 --- a/conf/airframes/tudelft/splash4.xml +++ b/conf/airframes/tudelft/splash4.xml @@ -87,8 +87,8 @@ --> - - + + diff --git a/conf/modules/wls.xml b/conf/modules/wls.xml index ef0393529d..ce397c51fc 100644 --- a/conf/modules/wls.xml +++ b/conf/modules/wls.xml @@ -11,8 +11,8 @@ The size of the matrix (output vs. objectives) must be defined large enough for all the controllers - - + +
diff --git a/conf/telemetry/highspeed_rotorcraft.xml b/conf/telemetry/highspeed_rotorcraft.xml index a25ca6910c..d70f85faf2 100644 --- a/conf/telemetry/highspeed_rotorcraft.xml +++ b/conf/telemetry/highspeed_rotorcraft.xml @@ -60,6 +60,8 @@ + + @@ -260,6 +262,8 @@ + + diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c index 4fd71572a0..e5562815c2 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c @@ -95,12 +95,12 @@ float guidance_indi_pitch_pref_deg = 0; // If using WLS, check that the matrix size is sufficient #if GUIDANCE_INDI_HYBRID_USE_WLS -#if GUIDANCE_INDI_HYBRID_U > WLS_N_U -#error Matrix-WLS_N_U too small: increase WLS_N_U in airframe file +#if GUIDANCE_INDI_HYBRID_U > WLS_N_U_MAX +#error Matrix-WLS_N_U_MAX too small: increase WLS_N_U_MAX in airframe file #endif -#if GUIDANCE_INDI_HYBRID_V > WLS_N_V -#error Matrix-WLS_N_V too small: increase WLS_N_V in airframe file +#if GUIDANCE_INDI_HYBRID_V > WLS_N_V_MAX +#error Matrix-WLS_N_V_MAX too small: increase WLS_N_V_MAX in airframe file #endif #endif @@ -200,22 +200,30 @@ float du_gih[GUIDANCE_INDI_HYBRID_U]; // = {0.0f, 0.0f, 0.0f}; #if GUIDANCE_INDI_HYBRID_USE_WLS #include "math/wls/wls_alloc.h" -float du_min_gih[GUIDANCE_INDI_HYBRID_U]; -float du_max_gih[GUIDANCE_INDI_HYBRID_U]; -float du_pref_gih[GUIDANCE_INDI_HYBRID_U]; float *Bwls_gih[GUIDANCE_INDI_HYBRID_V]; +struct WLS_t wls_guid_p = { + .nu = GUIDANCE_INDI_HYBRID_U, + .nv = GUIDANCE_INDI_HYBRID_V, + .gamma_sq = 100000.0, + .v = {0.0}, #ifdef GUIDANCE_INDI_WLS_PRIORITIES -float Wv_gih[GUIDANCE_INDI_HYBRID_V] = GUIDANCE_INDI_WLS_PRIORITIES; + .Wv = GUIDANCE_INDI_WLS_PRIORITIES, +#else // X,Y accel, Z accel + .Wv = { 100.f, 100.f, 1.f }, +#endif +#ifdef GUIDANCE_INDI_WLS_WU + .Wu = GUIDANCE_INDI_WLS_WU, #else -float Wv_gih[GUIDANCE_INDI_HYBRID_V] = { 100.f, 100.f, 1.f }; // X,Y accel, Z accel + .Wu = {[0 ... GUIDANCE_INDI_HYBRID_U - 1] = 1.0}, #endif -#ifdef GUIDANCE_INDI_WLS_WU -float Wu_gih[GUIDANCE_INDI_HYBRID_U] = GUIDANCE_INDI_WLS_WU; -#else -float Wu_gih[GUIDANCE_INDI_HYBRID_U] = { 1.f, 1.f, 1.f }; + .u_pref = {0.0}, + .u_min = {0.0}, + .u_max = {0.0}, + .PC = 0.0, + .SC = 0.0, + .iter = 0 +}; #endif -#endif - // The control objective float v_gih[3]; @@ -260,45 +268,13 @@ static void send_guidance_indi_hybrid(struct transport_tx *trans, struct link_de } #if GUIDANCE_INDI_HYBRID_USE_WLS -static void debug(struct transport_tx *trans, struct link_device *dev, char* name, float* data, int datasize) +static void send_wls_v_guid(struct transport_tx *trans, struct link_device *dev) { - pprz_msg_send_DEBUG_VECT(trans, dev,AC_ID, - strlen(name), name, - datasize, data); + send_wls_v("guid", &wls_guid_p, trans, dev); } - -static void send_guidance_indi_debug(struct transport_tx *trans, struct link_device *dev) -{ - static int c = 0; - switch (c++) - { - case 0: - debug(trans, dev, "v_gih", v_gih, 3); - break; - case 1: - debug(trans, dev, "du_min_gih", du_min_gih, GUIDANCE_INDI_HYBRID_U); - break; - case 2: - debug(trans, dev, "du_max_gih", du_max_gih, GUIDANCE_INDI_HYBRID_U); - break; - case 3: - debug(trans, dev, "du_pref_gih", du_pref_gih, GUIDANCE_INDI_HYBRID_U); - break; - case 4: - debug(trans, dev, "Wu_gih", Wu_gih, GUIDANCE_INDI_HYBRID_U); - break; - case 5: - debug(trans, dev, "Wv_gih", Wv_gih, GUIDANCE_INDI_HYBRID_V); - break; - default: - debug(trans, dev, "Bwls_gih[0]", Bwls_gih[0], GUIDANCE_INDI_HYBRID_U); - c=0; - break; - } -} -#else -static void send_guidance_indi_debug(struct transport_tx *trans UNUSED, struct link_device *dev UNUSED) +static void send_wls_u_guid(struct transport_tx *trans, struct link_device *dev) { + send_wls_u("guid", &wls_guid_p, trans, dev); } #endif // GUIDANCE_INDI_HYBRID_USE_WLS @@ -341,7 +317,10 @@ void guidance_indi_init(void) #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE_INDI_HYBRID, send_guidance_indi_hybrid); - register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DEBUG_VECT, send_guidance_indi_debug); +#if GUIDANCE_INDI_HYBRID_USE_WLS + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WLS_V, send_wls_v_guid); + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WLS_U, send_wls_u_guid); +#endif #endif } @@ -443,11 +422,13 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa float du_gih[GUIDANCE_INDI_HYBRID_U]; // = {0.0f, 0.0f, 0.0f}; - 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, - GUIDANCE_INDI_HYBRID_U, GUIDANCE_INDI_HYBRID_V); - + for (int i = 0; i < GUIDANCE_INDI_HYBRID_V; i++) { + wls_guid_p.v[i] = v_gih[i]; + } + wls_alloc(&wls_guid_p, Bwls_gih, 0, 0, 10); + for (int i = 0; i < GUIDANCE_INDI_HYBRID_U; i++) { + du_gih[i] = wls_guid_p.u[i]; + } 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/guidance/guidance_indi_hybrid.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h index ae0c3cb0e1..7385d1fff3 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h @@ -51,6 +51,8 @@ extern float guidance_indi_get_liftd(float pitch, float theta); extern void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U], struct FloatVect3 a_diff, float v_body[GUIDANCE_INDI_HYBRID_V]); #if GUIDANCE_INDI_HYBRID_USE_WLS +#include "math/wls/wls_alloc.h" +extern struct WLS_t wls_guid_p; extern void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle); #endif diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c index a482572437..273cc93fe0 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c @@ -133,22 +133,22 @@ void WEAK guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angl float pitch_pref_rad = RadOfDeg(guidance_indi_pitch_pref_deg); // Set lower limits - du_min_gih[0] = -guidance_indi_max_bank - roll_angle; //roll - du_min_gih[1] = min_pitch_limit_rad - pitch_angle; // pitch - du_min_gih[2] = (MAX_PPRZ - stabilization.cmd[COMMAND_THRUST]) * guidance_indi_thrust_z_eff; - du_min_gih[3] = -stabilization.cmd[COMMAND_THRUST_X]*g1g2[4][GUIDANCE_INDI_PUSHER_INDEX]; + wls_guid_p.u_min[0] = -guidance_indi_max_bank - roll_angle; //roll + wls_guid_p.u_min[1] = min_pitch_limit_rad - pitch_angle; // pitch + wls_guid_p.u_min[2] = (MAX_PPRZ - stabilization.cmd[COMMAND_THRUST]) * guidance_indi_thrust_z_eff; + wls_guid_p.u_min[3] = -stabilization.cmd[COMMAND_THRUST_X]*g1g2[4][GUIDANCE_INDI_PUSHER_INDEX]; // Set upper limits limits - du_max_gih[0] = guidance_indi_max_bank - roll_angle; //roll - du_max_gih[1] = max_pitch_limit_rad - pitch_angle; // pitch - du_max_gih[2] = -stabilization.cmd[COMMAND_THRUST] * guidance_indi_thrust_z_eff; - du_max_gih[3] = (MAX_PPRZ - stabilization.cmd[COMMAND_THRUST_X])*g1g2[4][GUIDANCE_INDI_PUSHER_INDEX]; + wls_guid_p.u_max[0] = guidance_indi_max_bank - roll_angle; //roll + wls_guid_p.u_max[1] = max_pitch_limit_rad - pitch_angle; // pitch + wls_guid_p.u_max[2] = -stabilization.cmd[COMMAND_THRUST] * guidance_indi_thrust_z_eff; + wls_guid_p.u_max[3] = (MAX_PPRZ - stabilization.cmd[COMMAND_THRUST_X])*g1g2[4][GUIDANCE_INDI_PUSHER_INDEX]; // Set prefered states - du_pref_gih[0] = -roll_angle; // prefered delta roll angle - du_pref_gih[1] = -pitch_angle + pitch_pref_rad;// prefered delta pitch angle - du_pref_gih[2] = du_max_gih[2]; // Low thrust better for efficiency - du_pref_gih[3] = body_v[0]; // solve the body acceleration + wls_guid_p.u_pref[0] = -roll_angle; // prefered delta roll angle + wls_guid_p.u_pref[1] = -pitch_angle + pitch_pref_rad;// prefered delta pitch angle + wls_guid_p.u_pref[2] = wls_guid_p.u_max[2]; // Low thrust better for efficiency + wls_guid_p.u_pref[3] = body_v[0]; // solve the body acceleration } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitter.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitter.c index c4abe22813..e7bdf98fb7 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitter.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitter.c @@ -89,19 +89,19 @@ void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_H void guidance_indi_hybrid_set_wls_settings(float body_v[3] UNUSED, float roll_angle, float pitch_angle) { // Set lower limits - du_min_gih[0] = -guidance_indi_max_bank - roll_angle; // roll - du_min_gih[1] = RadOfDeg(guidance_indi_min_pitch) - pitch_angle; // 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]; + wls_guid_p.u_min[0] = -guidance_indi_max_bank - roll_angle; // roll + wls_guid_p.u_min[1] = RadOfDeg(guidance_indi_min_pitch) - pitch_angle; // pitch + wls_guid_p.u_min[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_angle; //roll - du_max_gih[1] = RadOfDeg(GUIDANCE_INDI_MAX_PITCH) - pitch_angle; // 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]); + wls_guid_p.u_max[0] = guidance_indi_max_bank - roll_angle; //roll + wls_guid_p.u_max[1] = RadOfDeg(GUIDANCE_INDI_MAX_PITCH) - pitch_angle; // pitch + wls_guid_p.u_max[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_angle; // prefered delta roll angle - du_pref_gih[1] = -pitch_angle; // prefered delta pitch angle - du_pref_gih[2] = du_max_gih[2]; + wls_guid_p.u_pref[0] = -roll_angle; // prefered delta roll angle + wls_guid_p.u_pref[1] = -pitch_angle; // prefered delta pitch angle + wls_guid_p.u_pref[2] = wls_guid_p.u_max[2]; } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c index 45c10a94f8..f3e2d1079b 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c @@ -96,20 +96,41 @@ #if !STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE -#if INDI_NUM_ACT > WLS_N_U -#error Matrix-WLS_N_U too small or not defined: define WLS_N_U >= INDI_NUM_ACT in airframe file +#if INDI_NUM_ACT > WLS_N_U_MAX +#error Matrix-WLS_N_U_MAX too small or not defined: define WLS_N_U_MAX >= INDI_NUM_ACT in airframe file #endif -#if INDI_OUTPUTS > WLS_N_V -#error Matrix-WLS_N_V too small or not defined: define WLS_N_U >= INDI_OUTPUTS in airframe file +#if INDI_OUTPUTS > WLS_N_V_MAX +#error Matrix-WLS_N_V_MAX too small or not defined: define WLS_N_V_MAX >= INDI_OUTPUTS in airframe file #endif +struct WLS_t wls_stab_p = { + .nu = INDI_NUM_ACT, + .nv = INDI_OUTPUTS, + .gamma_sq = 10000.0, + .v = {0.0}, +#ifdef STABILIZATION_INDI_WLS_PRIORITIES + .Wv = STABILIZATION_INDI_WLS_PRIORITIES, +#else //State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST} +#if INDI_OUTPUTS == 5 + .Wv = {1000, 1000, 1, 100, 100}, +#else + .Wv = {1000, 1000, 1, 100}, +#endif +#endif +#ifdef STABILIZATION_INDI_WLS_WU //Weighting of different actuators in the cost function + .Wu = STABILIZATION_INDI_WLS_WU, +#else + .Wu = {[0 ... INDI_NUM_ACT - 1] = 1.0}, +#endif + .u_pref = {0.0}, + .u_min = {0.0}, + .u_max = {0.0}, + .PC = 0.0, + .SC = 0.0, + .iter = 0 +}; #endif - -float u_min_stab_indi[INDI_NUM_ACT]; -float u_max_stab_indi[INDI_NUM_ACT]; -float u_pref_stab_indi[INDI_NUM_ACT]; float indi_v[INDI_OUTPUTS]; float *Bwls[INDI_OUTPUTS]; -int num_iter = 0; static void lms_estimation(void); static void get_actuator_state(void); @@ -176,26 +197,6 @@ float act_first_order_cutoff[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_FREQ; float act_dyn_discrete[INDI_NUM_ACT]; // will be computed from freq at init #endif -#ifdef STABILIZATION_INDI_WLS_PRIORITIES -static float Wv[INDI_OUTPUTS] = STABILIZATION_INDI_WLS_PRIORITIES; -#else -//State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST} -#if INDI_OUTPUTS == 5 -static float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100, 100}; -#else -static float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100}; -#endif -#endif - -/** - * Weighting of different actuators in the cost function - */ -#ifdef STABILIZATION_INDI_WLS_WU -float indi_Wu[INDI_NUM_ACT] = STABILIZATION_INDI_WLS_WU; -#else -float indi_Wu[INDI_NUM_ACT] = {[0 ... INDI_NUM_ACT - 1] = 1.0}; -#endif - /** * Limit the maximum specific moment that can be compensated (units rad/s^2) */ @@ -296,6 +297,15 @@ void sum_g1_g2(void); #if PERIODIC_TELEMETRY #include "modules/datalink/telemetry.h" +static void send_wls_v_stab(struct transport_tx *trans, struct link_device *dev) +{ + send_wls_v("stab", &wls_stab_p, trans, dev); +} +static void send_wls_u_stab(struct transport_tx *trans, struct link_device *dev) +{ + send_wls_u("stab", &wls_stab_p, trans, dev); +} + static void send_eff_mat_g_indi(struct transport_tx *trans, struct link_device *dev) { float zero = 0.0; @@ -422,6 +432,8 @@ void stabilization_indi_init(void) register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_EFF_MAT_G, send_eff_mat_g_indi); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_REF_QUAT, send_ahrs_ref_quat); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE, send_att_full_indi); + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WLS_V, send_wls_v_stab); + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WLS_U, send_wls_u_stab); #endif } @@ -675,9 +687,13 @@ void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *s stabilization_indi_set_wls_settings(); // WLS Control Allocator - num_iter = - wls_alloc(indi_u, indi_v, u_min_stab_indi, u_max_stab_indi, Bwls, 0, 0, Wv, indi_Wu, u_pref_stab_indi, 10000, 10, - INDI_NUM_ACT, INDI_OUTPUTS); + for (i = 0; i < INDI_OUTPUTS; i++) { + wls_stab_p.v[i] = indi_v[i]; + } + wls_alloc(&wls_stab_p, Bwls, 0, 0, 10); + for (i = 0; i < INDI_NUM_ACT; i++) { + indi_u [i] = wls_stab_p.u[i]; + } #endif // Bound the inputs to the actuators @@ -719,9 +735,9 @@ void WEAK stabilization_indi_set_wls_settings(void) { // Calculate the min and max increments for (uint8_t i = 0; i < INDI_NUM_ACT; i++) { - u_min_stab_indi[i] = -MAX_PPRZ * act_is_servo[i]; - u_max_stab_indi[i] = MAX_PPRZ; - u_pref_stab_indi[i] = act_pref[i]; + wls_stab_p.u_min[i] = -MAX_PPRZ * act_is_servo[i]; + wls_stab_p.u_max[i] = MAX_PPRZ; + wls_stab_p.u_pref[i] = act_pref[i]; #ifdef GUIDANCE_INDI_MIN_THROTTLE float airspeed = stateGetAirspeed_f(); @@ -729,9 +745,9 @@ void WEAK stabilization_indi_set_wls_settings(void) if (!act_is_servo[i]) { if ((guidance_h.mode == GUIDANCE_H_MODE_HOVER) || (guidance_h.mode == GUIDANCE_H_MODE_NAV)) { if (airspeed < STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD) { - u_min_stab_indi[i] = GUIDANCE_INDI_MIN_THROTTLE; + wls_stab_p.u_min[i] = GUIDANCE_INDI_MIN_THROTTLE; } else { - u_min_stab_indi[i] = GUIDANCE_INDI_MIN_THROTTLE_FWD; + wls_stab_p.u_min[i] = GUIDANCE_INDI_MIN_THROTTLE_FWD; } } } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h index db589c367e..8e6cc783cb 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h @@ -37,9 +37,6 @@ extern bool act_is_servo[INDI_NUM_ACT]; extern bool indi_use_adaptive; -extern float u_min_stab_indi[INDI_NUM_ACT]; -extern float u_max_stab_indi[INDI_NUM_ACT]; -extern float u_pref_stab_indi[INDI_NUM_ACT]; extern float *Bwls[INDI_OUTPUTS]; extern float thrust_bx_eff; @@ -50,7 +47,6 @@ extern float thrust_bx_state_filt; extern float act_pref[INDI_NUM_ACT]; extern float indi_Wu[INDI_NUM_ACT]; - struct Indi_gains { struct FloatRates att; struct FloatRates rate; @@ -68,5 +64,9 @@ extern void stabilization_indi_attitude_run(bool in_flight, struct Stabilization extern void stabilization_indi_set_wls_settings(void); extern void stabilization_indi_update_filt_freq(float freq); // setting handler +#if !STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE +#include "math/wls/wls_alloc.h" +extern struct WLS_t wls_stab_p; +#endif #endif /* STABILIZATION_INDI */ diff --git a/sw/airborne/math/wls/wls_alloc.c b/sw/airborne/math/wls/wls_alloc.c index 8384073143..b329ad0168 100644 --- a/sw/airborne/math/wls/wls_alloc.c +++ b/sw/airborne/math/wls/wls_alloc.c @@ -51,12 +51,34 @@ #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); +static void print_final_values(struct WLS_t* WLS_p, float **B); +static void print_in_and_outputs(int n_c, int n_free, float **A_free_ptr, float *d, float *p_free); #endif - -#define WLS_N_C ((WLS_N_U)+(WLS_N_V)) +/* Define messages of the module*/ +#if PERIODIC_TELEMETRY +#include "modules/datalink/telemetry.h" +void send_wls_v(char *name, struct WLS_t *WLS_p, struct transport_tx *trans, struct link_device *dev) +{ + uint8_t iter_temp = (uint8_t)WLS_p->iter; + pprz_msg_send_WLS_V(trans, dev, AC_ID, + strlen(name),name, + &WLS_p->gamma_sq, + &iter_temp, + WLS_p->nv, WLS_p->v, + WLS_p->nv, WLS_p->Wv); +} +void send_wls_u(char *name, struct WLS_t *WLS_p, struct transport_tx *trans, struct link_device *dev) +{ + pprz_msg_send_WLS_U(trans, dev, AC_ID, + strlen(name),name, + WLS_p->nu, WLS_p->Wu, + WLS_p->nu, WLS_p->u_pref, + WLS_p->nu, WLS_p->u_min, + WLS_p->nu, WLS_p->u_max, + WLS_p->nu, WLS_p->u); +} +#endif /** * @brief Wrapper for qr solve @@ -67,7 +89,7 @@ static void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* * @param m number of rows * @param n number of columns */ -static 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; @@ -86,77 +108,64 @@ static void qr_solve_wrapper(int m, int n, float** A, float* b, float* x) { * Takes the control objective and max and min inputs from pprz and calculates * the inputs that will satisfy most of the control objective, subject to the * weighting matrices Wv and Wu - * - * @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 WLS_p Struct that contains most of the WLS parameters * @param B The control effectiveness matrix * @param u_guess Initial value for u * @param W_init Initial working set, if known - * @param Wv Weighting on different control objectives - * @param Wu Weighting on different controls - * @param up Preferred control vector - * @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 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 n_u, int n_v) { + +void wls_alloc(struct WLS_t* WLS_p, float **B, float *u_guess, float *W_init, int imax) { // allocate variables, use defaults where parameters are set to 0 - if(!gamma_sq) gamma_sq = 100000; - if(!imax) imax = 100; + if (!WLS_p->gamma_sq) WLS_p->gamma_sq = 100000; + if (!imax) imax = 100; - int n_c = n_u + n_v; + int n_c = WLS_p->nu + WLS_p->nv; - float A[WLS_N_C][WLS_N_U]; - float A_free[WLS_N_C][WLS_N_U]; + float A[n_c][WLS_p->nu]; + float A_free[n_c][WLS_p->nu]; // Create a pointer array to the rows of A_free // such that we can pass it to a function - float * A_free_ptr[WLS_N_C]; + float *A_free_ptr[n_c]; for(int i = 0; i < n_c; i++) A_free_ptr[i] = A_free[i]; - float b[WLS_N_C]; - float d[WLS_N_C]; + float b[n_c]; + float d[n_c]; - int free_index[WLS_N_U]; - int free_index_lookup[WLS_N_U]; + int free_index[WLS_p->nu]; + int free_index_lookup[WLS_p->nu]; int n_free = 0; int free_chk = -1; int iter = 0; - float p_free[WLS_N_U]; - float p[WLS_N_U]; - float u_opt[WLS_N_U]; - int infeasible_index[WLS_N_U] UNUSED; + float p_free[WLS_p->nu]; + float p[WLS_p->nu]; + float u_opt[WLS_p->nu]; + int infeasible_index[WLS_p->nu] UNUSED; int n_infeasible = 0; - float lambda[WLS_N_U]; - float W[WLS_N_U]; + float lambda[WLS_p->nu]; + float W[WLS_p->nu]; // Initialize u and the working set, if provided from input if (!u_guess) { - for (int i = 0; i < n_u; i++) { - u[i] = (umax[i] + umin[i]) * 0.5; + for (int i = 0; i < WLS_p->nu; i++) { + WLS_p->u[i] = (WLS_p->u_max[i] + WLS_p->u_min[i]) * 0.5; } } else { - for (int i = 0; i < n_u; i++) { - u[i] = u_guess[i]; + for (int i = 0; i < WLS_p->nu; i++) { + WLS_p->u[i] = u_guess[i]; } } - W_init ? memcpy(W, W_init, n_u * sizeof(float)) - : memset(W, 0, n_u * sizeof(float)); + W_init ? memcpy(W, W_init, WLS_p->nu * sizeof(float)) + : memset(W, 0, WLS_p->nu * sizeof(float)); - memset(free_index_lookup, -1, n_u * sizeof(float)); + memset(free_index_lookup, -1, WLS_p->nu * sizeof(float)); // find free indices - for (int i = 0; i < n_u; i++) { + for (int i = 0; i < WLS_p->nu; i++) { if (W[i] == 0) { free_index_lookup[i] = n_free; free_index[n_free++] = i; @@ -164,28 +173,27 @@ int wls_alloc(float* u, float* v, float* umin, float* umax, float** B, } // fill up A, A_free, b and d - for (int i = 0; i < n_v; i++) { - // If Wv is a NULL pointer, use Wv = identity - b[i] = Wv ? gamma_sq * Wv[i] * v[i] : gamma_sq * v[i]; + for (int i = 0; i < WLS_p->nv; i++) { + b[i] = WLS_p->gamma_sq * WLS_p->Wv[i] * WLS_p->v[i]; d[i] = b[i]; - for (int j = 0; j < n_u; j++) { + for (int j = 0; j < WLS_p->nu; j++) { // If Wv is a NULL pointer, use Wv = identity - A[i][j] = Wv ? gamma_sq * Wv[i] * B[i][j] : gamma_sq * B[i][j]; - d[i] -= A[i][j] * u[j]; + A[i][j] = WLS_p->gamma_sq * WLS_p->Wv[i] * B[i][j]; + d[i] -= A[i][j] * WLS_p->u[j]; } } - for (int i = n_v; i < n_c; i++) { - memset(A[i], 0, n_u * sizeof(float)); - A[i][i - n_v] = Wu ? Wu[i - n_v] : 1.0; - b[i] = up ? (Wu ? Wu[i-n_v] * up[i-n_v] : up[i-n_v]) : 0; - d[i] = b[i] - A[i][i - n_v] * u[i - n_v]; + for (int i = WLS_p->nv; i < n_c; i++) { + memset(A[i], 0, WLS_p->nu * sizeof(float)); + A[i][i - WLS_p->nv] = WLS_p->Wu[i - WLS_p->nv]; + b[i] = WLS_p->Wu[i - WLS_p->nv] * WLS_p->u_pref[i - WLS_p->nv]; + d[i] = b[i] - A[i][i - WLS_p->nv] * WLS_p->u[i - WLS_p->nv]; } // -------------- Start loop ------------ while (iter++ < imax) { // clear p, copy u to u_opt - memset(p, 0, n_u * sizeof(float)); - memcpy(u_opt, u, n_u * sizeof(float)); + memset(p, 0, WLS_p->nu * sizeof(float)); + memcpy(u_opt, WLS_p->u, WLS_p->nu * sizeof(float)); // Construct a matrix with the free columns of A if (free_chk != n_free) { @@ -218,7 +226,7 @@ int wls_alloc(float* u, float* v, float* umin, float* umax, float** B, u_opt[free_index[i]] += p_free[i]; // check limits - if ( (u_opt[free_index[i]] > umax[free_index[i]] || u_opt[free_index[i]] < umin[free_index[i]])) { + if ((u_opt[free_index[i]] > WLS_p->u_max[free_index[i]] || u_opt[free_index[i]] < WLS_p->u_min[free_index[i]])) { infeasible_index[n_infeasible++] = free_index[i]; } } @@ -227,22 +235,22 @@ int wls_alloc(float* u, float* v, float* umin, float* umax, float** B, // Check feasibility of the solution if (n_infeasible == 0) { // all variables are within limits - memcpy(u, u_opt, n_u * sizeof(float)); - memset(lambda, 0, n_u * sizeof(float)); + memcpy(WLS_p->u, u_opt, WLS_p->nu * sizeof(float)); + memset(lambda, 0, WLS_p->nu * sizeof(float)); // d = d + A_free*p_free; lambda = A*d; for (int i = 0; i < n_c; i++) { for (int k = 0; k < n_free; k++) { d[i] -= A_free[i][k] * p_free[k]; } - for (int k = 0; k < n_u; k++) { + for (int k = 0; k < WLS_p->nu; k++) { lambda[k] += A[i][k] * d[i]; } } bool break_flag = true; // lambda = lambda x W; - for (int i = 0; i < n_u; i++) { + for (int i = 0; i < WLS_p->nu; i++) { lambda[i] *= W[i]; // if any lambdas are negative, keep looking for solution if (lambda[i] < -FLT_EPSILON) { @@ -258,11 +266,11 @@ int wls_alloc(float* u, float* v, float* umin, float* umax, float** B, if (break_flag) { #if WLS_VERBOSE - print_final_values(n_u, n_v, u, B, v, umin, umax); + print_final_values(WLS_p, B); #endif - + // if solution is found, return number of iterations - return iter; + WLS_p->iter = iter; } } else { // scaling back actuator command (0-1) @@ -274,8 +282,8 @@ int wls_alloc(float* u, float* v, float* umin, float* umax, float** B, for (int i = 0; i < n_free; i++) { int id = free_index[i]; - alpha_tmp = (p[id] < 0) ? (umin[id] - u[id]) / p[id] - : (umax[id] - u[id]) / p[id]; + alpha_tmp = (p[id] < 0) ? (WLS_p->u_min[id] - WLS_p->u[id]) / p[id] + : (WLS_p->u_max[id] - WLS_p->u[id]) / p[id]; if (isnan(alpha_tmp) || alpha_tmp < 0.f) { alpha_tmp = 1.0f; @@ -287,9 +295,9 @@ int wls_alloc(float* u, float* v, float* umin, float* umax, float** B, } // 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]); + for (int i = 0; i < WLS_p->nu; i++) { + WLS_p->u[i] += alpha * p[i]; + Bound(WLS_p->u[i], WLS_p->u_min[i], WLS_p->u_max[i]); } // update d = d-alpha*A*p_free for (int i = 0; i < n_c; i++) { @@ -302,20 +310,19 @@ int wls_alloc(float* u, float* v, float* umin, float* umax, float** B, free_index[free_index_lookup[id_alpha]] = free_index[--n_free]; free_index_lookup[free_index[free_index_lookup[id_alpha]]] = - free_index_lookup[id_alpha]; + free_index_lookup[id_alpha]; free_index_lookup[id_alpha] = -1; } } - return iter; + WLS_p->iter = iter; } #if WLS_VERBOSE -static 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); printf("A_free =\n"); - for(int i = 0; i < n_c; i++) { + for (int i = 0; i < n_c; i++) { for (int j = 0; j < n_free; j++) { printf("%f ", A_free_ptr[i][j]); } @@ -334,39 +341,39 @@ static void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* printf("\n\n"); } -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); +static void print_final_values(struct WLS_t* WLS_p, float **B) { + printf("n_u = %d n_v = %d\n", WLS_p->nu, WLS_p->nv); printf("B =\n"); - for(int i = 0; i < n_v; i++) { - for (int j = 0; j < n_u; j++) { + for (int i = 0; i < WLS_p->nv; i++) { + for (int j = 0; j < WLS_p->nu; j++) { printf("%f ", B[i][j]); } printf("\n"); } printf("v = "); - for (int j = 0; j < n_v; j++) { - printf("%f ", v[j]); + for (int j = 0; j < WLS_p->nv; j++) { + printf("%f ", WLS_p->v[j]); } printf("\nu = "); - for (int j = 0; j < n_u; j++) { + for (int j = 0; j < WLS_p->nu; j++) { printf("%f ", u[j]); } printf("\n"); printf("\numin = "); - for (int j = 0; j < n_u; j++) { - printf("%f ", umin[j]); + for (int j = 0; j < WLS_p->nu; j++) { + printf("%f ", WLS_p->u_min[j]); } printf("\n"); printf("\numax = "); - for (int j = 0; j < n_u; j++) { - printf("%f ", umax[j]); + for (int j = 0; j < WLS_p->nu; j++) { + printf("%f ", WLS_p->u_max[j]); } printf("\n\n"); - + } #endif diff --git a/sw/airborne/math/wls/wls_alloc.h b/sw/airborne/math/wls/wls_alloc.h index 4a29468923..490db63241 100644 --- a/sw/airborne/math/wls/wls_alloc.h +++ b/sw/airborne/math/wls/wls_alloc.h @@ -52,21 +52,41 @@ #ifndef WLS_ALLOC_HEADER #define WLS_ALLOC_HEADER +#ifndef TEST_ALLOC #include "generated/airframe.h" - -#ifndef WLS_N_U -#define WLS_N_U 6 #endif -#ifndef WLS_N_V -#define WLS_N_V 4 +#ifndef WLS_N_U_MAX +#define WLS_N_U_MAX 6 +#endif + +#ifndef WLS_N_V_MAX +#define WLS_N_V_MAX 4 +#endif +struct WLS_t{ + int nu; // number of actuators + int nv; // number of controlled axes + float gamma_sq; // weighting factor WLS + float v[WLS_N_V_MAX]; // Pseudo Control Vector + float u[WLS_N_U_MAX]; // Allocation of Controls + float Wv[WLS_N_V_MAX]; // Weighting on different control objectives + float Wu[WLS_N_U_MAX]; // Weighting on different actuators + float u_pref[WLS_N_U_MAX]; // Preferred control vector + float u_min[WLS_N_U_MAX]; // Minimum control vector + float u_max[WLS_N_U_MAX]; // Maximum control vector + float PC; // Primary cost + float SC; // Secondary cost + int iter; // Number of iterations +}; + +#if PERIODIC_TELEMETRY +#include "modules/datalink/telemetry.h" +extern void send_wls_v(char *name, struct WLS_t* WLS_p, struct transport_tx *trans, struct link_device *dev); +extern void send_wls_u(char *name, struct WLS_t* WLS_p, struct transport_tx *trans, struct link_device *dev); #endif -extern 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, int n_u, int n_v); +extern void wls_alloc(struct WLS_t* WLS_p, float **B, float *u_guess, float *W_init, int imax); #endif \ No newline at end of file diff --git a/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c b/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c index 942c998128..0a737b20b0 100644 --- a/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c +++ b/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c @@ -424,22 +424,22 @@ void stabilization_indi_set_wls_settings(void) { // Calculate the min and max increments for (uint8_t i = 0; i < INDI_NUM_ACT; i++) { - u_min_stab_indi[i] = -MAX_PPRZ * act_is_servo[i]; - u_max_stab_indi[i] = MAX_PPRZ; - u_pref_stab_indi[i] = act_pref[i]; + wls_stab_p.u_min[i] = -MAX_PPRZ * act_is_servo[i]; + wls_stab_p.u_max[i] = MAX_PPRZ; + wls_stab_p.u_pref[i] = act_pref[i]; if (i == 5) { - u_pref_stab_indi[i] = actuator_state_filt_vect[i]; // Set change in prefered state to 0 for elevator - u_min_stab_indi[i] = 0; // cmd 0 is lowest position for elevator + wls_stab_p.u_pref[i] = actuator_state_filt_vect[i]; // Set change in preferred state to 0 for elevator + wls_stab_p.u_min[i] = 0; // cmd 0 is lowest position for elevator } if (i==8) { // dt (min to max) MAX_PPRZ / (dt * f) dt_min == 0.002 Bound(eff_sched_pusher_time, 0.002, 5.); float max_increment = MAX_PPRZ / (eff_sched_pusher_time * 500); - u_min_stab_indi[i] = actuators_pprz[i] - max_increment; - u_max_stab_indi[i] = actuators_pprz[i] + max_increment; + wls_stab_p.u_min[i] = actuators_pprz[i] - max_increment; + wls_stab_p.u_max[i] = actuators_pprz[i] + max_increment; - Bound(u_min_stab_indi[i], 0, MAX_PPRZ); - Bound(u_max_stab_indi[i], 0, MAX_PPRZ); + Bound(wls_stab_p.u_min[i], 0, MAX_PPRZ); + Bound(wls_stab_p.u_max[i], 0, MAX_PPRZ); } } } diff --git a/sw/airborne/modules/rot_wing_drone/rotwing_state.c b/sw/airborne/modules/rot_wing_drone/rotwing_state.c index 94f453fd02..c1a1103634 100644 --- a/sw/airborne/modules/rot_wing_drone/rotwing_state.c +++ b/sw/airborne/modules/rot_wing_drone/rotwing_state.c @@ -734,7 +734,7 @@ void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, fl float Wv_original[GUIDANCE_INDI_HYBRID_V] = GUIDANCE_INDI_WLS_PRIORITIES; // Increase importance of forward acceleration in forward flight - Wv_gih[0] = Wv_original[0] * (1.0f + fixed_wing_percentage * + wls_guid_p.Wv[0] = Wv_original[0] * (1.0f + fixed_wing_percentage * AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT); // stall n low hover motor_off (weight 16x more important than vertical weight) struct FloatEulers eulers_zxy; @@ -772,12 +772,12 @@ void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, fl float pitch_angle_range = 3.; if (rotwing_state_skewing.wing_angle_deg < 55) { scheduled_pitch_angle = 0; - Wu_gih[1] = Wu_gih_original[1]; + wls_guid_p.Wu[1] = Wu_gih_original[1]; max_pitch_limit_rad = quad_pitch_limit_rad; } else { float pitch_progression = (rotwing_state_skewing.wing_angle_deg - 55) / 35.; scheduled_pitch_angle = pitch_angle_range * pitch_progression; - Wu_gih[1] = Wu_gih_original[1] * (1.f - pitch_progression*0.9); + wls_guid_p.Wu[1] = Wu_gih_original[1] * (1.f - pitch_progression*0.9); max_pitch_limit_rad = quad_pitch_limit_rad + (fwd_pitch_limit_rad - quad_pitch_limit_rad) * pitch_progression; } if (!hover_motors_active) { @@ -790,20 +790,20 @@ void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, fl float pitch_pref_rad = RadOfDeg(guidance_indi_pitch_pref_deg); // Set lower limits - du_min_gih[0] = -roll_limit_rad - roll_angle; //roll - du_min_gih[1] = min_pitch_limit_rad - pitch_angle; // pitch - du_min_gih[2] = du_min_thrust_z; - du_min_gih[3] = (-actuator_state_filt_vect[8] * g1g2[4][8]); + wls_guid_p.u_min[0] = -roll_limit_rad - roll_angle; //roll + wls_guid_p.u_min[1] = min_pitch_limit_rad - pitch_angle; // pitch + wls_guid_p.u_min[2] = du_min_thrust_z; + wls_guid_p.u_min[3] = (-actuator_state_filt_vect[8] * g1g2[4][8]); // Set upper limits limits - du_max_gih[0] = roll_limit_rad - roll_angle; //roll - du_max_gih[1] = max_pitch_limit_rad - pitch_angle; // pitch - du_max_gih[2] = du_max_thrust_z; - du_max_gih[3] = 9.0; // Hacky value to prevent drone from pitching down in transition + wls_guid_p.u_max[0] = roll_limit_rad - roll_angle; //roll + wls_guid_p.u_max[1] = max_pitch_limit_rad - pitch_angle; // pitch + wls_guid_p.u_max[2] = du_max_thrust_z; + wls_guid_p.u_max[3] = 9.0; // Hacky value to prevent drone from pitching down in transition // Set prefered states - du_pref_gih[0] = 0; // prefered delta roll angle - du_pref_gih[1] = -pitch_angle + pitch_pref_rad;// prefered delta pitch angle - du_pref_gih[2] = du_max_gih[2]; // Low thrust better for efficiency - du_pref_gih[3] = body_v[0]; // solve the body acceleration + wls_guid_p.u_pref[0] = 0; // prefered delta roll angle + wls_guid_p.u_pref[1] = -pitch_angle + pitch_pref_rad;// prefered delta pitch angle + wls_guid_p.u_pref[2] = wls_guid_p.u_max[2]; // Low thrust better for efficiency + wls_guid_p.u_pref[3] = body_v[0]; // solve the body acceleration } diff --git a/sw/airborne/test/test_alloc.c b/sw/airborne/test/test_alloc.c index b81241d06b..3cbc260f07 100644 --- a/sw/airborne/test/test_alloc.c +++ b/sw/airborne/test/test_alloc.c @@ -32,7 +32,8 @@ #include "std.h" #include "math/wls/wls_alloc.h" -#define INDI_OUTPUTS 4 +#define INDI_NUM_ACT WLS_N_U_MAX +#define INDI_OUTPUTS WLS_N_V_MAX void test_overdetermined(void); void test_overdetermined2(void); @@ -89,6 +90,82 @@ void test_four_by_four(void) printf("du = %f, %f, %f, %f\n", indi_du[0], indi_du[1], indi_du[2], indi_du[3]); } +/* function to test wls for the oneloop controller 6x11 (outputs x inputs) system */ +void test_oneloop_rw3c(void) +{ + float gamma = 20; + float du_min[WLS_N_U_MAX] = {0, 0, 0, 0, -0.999999000000000, -0.818607651741293, -0.906420781094527, -1.00441964179104, -1.00551747263682, -0.625892592039801, -1.38242921393035}; + float du_max[WLS_N_U_MAX] = {0, 0, 0, 0, 1.00000000000000e-06, 0.181392348258707, 1.09357921890547, 0.995580358208955, 0.994482527363184, 1.37410740796020, 0.617570786069651}; + float du_pref[WLS_N_U_MAX] = {0, 0, 0, 0, -0.999999000000000, -0.818607651741293, 0.0935792189054731, -0.00441964179104483, -0.00551747263681596, 0.374107407960200, -0.382429213930349}; + + + float Wv[WLS_N_V_MAX] = {4,4,4,8,8,1}; + float Wu[WLS_N_U_MAX] = {0.75,0.75,0.75,0.75,1,0.1,0.1,0.1,0.1,1.2,1.2}; + float wls_v[WLS_N_V_MAX] = {-4.86661635467990, 7.02758381773395, 3.20567040886711, -1.05634927093606, -4.53403714285678, -0.125133236453181}; + //float wls_v[WLS_N_V_MAX] = {0,0,0,0,0,0}; + float wls_du[WLS_N_U_MAX] = {0}; + float g1g2[WLS_N_V_MAX][WLS_N_U_MAX] = { + {0, 0, 0, 0, -47.9216734146340, 0, 0, 0, 0, -23.2233317560976, 18.3478526585367}, + {0, 0, 0, 0, 104.951841268293, 0, 0, 0, 0, -13.1313056097561, 8.31975553658538}, + {0, 0, 0, 0, -33.4459766585367, 0, 0, 0, 0, -7.95380339024395, -67.3910292682927}, + {0, 0, 0, 0, 0, 0, 0, 834.716482762376, 623.043724059406, 0, 0}, + {0, 0, 0, 0, 0, 1841.05446648515, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 79.4693096831683, 0, 0, 0, 0} + }; + + // Scale to test numerical errors + float scaler = 1; // replace with your desired value + gamma *= scaler; + for (int i = 0; i < WLS_N_U_MAX; i++) { + du_min[i] *= scaler; + du_max[i] *= scaler; + du_pref[i] *= scaler; + } + + for (int i = 0; i < WLS_N_V_MAX; i++) { + for (int j = 0; j < WLS_N_U_MAX; j++) { + g1g2[i][j] /= scaler; + } + } + // Initialize the array of pointers to the rows of g1g2 + float *Bwls[WLS_N_V_MAX]; + uint8_t i; + for (i = 0; i < WLS_N_V_MAX; i++) { + Bwls[i] = g1g2[i]; + } + // 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, INDI_NUM_ACT, INDI_OUTPUTS); + int num_iter = wls_alloc(wls_du, wls_v, du_min, du_max, Bwls, du_pref, 0, Wv, Wu, du_pref, gamma, 10, WLS_N_U_MAX, WLS_N_V_MAX); + float nu_out[WLS_N_V_MAX] = {0.0f}; + calc_nu_out(Bwls, wls_du, nu_out); + float SC = 0; + calc_secondary_cost(wls_du, du_pref, Wu, &SC); + printf("finished in %d iterations\n", num_iter); + printf("Input:\n"); + printf("--------- jN jE jD jp jq jr\n"); + printf("v = %-13f %-13f %-13f %-13f %-13f %-13f\n", wls_v[0], wls_v[1], wls_v[2], wls_v[3], wls_v[4], wls_v[5]); + printf("v_out = %-13f %-13f %-13f %-13f %-13f %-13f\n", nu_out[0], nu_out[1], nu_out[2], nu_out[3], nu_out[4], nu_out[5]); + printf("Wv = %-13f %-13f %-13f %-13f %-13f %-13f\n", Wv[0], Wv[1], Wv[2], Wv[3], Wv[4], Wv[5]); + printf("Control effectiveness matrix g1g2 = \n"); + printf("--------- mF mR mB mL mP ele rud ail flp phi theta\n"); + printf("jN %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f\n", g1g2[0][0], g1g2[0][1], g1g2[0][2], g1g2[0][3], g1g2[0][4], g1g2[0][5], g1g2[0][6], g1g2[0][7], g1g2[0][8], g1g2[0][9], g1g2[0][10]); + printf("jE %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f\n", g1g2[1][0], g1g2[1][1], g1g2[1][2], g1g2[1][3], g1g2[1][4], g1g2[1][5], g1g2[1][6], g1g2[1][7], g1g2[1][8], g1g2[1][9], g1g2[1][10]); + printf("jD %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f\n", g1g2[2][0], g1g2[2][1], g1g2[2][2], g1g2[2][3], g1g2[2][4], g1g2[2][5], g1g2[2][6], g1g2[2][7], g1g2[2][8], g1g2[2][9], g1g2[2][10]); + printf("jp %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f\n", g1g2[3][0], g1g2[3][1], g1g2[3][2], g1g2[3][3], g1g2[3][4], g1g2[3][5], g1g2[3][6], g1g2[3][7], g1g2[3][8], g1g2[3][9], g1g2[3][10]); + printf("jq %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f\n", g1g2[4][0], g1g2[4][1], g1g2[4][2], g1g2[4][3], g1g2[4][4], g1g2[4][5], g1g2[4][6], g1g2[4][7], g1g2[4][8], g1g2[4][9], g1g2[4][10]); + printf("jr %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f\n", g1g2[5][0], g1g2[5][1], g1g2[5][2], g1g2[5][3], g1g2[5][4], g1g2[5][5], g1g2[5][6], g1g2[5][7], g1g2[5][8], g1g2[5][9], g1g2[5][10]); + printf("\n"); + printf("du_min = %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f\n", du_min[0], du_min[1], du_min[2], du_min[3], du_min[4], du_min[5], du_min[6], du_min[7], du_min[8], du_min[9], du_min[10]); + printf("\n"); + printf("du_max = %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f\n", du_max[0], du_max[1], du_max[2], du_max[3], du_max[4], du_max[5], du_max[6], du_max[7], du_max[8], du_max[9], du_max[10]); + printf("\n"); + printf("du_pref = %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f\n", du_pref[0], du_pref[1], du_pref[2], du_pref[3], du_pref[4], du_pref[5], du_pref[6], du_pref[7], du_pref[8], du_pref[9], du_pref[10]); + printf("\n"); + printf("Wu = %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f\n", Wu[0], Wu[1], Wu[2], Wu[3], Wu[4], Wu[5], Wu[6], Wu[7], Wu[8], Wu[9], Wu[10]); + printf("\n"); + printf("du = %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f %-13f\n", wls_du[0], wls_du[1], wls_du[2], wls_du[3], wls_du[4], wls_du[5], wls_du[6], wls_du[7], wls_du[8], wls_du[9], wls_du[10]); + printf("Secondary cost = %f\n", SC); +} /* * function to test wls for an overdetermined 4x6 (outputs x inputs) system diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink index 22115a818b..4016c70c55 160000 --- a/sw/ext/pprzlink +++ b/sw/ext/pprzlink @@ -1 +1 @@ -Subproject commit 22115a818b853266577e0594a8e10fd68f0a971c +Subproject commit 4016c70c558e65649b3599912b7535a858037e49