Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-20 11:06:26 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

[INDI] flexible priorities for WLS allocation (#2200)

Browse Source 
* [INDI] flexible priorities for WLS allocation

* [INDI] du_pref was not used

Moved WLS specific variables du_min and du_max within the else
This commit is contained in:
Ewoud Smeur
2017-12-01 16:04:16 +01:00
committed by Kirk Scheper
parent 0adf61c4b0
commit dab1bf2fcf
2 changed files with 18 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/bebop_indi_actuators.xml
+3
View File
@@ -134,6 +134,9 @@
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="TRUE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
<!--Priority for each axis (roll, pitch, yaw and thrust)-->
<define name="WLS_PRIORITIES" value="{1000, 1000, 1, 100}"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+15 -11
View File
@@ -109,6 +109,13 @@ float act_dyn[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN;
#define STABILIZATION_INDI_MAX_RATE 6.0
#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}
static float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100};
#endif
// variables needed for control
float actuator_state_filt_vect[INDI_NUM_ACT];
struct FloatRates angular_accel_ref = {0., 0., 0.};
@@ -396,16 +403,6 @@ static void stabilization_indi_calc_cmd(struct Int32Quat *att_err, bool rate_con
}
}
// Calculate the min and max increments
for (i = 0; i < INDI_NUM_ACT; i++) {
du_min[i] = -MAX_PPRZ * act_is_servo[i] - actuator_state_filt_vect[i];
du_max[i] = MAX_PPRZ - actuator_state_filt_vect[i];
du_pref[i] = act_pref[i] - actuator_state_filt_vect[i];
}
//State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST}
static float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100};
// The control objective in array format
indi_v[0] = (angular_accel_ref.p - angular_acceleration[0]);
indi_v[1] = (angular_accel_ref.q - angular_acceleration[1]);
@@ -421,9 +418,16 @@ static void stabilization_indi_calc_cmd(struct Int32Quat *att_err, bool rate_con
+ (g1g2_pseudo_inv[i][3] * indi_v[3]);
}
#else
// Calculate the min and max increments
for (i = 0; i < INDI_NUM_ACT; i++) {
du_min[i] = -MAX_PPRZ * act_is_servo[i] - actuator_state_filt_vect[i];
du_max[i] = MAX_PPRZ - actuator_state_filt_vect[i];
du_pref[i] = act_pref[i] - actuator_state_filt_vect[i];
}
// WLS Control Allocator
num_iter =
wls_alloc(indi_du, indi_v, du_min, du_max, Bwls, INDI_NUM_ACT, INDI_OUTPUTS, 0, 0, Wv, 0, du_min, 10000, 10);
wls_alloc(indi_du, indi_v, du_min, du_max, Bwls, INDI_NUM_ACT, INDI_OUTPUTS, 0, 0, Wv, 0, du_pref, 10000, 10);
#endif
// Add the increments to the actuators