Skip to content

Commit

Permalink
Revert "WLS control allocation (#2075)"
Browse files Browse the repository at this point in the history 
This reverts commit b2e22be.
  • Loading branch information
@EwoudSmeur
EwoudSmeur committed Jun 16, 2017
1 parent b2e22be commit b96b8b0
Show file tree
Hide file tree
Showing 11 changed files with 76 additions and 2,721 deletions.
23 changes: 22 additions & 1 deletion conf/airframes/TUDELFT/tudelft_bebop_indi_actuators.xml
View file
Expand Up @@ -100,12 +100,30 @@
<!-- setpoint limits for attitude stabilization rc flight -->
<define name="SP_MAX_PHI" value="45" unit="deg"/>
<define name="SP_MAX_THETA" value="45" unit="deg"/>
<define name="SP_MAX_R" value="400" unit="deg/s"/>
<define name="SP_MAX_R" value="120" unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="50"/>
</section>

<section name="ATTITUDE_REFERENCE" prefix="STABILIZATION_ATTITUDE_">
<!-- attitude reference generation model -->
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.9"/>
<define name="REF_MAX_P" value="600." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>

<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.9"/>
<define name="REF_MAX_Q" value="600." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>

<define name="REF_OMEGA_R" value="450" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.9"/>
<define name="REF_MAX_R" value="600." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(8000.)"/>
</section>

<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- control effectiveness -->
<define name="G1_ROLL" value="{20 , -20, -20 , 20 }"/>
Expand All @@ -123,6 +141,9 @@
<define name="REF_RATE_Q" value="28.0"/>
<define name="REF_RATE_R" value="28.0"/>

<!--Maxium yaw rate, to avoid instability-->
<define name="MAX_R" value="120.0" unit="deg/s"/>

<define name="ESTIMATION_FILT_CUTOFF" value="4.0"/>
<define name="FILT_CUTOFF" value="5.0"/>

