Paparazzi UAS
v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
Rotorcraft quaternion attitude stabilization. More...
#include "generated/airframe.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 "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 | STABILIZATION_ATTITUDE_PHI_FFDGAIN 0 |
explicitly define to zero to disable feed-forward rate term by default More... | |
#define | STABILIZATION_ATTITUDE_THETA_FFDGAIN 0 |
#define | STABILIZATION_ATTITUDE_PSI_FFDGAIN 0 |
#define | IERROR_SCALE 128 |
#define | GAIN_PRESCALER_FF 48 |
#define | GAIN_PRESCALER_P 12 |
#define | GAIN_PRESCALER_D 3 |
#define | GAIN_PRESCALER_I 3 |
#define | OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b)) |
#define | OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b)) |
#define | INTEGRATOR_BOUND 100000 |
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_init (void) |
stabilization_attitude_init More... | |
void | stabilization_attitude_enter (void) |
void | stabilization_attitude_set_failsafe_setpoint (void) |
void | stabilization_attitude_set_rpy_setpoint_i (struct Int32Eulers *rpy) |
void | stabilization_attitude_set_quat_setpoint_i (struct Int32Quat *quat) |
void | stabilization_attitude_set_earth_cmd_i (struct Int32Vect2 *cmd, int32_t heading) |
void | stabilization_attitude_set_stab_sp (struct StabilizationSetpoint *sp) |
static void | attitude_run_ff (int32_t ff_commands[], struct Int32AttitudeGains *gains, struct Int32Rates *ref_accel) |
static void | attitude_run_fb (int32_t fb_commands[], struct Int32AttitudeGains *gains, struct Int32Quat *att_err, struct Int32Rates *rate_err, struct Int32Quat *sum_err) |
void | stabilization_attitude_run (bool enable_integrator) |
void | stabilization_attitude_read_rc (bool in_flight, bool in_carefree, bool coordinated_turn) |
Variables | |
struct Int32AttitudeGains | stabilization_gains |
struct Int32Quat | stabilization_att_sum_err_quat |
int32_t | stabilization_att_fb_cmd [COMMANDS_NB] |
int32_t | stabilization_att_ff_cmd [COMMANDS_NB] |
struct Int32Quat | stab_att_sp_quat |
with INT32_QUAT_FRAC More... | |
struct Int32Eulers | stab_att_sp_euler |
with INT32_ANGLE_FRAC More... | |
struct AttRefQuatInt | att_ref_quat_i |
Rotorcraft quaternion attitude stabilization.
Definition in file stabilization_attitude_quat_int.c.
#define GAIN_PRESCALER_D 3 |
Definition at line 83 of file stabilization_attitude_quat_int.c.
#define GAIN_PRESCALER_FF 48 |
Definition at line 81 of file stabilization_attitude_quat_int.c.
#define GAIN_PRESCALER_I 3 |
Definition at line 84 of file stabilization_attitude_quat_int.c.
#define GAIN_PRESCALER_P 12 |
Definition at line 82 of file stabilization_attitude_quat_int.c.
#define IERROR_SCALE 128 |
Definition at line 80 of file stabilization_attitude_quat_int.c.
#define INTEGRATOR_BOUND 100000 |
#define OFFSET_AND_ROUND | ( | _a, | |
_b | |||
) | (((_a)+(1<<((_b)-1)))>>(_b)) |
Definition at line 223 of file stabilization_attitude_quat_int.c.
#define OFFSET_AND_ROUND2 | ( | _a, | |
_b | |||
) | (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b)) |
Definition at line 224 of file stabilization_attitude_quat_int.c.
#define STABILIZATION_ATTITUDE_PHI_FFDGAIN 0 |
explicitly define to zero to disable feed-forward rate term by default
Definition at line 40 of file stabilization_attitude_quat_int.c.
#define STABILIZATION_ATTITUDE_PSI_FFDGAIN 0 |
Definition at line 46 of file stabilization_attitude_quat_int.c.
#define STABILIZATION_ATTITUDE_THETA_FFDGAIN 0 |
Definition at line 43 of file stabilization_attitude_quat_int.c.
|
static |
Definition at line 235 of file stabilization_attitude_quat_int.c.
References GAIN_PRESCALER_D, GAIN_PRESCALER_I, GAIN_PRESCALER_P, gains, Int32Rates::p, Int32Rates::q, QUAT1_FLOAT_OF_BFP, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, Int32Rates::r, and RATE_FLOAT_OF_BFP.
Referenced by stabilization_attitude_run().
|
static |
Definition at line 226 of file stabilization_attitude_quat_int.c.
References GAIN_PRESCALER_FF, gains, Int32Rates::p, Int32Rates::q, Int32Rates::r, and RATE_FLOAT_OF_BFP.
Referenced by stabilization_attitude_run().
|
static |
Definition at line 133 of file stabilization_attitude_quat_int.c.
References att_ref_quat_i, dev, Int32Quat::qi, AttRefQuatInt::quat, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, and stateGetNedToBodyQuat_i().
Referenced by stabilization_attitude_init().
|
static |
Definition at line 89 of file stabilization_attitude_quat_int.c.
References dev, Int32Rates::p, Int32Eulers::phi, Int32Eulers::psi, Int32Rates::q, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, Int32Rates::r, stab_att_sp_euler, stabilization_att_fb_cmd, stabilization_att_ff_cmd, stabilization_att_sum_err_quat, stabilization_cmd, stateGetBodyRates_i(), stateGetNedToBodyEulers_i(), and Int32Eulers::theta.
Referenced by stabilization_attitude_init().
|
static |
Definition at line 113 of file stabilization_attitude_quat_int.c.
References AttRefQuatInt::accel, att_ref_quat_i, dev, AttRefQuatInt::euler, INT32_ANGLE_FRAC, INT32_EULERS_LSHIFT, Int32Rates::p, Int32Eulers::phi, Int32Eulers::psi, Int32Rates::q, Int32Rates::r, AttRefQuatInt::rate, REF_ANGLE_FRAC, stab_att_sp_euler, and Int32Eulers::theta.
Referenced by stabilization_attitude_init().
void stabilization_attitude_enter | ( | void | ) |
Definition at line 162 of file stabilization_attitude_quat_int.c.
References att_ref_quat_i, attitude_ref_quat_int_enter(), int32_quat_identity(), Int32Eulers::psi, stab_att_sp_euler, stabilization_att_sum_err_quat, stabilization_attitude_get_heading_i(), and stateGetNedToBodyQuat_i().
void stabilization_attitude_init | ( | void | ) |
stabilization_attitude_init
Initialize the heli indi attitude controller.
Definition at line 148 of file stabilization_attitude_quat_int.c.
References att_ref_quat_i, attitude_ref_quat_int_init(), DefaultPeriodic, int32_quat_identity(), register_periodic_telemetry(), send_ahrs_ref_quat(), send_att(), send_att_ref(), and stabilization_att_sum_err_quat.
void stabilization_attitude_read_rc | ( | bool | in_flight, |
bool | in_carefree, | ||
bool | coordinated_turn | ||
) |
Definition at line 320 of file stabilization_attitude_quat_int.c.
References QUAT_BFP_OF_REAL, stab_att_sp_euler, stab_att_sp_quat, stabilization_attitude_read_rc_setpoint_eulers_f(), stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(), and stabilization_attitude_read_rc_setpoint_quat_f().
Referenced by guidance_h_read_rc().
void stabilization_attitude_run | ( | bool | enable_integrator | ) |
Definition at line 256 of file stabilization_attitude_quat_int.c.
References AttRefQuatInt::accel, att_ref_quat_i, attitude_ref_quat_int_update(), attitude_run_fb(), attitude_run_ff(), IERROR_SCALE, int32_quat_identity(), int32_quat_inv_comp(), int32_quat_normalize(), int32_quat_wrap_shortest(), INT32_RATE_FRAC, INTEGRATOR_BOUND, MAX_PPRZ, OFFSET_AND_ROUND, Int32Rates::p, Int32Rates::q, AttRefQuatInt::quat, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, Int32Rates::r, AttRefQuatInt::rate, RATES_DIFF, REF_RATE_FRAC, stab_att_sp_quat, stabilization_att_fb_cmd, stabilization_att_ff_cmd, stabilization_att_sum_err_quat, stabilization_cmd, stabilization_gains, stateGetBodyRates_i(), and stateGetNedToBodyQuat_i().
void stabilization_attitude_set_earth_cmd_i | ( | struct Int32Vect2 * | cmd, |
int32_t | heading | ||
) |
Definition at line 200 of file stabilization_attitude_quat_int.c.
References ANGLE_FLOAT_OF_BFP, heading, INT32_TRIG_FRAC, FloatEulers::phi, Int32Eulers::phi, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, FloatEulers::psi, Int32Eulers::psi, quat_from_earth_cmd_i(), stab_att_sp_euler, stab_att_sp_quat, stateGetNedToBodyEulers_f(), stateGetNedToBodyEulers_i(), FloatEulers::theta, Int32Eulers::theta, FloatVect2::x, Int32Vect2::x, FloatVect2::y, and Int32Vect2::y.
Referenced by guidance_flip_run().
void stabilization_attitude_set_failsafe_setpoint | ( | void | ) |
Definition at line 176 of file stabilization_attitude_quat_int.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_attitude_set_quat_setpoint_i | ( | struct Int32Quat * | quat | ) |
Definition at line 194 of file stabilization_attitude_quat_int.c.
References float_eulers_of_quat(), int32_eulers_of_quat(), QUAT_FLOAT_OF_BFP, stab_att_sp_euler, and stab_att_sp_quat.
Referenced by guidance_h_from_nav().
void stabilization_attitude_set_rpy_setpoint_i | ( | struct Int32Eulers * | rpy | ) |
Definition at line 186 of file stabilization_attitude_quat_int.c.
References EULERS_FLOAT_OF_BFP, int32_quat_of_eulers(), stab_att_sp_euler, and stab_att_sp_quat.
Referenced by guidance_h_from_nav(), guidance_h_module_run(), guidance_hybrid_reset_heading(), and horizontal_ctrl_module_run().
void stabilization_attitude_set_stab_sp | ( | struct StabilizationSetpoint * | sp | ) |
Definition at line 217 of file stabilization_attitude_quat_int.c.
References stab_att_sp_euler, stab_att_sp_quat, stab_sp_to_eulers_f(), stab_sp_to_eulers_i(), and stab_sp_to_quat_i().
Referenced by guidance_h_from_nav(), and guidance_h_guided_run().
struct AttRefQuatInt att_ref_quat_i |
Definition at line 73 of file stabilization_attitude_quat_int.c.
Referenced by send_ahrs_ref_quat(), send_att_ref(), stabilization_attitude_enter(), stabilization_attitude_init(), and stabilization_attitude_run().
struct Int32Eulers stab_att_sp_euler |
with INT32_ANGLE_FRAC
Definition at line 73 of file stabilization_attitude_quat_int.c.
Referenced by send_att(), send_att_ref(), stabilization_attitude_enter(), stabilization_attitude_set_earth_cmd_i(), stabilization_attitude_set_quat_setpoint_i(), stabilization_attitude_set_rpy_setpoint_i(), and stabilization_attitude_set_stab_sp().
struct Int32Quat stab_att_sp_quat |
with INT32_QUAT_FRAC
Definition at line 73 of file stabilization_attitude_quat_int.c.
Referenced by stabilization_attitude_read_rc(), stabilization_attitude_run(), stabilization_attitude_set_earth_cmd_i(), stabilization_attitude_set_failsafe_setpoint(), stabilization_attitude_set_quat_setpoint_i(), stabilization_attitude_set_rpy_setpoint_i(), and stabilization_attitude_set_stab_sp().
int32_t stabilization_att_fb_cmd[COMMANDS_NB] |
Definition at line 72 of file stabilization_attitude_quat_int.c.
Referenced by send_att(), and stabilization_attitude_run().
int32_t stabilization_att_ff_cmd[COMMANDS_NB] |
Definition at line 73 of file stabilization_attitude_quat_int.c.
Referenced by send_att(), and stabilization_attitude_run().
struct Int32Quat stabilization_att_sum_err_quat |
Definition at line 1 of file stabilization_attitude_quat_int.c.
Referenced by send_att(), stabilization_attitude_enter(), stabilization_attitude_init(), and stabilization_attitude_run().
struct Int32AttitudeGains stabilization_gains |
Definition at line 1 of file stabilization_attitude_quat_int.c.
Referenced by gain_scheduling_init(), gain_scheduling_periodic(), set_gainset(), and stabilization_attitude_run().