![]() |
Paparazzi UAS
v6.3_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h"
#include "filters/low_pass_filter.h"
Go to the source code of this file.
Data Structures | |
struct | Indi_gains |
struct | IndiEstimation |
struct | IndiVariables |
Functions | |
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) |
Set attitude quaternion setpoint from rpy. More... | |
void | stabilization_indi_set_quat_setpoint_i (struct Int32Quat *quat) |
void | stabilization_indi_set_earth_cmd_i (struct Int32Vect2 *cmd, int32_t heading) |
Set attitude setpoint from command in earth axes. More... | |
void | stabilization_indi_set_stab_sp (struct StabilizationSetpoint *sp) |
Set attitude setpoint from stabilization setpoint struct. More... | |
void | stabilization_indi_rate_run (struct FloatRates rates_sp, bool in_flight) |
Does the INDI calculations. More... | |
void | stabilization_indi_attitude_run (struct Int32Quat quat_sp, bool in_flight) |
runs stabilization indi More... | |
void | stabilization_indi_read_rc (bool in_flight, bool in_carefree, bool coordinated_turn) |
This function reads rc commands. More... | |
void | stabilization_indi_simple_reset_r_filter_cutoff (float new_cutoff) |
Variables | |
struct Int32Quat | stab_att_sp_quat |
with INT32_QUAT_FRAC More... | |
struct Int32Eulers | stab_att_sp_euler |
with INT32_ANGLE_FRAC More... | |
struct IndiVariables | indi |
struct Indi_gains |
Definition at line 55 of file stabilization_indi.h.
Data Fields | ||
---|---|---|
struct FloatRates | att | |
struct FloatRates | rate |
struct IndiEstimation |
Definition at line 49 of file stabilization_indi_simple.h.
Data Fields | ||
---|---|---|
struct FloatRates | g1 | |
float | g2 | |
float | mu | |
Butterworth2LowPass | rate[3] | |
float | rate_d[3] | |
float | rate_dd[3] | |
Butterworth2LowPass | u[3] | |
float | u_d[3] | |
float | u_dd[3] |
struct IndiVariables |
Definition at line 61 of file stabilization_indi_simple.h.
Data Fields | ||
---|---|---|
struct FloatRates | act_dyn | |
bool | adaptive | Enable adataptive estimation. |
struct FloatRates | angular_accel_ref | |
float | attitude_max_yaw_rate | Maximum yaw rate in atttiude control in rad/s. |
float | cutoff_r | |
struct FloatRates | du | |
struct IndiEstimation | est | Estimation parameters for adaptive INDI. |
struct FloatRates | g1 | |
float | g2 | |
struct Indi_gains | gains | |
float | max_rate | Maximum rate in rate control in rad/s. |
Butterworth2LowPass | rate[3] | |
float | rate_d[3] | |
Butterworth2LowPass | u[3] | |
struct FloatRates | u_act_dyn | |
struct FloatRates | u_in |
void stabilization_indi_attitude_run | ( | struct Int32Quat | quat_sp, |
bool | in_flight | ||
) |
runs stabilization indi
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
in_flight | not used |
rate_control | rate control enabled, otherwise attitude control |
Definition at line 770 of file stabilization_indi.c.
References angular_rate_ref, Indi_gains::att, float_quat_inv_comp_norm_shortest(), float_quat_tilt_twist(), IndiVariables::gains, indi, 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 420 of file stabilization_indi.c.
References IndiVariables::angular_accel_ref, ddu_estimation, du_estimation, FLOAT_RATES_ZERO, float_vect_zero(), indi, indi_init_filters(), Int32Eulers::psi, stab_att_sp_euler, stabilization_attitude_get_heading_i(), IndiVariables::u_act_dyn, and IndiVariables::u_in.
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 347 of file stabilization_indi.c.
References IndiVariables::act_dyn, act_dyn_discrete, act_feedback_cb(), act_feedback_ev, act_first_order_cutoff, 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, indi, indi_init_filters(), init_filters(), num_thrusters, num_thrusters_x, FloatRates::p, FloatRates::q, FloatRates::r, register_periodic_telemetry(), send_ahrs_ref_quat(), send_att_full_indi(), send_att_indi(), send_eff_mat_g_indi(), send_eff_mat_g_indi_simple(), sum_g1_g2(), thrust_cb(), thrust_ev, and THRUST_INCREMENT_ID.
void stabilization_indi_rate_run | ( | struct FloatRates | rate_sp, |
bool | in_flight | ||
) |
Does the INDI calculations.
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
indi_commands[] | Array of commands that the function will write to |
att_err | quaternion attitude error |
Definition at line 551 of file stabilization_indi.c.
References IndiVariables::act_dyn, 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, IndiVariables::angular_accel_ref, angular_acceleration, IndiVariables::attitude_max_yaw_rate, autopilot_get_motors_on(), Bwls, IndiVariables::du, 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, filter_pqr(), finite_difference_from_filter(), FLOAT_RATES_ZERO, float_vect_copy(), float_vect_sum(), IndiVariables::g1, g1g2_pseudo_inv, g2, IndiVariables::g2, g2_times_du, IndiVariables::gains, get_actuator_state(), indi, 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, IndiVariables::rate, IndiVariables::rate_d, rates_filt_fo, stabilization_cmd, stabilization_indi_set_wls_settings(), stateGetBodyRates_f(), IndiVariables::u, IndiVariables::u_act_dyn, IndiVariables::u_in, 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 | ||
) |
This function reads rc commands.
in_flight | boolean that states if the UAV is in flight or not |
Definition at line 822 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 | ||
) |
Set attitude setpoint from command in earth axes.
cmd | 2D command in North East axes |
heading | Heading of the setpoint |
Function that calculates the setpoint quaternion from a command in earth axes
cmd | The command in earth axes (North East) |
heading | The desired heading |
Definition at line 514 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 474 of file stabilization_indi.c.
References FLOAT_RATES_ZERO, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Quat::qi, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, stab_att_ff_rates, 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 501 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 | ) |
Set attitude quaternion setpoint from rpy.
rpy | rpy from which to calculate quaternion setpoint |
Function that calculates the setpoint quaternion from rpy
rpy | roll pitch yaw input |
Definition at line 489 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 537 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_simple_reset_r_filter_cutoff | ( | float | new_cutoff | ) |
Definition at line 251 of file stabilization_indi_simple.c.
References IndiVariables::cutoff_r, indi, init_first_order_low_pass(), rates_filt_fo, and stateGetBodyRates_f().
|
extern |
Definition at line 106 of file stabilization_indi_simple.c.
Referenced by ctrl_eff_scheduling_periodic(), indi_init_filters(), lms_estimation(), send_att_indi(), send_eff_mat_g_indi_simple(), stabilization_indi_attitude_run(), stabilization_indi_enter(), stabilization_indi_init(), stabilization_indi_rate_run(), and stabilization_indi_simple_reset_r_filter_cutoff().
|
extern |
with INT32_ANGLE_FRAC
Definition at line 1 of file stabilization_attitude_euler_float.c.
|
extern |
with INT32_QUAT_FRAC
Definition at line 1 of file stabilization_attitude_heli_indi.c.