|
Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
#include "firmwares/rotorcraft/stabilization/stabilization_indi.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 |
| #define | STABILIZATION_INDI_USE_ADAPTIVE false |
| #define | STABILIZATION_INDI_ADAPTIVE_MU 0.001f |
| #define | STABILIZATION_INDI_ACT_IS_SERVO {0} |
| #define | STABILIZATION_INDI_ACT_IS_THRUSTER_X {0} |
| #define | STABILIZATION_INDI_ACT_IS_THRUSTER_Y {0} |
| #define | STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT 99999.f |
| Limit the maximum specific moment that can be compensated (units rad/s^2). | |
| #define | STABILIZATION_INDI_WLS_PRIORITIES {1000, 1000, 1, 100} |
| #define | STABILIZATION_INDI_WLS_WU {[0 ... INDI_NUM_ACT - 1] = 1.0} |
| #define | STABILIZATION_INDI_ACT_PREF {0.0} |
Functions | |
| static void | lms_estimation (void) |
| Function that estimates the control effectiveness of each actuator online. | |
| 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 | init_filters (void) |
| Function that resets the filters to zeros. | |
| static struct FloatRates | filter_rates (struct FloatRates *rates) |
| 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 make the values readable. | |
| static void | send_wls_v_stab (struct transport_tx *trans, struct link_device *dev) |
| static void | send_wls_u_stab (struct transport_tx *trans, struct link_device *dev) |
| 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. | |
| void | stabilization_indi_enter (void) |
| Function that resets important values upon engaging INDI. | |
| void | stabilization_indi_update_filt_freq (float freq) |
| void | stabilization_indi_rate_run (bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd) |
| void | stabilization_indi_attitude_run (bool in_flight, struct StabilizationSetpoint *att_sp, struct ThrustSetpoint *thrust, int32_t *cmd) |
| struct FloatRates WEAK | stabilization_indi_attitude_controller (struct FloatQuat att, struct FloatQuat att_sp, struct FloatRates rates_ff) |
| Default PD angular rate controller. | |
| struct FloatRates WEAK | stabilization_indi_rate_controller (struct FloatRates rates, struct FloatRates sp) |
| Default PD angular acceleration controller. | |
| void WEAK | stabilization_indi_set_wls_settings (void) |
| Function that sets the u_min, u_max and u_pref if function not elsewhere defined. | |
| void WEAK | stabilization_indi_get_actuator_state (void) |
| Function that tries to get actuator feedback. | |
| void WEAK | stabilization_indi_commit_actuator_cmd (int32_t *cmd) |
| float * | stabilization_indi_get_act_state (void) |
| float * | stabilization_indi_get_act_dyn (void) |
| float * | stabilization_indi_get_indi_u (void) |
| bool * | stabilization_indi_get_act_is_thruster_z (void) |
| int32_t | stabilization_indi_get_num_thrusters (void) |
| uint8_t * | stabilization_indi_get_act_to_commands (void) |
Variables | |
| struct WLS_t | wls_stab_p |
| float | act_pref [INDI_NUM_ACT] = STABILIZATION_INDI_ACT_PREF |
| float | indi_v [INDI_OUTPUTS] |
| float * | Bwls [INDI_OUTPUTS] |
| int32_t | stabilization_att_indi_cmd [COMMANDS_NB] |
| struct Indi_gains | indi_gains |
| bool | indi_use_adaptive = STABILIZATION_INDI_USE_ADAPTIVE |
| bool | act_is_servo [INDI_NUM_ACT] = STABILIZATION_INDI_ACT_IS_SERVO |
| static bool | act_thrust_mat [3][INDI_NUM_ACT] |
| float | act_first_order_cutoff [INDI_NUM_ACT] = STABILIZATION_INDI_ACT_FREQ |
| float | act_dyn_discrete [INDI_NUM_ACT] |
| float | stablization_indi_yaw_dist_limit = STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT |
| 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] |
| struct FloatVect3 | stab_thrust_filt = { 0.f, 0.f, 0.f } |
| float | q_filt = 0.0 |
| float | r_filt = 0.0 |
| float | stabilization_indi_filter_freq = 20.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 |
| static struct Int32Eulers | stab_att_sp_euler |
| static struct Int32Quat | stab_att_sp_quat |
| 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] |
| 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 the command_laws are set up appropriately in the airframe file. | |
| 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 47 of file stabilization_indi.c.
Referenced by bound_g_mat().
| #define STABILIZATION_INDI_ACT_IS_SERVO {0} |
Definition at line 91 of file stabilization_indi.c.
| #define STABILIZATION_INDI_ACT_IS_THRUSTER_X {0} |
Definition at line 95 of file stabilization_indi.c.
| #define STABILIZATION_INDI_ACT_IS_THRUSTER_Y {0} |
Definition at line 99 of file stabilization_indi.c.
| #define STABILIZATION_INDI_ACT_PREF {0.0} |
Definition at line 142 of file stabilization_indi.c.
| #define STABILIZATION_INDI_ADAPTIVE_MU 0.001f |
Definition at line 87 of file stabilization_indi.c.
Referenced by lms_estimation().
| #define STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE FALSE |
Definition at line 69 of file stabilization_indi.c.
| #define STABILIZATION_INDI_FILT_CUTOFF_P 20.0 |
Definition at line 52 of file stabilization_indi.c.
Referenced by indi_init_filters(), and init_filters().
| #define STABILIZATION_INDI_FILT_CUTOFF_Q 20.0 |
Definition at line 58 of file stabilization_indi.c.
Referenced by indi_init_filters(), and init_filters().
| #define STABILIZATION_INDI_FILT_CUTOFF_R 20.0 |
Definition at line 64 of file stabilization_indi.c.
Referenced by indi_init_filters(), and init_filters().
| #define STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER FALSE |
Definition at line 73 of file stabilization_indi.c.
| #define STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD 8.0 |
Definition at line 79 of file stabilization_indi.c.
Referenced by stabilization_indi_set_wls_settings().
| #define STABILIZATION_INDI_USE_ADAPTIVE false |
Definition at line 83 of file stabilization_indi.c.
| #define STABILIZATION_INDI_WLS_PRIORITIES {1000, 1000, 1, 100} |
Definition at line 130 of file stabilization_indi.c.
| #define STABILIZATION_INDI_WLS_WU {[0 ... INDI_NUM_ACT - 1] = 1.0} |
Definition at line 136 of file stabilization_indi.c.
| #define STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT 99999.f |
Limit the maximum specific moment that can be compensated (units rad/s^2).
Definition at line 106 of file stabilization_indi.c.
|
static |
Definition at line 1161 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 985 of file stabilization_indi.c.
References du_estimation, and g1_est.
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 999 of file stabilization_indi.c.
References ddu_estimation, and g2_est.
Referenced by lms_estimation().
|
static |
Definition at line 562 of file stabilization_indi.c.
References filter_rates(), FloatRates::p, FloatRates::q, FloatRates::r, rates_filt_fo, update_butterworth_2_low_pass(), and update_first_order_low_pass().
Referenced by filter_rates(), and stabilization_indi_rate_run().
|
static |
Function that resets the filters to zeros.
Definition at line 523 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, 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 1009 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, STABILIZATION_INDI_ADAPTIVE_MU, stateGetAccelBody_i(), sum_g1_g2(), and update_butterworth_2_low_pass().
Referenced by stabilization_indi_rate_run().
|
static |
Definition at line 374 of file stabilization_indi.c.
References dev, Int32Quat::qi, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, stab_att_sp_quat, and stateGetNedToBodyQuat_i().
|
static |
Definition at line 388 of file stabilization_indi.c.
References angular_accel_ref, angular_acceleration, angular_rate_ref, dev, EULERS_FLOAT_OF_BFP, float_eulers_of_quat_zxy(), FloatRates::p, FloatEulers::phi, FloatEulers::psi, FloatRates::q, QUAT_FLOAT_OF_BFP, FloatRates::r, stab_att_sp_euler, stab_att_sp_quat, stateGetBodyRates_f(), stateGetNedToBodyEulers_f(), stateGetNedToBodyEulersZxy_f(), and FloatEulers::theta.
Referenced by stabilization_indi_init().
|
static |
Definition at line 343 of file stabilization_indi.c.
References dev, g1g2, and g2_est.
Referenced by stabilization_indi_init().
|
static |
Definition at line 338 of file stabilization_indi.c.
References dev, send_wls_u(), and wls_stab_p.
Referenced by stabilization_indi_init().
|
static |
Definition at line 334 of file stabilization_indi.c.
References dev, send_wls_v(), and wls_stab_p.
Referenced by stabilization_indi_init().
| struct FloatRates WEAK stabilization_indi_attitude_controller | ( | struct FloatQuat | att, |
| struct FloatQuat | att_sp, | ||
| struct FloatRates | rates_ff ) |
Default PD angular rate controller.
Takes the current attitude filtered state and setpoint and compute the desired rates. Can be redefined elsewhere to use an other control scheme.
Definition at line 846 of file stabilization_indi.c.
References float_quat_inv_comp_norm_shortest(), float_quat_tilt_twist(), indi_gains, FloatRates::p, FloatRates::q, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatRates::r, RATES_ADD, stabilization_indi_attitude_controller(), FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by stabilization_indi_attitude_controller(), and stabilization_indi_attitude_run().
| void stabilization_indi_attitude_run | ( | bool | in_flight, |
| struct StabilizationSetpoint * | att_sp, | ||
| struct ThrustSetpoint * | thrust, | ||
| int32_t * | cmd ) |
| in_flight | enable integrator only in flight | |
| att_sp | attitude stabilization setpoint | |
| thrust | thrust setpoint | |
| [out] | output | command vector |
Function that should be called to run the INDI controller
Definition at line 824 of file stabilization_indi.c.
References angular_rate_ref, StabilizationSetpoint::sp, stab_att_sp_euler, stab_att_sp_quat, stab_sp_from_rates_f(), stab_sp_to_eulers_i(), stab_sp_to_quat_f(), stab_sp_to_quat_i(), stab_sp_to_rates_f(), stabilization_indi_attitude_controller(), stabilization_indi_rate_run(), and stateGetNedToBodyQuat_f().
Referenced by stabilization_attitude_run().
| void WEAK stabilization_indi_commit_actuator_cmd | ( | int32_t * | cmd | ) |
Definition at line 1205 of file stabilization_indi.c.
References act_thrust_mat, actuator_state, actuators_pprz, indi_u, and num_thrusters.
Referenced by stabilization_indi_rate_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 498 of file stabilization_indi.c.
References ddu_estimation, du_estimation, and float_vect_zero().
Referenced by stabilization_attitude_enter(), and stabilization_rate_enter().
| float * stabilization_indi_get_act_dyn | ( | void | ) |
Definition at line 1228 of file stabilization_indi.c.
References act_dyn_discrete.
Referenced by stabilization_indi_get_actuator_state().
| bool * stabilization_indi_get_act_is_thruster_z | ( | void | ) |
Definition at line 1236 of file stabilization_indi.c.
References act_thrust_mat.
Referenced by stabilization_indi_commit_actuator_cmd().
| float * stabilization_indi_get_act_state | ( | void | ) |
Definition at line 1224 of file stabilization_indi.c.
References actuator_state.
Referenced by stabilization_indi_commit_actuator_cmd(), and stabilization_indi_get_actuator_state().
| uint8_t * stabilization_indi_get_act_to_commands | ( | void | ) |
Definition at line 1245 of file stabilization_indi.c.
Referenced by stabilization_indi_commit_actuator_cmd().
| void WEAK stabilization_indi_get_actuator_state | ( | void | ) |
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 925 of file stabilization_indi.c.
References act_dyn_discrete, act_obs, actuator_state, float_vect_copy(), indi_u, and UNUSED.
Referenced by stabilization_indi_rate_run().
| float * stabilization_indi_get_indi_u | ( | void | ) |
Definition at line 1232 of file stabilization_indi.c.
References indi_u.
Referenced by stabilization_indi_commit_actuator_cmd(), and stabilization_indi_get_actuator_state().
| int32_t stabilization_indi_get_num_thrusters | ( | void | ) |
Definition at line 1240 of file stabilization_indi.c.
References num_thrusters.
Referenced by stabilization_indi_commit_actuator_cmd().
| void stabilization_indi_init | ( | void | ) |
Function that initializes important values upon engaging INDI.
Definition at line 423 of file stabilization_indi.c.
References act_dyn_discrete, act_feedback_cb(), act_feedback_ev, act_first_order_cutoff, act_is_servo, act_thrust_mat, 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, register_periodic_telemetry(), send_ahrs_ref_quat(), send_att_full_indi(), send_eff_mat_g_indi(), send_wls_u_stab(), send_wls_v_stab(), and sum_g1_g2().
| struct FloatRates WEAK stabilization_indi_rate_controller | ( | struct FloatRates | rates, |
| struct FloatRates | sp ) |
Default PD angular acceleration controller.
Takes the current rates filtered state and setpoint and compute the desired acceleration. Can be redefined elsewhere to use an other control scheme.
Definition at line 880 of file stabilization_indi.c.
References indi_gains, FloatRates::p, FloatRates::q, FloatRates::r, and stabilization_indi_rate_controller().
Referenced by stabilization_indi_rate_controller(), and stabilization_indi_rate_run().
| void stabilization_indi_rate_run | ( | bool | in_flight, |
| struct StabilizationSetpoint * | sp, | ||
| struct ThrustSetpoint * | thrust, | ||
| int32_t * | cmd ) |
| in_flight | boolean that states if the UAV is in flight or not |
| sp | rate setpoint |
| thrust | thrust setpoint |
| cmd | output command array |
Function that calculates the INDI commands
Definition at line 603 of file stabilization_indi.c.
References ACCELS_FLOAT_OF_BFP, act_is_servo, act_thrust_mat, 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(), body_accel_f, Bwls, estimation_input_lowpass_filters, estimation_output_lowpass_filters, estimation_rate_d, estimation_rate_dd, filter_rates(), float_mat_vect_mul(), FLOAT_VECT3_ZERO, float_vect_diff(), float_vect_dot_product(), float_vect_zero(), g1g2_pseudo_inv, g2, INDI_G_SCALING, indi_u, indi_use_adaptive, indi_v, lms_estimation(), MAX_PPRZ, measurement_lowpass_filters, FloatRates::p, FloatRates::q, FloatRates::r, radio_control, stab_sp_to_rates_f(), stab_thrust_filt, stabilization_indi_commit_actuator_cmd(), stabilization_indi_get_actuator_state(), stabilization_indi_rate_controller(), stabilization_indi_set_wls_settings(), stablization_indi_yaw_dist_limit, stateGetAccelBody_i(), stateGetBodyRates_f(), th_sp_to_incr_f(), th_sp_to_thrust_i(), THRUST_AXIS_X, THRUST_AXIS_Y, THRUST_AXIS_Z, ThrustSetpoint::type, update_butterworth_2_low_pass(), VECT3_ADD, wls_alloc(), wls_stab_p, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by stabilization_indi_attitude_run(), and stabilization_rate_run().
| void stabilization_indi_set_wls_settings | ( | void | ) |
Function that sets the u_min, u_max and u_pref if function not elsewhere defined.
Definition at line 894 of file stabilization_indi.c.
References act_is_servo, act_pref, guidance_h, GUIDANCE_H_MODE_HOVER, GUIDANCE_H_MODE_NAV, MAX_PPRZ, STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD, stateGetAirspeed_f(), and wls_stab_p.
Referenced by stabilization_indi_rate_run().
| void stabilization_indi_update_filt_freq | ( | float | freq | ) |
Definition at line 504 of file stabilization_indi.c.
References init_butterworth_2_low_pass(), init_first_order_low_pass(), p, rates_filt_fo, stabilization_indi_filter_freq, and stateGetBodyRates_f().
|
static |
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 1084 of file stabilization_indi.c.
References g1, g1g2, g2, and INDI_G_SCALING.
Referenced by lms_estimation(), and stabilization_indi_init().
| Butterworth2LowPass acceleration_lowpass_filter |
Definition at line 319 of file stabilization_indi.c.
Referenced by init_filters(), and lms_estimation().
| float act_dyn_discrete[INDI_NUM_ACT] |
Definition at line 233 of file stabilization_indi.c.
Referenced by stabilization_indi_get_act_dyn(), stabilization_indi_get_actuator_state(), and stabilization_indi_init().
| float act_first_order_cutoff[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_FREQ |
Definition at line 232 of file stabilization_indi.c.
Referenced by stabilization_indi_init().
| bool act_is_servo[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_IS_SERVO |
Definition at line 204 of file stabilization_indi.c.
Referenced by ground_detect_periodic(), stabilization_indi_init(), stabilization_indi_rate_run(), and stabilization_indi_set_wls_settings().
| float act_obs[INDI_NUM_ACT] |
Definition at line 268 of file stabilization_indi.c.
Referenced by stabilization_indi_get_actuator_state().
| float act_pref[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_PREF |
Definition at line 167 of file stabilization_indi.c.
Referenced by ctrl_eff_scheduling_periodic(), and stabilization_indi_set_wls_settings().
|
static |
Definition at line 211 of file stabilization_indi.c.
Referenced by stabilization_indi_commit_actuator_cmd(), stabilization_indi_get_act_is_thruster_z(), stabilization_indi_init(), and stabilization_indi_rate_run().
| Butterworth2LowPass actuator_lowpass_filters[INDI_NUM_ACT] |
Definition at line 315 of file stabilization_indi.c.
| float actuator_state[INDI_NUM_ACT] |
Definition at line 243 of file stabilization_indi.c.
| float actuator_state_filt_vect[INDI_NUM_ACT] |
Definition at line 239 of file stabilization_indi.c.
Referenced by eff_scheduling_cyfoam_periodic(), eff_scheduling_periodic_b(), eff_scheduling_rotwing_update_cmd(), eff_scheduling_rotwing_update_hover_motor_effectiveness(), ground_detect_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 255 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 256 of file stabilization_indi.c.
Referenced by lms_estimation(), stabilization_indi_init(), and stabilization_indi_rate_run().
| 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 the command_laws are set up appropriately in the airframe file.
FIXME add an extra slot for specific case (e.g. rotwing in simulation)
Definition at line 313 of file stabilization_indi.c.
Referenced by actuators_hitl_periodic(), control_mixing_heewing_attitude_direct(), control_mixing_heewing_attitude_plane(), control_mixing_heewing_nav_run(), ekf_aw_wrapper_fetch(), nps_autopilot_run_step(), rotwing_state_periodic(), schdule_control_effectiveness(), stabilization_indi_commit_actuator_cmd(), and stabilization_indi_set_wls_settings().
| struct FloatRates angular_accel_ref = {0., 0., 0.} |
Definition at line 240 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 242 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 241 of file stabilization_indi.c.
Referenced by send_att_full_indi(), and stabilization_indi_attitude_run().
| struct FloatVect3 body_accel_f |
Definition at line 325 of file stabilization_indi.c.
Referenced by ekf_aw_wrapper_fetch(), lms_estimation(), stabilization_indi_rate_run(), and stateGetAccelBody_f().
| float* Bwls[INDI_OUTPUTS] |
Definition at line 172 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 260 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 259 of file stabilization_indi.c.
Referenced by calc_g1_element(), lms_estimation(), and stabilization_indi_enter().
| Butterworth2LowPass estimation_input_lowpass_filters[INDI_NUM_ACT] |
Definition at line 316 of file stabilization_indi.c.
Referenced by init_filters(), and stabilization_indi_rate_run().
| Butterworth2LowPass estimation_output_lowpass_filters[3] |
Definition at line 318 of file stabilization_indi.c.
Referenced by init_filters(), and stabilization_indi_rate_run().
| float estimation_rate_d[INDI_NUM_ACT] |
Definition at line 257 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 258 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 300 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 307 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 309 of file stabilization_indi.c.
Referenced by bound_g_mat(), and stabilization_indi_init().
| float g1g2[INDI_OUTPUTS][INDI_NUM_ACT] |
Definition at line 306 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_rotwing_update_aileron_effectiveness(), eff_scheduling_rotwing_update_elevator_effectiveness(), eff_scheduling_rotwing_update_flaperon_effectiveness(), eff_scheduling_rotwing_update_hover_motor_effectiveness(), eff_scheduling_rotwing_update_pusher_effectiveness(), eff_scheduling_rotwing_update_rudder_effectiveness(), ground_detect_periodic(), guidance_indi_hybrid_set_wls_settings(), guidance_indi_run(), 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 288 of file stabilization_indi.c.
Referenced by stabilization_indi_rate_run().
| float g1g2_trans_mult[INDI_OUTPUTS][INDI_OUTPUTS] |
Definition at line 253 of file stabilization_indi.c.
| float g1g2inv[INDI_OUTPUTS][INDI_OUTPUTS] |
Definition at line 254 of file stabilization_indi.c.
| float g2[INDI_NUM_ACT] = STABILIZATION_INDI_G2 |
Definition at line 289 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 308 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 310 of file stabilization_indi.c.
Referenced by bound_g_mat(), and stabilization_indi_init().
| struct Indi_gains indi_gains |
Definition at line 184 of file stabilization_indi.c.
Referenced by eff_scheduling_periodic_b(), stabilization_indi_attitude_controller(), and stabilization_indi_rate_controller().
| float indi_u[INDI_NUM_ACT] |
Definition at line 244 of file stabilization_indi.c.
Referenced by stabilization_indi_commit_actuator_cmd(), stabilization_indi_get_actuator_state(), stabilization_indi_get_indi_u(), and stabilization_indi_rate_run().
| bool indi_use_adaptive = STABILIZATION_INDI_USE_ADAPTIVE |
Definition at line 197 of file stabilization_indi.c.
Referenced by stabilization_indi_rate_run().
| float indi_v[INDI_OUTPUTS] |
Definition at line 171 of file stabilization_indi.c.
Referenced by stabilization_indi_rate_run().
| Butterworth2LowPass measurement_lowpass_filters[3] |
Definition at line 317 of file stabilization_indi.c.
| float mu1[INDI_OUTPUTS] = {0.00001, 0.00001, 0.000003, 0.000002} |
Definition at line 263 of file stabilization_indi.c.
Referenced by lms_estimation().
| float mu2 = 0.002 |
Definition at line 265 of file stabilization_indi.c.
Referenced by lms_estimation().
| int32_t num_thrusters |
Definition at line 271 of file stabilization_indi.c.
Referenced by stabilization_indi_commit_actuator_cmd(), stabilization_indi_get_num_thrusters(), and stabilization_indi_init().
| float q_filt = 0.0 |
Definition at line 247 of file stabilization_indi.c.
| float r_filt = 0.0 |
Definition at line 248 of file stabilization_indi.c.
|
static |
Definition at line 323 of file stabilization_indi.c.
Referenced by filter_rates(), indi_init_filters(), init_filters(), stabilization_indi_rate_run(), stabilization_indi_simple_reset_r_filter_cutoff(), and stabilization_indi_update_filt_freq().
|
static |
Definition at line 273 of file stabilization_indi.c.
|
static |
Definition at line 274 of file stabilization_indi.c.
| struct FloatVect3 stab_thrust_filt = { 0.f, 0.f, 0.f } |
Definition at line 245 of file stabilization_indi.c.
Referenced by guidance_indi_calcG(), guidance_indi_set_wls_settings(), and stabilization_indi_rate_run().
| int32_t stabilization_att_indi_cmd[COMMANDS_NB] |
Definition at line 183 of file stabilization_indi.c.
| float stabilization_indi_filter_freq = 20.0 |
Definition at line 250 of file stabilization_indi.c.
Referenced by stabilization_indi_update_filt_freq().
| float stablization_indi_yaw_dist_limit = STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT |
Definition at line 236 of file stabilization_indi.c.
Referenced by stabilization_indi_rate_run().
| struct WLS_t wls_stab_p |
Definition at line 152 of file stabilization_indi.c.