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

Allocation merge (#2078)

Browse Source 
* WLS control allocation

* comments, and configure preferred act values

* keep option open to use pseudo-inverse

* add link to burkardt

* Simplify input reset if not flying

* qr solve in math folder

Use INDI_NUM_ACT and INDI_OUTPUTS instead of hardcoded numbers.
Fix code style

* Removed hardcoded number of actuators

precompute number of thrusters
This commit is contained in:
Ewoud Smeur
2017-06-28 00:41:55 +02:00
committed by Gautier Hattenberger
parent 28c343468d
commit 676358ee6e
11 changed files with 2910 additions and 185 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/TUDELFT/tudelft_bebop_indi_actuators.xml
+1 -22
View File
@@ -100,30 +100,12 @@
<!-- setpoint limits for attitude stabilization rc flight --> <!-- setpoint limits for attitude stabilization rc flight -->
<define name="SP_MAX_PHI" value="45" unit="deg"/> <define name="SP_MAX_PHI" value="45" unit="deg"/>
<define name="SP_MAX_THETA" value="45" unit="deg"/> <define name="SP_MAX_THETA" value="45" unit="deg"/>
<define name="SP_MAX_R" value="120" unit="deg/s"/> <define name="SP_MAX_R" value="400" unit="deg/s"/>
<define name="DEADBAND_A" value="0"/> <define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/> <define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="50"/> <define name="DEADBAND_R" value="50"/>
</section> </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_"> <section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- control effectiveness --> <!-- control effectiveness -->
<define name="G1_ROLL" value="{20 , -20, -20 , 20 }"/> <define name="G1_ROLL" value="{20 , -20, -20 , 20 }"/>
@@ -141,9 +123,6 @@
<define name="REF_RATE_Q" value="28.0"/> <define name="REF_RATE_Q" value="28.0"/>
<define name="REF_RATE_R" 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="ESTIMATION_FILT_CUTOFF" value="4.0"/>
<define name="FILT_CUTOFF" value="5.0"/> <define name="FILT_CUTOFF" value="5.0"/>
conf/modules/stabilization_indi.xml
+12 -21
View File
@@ -11,25 +11,12 @@
<define name="SP_MAX_R" value="90." description="max setpoint for yaw rate" unit="deg/s"/> <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"/> <define name="DEADBAND_R" value="250" description="deadband on yaw rate input"/>
</section> </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_"> <section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<define name="G1_P" value="0.0639" description="control effectiveness G1 gain on roll rate"/> <define name="G1_ROLL" value="{20 , -20, -20 , 20 }" description="control effectiveness of every actuator on the roll axis"/>
<define name="G1_Q" value="0.0361" description="control effectiveness G1 gain on pitch rate"/> <define name="G1_PITCH" value="{14 , 14, -14 , -14 }" description="control effectiveness of every actuator on the pitch axis"/>
<define name="G1_R" value="0.0022" description="control effectiveness G1 gain on yaw rate"/> <define name="G1_YAW" value="{-1, 1, -1, 1}" description="control effectiveness of every actuator on the yaw axis"/>
<define name="G2_R" value="0.1450" description="control effectiveness G2 gain on yaw rate"/> <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="REF_ERR_P" value="600.0" description="reference acceleration"/> <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_Q" value="600.0" description="reference acceleration"/>
<define name="REF_ERR_R" value="600.0" description="reference acceleration"/> <define name="REF_ERR_R" value="600.0" description="reference acceleration"/>
@@ -39,9 +26,10 @@
<define name="MAX_R" value="120.0" description="max yaw rate" unit="deg/s"/> <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="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="ESTIMATION_FILT_CUTOFF" value="8.0" description="second order cutoff parameter"/>
<define name="ACT_DYN_P" value="0.1" description="first order actuator dynamics on roll rate"/> <define name="ACT_DYN" value="{0.1, 0.1, 0.1, 0.1}" description="actuator dynamics"/>
<define name="ACT_DYN_Q" value="0.1" description="first order actuator dynamics on pitch rate"/> <define name="ACT_IS_SERVO" value="{0,0,0,0}" description="1 for every actuator that is a servo"/>
<define name="ACT_DYN_R" value="0.1" description="first order actuator dynamics on yaw rate"/> <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="USE_ADAPTIVE" value="FALSE|TRUE" description="enable adaptive gains"/> <define name="USE_ADAPTIVE" value="FALSE|TRUE" description="enable adaptive gains"/>
<define name="ADAPTIVE_MU" value="0.0001" description="adaptation parameter"/> <define name="ADAPTIVE_MU" value="0.0001" description="adaptation parameter"/>
</section> </section>
@@ -69,6 +57,9 @@
<file name="stabilization_attitude_quat_indi.c" dir="$(SRC_FIRMWARE)/stabilization"/> <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_quat_transformations.c" dir="$(SRC_FIRMWARE)/stabilization"/>
<file name="stabilization_attitude_rc_setpoint.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="math/qr_solve"/>
<file name="r8lib_min.c" dir="math/qr_solve"/>
<define name="STABILIZATION_ATTITUDE_TYPE_INT"/> <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_TYPE_H" value="stabilization/stabilization_attitude_quat_indi.h" type="string"/>
<define name="STABILIZATION_ATTITUDE_INDI_FULL" value="true"/> <define name="STABILIZATION_ATTITUDE_INDI_FULL" value="true"/>
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+116 -57
View File
@@ -42,6 +42,24 @@
#include "subsystems/actuators.h" #include "subsystems/actuators.h"
#include "subsystems/abi.h" #include "subsystems/abi.h"
#include "filters/low_pass_filter.h" #include "filters/low_pass_filter.h"
#include "wls/wls_alloc.h"
#include <stdio.h>
//only 4 actuators supported for now
#define INDI_NUM_ACT 4
// outputs: roll, pitch, yaw, thrust
#define INDI_OUTPUTS 4
// Factor that the estimated G matrix is allowed to deviate from initial one
#define INDI_ALLOWED_G_FACTOR 2.0
// Scaling for the control effectiveness to make it readible
#define INDI_G_SCALING 1000.0
float du_min[INDI_NUM_ACT];
float du_max[INDI_NUM_ACT];
float du_pref[INDI_NUM_ACT];
float indi_v[INDI_OUTPUTS];
float *Bwls[INDI_OUTPUTS];
int num_iter = 0;
static void lms_estimation(void); static void lms_estimation(void);
static void get_actuator_state(void); static void get_actuator_state(void);
@@ -60,15 +78,6 @@ struct ReferenceSystem reference_acceleration = {
STABILIZATION_INDI_REF_RATE_R, STABILIZATION_INDI_REF_RATE_R,
}; };
//only 4 actuators supported for now
#define INDI_NUM_ACT 4
// outputs: roll, pitch, yaw, thrust
#define INDI_OUTPUTS 4
// Factor that the estimated G matrix is allowed to deviate from initial one
#define INDI_ALLOWED_G_FACTOR 2.0
// Scaling for the control effectiveness to make it readible
#define INDI_G_SCALING 1000.0
#if STABILIZATION_INDI_USE_ADAPTIVE #if STABILIZATION_INDI_USE_ADAPTIVE
bool indi_use_adaptive = true; bool indi_use_adaptive = true;
#else #else
@@ -85,6 +94,14 @@ bool act_is_servo[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_IS_SERVO;
bool act_is_servo[INDI_NUM_ACT] = {0}; bool act_is_servo[INDI_NUM_ACT] = {0};
#endif #endif
#ifdef STABILIZATION_INDI_ACT_PREF
// Preferred (neutral, least energy) actuator value
float act_pref[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_PREF;
#else
// Assume 0 is neutral
float act_pref[INDI_NUM_ACT] = {0.0};
#endif
float act_dyn[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN; float act_dyn[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN;
/** Maximum rate you can request in RC rate mode (rad/s)*/ /** Maximum rate you can request in RC rate mode (rad/s)*/
@@ -110,12 +127,18 @@ float estimation_rate_d[INDI_NUM_ACT];
float estimation_rate_dd[INDI_NUM_ACT]; float estimation_rate_dd[INDI_NUM_ACT];
float du_estimation[INDI_NUM_ACT]; float du_estimation[INDI_NUM_ACT];
float ddu_estimation[INDI_NUM_ACT]; float ddu_estimation[INDI_NUM_ACT];
float mu1[4] = {0.00001, 0.00001, 0.000003, 0.000002};
// The learning rate per axis (roll, pitch, yaw, thrust)
float mu1[INDI_OUTPUTS] = {0.00001, 0.00001, 0.000003, 0.000002};
// The learning rate for the propeller inertia (scaled by 512 wrt mu1)
float mu2 = 0.002; float mu2 = 0.002;
// other variables // other variables
float act_obs[INDI_NUM_ACT]; float act_obs[INDI_NUM_ACT];
// Number of actuators used to provide thrust
int32_t num_thrusters;
struct Int32Eulers stab_att_sp_euler; struct Int32Eulers stab_att_sp_euler;
struct Int32Quat stab_att_sp_quat; struct Int32Quat stab_att_sp_quat;
@@ -130,7 +153,8 @@ bool indi_thrust_increment_set = false;
float g1g2_pseudo_inv[INDI_NUM_ACT][INDI_OUTPUTS]; float g1g2_pseudo_inv[INDI_NUM_ACT][INDI_OUTPUTS];
float g2[INDI_NUM_ACT] = STABILIZATION_INDI_G2; //scaled by INDI_G_SCALING float g2[INDI_NUM_ACT] = STABILIZATION_INDI_G2; //scaled by INDI_G_SCALING
float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL, float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST}; STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST
};
float g1g2[INDI_OUTPUTS][INDI_NUM_ACT]; float g1g2[INDI_OUTPUTS][INDI_NUM_ACT];
float g1_est[INDI_OUTPUTS][INDI_NUM_ACT]; float g1_est[INDI_OUTPUTS][INDI_NUM_ACT];
float g2_est[INDI_NUM_ACT]; float g2_est[INDI_NUM_ACT];
@@ -193,6 +217,12 @@ void stabilization_indi_init(void)
//Calculate G1G2_PSEUDO_INVERSE //Calculate G1G2_PSEUDO_INVERSE
calc_g1g2_pseudo_inv(); 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 // Initialize the estimator matrices
float_vect_copy(g1_est[0], g1[0], INDI_OUTPUTS * INDI_NUM_ACT); float_vect_copy(g1_est[0], g1[0], INDI_OUTPUTS * INDI_NUM_ACT);
float_vect_copy(g2_est, g2, INDI_NUM_ACT); float_vect_copy(g2_est, g2, INDI_NUM_ACT);
@@ -200,6 +230,12 @@ void stabilization_indi_init(void)
float_vect_copy(g1_init[0], g1[0], INDI_OUTPUTS * INDI_NUM_ACT); float_vect_copy(g1_init[0], g1[0], INDI_OUTPUTS * INDI_NUM_ACT);
float_vect_copy(g2_init, g2, INDI_NUM_ACT); float_vect_copy(g2_init, g2, INDI_NUM_ACT);
// Assume all non-servos are delivering thrust
num_thrusters = INDI_NUM_ACT;
for (i = 0; i < INDI_NUM_ACT; i++) {
num_thrusters -= act_is_servo[i];
}
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INDI_G, send_indi_g); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INDI_G, send_indi_g);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_REF_QUAT, send_ahrs_ref_quat); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_REF_QUAT, send_ahrs_ref_quat);
@@ -226,7 +262,8 @@ void stabilization_indi_enter(void)
/** /**
* Function that resets the filters to zeros * Function that resets the filters to zeros
*/ */
void init_filters(void) { void init_filters(void)
{
// tau = 1/(2*pi*Fc) // tau = 1/(2*pi*Fc)
float tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF); float tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF);
float tau_est = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF); float tau_est = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF);
@@ -340,45 +377,57 @@ 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 is scaled by INDI_G_SCALING to make it readable
g2_times_du = g2_times_du / INDI_G_SCALING; g2_times_du = g2_times_du / INDI_G_SCALING;
// Calculate the increment for each actuator float v_thrust = 0.0;
if (indi_thrust_increment_set) {
v_thrust = indi_thrust_increment;
//update thrust command such that the current is correctly estimated
stabilization_cmd[COMMAND_THRUST] = 0;
for (i = 0; i < INDI_NUM_ACT; i++) { for (i = 0; i < INDI_NUM_ACT; i++) {
indi_du[i] = (g1g2_pseudo_inv[i][0] * (angular_accel_ref.p - angular_acceleration[0])) stabilization_cmd[COMMAND_THRUST] += actuator_state[i] * -((int32_t) act_is_servo[i] - 1);
+ (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)); stabilization_cmd[COMMAND_THRUST] /= num_thrusters;
} else {
// incremental thrust
for (i = 0; i < INDI_NUM_ACT; i++) {
v_thrust +=
(stabilization_cmd[COMMAND_THRUST] - actuator_state_filt_vect[i]) * Bwls[3][i];
}
} }
if(indi_thrust_increment_set){ // Calculate the min and max increments
// The required body-z acceleration is calculated by the outer loop INDI controller
for (i = 0; i < INDI_NUM_ACT; i++) { for (i = 0; i < INDI_NUM_ACT; i++) {
indi_du[i] = indi_du[i] + g1g2_pseudo_inv[i][3]*indi_thrust_increment; 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]);
indi_v[2] = (angular_accel_ref.r - angular_acceleration[2] + g2_times_du);
indi_v[3] = v_thrust;
#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
// Add the increments to the actuators // Add the increments to the actuators
float_vect_sum(indi_u, actuator_state_filt_vect, indi_du, INDI_NUM_ACT); 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);
// 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;
// Make sure the thrust is bounded
Bound(stabilization_cmd[COMMAND_THRUST],0, MAX_PPRZ);
//avoid dividing by zero
if(avg_u_in < 1.0) {
avg_u_in = 1.0;
}
// 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 // Bound the inputs to the actuators
for (i = 0; i < INDI_NUM_ACT; i++) { for (i = 0; i < INDI_NUM_ACT; i++) {
@@ -389,6 +438,12 @@ static void stabilization_indi_calc_cmd(struct Int32Quat *att_err, bool rate_con
} }
} }
//Don't increment if not flying (not armed)
if (!in_flight) {
float_vect_zero(indi_u, INDI_NUM_ACT);
float_vect_zero(indi_du, INDI_NUM_ACT);
}
// Propagate actuator filters // Propagate actuator filters
get_actuator_state(); get_actuator_state();
for (i = 0; i < INDI_NUM_ACT; i++) { for (i = 0; i < INDI_NUM_ACT; i++) {
@@ -402,12 +457,8 @@ static void stabilization_indi_calc_cmd(struct Int32Quat *att_err, bool rate_con
actuator_state_filt_vectdd[i] = (actuator_state_filt_vectd[i] - actuator_state_filt_vectd_prev) * PERIODIC_FREQUENCY; actuator_state_filt_vectdd[i] = (actuator_state_filt_vectd[i] - actuator_state_filt_vectd_prev) * PERIODIC_FREQUENCY;
} }
//Don't increment if thrust is off // Use online effectiveness estimation only when flying
if(!in_flight) { if (in_flight && indi_use_adaptive) {
float_vect_zero(indi_u, INDI_NUM_ACT);
float_vect_zero(actuator_state_filt_vect, INDI_NUM_ACT);
}
else if(indi_use_adaptive) {
lms_estimation(); lms_estimation();
} }
@@ -483,7 +534,8 @@ void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinat
* If this is not available it will use a first order filter to approximate the actuator state. * If this is not available it will use a first order filter to approximate the actuator state.
* It is also possible to model rate limits (unit: PPRZ/loop cycle) * It is also possible to model rate limits (unit: PPRZ/loop cycle)
*/ */
void get_actuator_state(void) { void get_actuator_state(void)
{
#if INDI_RPM_FEEDBACK #if INDI_RPM_FEEDBACK
float_vect_copy(actuator_state, act_obs, INDI_NUM_ACT); float_vect_copy(actuator_state, act_obs, INDI_NUM_ACT);
#else #else
@@ -518,7 +570,8 @@ void get_actuator_state(void) {
* The elements are stored in a different matrix, * The elements are stored in a different matrix,
* because the old matrix is necessary to caclulate more elements. * because the old matrix is necessary to caclulate more elements.
*/ */
void calc_g1_element(float ddx_error, int8_t i, int8_t j, float mu) { void calc_g1_element(float ddx_error, int8_t i, int8_t j, float mu)
{
g1_est[i][j] = g1_est[i][j] - du_estimation[j] * mu * ddx_error; g1_est[i][j] = g1_est[i][j] - du_estimation[j] * mu * ddx_error;
} }
@@ -531,7 +584,8 @@ void calc_g1_element(float ddx_error, int8_t i, int8_t j, float mu) {
* The elements are stored in a different matrix, * The elements are stored in a different matrix,
* because the old matrix is necessary to caclulate more elements. * because the old matrix is necessary to caclulate more elements.
*/ */
void calc_g2_element(float ddx_error, int8_t j, float mu) { void calc_g2_element(float ddx_error, int8_t j, float mu)
{
g2_est[j] = g2_est[j] - ddu_estimation[j] * mu * ddx_error; g2_est[j] = g2_est[j] - ddu_estimation[j] * mu * ddx_error;
} }
@@ -540,7 +594,8 @@ void calc_g2_element(float ddx_error, int8_t j, float mu) {
* It is assumed that disturbances do not play a large role. * It is assumed that disturbances do not play a large role.
* All elements of the G1 and G2 matrices are be estimated. * All elements of the G1 and G2 matrices are be estimated.
*/ */
void lms_estimation(void) { void lms_estimation(void)
{
// Get the acceleration in body axes // Get the acceleration in body axes
struct Int32Vect3 *body_accel_i; struct Int32Vect3 *body_accel_i;
@@ -608,19 +663,21 @@ void lms_estimation(void) {
/** /**
* Function that calculates the pseudo-inverse of (G1+G2). * Function that calculates the pseudo-inverse of (G1+G2).
*/ */
void calc_g1g2_pseudo_inv(void) { void calc_g1g2_pseudo_inv(void)
{
//sum of G1 and G2 //sum of G1 and G2
int8_t i; int8_t i;
int8_t j; int8_t j;
for (i = 0; i < INDI_OUTPUTS; i++) { for (i = 0; i < INDI_OUTPUTS; i++) {
for (j = 0; j < INDI_NUM_ACT; j++) { for (j = 0; j < INDI_NUM_ACT; j++) {
if(i!=2) if (i != 2) {
g1g2[i][j] = g1[i][j] / INDI_G_SCALING; g1g2[i][j] = g1[i][j] / INDI_G_SCALING;
else } else {
g1g2[i][j] = (g1[i][j] + g2[j]) / INDI_G_SCALING; g1g2[i][j] = (g1[i][j] + g2[j]) / INDI_G_SCALING;
} }
} }
}
//G1G2*transpose(G1G2) //G1G2*transpose(G1G2)
//calculate matrix multiplication of its transpose INDI_OUTPUTSxnum_act x num_actxINDI_OUTPUTS //calculate matrix multiplication of its transpose INDI_OUTPUTSxnum_act x num_actxINDI_OUTPUTS
@@ -666,6 +723,7 @@ static void rpm_cb(uint8_t __attribute__((unused)) sender_id, uint16_t UNUSED *r
for (i = 0; i < num_act; i++) { for (i = 0; i < num_act; i++) {
act_obs[i] = (rpm[i] - get_servo_min(i)); act_obs[i] = (rpm[i] - get_servo_min(i));
act_obs[i] *= (MAX_PPRZ / (float)(get_servo_max(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 #endif
} }
@@ -679,7 +737,8 @@ static void thrust_cb(uint8_t UNUSED sender_id, float thrust_increment)
indi_thrust_increment_set = true; indi_thrust_increment_set = true;
} }
static void bound_g_mat(void) { static void bound_g_mat(void)
{
int8_t i; int8_t i;
int8_t j; int8_t j;
for (j = 0; j < INDI_NUM_ACT; j++) { for (j = 0; j < INDI_NUM_ACT; j++) {
sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.c
+363
View File
@@ -0,0 +1,363 @@
/*
* Copyright (C) Anton Naruta && Daniel Hoppener
* MAVLab Delft University of Technology
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/** @file wls_alloc.c
* @brief This is an active set algorithm for WLS control allocation
*
* This algorithm will find the optimal inputs to produce the least error wrt
* the control objective, taking into account the weighting matrices on the
* control objective and the control effort.
*
* The algorithm is described in:
* Prioritized Control Allocation for Quadrotors Subject to Saturation -
* E.J.J. Smeur, D.C. Höppener, C. de Wagter. In IMAV 2017
*
* written by Anton Naruta && Daniel Hoppener 2016
* MAVLab Delft University of Technology
*/
#include "wls_alloc.h"
#include <stdio.h>
#include "std.h"
#include <string.h>
#include <math.h>
#include <float.h>
#include "math/qr_solve/qr_solve.h"
#include "math/qr_solve/r8lib_min.h"
void print_final_values(int n_u, int n_v, float* u, float** B, float* v, float* umin, float* umax);
void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, float* p_free);
// provide loop feedback
#define WLS_VERBOSE FALSE
/**
* @brief Wrapper for qr solve
*
* Possible to use a different solver if needed.
* Solves a system of the form Ax = b for x.
*
* @param m number of rows
* @param n number of columns
*/
void qr_solve_wrapper(int m, int n, float** A, float* b, float* x) {
float in[m * n];
// convert A to 1d array
int k = 0;
for (int j = 0; j < n; j++) {
for (int i = 0; i < m; i++) {
in[k++] = A[i][j];
}
}
// use solver
qr_solve(m, n, in, b, x);
}
/**
* @brief active set algorithm for control allocation
*
* 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
* @param umin The minimum u vector
* @param umax The maximum u vector
* @param B The control effectiveness matrix
* @param n_u Length of u
* @param n_v Lenght of v
* @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
*
* @return Number of iterations, -1 upon failure
*/
int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
int n_u, int n_v, float* u_guess, float* W_init, float* Wv,
float* Wu, float* up, float gamma_sq, int imax) {
// allocate variables, use defaults where parameters are set to 0
if(!gamma_sq) gamma_sq = 100000;
if(!imax) imax = 100;
int n_c = n_u + n_v;
float A[n_c][n_u];
float A_free[n_c][n_u];
// Create a pointer array to the rows of A_free
// such that we can pass it to a function
float * A_free_ptr[n_c];
for(int i = 0; i < n_c; i++)
A_free_ptr[i] = A_free[i];
float b[n_c];
float d[n_c];
int free_index[n_u];
int free_index_lookup[n_u];
int n_free = 0;
int free_chk = -1;
int iter = 0;
float p_free[n_u];
float p[n_u];
float u_opt[n_u];
int infeasible_index[n_u] UNUSED;
int n_infeasible = 0;
float lambda[n_u];
float W[n_u];
// 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;
}
} else {
for (int i = 0; i < n_u; i++) {
u[i] = u_guess[i];
}
}
W_init ? memcpy(W, W_init, n_u * sizeof(float))
: memset(W, 0, n_u * sizeof(float));
memset(free_index_lookup, -1, n_u * sizeof(float));
// find free indices
for (int i = 0; i < n_u; i++) {
if (W[i] == 0) {
free_index_lookup[i] = n_free;
free_index[n_free++] = i;
}
}
// 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];
d[i] = b[i];
for (int j = 0; j < n_u; 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];
}
}
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] * up[i] : up[i]) : 0;
d[i] = b[i] - A[i][i - n_v] * u[i - n_v];
}
// -------------- 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));
// Construct a matrix with the free columns of A
if (free_chk != n_free) {
for (int i = 0; i < n_c; i++) {
for (int j = 0; j < n_free; j++) {
A_free[i][j] = A[i][free_index[j]];
}
}
free_chk = n_free;
}
if (n_free) {
// Still free variables left, calculate corresponding solution
// use a solver to find the solution to A_free*p_free = d
qr_solve_wrapper(n_c, n_free, A_free_ptr, d, p_free);
//print results current step
#if WLS_VERBOSE
print_in_and_outputs(n_c, n_free, A_free_ptr, d, p_free);
#endif
}
// Set the nonzero values of p and add to u_opt
for (int i = 0; i < n_free; i++) {
p[free_index[i]] = p_free[i];
u_opt[free_index[i]] += p_free[i];
}
// check limits
n_infeasible = 0;
for (int i = 0; i < n_u; i++) {
if (u_opt[i] >= (umax[i] + 1.0) || u_opt[i] <= (umin[i] - 1.0)) {
infeasible_index[n_infeasible++] = i;
}
}
// 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));
// 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++) {
lambda[k] += A[i][k] * d[i];
}
}
bool break_flag = true;
// lambda = lambda x W;
for (int i = 0; i < n_u; i++) {
lambda[i] *= W[i];
// if any lambdas are negative, keep looking for solution
if (lambda[i] < -FLT_EPSILON) {
break_flag = false;
W[i] = 0;
// add a free index
if (free_index_lookup[i] < 0) {
free_index_lookup[i] = n_free;
free_index[n_free++] = i;
}
}
}
if (break_flag) {
#if WLS_VERBOSE
print_final_values(1, n_u, n_v, u, B, v, umin, umax);
#endif
// if solution is found, return number of iterations
return iter;
}
} else {
float alpha = INFINITY;
float alpha_tmp;
int id_alpha = 0;
// find the lowest distance from the limit among the free variables
for (int i = 0; i < n_free; i++) {
int id = free_index[i];
if(fabs(p[id]) > FLT_EPSILON) {
alpha_tmp = (p[id] < 0) ? (umin[id] - u[id]) / p[id]
: (umax[id] - u[id]) / p[id];
} else {
alpha_tmp = INFINITY;
}
if (alpha_tmp < alpha) {
alpha = alpha_tmp;
id_alpha = id;
}
}
// update input u = u + alpha*p
for (int i = 0; i < n_u; i++) {
u[i] += alpha * p[i];
}
// update d = d-alpha*A*p_free
for (int i = 0; i < n_c; i++) {
for (int k = 0; k < n_free; k++) {
d[i] -= A_free[i][k] * alpha * p_free[k];
}
}
// get rid of a free index
W[id_alpha] = (p[id_alpha] > 0) ? 1.0 : -1.0;
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] = -1;
}
}
// solution failed, return negative one to indicate failure
return -1;
}
#if WLS_VERBOSE
void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, float* p_free) {
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 j = 0; j < n_free; j++) {
printf("%f ", A_free_ptr[i][j]);
}
printf("\n");
}
printf("d = ");
for (int j = 0; j < n_c; j++) {
printf("%f ", d[j]);
}
printf("\noutput = ");
for (int j = 0; j < n_free; j++) {
printf("%f ", p_free[j]);
}
printf("\n\n");
}
void print_final_values(int n_u, int n_v, float* u, float** B, float* v, float* umin, float* umax) {
printf("n_u = %d n_v = %d\n", n_u, n_v);
printf("B =\n");
for(int i = 0; i < n_v; i++) {
for (int j = 0; j < n_u; j++) {
printf("%f ", B[i][j]);
}
printf("\n");
}
printf("v = ");
for (int j = 0; j < n_v; j++) {
printf("%f ", v[j]);
}
printf("\nu = ");
for (int j = 0; j < n_u; j++) {
printf("%f ", u[j]);
}
printf("\n");
printf("\numin = ");
for (int j = 0; j < n_u; j++) {
printf("%f ", umin[j]);
}
printf("\n");
printf("\numax = ");
for (int j = 0; j < n_u; j++) {
printf("%f ", umax[j]);
}
printf("\n\n");
}
#endif
sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.h
+61
View File
@@ -0,0 +1,61 @@
/*
* Copyright (C) Anton Naruta && Daniel Hoppener
* MAVLab Delft University of Technology
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @brief Wrapper for qr solve
*
* Possible to use a different solver if needed.
* Solves a system of the form Ax = b for x.
*
* @param m number of rows
* @param n number of columns
*/
void qr_solve_wrapper(int m, int n, float** A, float* b, float* x);
/**
* @brief active set algorithm for control allocation
*
* 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
* @param umin The minimum u vector
* @param umax The maximum u vector
* @param B The control effectiveness matrix
* @param n_u Length of u
* @param n_v Lenght of v
* @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
*
* @return Number of iterations, -1 upon failure
*/
int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
int n_u, int n_w, float* u_guess, float* W_init, float* Wv,
float* Wu, float* ud, float gamma, int imax);
sw/airborne/math/qr_solve/qr_solve.c
+1616
View File
File diff suppressed because it is too large Load Diff
sw/airborne/math/qr_solve/qr_solve.h
+27
View File
@@ -0,0 +1,27 @@
/*
* This is part of the qr_solve library from John Burkardt.
* http://people.sc.fsu.edu/~jburkardt/c_src/qr_solve/qr_solve.html
*
* It is slightly modified to make it compile on simple microprocessors,
* and to remove all dynamic memory.
*
* This code is distributed under the GNU LGPL license.
*/
void daxpy ( int n, float da, float dx[], int incx, float dy[], int incy );
float ddot ( int n, float dx[], int incx, float dy[], int incy );
float dnrm2 ( int n, float x[], int incx );
void dqrank ( float a[], int lda, int m, int n, float tol, int *kr,
int jpvt[], float qraux[] );
void dqrdc ( float a[], int lda, int n, int p, float qraux[], int jpvt[],
float work[], int job );
int dqrls ( float a[], int lda, int m, int n, float tol, int *kr, float b[],
float x[], float rsd[], int jpvt[], float qraux[], int itask );
void dqrlss ( float a[], int lda, int m, int n, int kr, float b[], float x[],
float rsd[], int jpvt[], float qraux[] );
int dqrsl ( float a[], int lda, int n, int k, float qraux[], float y[],
float qy[], float qty[], float b[], float rsd[], float ab[], int job );
void drotg ( float *sa, float *sb, float *c, float *s );
void dscal ( int n, float sa, float x[], int incx );
void dswap ( int n, float x[], int incx, float y[], int incy );
void qr_solve ( int m, int n, float a[], float b[], float x[] );
sw/airborne/math/qr_solve/r8lib_min.c
+554
View File
File diff suppressed because it is too large Load Diff
sw/airborne/math/qr_solve/r8lib_min.h
+25
View File
@@ -0,0 +1,25 @@
/*
* This file is a modified subset of the R8lib from John Burkardt.
* http://people.sc.fsu.edu/~jburkardt/c_src/r8lib/r8lib.html
*
* It is the minimal set of functions from r8lib needed to use qr_solve.
*
* This code is distributed under the GNU LGPL license.
*/
void r8mat_copy_new ( int m, int n, float a1[], float a2[] );
float r8_epsilon ( void );
float r8mat_amax ( int m, int n, float a[] );
float r8_sign ( float x );
float r8_max ( float x, float y );
float *r8mat_transpose_new ( int m, int n, float a[] );
float *r8mat_mm_new ( int n1, int n2, int n3, float a[], float b[] );
float *r8mat_cholesky_factor ( int n, float a[], int *flag );
float *r8mat_mv_new ( int m, int n, float a[], float x[] );
float *r8mat_cholesky_solve ( int n, float l[], float b[] );
float *r8mat_l_solve ( int n, float a[], float b[] );
float *r8mat_lt_solve ( int n, float a[], float b[] );
float *r8mat_mtv_new ( int m, int n, float a[], float x[] );
float r8vec_max ( int n, float r8vec[] );
int i4_min ( int i1, int i2 );
int i4_max ( int i1, int i2 );
sw/airborne/test/Makefile
+4 -1
View File
@@ -21,8 +21,11 @@ test_algebra: test_algebra.c ../math/pprz_trig_int.c ../math/pprz_algebra_int.c
test_bla: test_bla.c ../math/pprz_trig_int.c ../math/pprz_algebra_int.c ../math/pprz_algebra_float.c ../math/pprz_algebra_double.c test_bla: test_bla.c ../math/pprz_trig_int.c ../math/pprz_algebra_int.c ../math/pprz_algebra_float.c ../math/pprz_algebra_double.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS) $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
test_alloc: test_alloc.c ../firmwares/rotorcraft/stabilization/wls/wls_alloc.c ../math/qr_solve/r8lib_min.c ../math/qr_solve/qr_solve.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
%.exe : %.c %.exe : %.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS) $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
clean: clean:
$(Q)rm -f *~ test_matrix test_geodetic test_algebra test_bla *.exe $(Q)rm -f *~ test_matrix test_geodetic test_algebra test_bla test_alloc *.exe
sw/airborne/test/test_alloc.c
+47
View File
@@ -0,0 +1,47 @@
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "std.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_algebra_double.h"
#include "math/pprz_algebra_int.h"
#include "pprz_algebra_print.h"
#include "firmwares/rotorcraft/stabilization/wls/wls_alloc.h"
#define INDI_OUTPUTS 4
#define INDI_NUM_ACT 4
int main(int argc, char **argv)
{
float u_min[INDI_NUM_ACT] = { -107, -19093, 0, -95, };
float u_max[INDI_NUM_ACT] = {19093, 107, 4600, 4505, };
float g1g2[INDI_OUTPUTS][INDI_NUM_ACT] = {
{ 0, 0, -0.0105, 0.0107016},
{ -0.0030044, 0.0030044, 0.035, 0.035},
{ -0.004856, -0.004856, 0, 0},
{ 0, 0, -0.0011, -0.0011}
};
//State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST}
static float Wv[INDI_OUTPUTS] = {100, 1000, 0.1, 10};
/*static float Wv[INDI_OUTPUTS] = {10, 10, 0.1, 1};*/
// The control objective in array format
float indi_v[INDI_OUTPUTS] = {10.8487, -10.5658, 6.8383, 1.8532};
float indi_du[INDI_NUM_ACT];
// Initialize the array of pointers to the rows of g1g2
float *Bwls[INDI_OUTPUTS];
uint8_t i;
for (i = 0; i < INDI_OUTPUTS; i++) {
Bwls[i] = g1g2[i];
}
// WLS Control Allocator
int num_iter =
wls_alloc(indi_du, indi_v, u_min, u_max, Bwls, INDI_NUM_ACT, INDI_OUTPUTS, 0, 0, Wv, 0, 0, 10000, 10);
printf("du = %f, %f, %f, %f\n", indi_du[0], indi_du[1], indi_du[2], indi_du[3]);
}