![]() |
Paparazzi UAS
v6.3_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h"
#include "math/pprz_algebra_float.h"
#include "state.h"
#include "generated/airframe.h"
#include "modules/radio_control/radio_control.h"
#include "modules/actuators/actuators.h"
#include "modules/core/abi.h"
#include "filters/low_pass_filter.h"
#include "math/wls/wls_alloc.h"
#include <stdio.h>
#include "modules/datalink/telemetry.h"
Go to the source code of this file.
Macros | |
#define | INDI_ALLOWED_G_FACTOR 2.0 |
#define | STABILIZATION_INDI_FILT_CUTOFF_P 20.0 |
#define | STABILIZATION_INDI_FILT_CUTOFF_Q 20.0 |
#define | STABILIZATION_INDI_FILT_CUTOFF_R 20.0 |
#define | STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE FALSE |
#define | STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER FALSE |
#define | STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD 8.0 |
Functions | |
static void | lms_estimation (void) |
Function that estimates the control effectiveness of each actuator online. More... | |
static void | get_actuator_state (void) |
Function that tries to get actuator feedback. More... | |
static void | calc_g1_element (float dx_error, int8_t i, int8_t j, float mu_extra) |
static void | calc_g2_element (float dx_error, int8_t j, float mu_extra) |
static void | bound_g_mat (void) |
static void | thrust_cb (uint8_t sender_id, struct FloatVect3 thrust_increment) |
void | init_filters (void) |
Function that resets the filters to zeros. More... | |
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 make the values readable. More... | |
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) |
static void | send_att_full_indi (struct transport_tx *trans, struct link_device *dev) |
void | stabilization_indi_init (void) |
Function that initializes important values upon engaging INDI. More... | |
void | stabilization_indi_enter (void) |
Function that resets important values upon engaging INDI. More... | |
void | stabilization_indi_set_failsafe_setpoint (void) |
Function that calculates the failsafe setpoint. More... | |
void | stabilization_indi_set_rpy_setpoint_i (struct Int32Eulers *rpy) |
void | stabilization_indi_set_quat_setpoint_i (struct Int32Quat *quat) |
void | stabilization_indi_set_earth_cmd_i (struct Int32Vect2 *cmd, int32_t heading) |
void | stabilization_indi_set_stab_sp (struct StabilizationSetpoint *sp) |
Set attitude setpoint from stabilization setpoint struct. More... | |
void | stabilization_indi_rate_run (struct FloatRates rate_sp, bool in_flight) |
void WEAK | stabilization_indi_set_wls_settings (float use_increment) |
void | stabilization_indi_attitude_run (struct Int32Quat quat_sp, bool in_flight) |
void | stabilization_indi_read_rc (bool in_flight, bool in_carefree, bool coordinated_turn) |
static void | thrust_cb (uint8_t UNUSED sender_id, struct FloatVect3 thrust_increment) |
ABI callback that obtains the thrust increment from guidance INDI. More... | |
Variables | |
float | du_min_stab_indi [INDI_NUM_ACT] |
float | du_max_stab_indi [INDI_NUM_ACT] |
float | du_pref_stab_indi [INDI_NUM_ACT] |
float | indi_v [INDI_OUTPUTS] |
float * | Bwls [INDI_OUTPUTS] |
int | num_iter = 0 |
int32_t | stabilization_att_indi_cmd [COMMANDS_NB] |
struct Indi_gains | indi_gains |
bool | indi_use_adaptive = false |
bool | act_is_servo [INDI_NUM_ACT] = {0} |
bool | act_is_thruster_x [INDI_NUM_ACT] = {0} |
bool | act_is_thruster_z [INDI_NUM_ACT] |
float | act_pref [INDI_NUM_ACT] = {0.0} |
float | act_dyn [INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN |
static float | Wv [INDI_OUTPUTS] = {1000, 1000, 1, 100} |
float | indi_Wu [INDI_NUM_ACT] = {[0 ... INDI_NUM_ACT - 1] = 1.0} |
Weighting of different actuators in the cost function. More... | |
float | actuator_state_filt_vect [INDI_NUM_ACT] |
struct FloatRates | angular_accel_ref = {0., 0., 0.} |
struct FloatRates | angular_rate_ref = {0., 0., 0.} |
float | angular_acceleration [3] = {0., 0., 0.} |
float | actuator_state [INDI_NUM_ACT] |
float | indi_u [INDI_NUM_ACT] |
float | indi_du [INDI_NUM_ACT] |
float | g2_times_du |
float | q_filt = 0.0 |
float | r_filt = 0.0 |
float | g1g2_trans_mult [INDI_OUTPUTS][INDI_OUTPUTS] |
float | g1g2inv [INDI_OUTPUTS][INDI_OUTPUTS] |
float | actuator_state_filt_vectd [INDI_NUM_ACT] |
float | actuator_state_filt_vectdd [INDI_NUM_ACT] |
float | estimation_rate_d [INDI_NUM_ACT] |
float | estimation_rate_dd [INDI_NUM_ACT] |
float | du_estimation [INDI_NUM_ACT] |
float | ddu_estimation [INDI_NUM_ACT] |
float | mu1 [INDI_OUTPUTS] = {0.00001, 0.00001, 0.000003, 0.000002} |
float | mu2 = 0.002 |
float | act_obs [INDI_NUM_ACT] |
int32_t | num_thrusters |
int32_t | num_thrusters_x |
struct Int32Eulers | stab_att_sp_euler |
with INT32_ANGLE_FRAC More... | |
struct Int32Quat | stab_att_sp_quat |
with INT32_QUAT_FRAC More... | |
struct FloatRates | stab_att_ff_rates |
abi_event | thrust_ev |
struct FloatVect3 | indi_thrust_increment |
bool | indi_thrust_increment_set = false |
float | g1g2_pseudo_inv [INDI_NUM_ACT][INDI_OUTPUTS] |
float | g2 [INDI_NUM_ACT] = STABILIZATION_INDI_G2 |
float | g1 [INDI_OUTPUTS][INDI_NUM_ACT] |
float | g1g2 [INDI_OUTPUTS][INDI_NUM_ACT] |
float | g1_est [INDI_OUTPUTS][INDI_NUM_ACT] |
float | g2_est [INDI_NUM_ACT] |
float | g1_init [INDI_OUTPUTS][INDI_NUM_ACT] |
float | g2_init [INDI_NUM_ACT] |
Butterworth2LowPass | actuator_lowpass_filters [INDI_NUM_ACT] |
Butterworth2LowPass | estimation_input_lowpass_filters [INDI_NUM_ACT] |
Butterworth2LowPass | measurement_lowpass_filters [3] |
Butterworth2LowPass | estimation_output_lowpass_filters [3] |
Butterworth2LowPass | acceleration_lowpass_filter |
static struct FirstOrderLowPass | rates_filt_fo [3] |
struct FloatVect3 | body_accel_f |
#define INDI_ALLOWED_G_FACTOR 2.0 |
Definition at line 49 of file stabilization_indi.c.
#define STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE FALSE |
Definition at line 71 of file stabilization_indi.c.
#define STABILIZATION_INDI_FILT_CUTOFF_P 20.0 |
Definition at line 54 of file stabilization_indi.c.
#define STABILIZATION_INDI_FILT_CUTOFF_Q 20.0 |
Definition at line 60 of file stabilization_indi.c.
#define STABILIZATION_INDI_FILT_CUTOFF_R 20.0 |
Definition at line 66 of file stabilization_indi.c.
#define STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER FALSE |
Definition at line 75 of file stabilization_indi.c.
#define STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD 8.0 |
Definition at line 81 of file stabilization_indi.c.
|
static |
Definition at line 1053 of file stabilization_indi.c.
References g1_est, g1_init, g2_est, g2_init, and INDI_ALLOWED_G_FACTOR.
Referenced by lms_estimation().
ddx_error | error in output change |
i | row of the matrix element |
j | column of the matrix element |
mu | learning rate |
Function that calculates an element of the G1 matrix. The elements are stored in a different matrix, because the old matrix is necessary to caclulate more elements.
Definition at line 860 of file stabilization_indi.c.
References du_estimation, g1_est, and mu.
Referenced by lms_estimation().
|
static |
ddx_error | error in output change |
j | column of the matrix element |
mu | learning rate |
Function that calculates an element of the G2 matrix. The elements are stored in a different matrix, because the old matrix is necessary to caclulate more elements.
Definition at line 874 of file stabilization_indi.c.
References ddu_estimation, g2_est, and mu.
Referenced by lms_estimation().
|
static |
Function that tries to get actuator feedback.
If this is not available it will use a first order filter to approximate the actuator state. It is also possible to model rate limits (unit: PPRZ/loop cycle)
Definition at line 824 of file stabilization_indi.c.
References act_dyn, act_obs, actuator_state, float_vect_copy(), indi_u, and UNUSED.
Referenced by stabilization_indi_rate_run().
void init_filters | ( | void | ) |
Function that resets the filters to zeros.
Definition at line 416 of file stabilization_indi.c.
References acceleration_lowpass_filter, actuator_lowpass_filters, estimation_input_lowpass_filters, estimation_output_lowpass_filters, init_butterworth_2_low_pass(), init_first_order_low_pass(), measurement_lowpass_filters, p, FloatRates::q, FloatRates::r, rates_filt_fo, STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF, STABILIZATION_INDI_FILT_CUTOFF, STABILIZATION_INDI_FILT_CUTOFF_P, STABILIZATION_INDI_FILT_CUTOFF_Q, STABILIZATION_INDI_FILT_CUTOFF_R, and stateGetBodyRates_f().
Referenced by stabilization_indi_init().
|
static |
Function that estimates the control effectiveness of each actuator online.
It is assumed that disturbances do not play a large role. All elements of the G1 and G2 matrices are be estimated.
Definition at line 884 of file stabilization_indi.c.
References acceleration_lowpass_filter, ACCELS_FLOAT_OF_BFP, actuator_state_filt_vectd, actuator_state_filt_vectdd, body_accel_f, bound_g_mat(), calc_g1_element(), calc_g2_element(), ddu_estimation, du_estimation, estimation_rate_dd, float_vect_copy(), float_vect_smul(), g1, g1_est, g2, g2_est, mu1, mu2, SecondOrderLowPass::o, stateGetAccelBody_i(), sum_g1_g2(), update_butterworth_2_low_pass(), and FloatVect3::z.
Referenced by stabilization_indi_rate_run().
|
static |
Definition at line 298 of file stabilization_indi.c.
References dev, Int32Quat::qi, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, stab_att_sp_quat, and stateGetNedToBodyQuat_i().
Referenced by stabilization_indi_init().
|
static |
Definition at line 312 of file stabilization_indi.c.
References angular_accel_ref, angular_acceleration, angular_rate_ref, dev, indi_u, FloatRates::p, FloatRates::q, FloatRates::r, and stateGetBodyRates_f().
Referenced by stabilization_indi_init().
|
static |
Definition at line 284 of file stabilization_indi.c.
References dev, g1g2, and g2_est.
Referenced by stabilization_indi_init().
void stabilization_indi_attitude_run | ( | struct Int32Quat | quat_sp, |
bool | in_flight | ||
) |
enable_integrator | |
rate_control | boolean that determines if we are in rate control or attitude control |
Function that should be called to run the INDI controller
Definition at line 754 of file stabilization_indi.c.
References angular_rate_ref, Indi_gains::att, float_quat_inv_comp_norm_shortest(), float_quat_tilt_twist(), indi_gains, indi_thrust_increment_set, FloatRates::p, FloatRates::q, QUAT_FLOAT_OF_BFP, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatRates::r, Indi_gains::rate, RATES_ADD, stab_att_ff_rates, stabilization_indi_rate_run(), stateGetNedToBodyQuat_f(), FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by stabilization_attitude_run().
void stabilization_indi_enter | ( | void | ) |
Function that resets important values upon engaging INDI.
Don't reset inputs and filters, because it is unlikely to switch stabilization in flight, and there are multiple modes that use (the same) stabilization. Resetting the controller is not so nice when you are flying. FIXME: Ideally we should detect when coming from something that is not INDI
Definition at line 404 of file stabilization_indi.c.
References ddu_estimation, du_estimation, float_vect_zero(), Int32Eulers::psi, stab_att_sp_euler, and stabilization_attitude_get_heading_i().
Referenced by stabilization_attitude_enter(), and stabilization_rate_enter().
void stabilization_indi_init | ( | void | ) |
Function that initializes important values upon engaging INDI.
Definition at line 339 of file stabilization_indi.c.
References act_feedback_cb(), act_feedback_ev, act_is_servo, act_is_thruster_x, act_is_thruster_z, actuator_state_filt_vect, actuator_state_filt_vectd, actuator_state_filt_vectdd, Bwls, DefaultPeriodic, estimation_rate_d, estimation_rate_dd, float_vect_copy(), float_vect_zero(), g1, g1_est, g1_init, g1g2, g2, g2_est, g2_init, init_filters(), num_thrusters, num_thrusters_x, register_periodic_telemetry(), send_ahrs_ref_quat(), send_att_full_indi(), send_eff_mat_g_indi(), sum_g1_g2(), thrust_cb(), thrust_ev, and THRUST_INCREMENT_ID.
void stabilization_indi_rate_run | ( | struct FloatRates | rate_sp, |
bool | in_flight | ||
) |
att_err | attitude error |
rate_control | boolean that states if we are in rate control or attitude control |
in_flight | boolean that states if the UAV is in flight or not |
Function that calculates the INDI commands
Definition at line 535 of file stabilization_indi.c.
References act_is_servo, act_is_thruster_x, act_is_thruster_z, actuator_lowpass_filters, actuator_state, actuator_state_filt_vect, actuator_state_filt_vectd, actuator_state_filt_vectdd, angular_accel_ref, angular_acceleration, autopilot_get_motors_on(), Bwls, du_max_stab_indi, du_min_stab_indi, du_pref_stab_indi, estimation_input_lowpass_filters, estimation_output_lowpass_filters, estimation_rate_d, estimation_rate_dd, float_vect_copy(), float_vect_sum(), g1g2_pseudo_inv, g2, g2_times_du, get_actuator_state(), indi_du, INDI_G_SCALING, indi_gains, indi_thrust_increment, indi_thrust_increment_set, indi_u, indi_use_adaptive, indi_v, indi_Wu, lms_estimation(), MAX_PPRZ, measurement_lowpass_filters, num_iter, num_thrusters, num_thrusters_x, SecondOrderLowPass::o, FloatRates::p, FloatRates::q, FloatRates::r, radio_control, Indi_gains::rate, rates_filt_fo, stabilization_cmd, stabilization_indi_set_wls_settings(), stateGetBodyRates_f(), update_butterworth_2_low_pass(), update_first_order_low_pass(), use_increment, RadioControl::values, wls_alloc(), Wv, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by stabilization_indi_attitude_run(), and stabilization_rate_run().
void stabilization_indi_read_rc | ( | bool | in_flight, |
bool | in_carefree, | ||
bool | coordinated_turn | ||
) |
Definition at line 806 of file stabilization_indi.c.
References QUAT_BFP_OF_REAL, stab_att_sp_quat, stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(), and stabilization_attitude_read_rc_setpoint_quat_f().
Referenced by stabilization_attitude_read_rc().
void stabilization_indi_set_earth_cmd_i | ( | struct Int32Vect2 * | cmd, |
int32_t | heading | ||
) |
cmd | 2D command in North East axes |
heading | Heading of the setpoint |
Function that calculates the setpoint quaternion from a command in earth axes
Definition at line 498 of file stabilization_indi.c.
References FLOAT_RATES_ZERO, heading, INT32_TRIG_FRAC, Int32Eulers::phi, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Eulers::psi, quat_from_earth_cmd_i(), stab_att_ff_rates, stab_att_sp_euler, stab_att_sp_quat, stateGetNedToBodyEulers_i(), Int32Eulers::theta, Int32Vect2::x, and Int32Vect2::y.
Referenced by stabilization_attitude_set_earth_cmd_i().
void stabilization_indi_set_failsafe_setpoint | ( | void | ) |
Function that calculates the failsafe setpoint.
Definition at line 458 of file stabilization_indi.c.
References PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Quat::qi, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, stab_att_sp_quat, and stabilization_attitude_get_heading_i().
Referenced by stabilization_attitude_set_failsafe_setpoint().
void stabilization_indi_set_quat_setpoint_i | ( | struct Int32Quat * | quat | ) |
quat | quaternion setpoint |
Definition at line 485 of file stabilization_indi.c.
References FLOAT_RATES_ZERO, int32_eulers_of_quat(), stab_att_ff_rates, stab_att_sp_euler, and stab_att_sp_quat.
Referenced by stabilization_attitude_set_quat_setpoint_i().
void stabilization_indi_set_rpy_setpoint_i | ( | struct Int32Eulers * | rpy | ) |
rpy | rpy from which to calculate quaternion setpoint |
Function that calculates the setpoint quaternion from rpy
Definition at line 473 of file stabilization_indi.c.
References FLOAT_RATES_ZERO, int32_quat_of_eulers(), stab_att_ff_rates, stab_att_sp_euler, and stab_att_sp_quat.
Referenced by stabilization_attitude_set_rpy_setpoint_i(), and vertical_ctrl_module_run().
void stabilization_indi_set_stab_sp | ( | struct StabilizationSetpoint * | sp | ) |
Set attitude setpoint from stabilization setpoint struct.
sp | Stabilization setpoint structure |
Definition at line 521 of file stabilization_indi.c.
References stab_att_ff_rates, stab_att_sp_euler, stab_att_sp_quat, stab_sp_to_eulers_i(), stab_sp_to_quat_i(), and stab_sp_to_rates_f().
Referenced by stabilization_attitude_set_stab_sp().
void stabilization_indi_set_wls_settings | ( | float | use_increment | ) |
use_increment | Function that sets the du_min, du_max and du_pref if function not elsewhere defined |
Definition at line 724 of file stabilization_indi.c.
References act_is_servo, act_pref, actuator_state_filt_vect, du_max_stab_indi, du_min_stab_indi, du_pref_stab_indi, guidance_h, GUIDANCE_H_MODE_HOVER, GUIDANCE_H_MODE_NAV, MAX_PPRZ, HorizontalGuidance::mode, STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD, stateGetAirspeed_f(), and use_increment.
Referenced by stabilization_indi_rate_run().
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 make the values readable.
Definition at line 967 of file stabilization_indi.c.
References g1, g1g2, g2, and INDI_G_SCALING.
Referenced by lms_estimation(), and stabilization_indi_init().
|
static |
|
static |
ABI callback that obtains the thrust increment from guidance INDI.
Definition at line 1047 of file stabilization_indi.c.
References indi_thrust_increment, and indi_thrust_increment_set.
Butterworth2LowPass acceleration_lowpass_filter |
Definition at line 271 of file stabilization_indi.c.
Referenced by init_filters(), and lms_estimation().
float act_dyn[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN |
Definition at line 170 of file stabilization_indi.c.
Referenced by get_actuator_state().
bool act_is_servo[INDI_NUM_ACT] = {0} |
Definition at line 151 of file stabilization_indi.c.
Referenced by ground_detect_sensor_periodic(), stabilization_indi_init(), stabilization_indi_rate_run(), and stabilization_indi_set_wls_settings().
bool act_is_thruster_x[INDI_NUM_ACT] = {0} |
Definition at line 157 of file stabilization_indi.c.
Referenced by stabilization_indi_init(), and stabilization_indi_rate_run().
bool act_is_thruster_z[INDI_NUM_ACT] |
Definition at line 160 of file stabilization_indi.c.
Referenced by stabilization_indi_init(), and stabilization_indi_rate_run().
float act_obs[INDI_NUM_ACT] |
Definition at line 221 of file stabilization_indi.c.
Referenced by get_actuator_state().
float act_pref[INDI_NUM_ACT] = {0.0} |
Definition at line 167 of file stabilization_indi.c.
Referenced by ctrl_eff_scheduling_periodic(), and stabilization_indi_set_wls_settings().
Butterworth2LowPass actuator_lowpass_filters[INDI_NUM_ACT] |
Definition at line 267 of file stabilization_indi.c.
Referenced by init_filters(), and stabilization_indi_rate_run().
float actuator_state[INDI_NUM_ACT] |
Definition at line 197 of file stabilization_indi.c.
Referenced by get_actuator_state(), and stabilization_indi_rate_run().
float actuator_state_filt_vect[INDI_NUM_ACT] |
Definition at line 193 of file stabilization_indi.c.
Referenced by eff_scheduling_cyfoam_periodic(), eff_scheduling_periodic_b(), ground_detect_sensor_periodic(), guidance_indi_hybrid_set_wls_settings(), stabilization_indi_init(), stabilization_indi_rate_run(), and stabilization_indi_set_wls_settings().
float actuator_state_filt_vectd[INDI_NUM_ACT] |
Definition at line 208 of file stabilization_indi.c.
Referenced by lms_estimation(), stabilization_indi_init(), and stabilization_indi_rate_run().
float actuator_state_filt_vectdd[INDI_NUM_ACT] |
Definition at line 209 of file stabilization_indi.c.
Referenced by lms_estimation(), stabilization_indi_init(), and stabilization_indi_rate_run().
struct FloatRates angular_accel_ref = {0., 0., 0.} |
Definition at line 193 of file stabilization_indi.c.
Referenced by send_att_full_indi(), and stabilization_indi_rate_run().
float angular_acceleration[3] = {0., 0., 0.} |
Definition at line 196 of file stabilization_indi.c.
Referenced by send_att_full_indi(), and stabilization_indi_rate_run().
struct FloatRates angular_rate_ref = {0., 0., 0.} |
Definition at line 193 of file stabilization_indi.c.
Referenced by send_att_full_indi(), and stabilization_indi_attitude_run().
struct FloatVect3 body_accel_f |
Definition at line 271 of file stabilization_indi.c.
Referenced by ekf_aw_wrapper_fetch(), and lms_estimation().
float* Bwls[INDI_OUTPUTS] |
Definition at line 112 of file stabilization_indi.c.
Referenced by eff_scheduling_cyfoam_periodic(), stabilization_indi_init(), and stabilization_indi_rate_run().
float ddu_estimation[INDI_NUM_ACT] |
Definition at line 213 of file stabilization_indi.c.
Referenced by calc_g2_element(), lms_estimation(), and stabilization_indi_enter().
float du_estimation[INDI_NUM_ACT] |
Definition at line 212 of file stabilization_indi.c.
Referenced by calc_g1_element(), lms_estimation(), and stabilization_indi_enter().
float du_max_stab_indi[INDI_NUM_ACT] |
Definition at line 109 of file stabilization_indi.c.
Referenced by stabilization_indi_rate_run(), and stabilization_indi_set_wls_settings().
float du_min_stab_indi[INDI_NUM_ACT] |
Definition at line 108 of file stabilization_indi.c.
Referenced by stabilization_indi_rate_run(), and stabilization_indi_set_wls_settings().
float du_pref_stab_indi[INDI_NUM_ACT] |
Definition at line 110 of file stabilization_indi.c.
Referenced by stabilization_indi_rate_run(), and stabilization_indi_set_wls_settings().
Butterworth2LowPass estimation_input_lowpass_filters[INDI_NUM_ACT] |
Definition at line 268 of file stabilization_indi.c.
Referenced by init_filters(), and stabilization_indi_rate_run().
Butterworth2LowPass estimation_output_lowpass_filters[3] |
Definition at line 270 of file stabilization_indi.c.
Referenced by init_filters(), and stabilization_indi_rate_run().
float estimation_rate_d[INDI_NUM_ACT] |
Definition at line 210 of file stabilization_indi.c.
Referenced by stabilization_indi_init(), and stabilization_indi_rate_run().
float estimation_rate_dd[INDI_NUM_ACT] |
Definition at line 211 of file stabilization_indi.c.
Referenced by lms_estimation(), stabilization_indi_init(), and stabilization_indi_rate_run().
float g1[INDI_OUTPUTS][INDI_NUM_ACT] |
Definition at line 256 of file stabilization_indi.c.
Referenced by gain_scheduling_periodic(), lms_estimation(), stabilization_indi_init(), and sum_g1_g2().
float g1_est[INDI_OUTPUTS][INDI_NUM_ACT] |
Definition at line 262 of file stabilization_indi.c.
Referenced by bound_g_mat(), calc_g1_element(), lms_estimation(), and stabilization_indi_init().
float g1_init[INDI_OUTPUTS][INDI_NUM_ACT] |
Definition at line 264 of file stabilization_indi.c.
Referenced by bound_g_mat(), and stabilization_indi_init().
float g1g2[INDI_OUTPUTS][INDI_NUM_ACT] |
Definition at line 261 of file stabilization_indi.c.
Referenced by eff_scheduling_falcon_init(), eff_scheduling_falcon_periodic(), eff_scheduling_falcon_report(), eff_scheduling_generic_periodic(), eff_scheduling_periodic_a(), eff_scheduling_periodic_b(), eff_scheduling_rot_wing_update_aileron_effectiveness(), eff_scheduling_rot_wing_update_elevator_effectiveness(), eff_scheduling_rot_wing_update_flaperon_effectiveness(), eff_scheduling_rot_wing_update_hover_motor_effectiveness(), eff_scheduling_rot_wing_update_pusher_effectiveness(), eff_scheduling_rot_wing_update_rudder_effectiveness(), ground_detect_sensor_periodic(), guidance_indi_hybrid_set_wls_settings(), schdule_control_effectiveness(), send_eff_mat_g_indi(), stabilization_indi_init(), and sum_g1_g2().
float g1g2_pseudo_inv[INDI_NUM_ACT][INDI_OUTPUTS] |
Definition at line 248 of file stabilization_indi.c.
Referenced by stabilization_indi_rate_run().
float g1g2_trans_mult[INDI_OUTPUTS][INDI_OUTPUTS] |
Definition at line 206 of file stabilization_indi.c.
float g1g2inv[INDI_OUTPUTS][INDI_OUTPUTS] |
Definition at line 207 of file stabilization_indi.c.
float g2[INDI_NUM_ACT] = STABILIZATION_INDI_G2 |
Definition at line 249 of file stabilization_indi.c.
Referenced by gain_scheduling_periodic(), lms_estimation(), stabilization_indi_init(), stabilization_indi_rate_run(), and sum_g1_g2().
float g2_est[INDI_NUM_ACT] |
Definition at line 263 of file stabilization_indi.c.
Referenced by bound_g_mat(), calc_g2_element(), lms_estimation(), send_eff_mat_g_indi(), and stabilization_indi_init().
float g2_init[INDI_NUM_ACT] |
Definition at line 265 of file stabilization_indi.c.
Referenced by bound_g_mat(), and stabilization_indi_init().
float g2_times_du |
Definition at line 200 of file stabilization_indi.c.
Referenced by stabilization_indi_rate_run().
float indi_du[INDI_NUM_ACT] |
Definition at line 199 of file stabilization_indi.c.
Referenced by stabilization_indi_rate_run().
struct Indi_gains indi_gains |
Definition at line 124 of file stabilization_indi.c.
Referenced by eff_scheduling_periodic_b(), stabilization_indi_attitude_run(), and stabilization_indi_rate_run().
struct FloatVect3 indi_thrust_increment |
Definition at line 244 of file stabilization_indi.c.
Referenced by stabilization_indi_rate_run(), and thrust_cb().
bool indi_thrust_increment_set = false |
Definition at line 246 of file stabilization_indi.c.
Referenced by stabilization_indi_attitude_run(), stabilization_indi_rate_run(), and thrust_cb().
float indi_u[INDI_NUM_ACT] |
Definition at line 198 of file stabilization_indi.c.
Referenced by get_actuator_state(), send_att_full_indi(), and stabilization_indi_rate_run().
bool indi_use_adaptive = false |
Definition at line 141 of file stabilization_indi.c.
Referenced by stabilization_indi_rate_run().
float indi_v[INDI_OUTPUTS] |
Definition at line 111 of file stabilization_indi.c.
Referenced by stabilization_indi_rate_run().
float indi_Wu[INDI_NUM_ACT] = {[0 ... INDI_NUM_ACT - 1] = 1.0} |
Weighting of different actuators in the cost function.
Definition at line 189 of file stabilization_indi.c.
Referenced by schdule_control_effectiveness(), and stabilization_indi_rate_run().
Butterworth2LowPass measurement_lowpass_filters[3] |
Definition at line 269 of file stabilization_indi.c.
Referenced by init_filters(), and stabilization_indi_rate_run().
float mu1[INDI_OUTPUTS] = {0.00001, 0.00001, 0.000003, 0.000002} |
Definition at line 216 of file stabilization_indi.c.
Referenced by lms_estimation().
float mu2 = 0.002 |
Definition at line 218 of file stabilization_indi.c.
Referenced by lms_estimation().
int num_iter = 0 |
Definition at line 113 of file stabilization_indi.c.
Referenced by stabilization_indi_rate_run().
int32_t num_thrusters |
Definition at line 224 of file stabilization_indi.c.
Referenced by stabilization_indi_init(), and stabilization_indi_rate_run().
int32_t num_thrusters_x |
Definition at line 225 of file stabilization_indi.c.
Referenced by stabilization_indi_init(), and stabilization_indi_rate_run().
float q_filt = 0.0 |
Definition at line 202 of file stabilization_indi.c.
float r_filt = 0.0 |
Definition at line 203 of file stabilization_indi.c.
|
static |
Definition at line 271 of file stabilization_indi.c.
Referenced by init_filters(), and stabilization_indi_rate_run().
struct FloatRates stab_att_ff_rates |
Definition at line 225 of file stabilization_indi.c.
Referenced by stabilization_indi_attitude_run(), stabilization_indi_set_earth_cmd_i(), stabilization_indi_set_quat_setpoint_i(), stabilization_indi_set_rpy_setpoint_i(), and stabilization_indi_set_stab_sp().
struct Int32Eulers stab_att_sp_euler |
with INT32_ANGLE_FRAC
Definition at line 225 of file stabilization_indi.c.
Referenced by stabilization_indi_enter(), stabilization_indi_set_earth_cmd_i(), stabilization_indi_set_quat_setpoint_i(), stabilization_indi_set_rpy_setpoint_i(), and stabilization_indi_set_stab_sp().
struct Int32Quat stab_att_sp_quat |
with INT32_QUAT_FRAC
Definition at line 225 of file stabilization_indi.c.
Referenced by send_ahrs_ref_quat(), stabilization_indi_read_rc(), stabilization_indi_set_earth_cmd_i(), stabilization_indi_set_failsafe_setpoint(), stabilization_indi_set_quat_setpoint_i(), stabilization_indi_set_rpy_setpoint_i(), and stabilization_indi_set_stab_sp().
int32_t stabilization_att_indi_cmd[COMMANDS_NB] |
Definition at line 124 of file stabilization_indi.c.
abi_event thrust_ev |
Definition at line 243 of file stabilization_indi.c.
Referenced by stabilization_indi_init().
|
static |
Definition at line 179 of file stabilization_indi.c.
Referenced by stabilization_indi_rate_run().