|
Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
Quaternion attitude stabilization (floating point). More...
#include "generated/airframe.h"#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h"#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h"#include "std.h"#include "paparazzi.h"#include "math/pprz_algebra_float.h"#include "math/pprz_algebra_int.h"#include "state.h"#include "modules/datalink/telemetry.h"Go to the source code of this file.
Macros | |
| #define | IERROR_SCALE 1024 |
| #define | GAIN_PRESCALER_FF 1 |
| #define | GAIN_PRESCALER_P 1 |
| #define | GAIN_PRESCALER_D 1 |
| #define | GAIN_PRESCALER_I 1 |
| #define | INTEGRATOR_BOUND 1.0 |
Functions | |
| static void | send_att (struct transport_tx *trans, struct link_device *dev) |
| static void | send_att_ref (struct transport_tx *trans, struct link_device *dev) |
| static void | send_ahrs_ref_quat (struct transport_tx *trans, struct link_device *dev) |
| void | stabilization_attitude_quat_float_init (void) |
| void | stabilization_attitude_gain_schedule (uint8_t idx) |
| void | stabilization_attitude_enter (void) |
| Attitude control enter function. | |
| static void | attitude_run_ff (float ff_commands[], struct FloatAttitudeGains *gains, struct FloatRates *ref_accel) |
| static void | attitude_run_fb (float fb_commands[], struct FloatAttitudeGains *gains, struct FloatQuat *att_err, struct FloatRates *rate_err, struct FloatRates *rate_err_d, struct FloatQuat *sum_err) |
| void | stabilization_attitude_run (bool enable_integrator, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd) |
| Attitude control run function. | |
Variables | |
| struct FloatAttitudeGains | stabilization_gains [STABILIZATION_ATTITUDE_GAIN_NB] |
| static struct FloatEulers | stab_att_sp_euler |
| static struct FloatQuat | stab_att_sp_quat |
| struct AttRefQuatFloat | att_ref_quat_f |
| struct FloatQuat | stabilization_att_sum_err_quat |
| static struct FloatRates | last_body_rate |
| static struct FloatRates | body_rate_d |
| float | stabilization_att_fb_cmd [COMMANDS_NB] |
| float | stabilization_att_ff_cmd [COMMANDS_NB] |
| static int | gain_idx = STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT |
| static const float | phi_pgain [] = STABILIZATION_ATTITUDE_PHI_PGAIN |
| static const float | theta_pgain [] = STABILIZATION_ATTITUDE_THETA_PGAIN |
| static const float | psi_pgain [] = STABILIZATION_ATTITUDE_PSI_PGAIN |
| static const float | phi_dgain [] = STABILIZATION_ATTITUDE_PHI_DGAIN |
| static const float | theta_dgain [] = STABILIZATION_ATTITUDE_THETA_DGAIN |
| static const float | psi_dgain [] = STABILIZATION_ATTITUDE_PSI_DGAIN |
| static const float | phi_igain [] = STABILIZATION_ATTITUDE_PHI_IGAIN |
| static const float | theta_igain [] = STABILIZATION_ATTITUDE_THETA_IGAIN |
| static const float | psi_igain [] = STABILIZATION_ATTITUDE_PSI_IGAIN |
| static const float | phi_ddgain [] = STABILIZATION_ATTITUDE_PHI_DDGAIN |
| static const float | theta_ddgain [] = STABILIZATION_ATTITUDE_THETA_DDGAIN |
| static const float | psi_ddgain [] = STABILIZATION_ATTITUDE_PSI_DDGAIN |
| static const float | phi_dgain_d [] = STABILIZATION_ATTITUDE_PHI_DGAIN_D |
| static const float | theta_dgain_d [] = STABILIZATION_ATTITUDE_THETA_DGAIN_D |
| static const float | psi_dgain_d [] = STABILIZATION_ATTITUDE_PSI_DGAIN_D |
Quaternion attitude stabilization (floating point).
Definition in file stabilization_attitude_quat_float.c.
| #define GAIN_PRESCALER_D 1 |
Definition at line 234 of file stabilization_attitude_quat_float.c.
Referenced by attitude_run_fb(), and attitude_run_fb().
| #define GAIN_PRESCALER_FF 1 |
Definition at line 214 of file stabilization_attitude_quat_float.c.
Referenced by attitude_run_ff(), and attitude_run_ff().
| #define GAIN_PRESCALER_I 1 |
Definition at line 237 of file stabilization_attitude_quat_float.c.
Referenced by attitude_run_fb(), and attitude_run_fb().
| #define GAIN_PRESCALER_P 1 |
Definition at line 231 of file stabilization_attitude_quat_float.c.
Referenced by attitude_run_fb(), and attitude_run_fb().
| #define IERROR_SCALE 1024 |
Definition at line 97 of file stabilization_attitude_quat_float.c.
Referenced by stabilization_attitude_run().
| #define INTEGRATOR_BOUND 1.0 |
Referenced by stabilization_attitude_run().
|
static |
Definition at line 239 of file stabilization_attitude_quat_float.c.
References COMMAND_PITCH, COMMAND_ROLL, GAIN_PRESCALER_D, GAIN_PRESCALER_I, GAIN_PRESCALER_P, gains, FloatRates::p, FloatRates::q, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, and FloatRates::r.
Referenced by stabilization_attitude_run().
|
static |
Definition at line 216 of file stabilization_attitude_quat_float.c.
References COMMAND_PITCH, COMMAND_ROLL, GAIN_PRESCALER_FF, gains, FloatRates::p, FloatRates::q, and FloatRates::r.
Referenced by stabilization_attitude_run().
|
static |
Definition at line 144 of file stabilization_attitude_quat_float.c.
References att_ref_quat_f, dev, Int32Quat::qi, QUAT_BFP_OF_REAL, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, and stateGetNedToBodyQuat_i().
Referenced by stabilization_attitude_quat_float_init(), stabilization_attitude_quat_int_init(), and stabilization_indi_init().
|
static |
Definition at line 102 of file stabilization_attitude_quat_float.c.
References body_rate_d, COMMAND_PITCH, COMMAND_ROLL, dev, FloatRates::p, FloatEulers::phi, FloatEulers::psi, FloatRates::q, FloatRates::r, stab_att_sp_euler, stabilization, stabilization_att_fb_cmd, stabilization_att_ff_cmd, stabilization_att_sum_err_quat, stateGetBodyRates_f(), stateGetNedToBodyEulers_f(), and FloatEulers::theta.
|
static |
Definition at line 127 of file stabilization_attitude_quat_float.c.
References att_ref_quat_f, dev, and stab_att_sp_euler.
| void stabilization_attitude_enter | ( | void | ) |
Attitude control enter function.
Definition at line 205 of file stabilization_attitude_quat_float.c.
References att_ref_quat_f, attitude_ref_quat_float_enter(), float_quat_identity(), stabilization_att_sum_err_quat, and stateGetNedToBodyQuat_f().
| void stabilization_attitude_gain_schedule | ( | uint8_t | idx | ) |
Definition at line 195 of file stabilization_attitude_quat_float.c.
References att_ref_quat_f, attitude_ref_quat_float_schedule(), gain_idx, idx, and STABILIZATION_ATTITUDE_GAIN_NB.
| void stabilization_attitude_quat_float_init | ( | void | ) |
Definition at line 161 of file stabilization_attitude_quat_float.c.
References att_ref_quat_f, attitude_ref_quat_float_init(), attitude_ref_quat_float_schedule(), body_rate_d, DefaultPeriodic, FLOAT_EULERS_ZERO, float_quat_identity(), FLOAT_RATES_ZERO, last_body_rate, p, phi_ddgain, phi_dgain, phi_dgain_d, phi_igain, phi_pgain, psi_ddgain, psi_dgain, psi_dgain_d, psi_igain, psi_pgain, register_periodic_telemetry(), send_ahrs_ref_quat(), send_att(), send_att_ref(), stab_att_sp_euler, stab_att_sp_quat, stabilization_att_sum_err_quat, STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT, STABILIZATION_ATTITUDE_GAIN_NB, stabilization_gains, theta_ddgain, theta_dgain, theta_dgain_d, theta_igain, theta_pgain, and VECT3_ASSIGN.
| void stabilization_attitude_run | ( | bool | in_flight, |
| struct StabilizationSetpoint * | sp, | ||
| struct ThrustSetpoint * | thrust, | ||
| int32_t * | cmd ) |
Attitude control run function.
| [in] | in_flight | true if in flight |
| [in] | sp | pointer to the stabilization setpoint structure |
| [in] | thrust | pointer to the thrust setoint structure |
| [out] | cmd | pointer to the output command vector |
Definition at line 279 of file stabilization_attitude_quat_float.c.
References att_ref_quat_f, attitude_ref_quat_float_update(), attitude_run_fb(), attitude_run_ff(), body_rate_d, COMMAND_PITCH, COMMAND_ROLL, float_quat_identity(), float_quat_inv_comp(), float_quat_wrap_shortest(), gain_idx, IERROR_SCALE, INTEGRATOR_BOUND, last_body_rate, MAX_PPRZ, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, RATES_COPY, RATES_DIFF, stab_att_sp_euler, stab_att_sp_quat, stab_sp_to_eulers_f(), stab_sp_to_quat_f(), stabilization_att_fb_cmd, stabilization_att_ff_cmd, stabilization_att_sum_err_quat, stabilization_gains, stateGetBodyRates_f(), stateGetNedToBodyQuat_f(), th_sp_to_thrust_i(), and THRUST_AXIS_Z.
| struct AttRefQuatFloat att_ref_quat_f |
Definition at line 42 of file stabilization_attitude_quat_float.c.
Referenced by send_ahrs_ref_quat(), send_att_ref(), stabilization_attitude_enter(), stabilization_attitude_gain_schedule(), stabilization_attitude_quat_float_init(), and stabilization_attitude_run().
|
static |
Definition at line 47 of file stabilization_attitude_quat_float.c.
Referenced by send_att(), stabilization_attitude_quat_float_init(), and stabilization_attitude_run().
|
static |
Definition at line 52 of file stabilization_attitude_quat_float.c.
Referenced by stabilization_attitude_gain_schedule(), and stabilization_attitude_run().
|
static |
Definition at line 46 of file stabilization_attitude_quat_float.c.
Referenced by stabilization_attitude_quat_float_init(), and stabilization_attitude_run().
|
static |
Definition at line 66 of file stabilization_attitude_quat_float.c.
Referenced by stabilization_attitude_quat_float_init().
|
static |
Definition at line 58 of file stabilization_attitude_quat_float.c.
Referenced by stabilization_attitude_quat_float_init().
|
static |
Definition at line 70 of file stabilization_attitude_quat_float.c.
Referenced by stabilization_attitude_quat_float_init().
|
static |
Definition at line 62 of file stabilization_attitude_quat_float.c.
Referenced by stabilization_attitude_quat_float_init().
|
static |
Definition at line 54 of file stabilization_attitude_quat_float.c.
Referenced by stabilization_attitude_quat_float_init().
|
static |
Definition at line 68 of file stabilization_attitude_quat_float.c.
Referenced by stabilization_attitude_quat_float_init().
|
static |
Definition at line 60 of file stabilization_attitude_quat_float.c.
Referenced by stabilization_attitude_quat_float_init().
|
static |
Definition at line 72 of file stabilization_attitude_quat_float.c.
Referenced by stabilization_attitude_quat_float_init().
|
static |
Definition at line 64 of file stabilization_attitude_quat_float.c.
Referenced by stabilization_attitude_quat_float_init().
|
static |
Definition at line 56 of file stabilization_attitude_quat_float.c.
Referenced by stabilization_attitude_quat_float_init().
|
static |
Definition at line 39 of file stabilization_attitude_quat_float.c.
|
static |
Definition at line 40 of file stabilization_attitude_quat_float.c.
| float stabilization_att_fb_cmd[COMMANDS_NB] |
Definition at line 49 of file stabilization_attitude_quat_float.c.
| float stabilization_att_ff_cmd[COMMANDS_NB] |
Definition at line 50 of file stabilization_attitude_quat_float.c.
| struct FloatQuat stabilization_att_sum_err_quat |
Definition at line 44 of file stabilization_attitude_quat_float.c.
Referenced by send_att(), send_att(), stabilization_attitude_enter(), stabilization_attitude_quat_float_init(), stabilization_attitude_quat_int_init(), and stabilization_attitude_run().
| struct FloatAttitudeGains stabilization_gains[STABILIZATION_ATTITUDE_GAIN_NB] |
Definition at line 37 of file stabilization_attitude_quat_float.c.
|
static |
Definition at line 67 of file stabilization_attitude_quat_float.c.
Referenced by stabilization_attitude_quat_float_init().
|
static |
Definition at line 59 of file stabilization_attitude_quat_float.c.
Referenced by stabilization_attitude_quat_float_init().
|
static |
Definition at line 71 of file stabilization_attitude_quat_float.c.
Referenced by stabilization_attitude_quat_float_init().
|
static |
Definition at line 63 of file stabilization_attitude_quat_float.c.
Referenced by stabilization_attitude_quat_float_init().
|
static |
Definition at line 55 of file stabilization_attitude_quat_float.c.
Referenced by stabilization_attitude_quat_float_init().