39 #include "generated/airframe.h"
44 #if defined(STABILIZATION_INDI_ACT_DYN_P) && !defined(STABILIZATION_INDI_ACT_DYN_Q) && !defined(STABILIZATION_INDI_ACT_DYN_R)
45 #warning STABILIZATION_INDI_ACT_DYN is deprecated, use STABILIZATION_INDI_ACT_FREQ instead.
46 #warning You now have to define the continuous time corner frequency in rad/s of the actuators.
47 #warning "Use -ln(1 - old_number) * PERIODIC_FREQUENCY to compute it from the old values."
49 #if !defined(STABILIZATION_INDI_ACT_FREQ_P) && !defined(STABILIZATION_INDI_ACT_FREQ_Q) && !defined(STABILIZATION_INDI_ACT_FREQ_R)
50 #warning You have to define the corner frequency of the first order actuator dynamics model in rad/s!
56 #ifndef STABILIZATION_INDI_FILT_CUTOFF
57 #define STABILIZATION_INDI_FILT_CUTOFF 8.0
61 #ifndef STABILIZATION_INDI_FILT_CUTOFF_RDOT
62 #define STABILIZATION_INDI_FILT_CUTOFF_RDOT STABILIZATION_INDI_FILT_CUTOFF
65 #ifndef STABILIZATION_INDI_MAX_RATE
66 #define STABILIZATION_INDI_MAX_RATE 6.0
69 #if STABILIZATION_INDI_USE_ADAPTIVE
70 #warning "Use caution with adaptive indi. See the wiki for more info"
73 #ifndef STABILIZATION_INDI_MAX_R
74 #define STABILIZATION_INDI_MAX_R STABILIZATION_ATTITUDE_SP_MAX_R
77 #ifndef STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF
78 #define STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF 4.0
81 #ifdef STABILIZATION_INDI_FILT_CUTOFF_P
82 #define STABILIZATION_INDI_FILTER_ROLL_RATE TRUE
84 #define STABILIZATION_INDI_FILT_CUTOFF_P 20.0
87 #ifdef STABILIZATION_INDI_FILT_CUTOFF_Q
88 #define STABILIZATION_INDI_FILTER_PITCH_RATE TRUE
90 #define STABILIZATION_INDI_FILT_CUTOFF_Q 20.0
93 #ifdef STABILIZATION_INDI_FILT_CUTOFF_R
94 #define STABILIZATION_INDI_FILTER_YAW_RATE TRUE
96 #define STABILIZATION_INDI_FILT_CUTOFF_R 20.0
109 #define INDI_EST_SCALE 0.001
116 .g1 = {STABILIZATION_INDI_G1_P, STABILIZATION_INDI_G1_Q, STABILIZATION_INDI_G1_R},
117 .g2 = STABILIZATION_INDI_G2_R,
120 STABILIZATION_INDI_REF_ERR_P,
121 STABILIZATION_INDI_REF_ERR_Q,
122 STABILIZATION_INDI_REF_ERR_R
125 STABILIZATION_INDI_REF_RATE_P,
126 STABILIZATION_INDI_REF_RATE_Q,
127 STABILIZATION_INDI_REF_RATE_R
139 .mu = STABILIZATION_INDI_ADAPTIVE_MU,
142 #if STABILIZATION_INDI_USE_ADAPTIVE
149 #if PERIODIC_TELEMETRY
155 pprz_msg_send_STAB_ATTITUDE(trans,
dev, AC_ID,
178 pprz_msg_send_EFF_MAT_G(trans,
dev, AC_ID,
192 pprz_msg_send_AHRS_REF_QUAT(trans,
dev, AC_ID,
209 #ifdef STABILIZATION_INDI_ACT_FREQ_P
210 indi.
act_dyn.
p = 1-exp(-STABILIZATION_INDI_ACT_FREQ_P/PERIODIC_FREQUENCY);
211 indi.
act_dyn.
q = 1-exp(-STABILIZATION_INDI_ACT_FREQ_Q/PERIODIC_FREQUENCY);
212 indi.
act_dyn.
r = 1-exp(-STABILIZATION_INDI_ACT_FREQ_R/PERIODIC_FREQUENCY);
219 #if PERIODIC_TELEMETRY
231 float tau_axis[3] = {tau, tau, tau_rdot};
233 float sample_time = 1.0 / PERIODIC_FREQUENCY;
235 for (
int8_t i = 0; i < 3; i++) {
252 float sample_time = 1.0 / PERIODIC_FREQUENCY;
364 for (
int8_t i = 0; i < 3; i++) {
365 output[i] = (filter[i].
o[0] - filter[i].
o[1]) * PERIODIC_FREQUENCY;
378 for (
int8_t i = 0; i < 3; i++) {
379 output[i] = (
new[i] - old[i])*PERIODIC_FREQUENCY;
409 #if STABILIZATION_INDI_FILTER_ROLL_RATE
412 rates_filt.
p = body_rates->
p;
414 #if STABILIZATION_INDI_FILTER_PITCH_RATE
417 rates_filt.
q = body_rates->
q;
419 #if STABILIZATION_INDI_FILTER_YAW_RATE
422 rates_filt.
r = body_rates->
r;
461 #if STABILIZATION_INDI_FULL_AUTHORITY
510 att_fb.
x = att_err.
qx;
511 att_fb.
y = att_err.
qy;
512 att_fb.
z = att_err.
qz;
536 #if USE_EARTH_BOUND_RC_SETPOINT
559 float rate_d_prev[3];
575 float error = (est->
g1.
r * du[2] + est->
g2 * ddu - est->
rate_dd[2]);
576 est->
g1.
r = est->
g1.
r - error * du[2] * est->
mu / 3;
577 est->
g2 = est->
g2 - error * 1000 * ddu * est->
mu / 3;
580 if (est->
g1.
p < 0.01) { est->
g1.
p = 0.01; }
581 if (est->
g1.
q < 0.01) { est->
g1.
q = 0.01; }
582 if (est->
g1.
r < 0.01) { est->
g1.
r = 0.01; }
583 if (est->
g2 < 0.01) { est->
g2 = 0.01; }
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_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)
#define FLOAT_RATES_ZERO(_r)
#define RATES_SMUL(_ro, _ri, _s)
#define QUAT_BFP_OF_REAL(_qi, _qf)
#define RATES_ADD(_a, _b)
#define QUAT_FLOAT_OF_BFP(_qf, _qi)
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).
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.
#define PPRZ_ITRIG_SIN(_s, _a)
#define PPRZ_ITRIG_COS(_c, _a)
Generic interface for radio control modules.
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 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)
This function reads rc commands.
void stabilization_indi_enter(void)
Function that resets important values upon engaging INDI.
void stabilization_indi_set_failsafe_setpoint(void)
Function that calculates the failsafe setpoint.
static void finite_difference(float output[3], float new[3], float old[3])
Calculate derivative of an array via finite difference.
void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight)
runs stabilization indi
static struct FirstOrderLowPass rates_filt_fo[3]
#define STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF
static void lms_estimation(void)
This is a Least Mean Squares adaptive filter It estimates the actuator effectiveness online,...
void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
Set attitude setpoint from command in earth axes.
#define STABILIZATION_INDI_FILT_CUTOFF_P
#define STABILIZATION_INDI_FILT_CUTOFF_Q
static void send_att_indi(struct transport_tx *trans, struct link_device *dev)
void stabilization_indi_simple_reset_r_filter_cutoff(float new_cutoff)
static void finite_difference_from_filter(float *output, Butterworth2LowPass *filter)
Caclulate finite difference form a filter array The filter already contains the previous values.
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
#define STABILIZATION_INDI_MAX_RATE
static void send_eff_mat_g_indi_simple(struct transport_tx *trans, struct link_device *dev)
void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
Does the INDI calculations.
void stabilization_indi_init(void)
Function that initializes important values upon engaging INDI.
#define STABILIZATION_INDI_FILT_CUTOFF
void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp)
Set attitude setpoint from stabilization setpoint struct.
void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy)
Set attitude quaternion setpoint from rpy.
struct IndiVariables indi
#define STABILIZATION_INDI_MAX_R
#define STABILIZATION_INDI_FILT_CUTOFF_R
static void filter_pqr(Butterworth2LowPass *filter, struct FloatRates *new_values)
Update butterworth filter for p, q and r of a FloatRates struct.
struct Int32Quat stab_att_sp_quat
with INT32_QUAT_FRAC
#define STABILIZATION_INDI_FILT_CUTOFF_RDOT
void stabilization_indi_set_quat_setpoint_i(struct Int32Quat *quat)
static void indi_init_filters(void)
struct FloatRates stab_att_ff_rates
Butterworth2LowPass rate[3]
struct FloatRates act_dyn
struct IndiEstimation est
Estimation parameters for adaptive INDI.
struct FloatRates u_act_dyn
struct FloatRates angular_accel_ref
bool adaptive
Enable adataptive estimation.
float attitude_max_yaw_rate
Maximum yaw rate in atttiude control in rad/s.
Butterworth2LowPass rate[3]
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.
int int32_t
Typedef defining 32 bit int type.
signed char int8_t
Typedef defining 8 bit char type.