40 #include "generated/airframe.h"
49 #define INDI_ALLOWED_G_FACTOR 2.0
51 #ifdef STABILIZATION_INDI_FILT_CUTOFF_P
52 #define STABILIZATION_INDI_FILTER_ROLL_RATE TRUE
54 #define STABILIZATION_INDI_FILT_CUTOFF_P 20.0
57 #ifdef STABILIZATION_INDI_FILT_CUTOFF_Q
58 #define STABILIZATION_INDI_FILTER_PITCH_RATE TRUE
60 #define STABILIZATION_INDI_FILT_CUTOFF_Q 20.0
63 #ifdef STABILIZATION_INDI_FILT_CUTOFF_R
64 #define STABILIZATION_INDI_FILTER_YAW_RATE TRUE
66 #define STABILIZATION_INDI_FILT_CUTOFF_R 20.0
70 #ifndef STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
71 #define STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE FALSE
74 #ifndef STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
75 #define STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER FALSE
80 #ifndef STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD
81 #define STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD 8.0
85 #ifndef STABILIZATION_INDI_G1_THRUST_X
86 #error "You must define STABILIZATION_INDI_G1_THRUST_X for your number of INDI_OUTPUTS"
90 #ifdef SetCommandsFromRC
91 #warning SetCommandsFromRC not used: STAB_INDI writes actuators directly
94 #ifdef SetAutoCommandsFromRC
95 #warning SetAutoCommandsFromRC not used: STAB_INDI writes actuators directly
99 #if !STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
100 #if INDI_NUM_ACT > WLS_N_U
101 #error Matrix-WLS_N_U too small or not defined: define WLS_N_U >= INDI_NUM_ACT in airframe file
103 #if INDI_OUTPUTS > WLS_N_V
104 #error Matrix-WLS_N_V too small or not defined: define WLS_N_U >= INDI_OUTPUTS in airframe file
119 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
120 static void calc_g1g2_pseudo_inv(
void);
127 STABILIZATION_INDI_REF_ERR_P,
128 STABILIZATION_INDI_REF_ERR_Q,
129 STABILIZATION_INDI_REF_ERR_R
132 STABILIZATION_INDI_REF_RATE_P,
133 STABILIZATION_INDI_REF_RATE_Q,
134 STABILIZATION_INDI_REF_RATE_R
138 #if STABILIZATION_INDI_USE_ADAPTIVE
144 #ifdef STABILIZATION_INDI_ACT_RATE_LIMIT
145 float act_rate_limit[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_RATE_LIMIT;
148 #ifdef STABILIZATION_INDI_ACT_IS_SERVO
149 bool act_is_servo[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_IS_SERVO;
154 #ifdef STABILIZATION_INDI_ACT_IS_THRUSTER_X
162 #ifdef STABILIZATION_INDI_ACT_PREF
164 float act_pref[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_PREF;
170 #ifdef STABILIZATION_INDI_ACT_DYN
171 #warning STABILIZATION_INDI_ACT_DYN is deprecated, use STABILIZATION_INDI_ACT_FREQ instead.
172 #warning You now have to define the continuous time corner frequency in rad/s of the actuators.
173 #warning "Use -ln(1 - old_number) * PERIODIC_FREQUENCY to compute it from the old values."
180 #ifdef STABILIZATION_INDI_WLS_PRIORITIES
181 static float Wv[INDI_OUTPUTS] = STABILIZATION_INDI_WLS_PRIORITIES;
184 #if INDI_OUTPUTS == 5
185 static float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100, 100};
187 static float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100};
194 #ifdef STABILIZATION_INDI_WLS_WU
195 float indi_Wu[INDI_NUM_ACT] = STABILIZATION_INDI_WLS_WU;
197 float indi_Wu[INDI_NUM_ACT] = {[0 ... INDI_NUM_ACT - 1] = 1.0};
224 float mu1[INDI_OUTPUTS] = {0.00001, 0.00001, 0.000003, 0.000002};
240 #if STABILIZATION_INDI_RPM_FEEDBACK
241 #ifndef STABILIZATION_INDI_ACT_FEEDBACK_ID
242 #define STABILIZATION_INDI_ACT_FEEDBACK_ID ABI_BROADCAST
244 PRINT_CONFIG_VAR(STABILIZATION_INDI_ACT_FEEDBACK_ID)
257 float g2[INDI_NUM_ACT] = STABILIZATION_INDI_G2;
258 #ifdef STABILIZATION_INDI_G1
259 float g1[INDI_OUTPUTS][INDI_NUM_ACT] = STABILIZATION_INDI_G1;
261 #if INDI_OUTPUTS == 5
262 float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
263 STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW,
264 STABILIZATION_INDI_G1_THRUST, STABILIZATION_INDI_G1_THRUST_X
267 float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
268 STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST
273 float g1g2[INDI_OUTPUTS][INDI_NUM_ACT];
274 float g1_est[INDI_OUTPUTS][INDI_NUM_ACT];
284 #if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
294 #if PERIODIC_TELEMETRY
299 pprz_msg_send_EFF_MAT_G(trans,
dev, AC_ID,
303 INDI_NUM_ACT,
g1g2[0],
304 INDI_NUM_ACT,
g1g2[1],
305 INDI_NUM_ACT,
g1g2[2],
306 INDI_NUM_ACT,
g1g2[3],
313 pprz_msg_send_AHRS_REF_QUAT(trans,
dev, AC_ID,
328 pprz_msg_send_STAB_ATTITUDE(trans,
dev, AC_ID,
358 #ifdef STABILIZATION_INDI_ACT_FREQ
360 for (i = 0; i < INDI_NUM_ACT; i++) {
365 #if STABILIZATION_INDI_RPM_FEEDBACK
380 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
382 calc_g1g2_pseudo_inv();
386 for (i = 0; i < INDI_OUTPUTS; i++) {
400 for (i = 0; i < INDI_NUM_ACT; i++) {
409 #if PERIODIC_TELEMETRY
441 float sample_time = 1.0 / PERIODIC_FREQUENCY;
444 for (i = 0; i < 3; i++) {
450 for (i = 0; i < INDI_NUM_ACT; i++) {
458 #if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
559 float rate_vect[3] = {body_rates->
p, body_rates->
q, body_rates->
r};
561 for (i = 0; i < 3; i++) {
580 #if STABILIZATION_INDI_FILTER_ROLL_RATE
581 #if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
587 rates_filt.
p = body_rates->
p;
589 #if STABILIZATION_INDI_FILTER_PITCH_RATE
590 #if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
596 rates_filt.
q = body_rates->
q;
598 #if STABILIZATION_INDI_FILTER_YAW_RATE
599 #if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
605 rates_filt.
r = body_rates->
r;
614 for (i = 0; i < INDI_NUM_ACT; i++) {
634 for (i = 0; i < INDI_NUM_ACT; i++) {
639 #if INDI_OUTPUTS == 5
641 for (i = 0; i < INDI_NUM_ACT; i++) {
649 for (i = 0; i < INDI_NUM_ACT; i++) {
652 #if INDI_OUTPUTS == 5
665 #if INDI_OUTPUTS == 5
670 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
672 for (i = 0; i < INDI_NUM_ACT; i++) {
684 wls_alloc(
indi_du,
indi_v,
du_min_stab_indi,
du_max_stab_indi,
Bwls, 0, 0,
Wv,
indi_Wu,
du_pref_stab_indi, 10000, 10,
685 INDI_NUM_ACT, INDI_OUTPUTS);
697 for (i = 0; i < INDI_NUM_ACT; i++) {
711 for (i = 0; i < INDI_NUM_ACT; i++) {
729 for (i = 0; i < INDI_NUM_ACT; i++) {
742 for (
uint8_t i = 0; i < INDI_NUM_ACT; i++) {
747 #ifdef GUIDANCE_INDI_MIN_THROTTLE
789 att_fb.
x = att_err.
qx;
790 att_fb.
y = att_err.
qy;
791 att_fb.
z = att_err.
qz;
824 #if USE_EARTH_BOUND_RC_SETPOINT
841 #if STABILIZATION_INDI_RPM_FEEDBACK
846 float UNUSED prev_actuator_state;
847 for (i = 0; i < INDI_NUM_ACT; i++) {
853 #ifdef STABILIZATION_INDI_ACT_RATE_LIMIT
854 if ((
actuator_state[i] - prev_actuator_state) > act_rate_limit[i]) {
856 }
else if ((
actuator_state[i] - prev_actuator_state) < -act_rate_limit[i]) {
916 #ifndef STABILIZATION_INDI_ADAPTIVE_MU
917 float adaptive_mu_lr = 0.001;
919 float adaptive_mu_lr = STABILIZATION_INDI_ADAPTIVE_MU;
932 for (i = 0; i < INDI_OUTPUTS; i++) {
934 float ddx_error = - ddx_estimation[i];
936 for (j = 0; j < INDI_NUM_ACT; j++) {
946 for (j = 0; j < INDI_NUM_ACT; j++) {
951 if (fabs(indi_accel_d) > 60.0) {
957 for (j = 0; j < INDI_NUM_ACT; j++) {
972 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
974 calc_g1g2_pseudo_inv();
986 for (i = 0; i < INDI_OUTPUTS; i++) {
987 for (j = 0; j < INDI_NUM_ACT; j++) {
997 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
1002 void calc_g1g2_pseudo_inv(
void)
1010 for (row = 0; row < INDI_OUTPUTS; row++) {
1011 for (col = 0; col < INDI_OUTPUTS; col++) {
1013 for (i = 0; i < INDI_NUM_ACT; i++) {
1014 element = element +
g1g2[row][i] *
g1g2[col][i];
1031 for (row = 0; row < INDI_NUM_ACT; row++) {
1032 for (col = 0; col < INDI_OUTPUTS; col++) {
1034 for (i = 0; i < INDI_OUTPUTS; i++) {
1035 element = element +
g1g2[i][row] *
g1g2inv[col][i];
1043 #if STABILIZATION_INDI_RPM_FEEDBACK
1047 for (i = 0; i < num_act; i++) {
1049 if (feedback[i].
idx < INDI_NUM_ACT && feedback[i].set.rpm) {
1072 for (j = 0; j < INDI_NUM_ACT; j++) {
1077 for (i = 0; i < INDI_OUTPUTS; i++) {
1086 if (
g1_est[i][j] > max_limit) {
1087 g1_est[i][j] = max_limit;
1089 if (
g1_est[i][j] < min_limit) {
1090 g1_est[i][j] = min_limit;
1103 if (
g2_est[j] > max_limit) {
1106 if (
g2_est[j] < min_limit) {
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
#define THRUST_INCREMENT_ID
bool autopilot_get_motors_on(void)
get motors status
static void float_vect_sum(float *o, const float *a, const float *b, const int n)
o = a + b
static void float_vect_zero(float *a, const int n)
a = 0
void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
Composition (multiplication) of two quaternions with normalization.
static void float_vect_smul(float *o, const float *a, const float s, const int n)
o = a * s
static void float_vect_scale(float *a, const float s, const int n)
a *= s
static void float_vect_copy(float *a, const float *b, const int n)
a = b
void float_quat_tilt_twist(struct FloatQuat *tilt, struct FloatQuat *twist, struct FloatQuat *quat)
Tilt twist decomposition of a quaternion (z axis)
bool float_mat_inv_4d(float invOut[4][4], float mat_in[4][4])
4x4 Matrix inverse
#define FLOAT_RATES_ZERO(_r)
#define QUAT_BFP_OF_REAL(_qi, _qf)
#define RATES_ADD(_a, _b)
#define QUAT_FLOAT_OF_BFP(_qf, _qi)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
int32_t phi
in rad with INT32_ANGLE_FRAC
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t theta
in rad with INT32_ANGLE_FRAC
void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q)
void int32_quat_of_eulers(struct Int32Quat *q, struct Int32Eulers *e)
Quaternion from Euler angles.
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
static struct Int32Vect3 * stateGetAccelBody_i(void)
Get acceleration in Body coordinates (int).
static float stateGetAirspeed_f(void)
Get airspeed (float).
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
Simple first order low pass filter with bilinear transform.
static float update_first_order_low_pass(struct FirstOrderLowPass *filter, float value)
Update first order low pass filter state with a new value.
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
Init a second order Butterworth filter.
static void init_first_order_low_pass(struct FirstOrderLowPass *filter, float tau, float sample_time, float value)
Init first order low pass filter.
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
Update second order Butterworth low pass filter state with a new value.
First order low pass filter structure.
Second order low pass filter structure.
Hardware independent API for actuators (servos, motor controllers).
uint8_t idx
General index of the actuators (generated in airframe.h)
static float mu
Physical model parameters.
static float use_increment
static abi_event act_feedback_ev
Paparazzi floating point algebra.
#define PPRZ_ITRIG_SIN(_s, _a)
#define PPRZ_ITRIG_COS(_c, _a)
struct RadioControl radio_control
Generic interface for radio control modules.
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
struct HorizontalGuidance guidance_h
#define GUIDANCE_H_MODE_NAV
#define GUIDANCE_H_MODE_HOVER
General attitude stabilization interface for rotorcrafts.
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
struct FloatRates stab_sp_to_rates_f(struct StabilizationSetpoint *sp)
struct Int32Quat stab_sp_to_quat_i(struct StabilizationSetpoint *sp)
struct Int32Eulers stab_sp_to_eulers_i(struct StabilizationSetpoint *sp)
int32_t stabilization_attitude_get_heading_i(void)
void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat *q_sp, bool in_flight, bool in_carefree, bool coordinated_turn)
void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool in_flight, bool in_carefree, bool coordinated_turn)
Read attitude setpoint from RC as quaternion Interprets the stick positions as axes.
Read an attitude setpoint from the RC.
static void calc_g1_element(float dx_error, int8_t i, int8_t j, float mu_extra)
Butterworth2LowPass acceleration_lowpass_filter
static void send_eff_mat_g_indi(struct transport_tx *trans, struct link_device *dev)
static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
void stabilization_indi_enter(void)
Function that resets important values upon engaging INDI.
static float Wv[INDI_OUTPUTS]
float act_pref[INDI_NUM_ACT]
float * Bwls[INDI_OUTPUTS]
int32_t stabilization_att_indi_cmd[COMMANDS_NB]
struct Indi_gains indi_gains
#define STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD
void stabilization_indi_set_failsafe_setpoint(void)
Function that calculates the failsafe setpoint.
bool act_is_thruster_x[INDI_NUM_ACT]
float du_min_stab_indi[INDI_NUM_ACT]
bool act_is_servo[INDI_NUM_ACT]
void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight)
static struct FirstOrderLowPass rates_filt_fo[3]
float angular_acceleration[3]
float du_pref_stab_indi[INDI_NUM_ACT]
void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
float actuator_state_filt_vectd[INDI_NUM_ACT]
float indi_Wu[INDI_NUM_ACT]
Weighting of different actuators in the cost function.
#define STABILIZATION_INDI_FILT_CUTOFF_P
static void bound_g_mat(void)
#define STABILIZATION_INDI_FILT_CUTOFF_Q
float g1g2[INDI_OUTPUTS][INDI_NUM_ACT]
static void calc_g2_element(float dx_error, int8_t j, float mu_extra)
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
float du_max_stab_indi[INDI_NUM_ACT]
void WEAK stabilization_indi_set_wls_settings(float use_increment)
float estimation_rate_d[INDI_NUM_ACT]
float du_estimation[INDI_NUM_ACT]
float actuator_state_filt_vectdd[INDI_NUM_ACT]
static void thrust_cb(uint8_t sender_id, struct FloatVect3 thrust_increment)
Butterworth2LowPass measurement_lowpass_filters[3]
bool act_is_thruster_z[INDI_NUM_ACT]
bool indi_thrust_increment_set
void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
struct FloatVect3 body_accel_f
float ddu_estimation[INDI_NUM_ACT]
float estimation_rate_dd[INDI_NUM_ACT]
float act_obs[INDI_NUM_ACT]
float g1[INDI_OUTPUTS][INDI_NUM_ACT]
void stabilization_indi_init(void)
Function that initializes important values upon engaging INDI.
struct FloatRates angular_rate_ref
void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp)
Set attitude setpoint from stabilization setpoint struct.
float g1g2inv[INDI_OUTPUTS][INDI_OUTPUTS]
void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy)
float actuator_state_filt_vect[INDI_NUM_ACT]
float act_first_order_cutoff[INDI_NUM_ACT]
float act_dyn_discrete[INDI_NUM_ACT]
struct FloatRates angular_accel_ref
float g1g2_trans_mult[INDI_OUTPUTS][INDI_OUTPUTS]
#define STABILIZATION_INDI_FILT_CUTOFF_R
float g2_init[INDI_NUM_ACT]
struct FloatVect3 indi_thrust_increment
Butterworth2LowPass estimation_input_lowpass_filters[INDI_NUM_ACT]
void sum_g1_g2(void)
Function that sums g1 and g2 to obtain the g1g2 matrix It also undoes the scaling that was done to ma...
float g1g2_pseudo_inv[INDI_NUM_ACT][INDI_OUTPUTS]
float g2_est[INDI_NUM_ACT]
float g1_init[INDI_OUTPUTS][INDI_NUM_ACT]
struct Int32Quat stab_att_sp_quat
with INT32_QUAT_FRAC
float actuator_state[INDI_NUM_ACT]
static void lms_estimation(void)
Function that estimates the control effectiveness of each actuator online.
static void send_att_full_indi(struct transport_tx *trans, struct link_device *dev)
#define INDI_ALLOWED_G_FACTOR
float g1_est[INDI_OUTPUTS][INDI_NUM_ACT]
float indi_v[INDI_OUTPUTS]
void stabilization_indi_set_quat_setpoint_i(struct Int32Quat *quat)
float indi_u[INDI_NUM_ACT]
static void get_actuator_state(void)
Function that tries to get actuator feedback.
Butterworth2LowPass actuator_lowpass_filters[INDI_NUM_ACT]
struct FloatRates stab_att_ff_rates
void init_filters(void)
Function that resets the filters to zeros.
Butterworth2LowPass estimation_output_lowpass_filters[3]
float indi_du[INDI_NUM_ACT]
#define STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF
#define STABILIZATION_INDI_FILT_CUTOFF
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
static void act_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback, uint8_t num_act)
RPM callback for RPM based control throttle curves.
int int32_t
Typedef defining 32 bit int type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
signed char int8_t
Typedef defining 8 bit char type.
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)
active set algorithm for control allocation