mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-28 18:07:25 +08:00
WLS Debug (#3320)
* WLS Debug WLS Debug * track new develop branch * WLS to use pointer of struct to allow multiple calls of WLS in the same control loop * additional changes and bug fixes to wls struct pointer rework * Track new pprzling branch with wls msg additions * code clean up * Implement new WLS in guidance and stabilization * code clean up * add wls mesg to telemetry. Fix stabilization wanring. * track pprzlink * Better casing * Use WLS_N_U_MAX and WLS_N_V_MAX * Register periodic telemetry of WLS only if WLS is used * Track master pprzlink * Shortened explanation of parameters * track pprzlink master --------- Co-authored-by: Tomaso Maria Luigi De Ponti <48210579+tmldeponti@users.noreply.github.com> Co-authored-by: tomaso_de_ponti <tomasodp@gmail.com>
This commit is contained in:
Christophe De Wagter
committed by
GitHub
GitHub
parent
455d883677
commit
4e00e22993
@@ -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];
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -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];
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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 */
|
||||
|
||||
|
||||
@@ -51,12 +51,34 @@
|
||||
|
||||
#if WLS_VERBOSE
|
||||
#include <stdio.h>
|
||||
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
|
||||
|
||||
@@ -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
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
+1
-1
Submodule sw/ext/pprzlink updated: 22115a818b...4016c70c55
Reference in New Issue
Block a user