39 #include "generated/airframe.h"
43 #if !defined(STABILIZATION_INDI_ACT_DYN_P) && !defined(STABILIZATION_INDI_ACT_DYN_Q) && !defined(STABILIZATION_INDI_ACT_DYN_R)
44 #error You have to define the first order time constant of the actuator dynamics!
49 #ifndef STABILIZATION_INDI_FILT_CUTOFF
50 #define STABILIZATION_INDI_FILT_CUTOFF 8.0
54 #ifndef STABILIZATION_INDI_FILT_CUTOFF_R
55 #define STABILIZATION_INDI_FILT_CUTOFF_R STABILIZATION_INDI_FILT_CUTOFF
58 #ifndef STABILIZATION_INDI_MAX_RATE
59 #define STABILIZATION_INDI_MAX_RATE 6.0
62 #if STABILIZATION_INDI_USE_ADAPTIVE
63 #warning "Use caution with adaptive indi. See the wiki for more info"
66 #ifndef STABILIZATION_INDI_MAX_R
67 #define STABILIZATION_INDI_MAX_R STABILIZATION_ATTITUDE_SP_MAX_R
70 #ifndef STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF
71 #define STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF 4.0
83 #define INDI_EST_SCALE 0.001
89 .g1 = {STABILIZATION_INDI_G1_P, STABILIZATION_INDI_G1_Q, STABILIZATION_INDI_G1_R},
90 .g2 = STABILIZATION_INDI_G2_R,
91 .reference_acceleration = {
92 STABILIZATION_INDI_REF_ERR_P,
93 STABILIZATION_INDI_REF_ERR_Q,
94 STABILIZATION_INDI_REF_ERR_R,
95 STABILIZATION_INDI_REF_RATE_P,
96 STABILIZATION_INDI_REF_RATE_Q,
97 STABILIZATION_INDI_REF_RATE_R},
106 .mu = STABILIZATION_INDI_ADAPTIVE_MU,
109 #if STABILIZATION_INDI_USE_ADAPTIVE
116 #if PERIODIC_TELEMETRY
126 pprz_msg_send_STAB_ATTITUDE_INDI(trans, dev, AC_ID,
142 pprz_msg_send_AHRS_REF_QUAT(trans, dev, AC_ID,
159 #if PERIODIC_TELEMETRY
169 float tau_axis[3] = {tau, tau, tau_r};
171 float sample_time = 1.0/PERIODIC_FREQUENCY;
173 for(
int8_t i=0; i<3; i++) {
260 for(
int8_t i=0; i<3; i++) {
261 output[i] = (filter[i].
o[0] - filter[i].
o[1])*PERIODIC_FREQUENCY;
273 for(
int8_t i=0; i<3; i++) {
274 output[i] = (
new[i] - old[i])*PERIODIC_FREQUENCY;
297 RATES_COPY(rates_for_feedback, (*body_rates));
301 #if STABILIZATION_INDI_FILTER_ROLL_RATE
302 rates_for_feedback.
p = indi.
rate[0].
o[0];
304 #if STABILIZATION_INDI_FILTER_PITCH_RATE
305 rates_for_feedback.
q = indi.
rate[1].
o[0];
307 #if STABILIZATION_INDI_FILTER_YAW_RATE
308 rates_for_feedback.
r = indi.
rate[2].
o[0];
343 Bound(indi.
u_in.
p, -4500, 4500);
344 Bound(indi.
u_in.
q, -4500, 4500);
345 Bound(indi.
u_in.
r, -4500, 4500);
366 indi_commands[COMMAND_ROLL] = indi.
u_in.
p;
367 indi_commands[COMMAND_PITCH] = indi.
u_in.
q;
368 indi_commands[COMMAND_YAW] = indi.
u_in.
r;
409 #if USE_EARTH_BOUND_RC_SETPOINT
432 float rate_d_prev[3];
448 float error = (est->
g1.
r * du[2] + est->
g2 * ddu - est->
rate_dd[2]);
449 est->
g1.
r = est->
g1.
r - error * du[2] * est->
mu / 3;
450 est->
g2 = est->
g2 - error * 1000 * ddu * est->
mu / 3;
453 if (est->
g1.
p < 0.01) { est->
g1.
p = 0.01; }
454 if (est->
g1.
q < 0.01) { est->
g1.
q = 0.01; }
455 if (est->
g1.
r < 0.01) { est->
g1.
r = 0.01; }
456 if (est->
g2 < 0.01) { est->
g2 = 0.01; }
int32_t psi
in rad with INT32_ANGLE_FRAC
struct Int32Quat stab_att_sp_quat
with INT32_QUAT_FRAC
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
Update second order Butterworth low pass filter state with a new value.
#define STABILIZATION_INDI_MAX_R
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.
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
Init a second order Butterworth filter.
static void int32_quat_normalize(struct Int32Quat *q)
normalize a quaternion inplace
bool adaptive
Enable adataptive estimation.
void int32_quat_inv_comp(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
Composition (multiplication) of two quaternions.
struct ReferenceSystem reference_acceleration
#define STABILIZATION_INDI_MAX_RATE
static void indi_init_filters(void)
void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
This function reads rc commands.
static void send_att_indi(struct transport_tx *trans, struct link_device *dev)
Periodic telemetry system header (includes downlink utility and generated code).
General attitude stabilization interface for rotorcrafts.
void stabilization_indi_enter(void)
Function that resets important values upon engaging INDI.
int32_t theta
in rad with INT32_ANGLE_FRAC
static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
void int32_quat_of_eulers(struct Int32Quat *q, struct Int32Eulers *e)
Quaternion from Euler angles.
int32_t stabilization_attitude_get_heading_i(void)
Read an attitude setpoint from the RC.
float max_rate
Maximum rate in rate control in rad/s.
static void filter_pqr(Butterworth2LowPass *filter, struct FloatRates *new_values)
Update butterworth filter for p, q and r of a FloatRates struct.
#define FLOAT_RATES_ZERO(_r)
void stabilization_indi_set_failsafe_setpoint(void)
Function that calculates the failsafe setpoint.
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
void stabilization_indi_init(void)
Function that initializes important values upon engaging INDI.
static void stabilization_indi_calc_cmd(int32_t indi_commands[], struct Int32Quat *att_err, bool rate_control)
Does the INDI calculations.
void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat *q_sp, bool in_flight, bool in_carefree, bool coordinated_turn)
static void finite_difference(float output[3], float new[3], float old[3])
Calculate derivative of an array via finite difference.
void stabilization_indi_run(bool in_flight, bool rate_control)
runs stabilization indi
#define PPRZ_ITRIG_SIN(_s, _a)
#define STABILIZATION_INDI_FILT_CUTOFF_R
struct IndiVariables indi
Butterworth2LowPass rate[3]
Butterworth2LowPass rate[3]
static int32_t stabilization_att_indi_cmd[COMMANDS_NB]
#define QUAT_BFP_OF_REAL(_qi, _qf)
#define DefaultPeriodic
Set default periodic telemetry.
static void lms_estimation(void)
This is a Least Mean Squares adaptive filter It estimates the actuator effectiveness online...
struct IndiEstimation est
Estimation parameters for adaptive INDI.
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
#define STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF
struct RadioControl radio_control
static void finite_difference_from_filter(float *output, Butterworth2LowPass *filter)
Caclulate finite difference form a filter array The filter already contains the previous values...
#define RATES_SMUL(_ro, _ri, _s)
void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy)
Set attitude quaternion setpoint from rpy.
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
#define QUAT1_FLOAT_OF_BFP(_qi)
static const struct usb_device_descriptor dev
#define STABILIZATION_INDI_FILT_CUTOFF
void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
Set attitude setpoint from command in earth axes.
int32_t phi
in rad with INT32_ANGLE_FRAC
struct FloatRates angular_accel_ref
API to get/set the generic vehicle states.
static void float_vect_scale(float *a, const float s, const int n)
a *= s
static void int32_quat_wrap_shortest(struct Int32Quat *q)
static void float_vect_copy(float *a, const float *b, const int n)
a = b
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
#define RATES_COPY(_a, _b)
Second order low pass filter structure.
float attitude_max_yaw_rate
Maximum yaw rate in atttiude control in rad/s.
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
struct FloatRates u_act_dyn
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
#define PPRZ_ITRIG_COS(_c, _a)