Expand Down
33 changes: 21 additions & 12 deletions conf/modules/stabilization_indi.xml
View file
Expand Up @@ -11,12 +11,25 @@
<define name="SP_MAX_R" value="90." description="max setpoint for yaw rate" unit="deg/s"/>
<define name="DEADBAND_R" value="250" description="deadband on yaw rate input"/>
</section>
<section name="ATTITUDE_REFERENCE" prefix="STABILIZATION_ATTITUDE_">
<define name="REF_OMEGA_P" value="400" description="reference generator omega param on roll rate" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.9" description="reference generator zeta param on roll rate"/>
<define name="REF_MAX_P" value="300." description="reference generator max roll rate" unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)" description="reference generator max roll acceleration"/>
<define name="REF_OMEGA_Q" value="400" description="reference generator omega param on pitch rate" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.9" description="reference generator zeta param on pitch rate"/>
<define name="REF_MAX_Q" value="300." description="reference generator max pitch rate" unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)" description="reference generator max pitch acceleration"/>
<define name="REF_OMEGA_R" value="250" description="reference generator omega param on yaw rate" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.9" description="reference generator zeta param on yaw rate"/>
<define name="REF_MAX_R" value="180." description="reference generator max yaw rate" unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)" description="reference generator max yaw acceleration"/>
</section>
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<define name="G1_ROLL" value="{20 , -20, -20 , 20 }" description="control effectiveness of every actuator on the roll axis"/>
<define name="G1_PITCH" value="{14 , 14, -14 , -14 }" description="control effectiveness of every actuator on the pitch axis"/>
<define name="G1_YAW" value="{-1, 1, -1, 1}" description="control effectiveness of every actuator on the yaw axis"/>
<define name="G1_THRUST" value="{-.4, -.4, -.4, -.4}" description="control effectiveness of every actuator on the thrust axis"/>
<define name="G2" value="{-60.0, 60.0, -60.0, 60.0}" description="control effectiveness of every actuator derivative on the yaw axis (important for propellers with strong torque changes)"/>
<define name="G1_P" value="0.0639" description="control effectiveness G1 gain on roll rate"/>
<define name="G1_Q" value="0.0361" description="control effectiveness G1 gain on pitch rate"/>
<define name="G1_R" value="0.0022" description="control effectiveness G1 gain on yaw rate"/>
<define name="G2_R" value="0.1450" description="control effectiveness G2 gain on yaw rate"/>
<define name="REF_ERR_P" value="600.0" description="reference acceleration"/>
<define name="REF_ERR_Q" value="600.0" description="reference acceleration"/>
<define name="REF_ERR_R" value="600.0" description="reference acceleration"/>
Expand All @@ -26,10 +39,9 @@
<define name="MAX_R" value="120.0" description="max yaw rate" unit="deg/s"/>
<define name="FILT_CUTOFF" value="8.0" description="second order cutoff parameter"/>
<define name="ESTIMATION_FILT_CUTOFF" value="8.0" description="second order cutoff parameter"/>
<define name="ACT_DYN" value="{0.1, 0.1, 0.1, 0.1}" description="actuator dynamics"/>
<define name="ACT_IS_SERVO" value="{0,0,0,0}" description="1 for every actuator that is a servo"/>
<define name="ACT_RATE_LIMIT" value="{9600,9600,9600,9600}" description="rate limit in PPRZ units per timestep (depends on control frequency)"/>
<define name="ACT_PREF" value="{0.0, 0.0, 0.0, 0.0}" description="preferred (low energy) actuator value. Important when the system is over-determined!"/>
<define name="ACT_DYN_P" value="0.1" description="first order actuator dynamics on roll rate"/>
<define name="ACT_DYN_Q" value="0.1" description="first order actuator dynamics on pitch rate"/>
<define name="ACT_DYN_R" value="0.1" description="first order actuator dynamics on yaw rate"/>
<define name="USE_ADAPTIVE" value="FALSE|TRUE" description="enable adaptive gains"/>
<define name="ADAPTIVE_MU" value="0.0001" description="adaptation parameter"/>
</section>
Expand Down Expand Up @@ -57,9 +69,6 @@
<file name="stabilization_attitude_quat_indi.c" dir="$(SRC_FIRMWARE)/stabilization"/>
<file name="stabilization_attitude_quat_transformations.c" dir="$(SRC_FIRMWARE)/stabilization"/>
<file name="stabilization_attitude_rc_setpoint.c" dir="$(SRC_FIRMWARE)/stabilization"/>
<file name="wls_alloc.c" dir="$(SRC_FIRMWARE)/stabilization/wls"/>
<file name="qr_solve.c" dir="$(SRC_FIRMWARE)/stabilization/wls"/>
<file name="r8lib_min.c" dir="$(SRC_FIRMWARE)/stabilization/wls"/>
<define name="STABILIZATION_ATTITUDE_TYPE_INT"/>
<define name="STABILIZATION_ATTITUDE_TYPE_H" value="stabilization/stabilization_attitude_quat_indi.h" type="string"/>
<define name="STABILIZATION_ATTITUDE_INDI_FULL" value="true"/>
Expand Down
100 changes: 32 additions & 68 deletions sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
View file
Expand Up @@ -42,15 +42,6 @@
#include "subsystems/actuators.h"
#include "subsystems/abi.h"
#include "filters/low_pass_filter.h"
#include "wls/wls_alloc.h"
#include <stdio.h>

float du_min[4];
float du_max[4];
float du_pref[4];
float indi_v[4];
float* Bwls[4];
int num_iter = 0;

static void lms_estimation(void);
static void get_actuator_state(void);
Expand Down Expand Up @@ -94,14 +85,6 @@ bool act_is_servo[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_IS_SERVO;
bool act_is_servo[INDI_NUM_ACT] = {0};
#endif

#ifdef STABILIZATION_INDI_ACT_PREF
// Preferred (neutral, least energy) actuator value
float act_pref[4] = STABILIZATION_INDI_ACT_PREF;
#else
// Assume 0 is neutral
float act_pref[4] = {0.0};
#endif

float act_dyn[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN;

/** Maximum rate you can request in RC rate mode (rad/s)*/
Expand Down Expand Up @@ -210,12 +193,6 @@ void stabilization_indi_init(void)
//Calculate G1G2_PSEUDO_INVERSE
calc_g1g2_pseudo_inv();

// Initialize the array of pointers to the rows of g1g2
uint8_t i;
for(i=0; i<INDI_OUTPUTS; i++) {
Bwls[i] = g1g2[i];
}

// Initialize the estimator matrices
float_vect_copy(g1_est[0], g1[0], INDI_OUTPUTS*INDI_NUM_ACT);
float_vect_copy(g2_est, g2, INDI_NUM_ACT);
Expand Down Expand Up @@ -319,7 +296,7 @@ void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)

quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading);
}
#include "subsystems/radio_control.h"

