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
86 STABILIZATION_INDI_REF_ERR_P,
87 STABILIZATION_INDI_REF_ERR_Q,
88 STABILIZATION_INDI_REF_ERR_R
91 STABILIZATION_INDI_REF_RATE_P,
92 STABILIZATION_INDI_REF_RATE_Q,
93 STABILIZATION_INDI_REF_RATE_R
97 #if STABILIZATION_INDI_USE_ADAPTIVE
103 #ifdef STABILIZATION_INDI_ACT_RATE_LIMIT
104 float act_rate_limit[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_RATE_LIMIT;
107 #ifdef STABILIZATION_INDI_ACT_IS_SERVO
108 bool act_is_servo[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_IS_SERVO;
113 #ifdef STABILIZATION_INDI_ACT_PREF
115 float act_pref[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_PREF;
121 float act_dyn[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN;
123 #ifdef STABILIZATION_INDI_WLS_PRIORITIES
124 static float Wv[INDI_OUTPUTS] = STABILIZATION_INDI_WLS_PRIORITIES;
127 static float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100};
153 float mu1[INDI_OUTPUTS] = {0.00001, 0.00001, 0.000003, 0.000002};
175 float g2[INDI_NUM_ACT] = STABILIZATION_INDI_G2;
176 float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
177 STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST
179 float g1g2[INDI_OUTPUTS][INDI_NUM_ACT];
180 float g1_est[INDI_OUTPUTS][INDI_NUM_ACT];
197 #if PERIODIC_TELEMETRY
201 pprz_msg_send_INDI_G(trans,
dev, AC_ID, INDI_NUM_ACT,
g1_est[0],
211 pprz_msg_send_AHRS_REF_QUAT(trans,
dev, AC_ID,
246 for (i = 0; i < INDI_OUTPUTS; i++) {
259 for (i = 0; i < INDI_NUM_ACT; i++) {
263 #if PERIODIC_TELEMETRY
294 float sample_time = 1.0 / PERIODIC_FREQUENCY;
297 for (i = 0; i < 3; i++) {
303 for (i = 0; i < INDI_NUM_ACT; i++) {
379 float rate_vect[3] = {body_rates->
p, body_rates->
q, body_rates->
r};
381 for (i = 0; i < 3; i++) {
400 #if STABILIZATION_INDI_FILTER_ROLL_RATE
403 rates_filt.
p = body_rates->
p;
405 #if STABILIZATION_INDI_FILTER_PITCH_RATE
408 rates_filt.
q = body_rates->
q;
410 #if STABILIZATION_INDI_FILTER_YAW_RATE
413 rates_filt.
r = body_rates->
r;
422 for (i = 0; i < INDI_NUM_ACT; i++) {
428 float v_thrust = 0.0;
434 for (i = 0; i < INDI_NUM_ACT; i++) {
441 for (i = 0; i < INDI_NUM_ACT; i++) {
454 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
456 for (i = 0; i < INDI_NUM_ACT; i++) {
464 for (i = 0; i < INDI_NUM_ACT; i++) {
469 #ifdef GUIDANCE_INDI_MIN_THROTTLE
474 if (airspeed < 8.0) {
486 wls_alloc(
indi_du,
indi_v,
du_min,
du_max,
Bwls, 0, 0,
Wv, 0,
du_pref, 10000, 10);
493 for (i = 0; i < INDI_NUM_ACT; i++) {
514 for (i = 0; i < INDI_NUM_ACT; i++) {
532 for (i = 0; i < INDI_NUM_ACT; i++) {
568 att_fb.
x = att_err.
qx;
569 att_fb.
y = att_err.
qy;
570 att_fb.
z = att_err.
qz;
595 #if USE_EARTH_BOUND_RC_SETPOINT
612 #if INDI_RPM_FEEDBACK
617 float UNUSED prev_actuator_state;
618 for (i = 0; i < INDI_NUM_ACT; i++) {
624 #ifdef STABILIZATION_INDI_ACT_RATE_LIMIT
625 if ((
actuator_state[i] - prev_actuator_state) > act_rate_limit[i]) {
627 }
else if ((
actuator_state[i] - prev_actuator_state) < -act_rate_limit[i]) {
687 #ifndef STABILIZATION_INDI_ADAPTIVE_MU
688 float adaptive_mu_lr = 0.001;
690 float adaptive_mu_lr = STABILIZATION_INDI_ADAPTIVE_MU;
703 for (i = 0; i < INDI_OUTPUTS; i++) {
705 float ddx_error = - ddx_estimation[i];
707 for (j = 0; j < INDI_NUM_ACT; j++) {
717 for (j = 0; j < INDI_NUM_ACT; j++) {
722 if (fabs(indi_accel_d) > 60.0) {
728 for (j = 0; j < INDI_NUM_ACT; j++) {
743 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
756 for (i = 0; i < INDI_OUTPUTS; i++) {
757 for (j = 0; j < INDI_NUM_ACT; j++) {
779 for (row = 0; row < INDI_OUTPUTS; row++) {
780 for (col = 0; col < INDI_OUTPUTS; col++) {
782 for (i = 0; i < INDI_NUM_ACT; i++) {
783 element = element +
g1g2[row][i] *
g1g2[col][i];
800 for (row = 0; row < INDI_NUM_ACT; row++) {
801 for (col = 0; col < INDI_OUTPUTS; col++) {
803 for (i = 0; i < INDI_OUTPUTS; i++) {
804 element = element +
g1g2[i][row] *
g1g2inv[col][i];
813 #if INDI_RPM_FEEDBACK
815 for (i = 0; i < num_act; i++) {
817 act_obs[i] *= (
MAX_PPRZ / (float)(get_servo_max(i) - get_servo_min(i)));
836 for (j = 0; j < INDI_NUM_ACT; j++) {
841 for (i = 0; i < INDI_OUTPUTS; i++) {
850 if (
g1_est[i][j] > max_limit) {
853 if (
g1_est[i][j] < min_limit) {
867 if (
g2_est[j] > max_limit) {
870 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
bool float_mat_inv_4d(float invOut[16], float mat_in[16])
4x4 Matrix inverse
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)
#define QUAT_BFP_OF_REAL(_qi, _qf)
#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_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).
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).
static float mu
Physical model parameters.
Paparazzi floating point algebra.
#define PPRZ_ITRIG_SIN(_s, _a)
#define PPRZ_ITRIG_COS(_c, _a)
Generic interface for radio control modules.
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.
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_g1g2_pseudo_inv(void)
Function that calculates the pseudo-inverse of (G1+G2).
static void calc_g1_element(float dx_error, int8_t i, int8_t j, float mu_extra)
float du_max[INDI_NUM_ACT]
Butterworth2LowPass acceleration_lowpass_filter
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
void stabilization_indi_set_failsafe_setpoint(void)
Function that calculates the failsafe setpoint.
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]
void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
float actuator_state_filt_vectd[INDI_NUM_ACT]
#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 estimation_rate_d[INDI_NUM_ACT]
float indi_thrust_increment
float du_estimation[INDI_NUM_ACT]
float actuator_state_filt_vectdd[INDI_NUM_ACT]
Butterworth2LowPass measurement_lowpass_filters[3]
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.
static void send_indi_g(struct transport_tx *trans, struct link_device *dev)
float act_dyn[INDI_NUM_ACT]
float du_min[INDI_NUM_ACT]
float g1g2inv[INDI_OUTPUTS][INDI_OUTPUTS]
void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy)
float actuator_state_filt_vect[INDI_NUM_ACT]
struct FloatRates angular_accel_ref
float g1g2_trans_mult[INDI_OUTPUTS][INDI_OUTPUTS]
static void rpm_cb(uint8_t sender_id, uint16_t *rpm, uint8_t num_act)
#define STABILIZATION_INDI_FILT_CUTOFF_R
float g2_init[INDI_NUM_ACT]
float du_pref[INDI_NUM_ACT]
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 thrust_cb(uint8_t sender_id, float thrust_increment)
static void lms_estimation(void)
Function that estimates the control effectiveness of each actuator online.
#define INDI_ALLOWED_G_FACTOR
float g1_est[INDI_OUTPUTS][INDI_NUM_ACT]
float indi_v[INDI_OUTPUTS]
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]
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.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
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)
active set algorithm for control allocation