![]() |
Paparazzi UAS
v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
#include "firmwares/rotorcraft/stabilization/stabilization_indi_simple.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 "state.h"
#include "generated/airframe.h"
#include "paparazzi.h"
#include "modules/radio_control/radio_control.h"
#include "filters/low_pass_filter.h"
#include "modules/datalink/telemetry.h"
Go to the source code of this file.
Macros | |
#define | STABILIZATION_INDI_FILT_CUTOFF 8.0 |
#define | STABILIZATION_INDI_FILT_CUTOFF_RDOT STABILIZATION_INDI_FILT_CUTOFF |
#define | STABILIZATION_INDI_MAX_RATE 6.0 |
#define | STABILIZATION_INDI_MAX_R STABILIZATION_ATTITUDE_SP_MAX_R |
#define | STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF 4.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 | INDI_EST_SCALE 0.001 |
Functions | |
static void | lms_estimation (void) |
This is a Least Mean Squares adaptive filter It estimates the actuator effectiveness online, by comparing the expected angular acceleration based on the inputs with the measured angular acceleration. More... | |
static void | indi_init_filters (void) |
static void | send_att_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_init (void) |
Function that initializes important values upon engaging INDI. More... | |
void | stabilization_indi_simple_reset_r_filter_cutoff (float new_cutoff) |
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_earth_cmd_i (struct Int32Vect2 *cmd, int32_t heading) |
Set attitude setpoint from command in earth axes. More... | |
static void | filter_pqr (Butterworth2LowPass *filter, struct FloatRates *new_values) |
Update butterworth filter for p, q and r of a FloatRates struct. More... | |
static void | finite_difference_from_filter (float *output, Butterworth2LowPass *filter) |
Caclulate finite difference form a filter array The filter already contains the previous values. More... | |
static void | finite_difference (float output[3], float new[3], float old[3]) |
Calculate derivative of an array via finite difference. More... | |
void | stabilization_indi_rate_run (struct FloatRates rate_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... | |
Variables | |
struct Int32Eulers | stab_att_sp_euler |
with INT32_ANGLE_FRAC More... | |
struct Int32Quat | stab_att_sp_quat |
with INT32_QUAT_FRAC More... | |
static struct FirstOrderLowPass | rates_filt_fo [3] |
struct IndiVariables | indi |
#define INDI_EST_SCALE 0.001 |
Definition at line 102 of file stabilization_indi_simple.c.
#define STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF 4.0 |
Definition at line 72 of file stabilization_indi_simple.c.
#define STABILIZATION_INDI_FILT_CUTOFF 8.0 |
Definition at line 51 of file stabilization_indi_simple.c.
#define STABILIZATION_INDI_FILT_CUTOFF_P 20.0 |
Definition at line 78 of file stabilization_indi_simple.c.
#define STABILIZATION_INDI_FILT_CUTOFF_Q 20.0 |
Definition at line 84 of file stabilization_indi_simple.c.
#define STABILIZATION_INDI_FILT_CUTOFF_R 20.0 |
Definition at line 90 of file stabilization_indi_simple.c.
#define STABILIZATION_INDI_FILT_CUTOFF_RDOT STABILIZATION_INDI_FILT_CUTOFF |
Definition at line 56 of file stabilization_indi_simple.c.
#define STABILIZATION_INDI_MAX_R STABILIZATION_ATTITUDE_SP_MAX_R |
Definition at line 68 of file stabilization_indi_simple.c.
#define STABILIZATION_INDI_MAX_RATE 6.0 |
Definition at line 60 of file stabilization_indi_simple.c.
|
inlinestatic |
Update butterworth filter for p, q and r of a FloatRates struct.
filter | The filter array to use |
new_values | The new values |
Definition at line 288 of file stabilization_indi_simple.c.
References FloatRates::p, FloatRates::q, FloatRates::r, and update_butterworth_2_low_pass().
Referenced by lms_estimation(), and stabilization_indi_rate_run().
|
inlinestatic |
Calculate derivative of an array via finite difference.
output[3] | The output array |
new[3] | The newest values |
old[3] | The values of the previous timestep |
Definition at line 316 of file stabilization_indi_simple.c.
Referenced by lms_estimation().
|
inlinestatic |
Caclulate finite difference form a filter array The filter already contains the previous values.
output | The output array |
filter | The filter array input |
Definition at line 302 of file stabilization_indi_simple.c.
References SecondOrderLowPass::o.
Referenced by lms_estimation(), and stabilization_indi_rate_run().
|
static |
Definition at line 191 of file stabilization_indi_simple.c.
References IndiVariables::est, indi, init_butterworth_2_low_pass(), init_first_order_low_pass(), p, IndiEstimation::rate, IndiVariables::rate, 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, STABILIZATION_INDI_FILT_CUTOFF_RDOT, stateGetBodyRates_f(), IndiEstimation::u, and IndiVariables::u.
Referenced by stabilization_indi_enter(), and stabilization_indi_init().
|
inlinestatic |
This is a Least Mean Squares adaptive filter It estimates the actuator effectiveness online, by comparing the expected angular acceleration based on the inputs with the measured angular acceleration.
Definition at line 487 of file stabilization_indi_simple.c.
References IndiVariables::adaptive, IndiVariables::est, filter_pqr(), finite_difference(), finite_difference_from_filter(), float_vect_copy(), float_vect_scale(), IndiEstimation::g1, IndiVariables::g1, IndiEstimation::g2, IndiVariables::g2, indi, INDI_EST_SCALE, IndiEstimation::mu, FloatRates::p, FloatRates::q, FloatRates::r, IndiEstimation::rate, IndiEstimation::rate_d, IndiEstimation::rate_dd, stateGetBodyRates_f(), IndiEstimation::u, IndiVariables::u_act_dyn, IndiEstimation::u_d, and IndiEstimation::u_dd.
Referenced by stabilization_indi_rate_run().
|
static |
Definition at line 165 of file stabilization_indi_simple.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 145 of file stabilization_indi_simple.c.
References IndiVariables::angular_accel_ref, dev, IndiVariables::est, IndiEstimation::g1, IndiEstimation::g2, indi, INDI_EST_SCALE, FloatRates::p, FloatRates::q, FloatRates::r, IndiVariables::rate_d, and RATES_SMUL.
Referenced by stabilization_indi_init().
void stabilization_indi_attitude_run | ( | struct Int32Quat | quat_sp, |
bool | in_flight | ||
) |
runs stabilization indi
in_flight | not used |
rate_control | rate control enabled, otherwise attitude control |
Definition at line 430 of file stabilization_indi_simple.c.
References 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, stabilization_indi_rate_run(), stateGetNedToBodyQuat_f(), FloatVect3::x, FloatVect3::y, and FloatVect3::z.
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 223 of file stabilization_indi_simple.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.
void stabilization_indi_init | ( | void | ) |
Function that initializes important values upon engaging INDI.
Definition at line 180 of file stabilization_indi_simple.c.
References act_is_servo, actuator_state_filt_vect, actuator_state_filt_vectd, actuator_state_filt_vectdd, Bwls, calc_g1g2_pseudo_inv(), 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_init_filters(), init_filters(), num_thrusters, register_periodic_telemetry(), rpm_cb(), rpm_ev, RPM_SENSOR_ID, send_ahrs_ref_quat(), send_att_indi(), send_indi_g(), thrust_cb(), thrust_ev, and THRUST_INCREMENT_ID.
void stabilization_indi_rate_run | ( | struct FloatRates | rate_sp, |
bool | in_flight | ||
) |
Does the INDI calculations.
indi_commands[] | Array of commands that the function will write to |
att_err | quaternion attitude error |
Definition at line 329 of file stabilization_indi_simple.c.
References act_is_servo, act_pref, actuator_lowpass_filters, actuator_state, actuator_state_filt_vect, actuator_state_filt_vectd, actuator_state_filt_vectdd, IndiVariables::angular_accel_ref, angular_accel_ref, angular_acceleration, IndiVariables::attitude_max_yaw_rate, autopilot_get_motors_on(), Bwls, IndiVariables::du, du_max, du_min, du_pref, 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_sum(), float_vect_zero(), IndiVariables::g1, g1g2_pseudo_inv, IndiVariables::g2, g2, g2_times_du, IndiVariables::gains, get_actuator_state(), guidance_h, GUIDANCE_H_MODE_HOVER, GUIDANCE_H_MODE_NAV, indi, indi_du, INDI_G_SCALING, indi_gains, indi_thrust_increment, indi_thrust_increment_set, indi_u, indi_use_adaptive, indi_v, lms_estimation(), MAX_PPRZ, measurement_lowpass_filters, HorizontalGuidance::mode, num_iter, num_thrusters, SecondOrderLowPass::o, FloatRates::p, FloatRates::q, FloatRates::r, Indi_gains::rate, IndiVariables::rate, IndiVariables::rate_d, rates_filt_fo, stabilization_cmd, stateGetAirspeed_f(), stateGetBodyRates_f(), IndiVariables::u, IndiVariables::u_act_dyn, IndiVariables::u_in, update_butterworth_2_low_pass(), update_first_order_low_pass(), wls_alloc(), and Wv.
Referenced by stabilization_indi_attitude_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 470 of file stabilization_indi_simple.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().
void stabilization_indi_set_earth_cmd_i | ( | struct Int32Vect2 * | cmd, |
int32_t | heading | ||
) |
Set attitude setpoint from command in earth axes.
cmd | The command in earth axes (North East) |
heading | The desired heading |
Definition at line 265 of file stabilization_indi_simple.c.
References heading, INT32_TRIG_FRAC, Int32Eulers::phi, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Eulers::psi, quat_from_earth_cmd_i(), stab_att_sp_euler, stab_att_sp_quat, stateGetNedToBodyEulers_i(), Int32Eulers::theta, Int32Vect2::x, and Int32Vect2::y.
void stabilization_indi_set_failsafe_setpoint | ( | void | ) |
Function that calculates the failsafe setpoint.
Definition at line 236 of file stabilization_indi_simple.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().
void stabilization_indi_set_rpy_setpoint_i | ( | struct Int32Eulers * | rpy | ) |
Set attitude quaternion setpoint from rpy.
rpy | roll pitch yaw input |
Definition at line 251 of file stabilization_indi_simple.c.
References int32_quat_of_eulers(), stab_att_sp_euler, and stab_att_sp_quat.
void stabilization_indi_simple_reset_r_filter_cutoff | ( | float | new_cutoff | ) |
Definition at line 216 of file stabilization_indi_simple.c.
References IndiVariables::cutoff_r, indi, init_first_order_low_pass(), rates_filt_fo, and stateGetBodyRates_f().
struct IndiVariables indi |
Definition at line 104 of file stabilization_indi_simple.c.
Referenced by ctrl_eff_scheduling_periodic(), indi_init_filters(), lms_estimation(), send_att_indi(), stabilization_indi_attitude_run(), stabilization_indi_enter(), stabilization_indi_rate_run(), and stabilization_indi_simple_reset_r_filter_cutoff().
|
static |
Definition at line 96 of file stabilization_indi_simple.c.
Referenced by indi_init_filters(), stabilization_indi_rate_run(), and stabilization_indi_simple_reset_r_filter_cutoff().
struct Int32Eulers stab_att_sp_euler |
with INT32_ANGLE_FRAC
Definition at line 93 of file stabilization_indi_simple.c.
Referenced by stabilization_indi_enter(), stabilization_indi_set_earth_cmd_i(), and stabilization_indi_set_rpy_setpoint_i().
struct Int32Quat stab_att_sp_quat |
with INT32_QUAT_FRAC
Definition at line 94 of file stabilization_indi_simple.c.
Referenced by send_ahrs_ref_quat(), stabilization_indi_read_rc(), stabilization_indi_set_earth_cmd_i(), stabilization_indi_set_failsafe_setpoint(), and stabilization_indi_set_rpy_setpoint_i().