/**
* @param att_err attitude error
* @param rate_control boolean that states if we are in rate control or attitude control
Expand Down Expand Up @@ -363,52 +340,45 @@ static void stabilization_indi_calc_cmd(struct Int32Quat *att_err, bool rate_con
//G2 is scaled by INDI_G_SCALING to make it readable
g2_times_du = g2_times_du/INDI_G_SCALING;

float v_thrust = 0.0;
if(indi_thrust_increment_set){
v_thrust = indi_thrust_increment;
// Calculate the increment for each actuator
for(i=0; i<INDI_NUM_ACT; i++) {
indi_du[i] = (g1g2_pseudo_inv[i][0] * (angular_accel_ref.p - angular_acceleration[0]))
+ (g1g2_pseudo_inv[i][1] * (angular_accel_ref.q - angular_acceleration[1]))
+ (g1g2_pseudo_inv[i][2] * (angular_accel_ref.r - angular_acceleration[2] + g2_times_du));
}

//update thrust command such that the current is correctly estimated
stabilization_cmd[COMMAND_THRUST] = (actuator_state[0] + actuator_state[1] + actuator_state[2] + actuator_state[3])/4.0;
} else {
// incremental thrust
if(indi_thrust_increment_set){
// The required body-z acceleration is calculated by the outer loop INDI controller
for(i=0; i<INDI_NUM_ACT; i++) {
v_thrust +=
(stabilization_cmd[COMMAND_THRUST] - actuator_state_filt_vect[i])*Bwls[3][i];
indi_du[i] = indi_du[i] + g1g2_pseudo_inv[i][3]*indi_thrust_increment;
}
}

// 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];
}
// Add the increments to the actuators
float_vect_sum(indi_u, actuator_state_filt_vect, indi_du, INDI_NUM_ACT);
} else
{
// Add the increments to the actuators without the thrust
float_vect_sum(indi_u, actuator_state_filt_vect, indi_du, INDI_NUM_ACT);

//State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST}
static float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100};
// Calculate the average of the actuators as a measure for the thrust
float avg_u_in = 0;
for(i=0; i<INDI_NUM_ACT; i++) {
avg_u_in += indi_u[i];
}
avg_u_in /= INDI_NUM_ACT;

// 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]);
indi_v[2] = (angular_accel_ref.r - angular_acceleration[2] + g2_times_du);
indi_v[3] = v_thrust;
// Make sure the thrust is bounded
Bound(stabilization_cmd[COMMAND_THRUST],0, MAX_PPRZ);

#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
// Calculate the increment for each actuator
for(i=0; i<INDI_NUM_ACT; i++) {
indi_du[i] = (g1g2_pseudo_inv[i][0] * indi_v[0])
+ (g1g2_pseudo_inv[i][1] * indi_v[1])
+ (g1g2_pseudo_inv[i][2] * indi_v[2])
+ (g1g2_pseudo_inv[i][3] * indi_v[3]);
}
#else
// 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);
#endif
//avoid dividing by zero
if(avg_u_in < 1.0) {
avg_u_in = 1.0;
}

// Add the increments to the actuators
float_vect_sum(indi_u, actuator_state_filt_vect, indi_du, INDI_NUM_ACT);
// Rescale the command to the actuators to get the desired thrust
float indi_cmd_scaling = stabilization_cmd[COMMAND_THRUST] / avg_u_in;
float_vect_smul(indi_u, indi_u, indi_cmd_scaling, INDI_NUM_ACT);
}

// Bound the inputs to the actuators
for(i=0; i<INDI_NUM_ACT; i++) {
Expand All @@ -419,11 +389,6 @@ static void stabilization_indi_calc_cmd(struct Int32Quat *att_err, bool rate_con
}
}

if(radio_control.values[RADIO_THROTTLE] < 300) {
float_vect_zero(indi_u, INDI_NUM_ACT);
float_vect_zero(indi_du, INDI_NUM_ACT);
}

// Propagate actuator filters
get_actuator_state();
for(i=0; i<INDI_NUM_ACT; i++) {
Expand Down Expand Up @@ -701,7 +666,6 @@ static void rpm_cb(uint8_t __attribute__((unused)) sender_id, uint16_t UNUSED *r
for(i=0; i<num_act; i++) {
act_obs[i] = (rpm[i] - get_servo_min(i));
act_obs[i] *= (MAX_PPRZ / (float)(get_servo_max(i)-get_servo_min(i)));
Bound(act_obs[i], 0, MAX_PPRZ);
}
#endif
}
Expand Down

0 comments on commit b96b8b0

Please sign in to comment.