|
Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
#include "firmwares/rotorcraft/stabilization/stabilization_indi_simple.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. | |
| static void | indi_init_filters (void) |
| static void | send_att_indi (struct transport_tx *trans, struct link_device *dev) |
| static void | send_eff_mat_g_indi_simple (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. | |
| void | stabilization_indi_simple_reset_r_filter_cutoff (float new_cutoff) |
| void | stabilization_indi_enter (void) |
| Function that resets important values upon engaging INDI. | |
| static void | filter_pqr (Butterworth2LowPass *filter, struct FloatRates *new_values) |
| Update butterworth filter for p, q and r of a FloatRates struct. | |
| static void | finite_difference_from_filter (float *output, Butterworth2LowPass *filter) |
| Caclulate finite difference form a filter array The filter already contains the previous values. | |
| static void | finite_difference (float output[3], float new[3], float old[3]) |
| Calculate derivative of an array via finite difference. | |
| void | stabilization_indi_rate_run (bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd) |
| Does the INDI calculations. | |
| void | stabilization_indi_attitude_run (bool in_flight, struct StabilizationSetpoint *att_sp, struct ThrustSetpoint *thrust, int32_t *cmd) |
| runs stabilization indi | |
Variables | |
| struct Int32Eulers | stab_att_sp_euler |
| struct Int32Quat | stab_att_sp_quat |
| static struct FirstOrderLowPass | rates_filt_fo [3] |
| struct IndiVariables | indi |
| #define INDI_EST_SCALE 0.001 |
Definition at line 107 of file stabilization_indi_simple.c.
Referenced by lms_estimation(), and send_eff_mat_g_indi_simple().
| #define STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF 4.0 |
Definition at line 77 of file stabilization_indi_simple.c.
Referenced by indi_init_filters(), and init_filters().
| #define STABILIZATION_INDI_FILT_CUTOFF 8.0 |
Definition at line 56 of file stabilization_indi_simple.c.
Referenced by indi_init_filters(), and init_filters().
| #define STABILIZATION_INDI_FILT_CUTOFF_P 20.0 |
Definition at line 83 of file stabilization_indi_simple.c.
| #define STABILIZATION_INDI_FILT_CUTOFF_Q 20.0 |
Definition at line 89 of file stabilization_indi_simple.c.
| #define STABILIZATION_INDI_FILT_CUTOFF_R 20.0 |
Definition at line 95 of file stabilization_indi_simple.c.
| #define STABILIZATION_INDI_FILT_CUTOFF_RDOT STABILIZATION_INDI_FILT_CUTOFF |
Definition at line 61 of file stabilization_indi_simple.c.
Referenced by indi_init_filters().
| #define STABILIZATION_INDI_MAX_R STABILIZATION_ATTITUDE_SP_MAX_R |
Definition at line 73 of file stabilization_indi_simple.c.
| #define STABILIZATION_INDI_MAX_RATE 6.0 |
Definition at line 65 of file stabilization_indi_simple.c.
Referenced by stabilization_rate_read_rc().
|
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 267 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 295 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 281 of file stabilization_indi_simple.c.
References SecondOrderLowPass::o.
Referenced by lms_estimation(), and stabilization_indi_rate_run().
|
static |
Definition at line 216 of file stabilization_indi_simple.c.
References indi, init_butterworth_2_low_pass(), init_first_order_low_pass(), 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, STABILIZATION_INDI_FILT_CUTOFF_RDOT, and stateGetBodyRates_f().
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 462 of file stabilization_indi_simple.c.
References filter_pqr(), finite_difference(), finite_difference_from_filter(), float_vect_copy(), float_vect_scale(), IndiEstimation::g1, IndiEstimation::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, IndiEstimation::u_d, and IndiEstimation::u_dd.
Referenced by stabilization_indi_rate_run().
|
static |
Definition at line 179 of file stabilization_indi_simple.c.
References dev, Int32Quat::qi, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, stab_att_sp_quat, and stateGetNedToBodyQuat_i().
|
static |
Definition at line 150 of file stabilization_indi_simple.c.
Referenced by stabilization_indi_init().
|
static |
Definition at line 166 of file stabilization_indi_simple.c.
References dev, indi, INDI_EST_SCALE, FloatRates::p, FloatRates::q, FloatRates::r, and RATES_SMUL.
Referenced by stabilization_indi_init().
| void stabilization_indi_attitude_run | ( | bool | in_flight, |
| struct StabilizationSetpoint * | att_sp, | ||
| struct ThrustSetpoint * | thrust, | ||
| int32_t * | cmd ) |
runs stabilization indi
| in_flight | not used |
| rate_control | rate control enabled, otherwise attitude control |
Definition at line 415 of file stabilization_indi_simple.c.
References float_quat_inv_comp_norm_shortest(), float_quat_tilt_twist(), indi, FloatRates::p, FloatRates::q, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatRates::r, RATES_ADD, 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_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 248 of file stabilization_indi_simple.c.
References FLOAT_RATES_ZERO, indi, indi_init_filters(), stab_att_sp_euler, and stabilization_attitude_get_heading_i().
| void stabilization_indi_init | ( | void | ) |
Function that initializes important values upon engaging INDI.
Definition at line 194 of file stabilization_indi_simple.c.
References DefaultPeriodic, indi, indi_init_filters(), register_periodic_telemetry(), send_ahrs_ref_quat(), send_att_indi(), and send_eff_mat_g_indi_simple().
| void stabilization_indi_rate_run | ( | bool | in_flight, |
| struct StabilizationSetpoint * | sp, | ||
| struct ThrustSetpoint * | thrust, | ||
| int32_t * | cmd ) |
Does the INDI calculations.
| in_flight | true aircraft is flying |
| sp | rate setpoint |
| thrust | thrust setpoint |
| cmd | output command array |
Definition at line 310 of file stabilization_indi_simple.c.
References COMMAND_PITCH, COMMAND_ROLL, filter_pqr(), finite_difference_from_filter(), FLOAT_RATES_ZERO, indi, lms_estimation(), MAX_PPRZ, FloatRates::p, FloatRates::q, FloatRates::r, rates_filt_fo, stab_sp_to_rates_f(), stateGetBodyRates_f(), th_sp_to_thrust_i(), THRUST_AXIS_Z, and update_first_order_low_pass().
Referenced by stabilization_indi_attitude_run().
| void stabilization_indi_simple_reset_r_filter_cutoff | ( | float | new_cutoff | ) |
Definition at line 241 of file stabilization_indi_simple.c.
References indi, init_first_order_low_pass(), rates_filt_fo, and stateGetBodyRates_f().
| struct IndiVariables indi |
Definition at line 109 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().
|
static |
Definition at line 101 of file stabilization_indi_simple.c.
| struct Int32Eulers stab_att_sp_euler |
Definition at line 98 of file stabilization_indi_simple.c.
| struct Int32Quat stab_att_sp_quat |
Definition at line 99 of file stabilization_indi_simple.c.