38#include "generated/airframe.h"
47#define INDI_ALLOWED_G_FACTOR 2.0
49#ifdef STABILIZATION_INDI_FILT_CUTOFF_P
50#define STABILIZATION_INDI_FILTER_ROLL_RATE TRUE
52#define STABILIZATION_INDI_FILT_CUTOFF_P 20.0
55#ifdef STABILIZATION_INDI_FILT_CUTOFF_Q
56#define STABILIZATION_INDI_FILTER_PITCH_RATE TRUE
58#define STABILIZATION_INDI_FILT_CUTOFF_Q 20.0
61#ifdef STABILIZATION_INDI_FILT_CUTOFF_R
62#define STABILIZATION_INDI_FILTER_YAW_RATE TRUE
64#define STABILIZATION_INDI_FILT_CUTOFF_R 20.0
68#ifndef STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
69#define STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE FALSE
72#ifndef STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
73#define STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER FALSE
78#ifndef STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD
79#define STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD 8.0
82#ifndef STABILIZATION_INDI_USE_ADAPTIVE
83#define STABILIZATION_INDI_USE_ADAPTIVE false
86#ifndef STABILIZATION_INDI_ADAPTIVE_MU
87#define STABILIZATION_INDI_ADAPTIVE_MU 0.001f
90#ifndef STABILIZATION_INDI_ACT_IS_SERVO
91#define STABILIZATION_INDI_ACT_IS_SERVO {0}
94#ifndef STABILIZATION_INDI_ACT_IS_THRUSTER_X
95#define STABILIZATION_INDI_ACT_IS_THRUSTER_X {0}
98#ifndef STABILIZATION_INDI_ACT_IS_THRUSTER_Y
99#define STABILIZATION_INDI_ACT_IS_THRUSTER_Y {0}
105#ifndef STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT
106#define STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT 99999.f
110#ifndef STABILIZATION_INDI_G1_THRUST_X
111#error "You must define STABILIZATION_INDI_G1_THRUST_X for your number of INDI_OUTPUTS"
115#ifdef SetCommandsFromRC
116#warning SetCommandsFromRC not used: STAB_INDI writes actuators directly
119#ifdef SetAutoCommandsFromRC
120#warning SetAutoCommandsFromRC not used: STAB_INDI writes actuators directly
123#if !STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
126#ifndef STABILIZATION_INDI_WLS_PRIORITIES
128#define STABILIZATION_INDI_WLS_PRIORITIES {1000, 1000, 1, 100, 100}
130#define STABILIZATION_INDI_WLS_PRIORITIES {1000, 1000, 1, 100}
135#ifndef STABILIZATION_INDI_WLS_WU
136#define STABILIZATION_INDI_WLS_WU {[0 ... INDI_NUM_ACT - 1] = 1.0}
141#ifndef STABILIZATION_INDI_ACT_PREF
142#define STABILIZATION_INDI_ACT_PREF {0.0}
145#if INDI_NUM_ACT > WLS_N_U_MAX
146#error Matrix-WLS_N_U_MAX too small or not defined: define WLS_N_U_MAX >= INDI_NUM_ACT in airframe file
148#if INDI_OUTPUTS > WLS_N_V_MAX
149#error Matrix-WLS_N_V_MAX too small or not defined: define WLS_N_V_MAX >= INDI_OUTPUTS in airframe file
178#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
179static void calc_g1g2_pseudo_inv(
void);
186 STABILIZATION_INDI_REF_ERR_P,
187 STABILIZATION_INDI_REF_ERR_Q,
188 STABILIZATION_INDI_REF_ERR_R
191 STABILIZATION_INDI_REF_RATE_P,
192 STABILIZATION_INDI_REF_RATE_Q,
193 STABILIZATION_INDI_REF_RATE_R
199#ifdef STABILIZATION_INDI_ACT_RATE_LIMIT
200float act_rate_limit[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_RATE_LIMIT;
208#ifdef STABILIZATION_INDI_ACT_THRUST_MAT
209static bool act_thrust_mat[3][INDI_NUM_ACT] = STABILIZATION_INDI_ACT_THRUST_MAT;
214#ifdef STABILIZATION_INDI_ACT_IS_THRUSTER_Z
215 STABILIZATION_INDI_ACT_IS_THRUSTER_Z
222#ifdef STABILIZATION_INDI_COMMANDS
223static uint8_t act_to_commands[INDI_NUM_ACT] = STABILIZATION_INDI_COMMANDS;
226#ifdef STABILIZATION_INDI_ACT_DYN
227#warning STABILIZATION_INDI_ACT_DYN is deprecated, use STABILIZATION_INDI_ACT_FREQ instead.
228#warning You now have to define the continuous time corner frequency in rad/s of the actuators.
229#warning "Use -ln(1 - old_number) * PERIODIC_FREQUENCY to compute it from the old values."
263float mu1[INDI_OUTPUTS] = {0.00001, 0.00001, 0.000003, 0.000002};
277#if STABILIZATION_INDI_RPM_FEEDBACK
278#ifndef STABILIZATION_INDI_ACT_FEEDBACK_ID
279#define STABILIZATION_INDI_ACT_FEEDBACK_ID ABI_BROADCAST
289float g2[INDI_NUM_ACT] = STABILIZATION_INDI_G2;
290#ifdef STABILIZATION_INDI_G1
291float g1[INDI_OUTPUTS][INDI_NUM_ACT] = STABILIZATION_INDI_G1;
294float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
295 STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW,
296 STABILIZATION_INDI_G1_THRUST, STABILIZATION_INDI_G1_THRUST_X
300float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
301 STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST
306float g1g2[INDI_OUTPUTS][INDI_NUM_ACT];
320#if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
331#if PERIODIC_TELEMETRY
333#if !STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
345 pprz_msg_send_EFF_MAT_STAB(trans,
dev, AC_ID,
346 INDI_NUM_ACT,
g1g2[0],
347 INDI_NUM_ACT,
g1g2[1],
348 INDI_NUM_ACT,
g1g2[2],
356 pprz_msg_send_EFF_MAT_STAB_THRUST(trans,
dev, AC_ID,
360 INDI_NUM_ACT,
g1g2[3]
361#elif INDI_OUTPUTS == 5
362 INDI_NUM_ACT,
g1g2[4],
364 INDI_NUM_ACT,
g1g2[3]
365#elif INDI_OUTPUTS == 6
366 INDI_NUM_ACT,
g1g2[3],
367 INDI_NUM_ACT,
g1g2[4],
368 INDI_NUM_ACT,
g1g2[5]
377 pprz_msg_send_AHRS_REF_QUAT(trans,
dev, AC_ID,
393#if GUIDANCE_INDI_HYBRID
403 float temp_att_ref[3] = {att_sp.
phi, att_sp.
theta, att_sp.
psi};
404 float temp_rate[3] = {body_rates->
p, body_rates->
q, body_rates->
r};
407 pprz_msg_send_STAB_ATTITUDE(trans,
dev, AC_ID,
430#ifdef STABILIZATION_INDI_ACT_FREQ
432 for (i = 0; i < INDI_NUM_ACT; i++) {
437#if STABILIZATION_INDI_RPM_FEEDBACK
451#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
453 calc_g1g2_pseudo_inv();
457 for (i = 0; i < INDI_OUTPUTS; i++) {
469 for (i = 0; i < INDI_NUM_ACT; i++) {
470#ifndef STABILIZATION_INDI_ACT_IS_THRUSTER_Z
479#if PERIODIC_TELEMETRY
483 #if !STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
507 float tau = 1.0 / (2.0 * M_PI * freq);
508 float sample_time = 1.0 / PERIODIC_FREQUENCY;
509#if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
528 float sample_time = 1.0 / PERIODIC_FREQUENCY;
531 for (i = 0; i < 3; i++) {
537 for (i = 0; i < INDI_NUM_ACT; i++) {
545#if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
565#if STABILIZATION_INDI_FILTER_ROLL_RATE
566#if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
572 rates_filt.
p = rates->p;
574#if STABILIZATION_INDI_FILTER_PITCH_RATE
575#if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
581 rates_filt.
q = rates->q;
583#if STABILIZATION_INDI_FILTER_YAW_RATE
584#if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
590 rates_filt.
r = rates->r;
608 float actuator_state_filt_vect_prev[INDI_NUM_ACT];
609 for (
int i = 0; i < INDI_NUM_ACT; i++) {
626 float angular_acc_prediction_filt[INDI_OUTPUTS];
628 angular_acc_prediction_filt[2] -= g2_times_u_act_filt;
632 float rate_vect[3] = {body_rates->
p, body_rates->
q, body_rates->
r};
640 for (i = 0; i < 3; i++) {
656 float angular_acc_disturbance_estimate[INDI_OUTPUTS];
678 for (i = 0; i < INDI_NUM_ACT; i++) {
694 struct FloatVect3 v_thrust = { 0.f, 0.f, 0.f };
695 if (thrust->
type == THRUST_INCR_SP) {
708#if (INDI_OUTPUTS == 5) && (defined RADIO_CONTROL_THRUST_X) && (defined COMMAND_THRUST_X)
710 cmd[COMMAND_THRUST_X] =
radio_control.values[RADIO_CONTROL_THRUST_X];
711 th_cmd.
x = (float)cmd[COMMAND_THRUST_X];
716 for (i = 0; i < INDI_NUM_ACT; i++) {
732#ifdef STABILIZATION_INDI_V_THRUST_X
733 v_thrust.
x = th_cmd.
x * STABILIZATION_INDI_V_THRUST_X /
MAX_PPRZ;
735#ifdef STABILIZATION_INDI_V_THRUST_Y
736 v_thrust.
y = th_cmd.
y * STABILIZATION_INDI_V_THRUST_Y /
MAX_PPRZ;
738#ifdef STABILIZATION_INDI_V_THRUST_Z
739 v_thrust.
z = th_cmd.
z * STABILIZATION_INDI_V_THRUST_Z /
MAX_PPRZ;
771#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
773 for (i = 0; i < INDI_NUM_ACT; i++) {
783 for (i = 0; i < INDI_OUTPUTS; i++) {
787 for (i = 0; i < INDI_NUM_ACT; i++) {
798 cmd[COMMAND_THRUST] = 0;
799 for (i = 0; i < INDI_NUM_ACT; i++) {
861 att_fb.
x = att_err.
qx;
862 att_fb.
y = att_err.
qy;
863 att_fb.
z = att_err.
qz;
883 accel_ref.
p = (sp.p - rates.p) *
indi_gains.rate.p;
884 accel_ref.
q = (sp.q - rates.q) *
indi_gains.rate.q;
885 accel_ref.
r = (sp.r - rates.r) *
indi_gains.rate.r;
893#if !STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
897 for (
uint8_t i = 0; i < INDI_NUM_ACT; i++) {
902#ifdef GUIDANCE_INDI_MIN_THROTTLE
908 wls_stab_p.u_min[i] = GUIDANCE_INDI_MIN_THROTTLE;
910 wls_stab_p.u_min[i] = GUIDANCE_INDI_MIN_THROTTLE_FWD;
927#if STABILIZATION_INDI_RPM_FEEDBACK
932 float UNUSED prev_actuator_state;
933 for (i = 0; i < INDI_NUM_ACT; i++) {
939#ifdef STABILIZATION_INDI_ACT_RATE_LIMIT
940 if ((
actuator_state[i] - prev_actuator_state) > act_rate_limit[i]) {
942 }
else if ((
actuator_state[i] - prev_actuator_state) < -act_rate_limit[i]) {
951#ifdef STABILIZATION_INDI_COMMANDS
959 for (
int i = 0; i < INDI_NUM_ACT; i++) {
961 if (is_motor && !motors_on) {
962 cmd_out[act_to_commands[i]] =
MIN_PPRZ;
964 cmd_out[act_to_commands[i]] = cmd_in[act_to_commands[i]];
969 cmd_in[COMMAND_THRUST] = 0;
971 cmd_out[COMMAND_THRUST] = cmd_in[COMMAND_THRUST];
1034 for (i = 0; i < INDI_OUTPUTS; i++) {
1036 float ddx_error = - ddx_estimation[i];
1038 for (j = 0; j < INDI_NUM_ACT; j++) {
1048 for (j = 0; j < INDI_NUM_ACT; j++) {
1051 }
else if (i == 3) {
1053 if (fabs(indi_accel_d) > 60.0) {
1059 for (j = 0; j < INDI_NUM_ACT; j++) {
1074#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
1076 calc_g1g2_pseudo_inv();
1088 for (i = 0; i < INDI_OUTPUTS; i++) {
1089 for (j = 0; j < INDI_NUM_ACT; j++) {
1099#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
1104void calc_g1g2_pseudo_inv(
void)
1112 for (row = 0; row < INDI_OUTPUTS; row++) {
1113 for (col = 0; col < INDI_OUTPUTS; col++) {
1115 for (i = 0; i < INDI_NUM_ACT; i++) {
1116 element = element +
g1g2[row][i] *
g1g2[col][i];
1133 for (row = 0; row < INDI_NUM_ACT; row++) {
1134 for (col = 0; col < INDI_OUTPUTS; col++) {
1136 for (i = 0; i < INDI_OUTPUTS; i++) {
1137 element = element +
g1g2[i][row] *
g1g2inv[col][i];
1145#if STABILIZATION_INDI_RPM_FEEDBACK
1149 for (i = 0; i < num_act; i++) {
1151 if (feedback[i].
idx < INDI_NUM_ACT && feedback[i].set.rpm) {
1165 for (j = 0; j < INDI_NUM_ACT; j++) {
1170 for (i = 0; i < INDI_OUTPUTS; i++) {
1179 if (
g1_est[i][j] > max_limit) {
1180 g1_est[i][j] = max_limit;
1182 if (
g1_est[i][j] < min_limit) {
1183 g1_est[i][j] = min_limit;
1196 if (
g2_est[j] > max_limit) {
1199 if (
g2_est[j] < min_limit) {
1206 for (
uint8_t i = 0; i < INDI_NUM_ACT; i++) {
1209#ifdef STABILIZATION_INDI_COMMANDS
1216 cmd[COMMAND_THRUST] = 0;
1217 for (
uint8_t i = 0; i < INDI_NUM_ACT; i++) {
1246#ifdef STABILIZATION_INDI_COMMANDS
1247 return act_to_commands;
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
bool autopilot_get_motors_on(void)
get motors status
static struct uart_periph * dev
static void float_vect_zero(float *a, const int n)
a = 0
void float_eulers_of_quat_zxy(struct FloatEulers *e, const struct FloatQuat *q)
euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
static void float_vect_smul(float *o, const float *a, const float s, const int n)
o = a * s
#define FLOAT_VECT3_ZERO(_v)
static void float_vect_scale(float *a, const float s, const int n)
a *= s
static void float_mat_vect_mul(float *o, float **a, const float *b, const int m, const int n)
o = a * b
static float float_vect_dot_product(const float *a, const float *b, const int n)
a.b
static void float_vect_copy(float *a, const float *b, const int n)
a = b
static void float_vect_diff(float *o, const float *a, const float *b, const int n)
o = a - b
bool float_mat_inv_4d(float invOut[4][4], const float mat_in[4][4])
4x4 Matrix inverse
void float_quat_tilt_twist(struct FloatQuat *tilt, struct FloatQuat *twist, const struct FloatQuat *quat)
Tilt twist decomposition of quaternion.
void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, const struct FloatQuat *a2b, const struct FloatQuat *a2c)
Composition (multiplication) of two quaternions with normalization.
#define RATES_ADD(_a, _b)
#define QUAT_FLOAT_OF_BFP(_qf, _qi)
#define VECT3_ADD(_a, _b)
#define EULERS_FLOAT_OF_BFP(_ef, _ei)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
static struct FloatEulers * stateGetNedToBodyEulersZxy_f(void)
Get vehicle body attitude ZXY euler angles (float).
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
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.
struct SecondOrderLowPass Butterworth2LowPass
Second order Butterworth low pass filter.
static void init_first_order_low_pass(struct FirstOrderLowPass *filter, float tau, const float sample_time, float value)
Init first order low pass filter.
static float update_first_order_low_pass(struct FirstOrderLowPass *filter, const float value)
Update first order low pass filter state with a new value.
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, const float tau, const float sample_time, const float value)
Init a second order Butterworth filter.
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, const float value)
Update second order Butterworth low pass filter state with a new value.
First 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).
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
static abi_event act_feedback_ev
Paparazzi floating point algebra.
struct RadioControl radio_control
Generic interface for radio control modules.
void WEAK set_rotorcraft_commands(pprz_t *cmd_out, int32_t *cmd_in, bool in_flight, bool motors_on)
Display descent speed in failsafe mode if needed.
struct HorizontalGuidance guidance_h
#define GUIDANCE_H_MODE_NAV
#define GUIDANCE_H_MODE_HOVER
struct FloatRates stab_sp_to_rates_f(struct StabilizationSetpoint *sp)
float th_sp_to_incr_f(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)
struct Int32Quat stab_sp_to_quat_i(struct StabilizationSetpoint *sp)
struct Int32Eulers stab_sp_to_eulers_i(struct StabilizationSetpoint *sp)
struct StabilizationSetpoint stab_sp_from_rates_f(struct FloatRates *rates)
struct FloatQuat stab_sp_to_quat_f(struct StabilizationSetpoint *sp)
int32_t th_sp_to_thrust_i(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)
float actuator_state[ANDI_NUM_ACT]
static struct FloatEulers stab_att_sp_euler
Butterworth2LowPass_int measurement_lowpass_filters[INDI_DOF]
Butterworth2LowPass_int actuator_lowpass_filters[INDI_DOF]
static struct Int32Quat stab_att_sp_quat
static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
Read an attitude setpoint from the RC.
static void calc_g1_element(float dx_error, int8_t i, int8_t j, float mu_extra)
#define STABILIZATION_INDI_WLS_WU
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_enter(void)
Function that resets important values upon engaging INDI.
float act_pref[INDI_NUM_ACT]
struct FloatRates WEAK stabilization_indi_attitude_controller(struct FloatQuat att, struct FloatQuat att_sp, struct FloatRates rates_ff)
Default PD angular rate controller.
#define STABILIZATION_INDI_ACT_PREF
float * Bwls[INDI_OUTPUTS]
int32_t stabilization_att_indi_cmd[COMMANDS_NB]
struct Indi_gains indi_gains
void WEAK stabilization_indi_set_wls_settings(void)
Function that sets the u_min, u_max and u_pref if function not elsewhere defined.
#define STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD
float * stabilization_indi_get_act_state(void)
static bool act_thrust_mat[3][INDI_NUM_ACT]
bool act_is_servo[INDI_NUM_ACT]
static struct FirstOrderLowPass rates_filt_fo[3]
float angular_acceleration[3]
static void send_wls_v_stab(struct transport_tx *trans, struct link_device *dev)
float actuator_state_filt_vectd[INDI_NUM_ACT]
#define STABILIZATION_INDI_ADAPTIVE_MU
#define STABILIZATION_INDI_FILT_CUTOFF_P
static void bound_g_mat(void)
#define STABILIZATION_INDI_FILT_CUTOFF_Q
#define STABILIZATION_INDI_ACT_IS_THRUSTER_X
float g1g2[INDI_OUTPUTS][INDI_NUM_ACT]
static void calc_g2_element(float dx_error, int8_t j, float mu_extra)
static void send_wls_u_stab(struct transport_tx *trans, struct link_device *dev)
bool * stabilization_indi_get_act_is_thruster_z(void)
float estimation_rate_d[INDI_NUM_ACT]
static void init_filters(void)
Function that resets the filters to zeros.
#define STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT
Limit the maximum specific moment that can be compensated (units rad/s^2).
float du_estimation[INDI_NUM_ACT]
float * stabilization_indi_get_act_dyn(void)
float actuator_state_filt_vectdd[INDI_NUM_ACT]
void WEAK stabilization_indi_commit_actuator_cmd(int32_t *cmd)
void stabilization_indi_update_filt_freq(float freq)
static struct FloatRates filter_rates(struct FloatRates *rates)
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
struct FloatVect3 stab_thrust_filt
#define STABILIZATION_INDI_WLS_PRIORITIES
float stablization_indi_yaw_dist_limit
float g1g2inv[INDI_OUTPUTS][INDI_OUTPUTS]
void stabilization_indi_attitude_run(bool in_flight, struct StabilizationSetpoint *att_sp, struct ThrustSetpoint *thrust, int32_t *cmd)
float actuator_state_filt_vect[INDI_NUM_ACT]
#define STABILIZATION_INDI_USE_ADAPTIVE
#define STABILIZATION_INDI_ACT_IS_SERVO
void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
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
#define STABILIZATION_INDI_ACT_IS_THRUSTER_Y
float g2_init[INDI_NUM_ACT]
Butterworth2LowPass estimation_input_lowpass_filters[INDI_NUM_ACT]
static 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 FloatRates WEAK stabilization_indi_rate_controller(struct FloatRates rates, struct FloatRates sp)
Default PD angular acceleration controller.
uint8_t * stabilization_indi_get_act_to_commands(void)
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
int32_t stabilization_indi_get_num_thrusters(void)
float g1_est[INDI_OUTPUTS][INDI_NUM_ACT]
float indi_v[INDI_OUTPUTS]
void WEAK stabilization_indi_get_actuator_state(void)
Function that tries to get actuator feedback.
float stabilization_indi_filter_freq
int16_t actuators_pprz[INDI_NUM_ACT+1]
PPRZ command to each actuator Can be used to directly control actuators from the control algorithm if...
float indi_u[INDI_NUM_ACT]
float * stabilization_indi_get_indi_u(void)
Butterworth2LowPass estimation_output_lowpass_filters[3]
#define STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF
#define STABILIZATION_INDI_FILT_CUTOFF
API to get/set the generic vehicle states.
union StabilizationSetpoint::@354356152067340015216250042204214023005305374154 sp
Thrust setpoint // TODO to a setpoint header Structure to store the desired thrust vector with differ...
enum ThrustSetpoint::@227227372251370165225315201346301141210232313211 type
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_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.
void send_wls_v(char *name, struct WLS_t *WLS_p, struct transport_tx *trans, struct link_device *dev)
void wls_alloc(struct WLS_t *WLS_p, float **B, float *u_guess, float *W_init, int imax)
active set algorithm for control allocation
void send_wls_u(char *name, struct WLS_t *WLS_p, struct transport_tx *trans, struct link_device *dev)