Paparazzi UAS
v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
|
Rotorcraft attitude reference generation. More...
#include "generated/airframe.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_defaults.h"
Go to the source code of this file.
Macros | |
#define | TWO_ZETA_OMEGA_RES 10 |
#define | TWO_OMEGA_2_RES 7 |
#define | F_UPDATE_RES 9 |
#define | OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b)) |
Rotorcraft attitude reference generation.
(quaternion int version)
Definition in file stabilization_attitude_ref_quat_int.c.
#define F_UPDATE_RES 9 |
Definition at line 92 of file stabilization_attitude_ref_quat_int.c.
Referenced by attitude_ref_quat_int_update().
#define OFFSET_AND_ROUND | ( | _a, | |
_b | |||
) | (((_a)+(1<<((_b)-1)))>>(_b)) |
Definition at line 94 of file stabilization_attitude_ref_quat_int.c.
Referenced by attitude_ref_quat_int_update().
#define TWO_OMEGA_2_RES 7 |
Definition at line 36 of file stabilization_attitude_ref_quat_int.c.
Referenced by attitude_ref_quat_int_update(), update_ref_model_p(), update_ref_model_q(), and update_ref_model_r().
#define TWO_ZETA_OMEGA_RES 10 |
Definition at line 35 of file stabilization_attitude_ref_quat_int.c.
Referenced by attitude_ref_quat_int_update(), update_ref_model_p(), update_ref_model_q(), and update_ref_model_r().
void attitude_ref_quat_int_enter | ( | struct AttRefQuatInt * | ref, |
int32_t | psi | ||
) |
Definition at line 78 of file stabilization_attitude_ref_quat_int.c.
References AttRefQuatInt::accel, AttRefQuatInt::euler, int32_quat_of_eulers(), int32_quat_wrap_shortest(), AttRefQuatInt::quat, AttRefQuatInt::rate, and reset_psi_ref().
Referenced by stabilization_attitude_enter().
void attitude_ref_quat_int_init | ( | struct AttRefQuatInt * | ref | ) |
Definition at line 51 of file stabilization_attitude_ref_quat_int.c.
References AttRefQuatInt::accel, attitude_ref_quat_int_set_max_p(), attitude_ref_quat_int_set_max_pdot(), attitude_ref_quat_int_set_max_q(), attitude_ref_quat_int_set_max_qdot(), attitude_ref_quat_int_set_max_r(), attitude_ref_quat_int_set_max_rdot(), attitude_ref_quat_int_set_omega(), attitude_ref_quat_int_set_zeta(), AttRefQuatInt::euler, int32_quat_identity(), INT_EULERS_ZERO, INT_RATES_ZERO, AttRefQuatInt::quat, AttRefQuatInt::rate, STABILIZATION_ATTITUDE_REF_MAX_P, STABILIZATION_ATTITUDE_REF_MAX_PDOT, STABILIZATION_ATTITUDE_REF_MAX_Q, STABILIZATION_ATTITUDE_REF_MAX_QDOT, STABILIZATION_ATTITUDE_REF_MAX_R, STABILIZATION_ATTITUDE_REF_MAX_RDOT, and update_ref_model().
Referenced by stabilization_attitude_init().
void attitude_ref_quat_int_set_max_p | ( | struct AttRefQuatInt * | ref, |
float | max_p | ||
) |
Definition at line 254 of file stabilization_attitude_ref_quat_int.c.
References BFP_OF_REAL, Int32RefSat::max_rate, Int32Rates::p, REF_RATE_FRAC, and AttRefQuatInt::saturation.
Referenced by attitude_ref_quat_int_init().
void attitude_ref_quat_int_set_max_pdot | ( | struct AttRefQuatInt * | ref, |
float | max_pdot | ||
) |
Definition at line 269 of file stabilization_attitude_ref_quat_int.c.
References BFP_OF_REAL, Int32RefSat::max_accel, Int32Rates::p, REF_ACCEL_FRAC, and AttRefQuatInt::saturation.
Referenced by attitude_ref_quat_int_init().
void attitude_ref_quat_int_set_max_q | ( | struct AttRefQuatInt * | ref, |
float | max_q | ||
) |
Definition at line 259 of file stabilization_attitude_ref_quat_int.c.
References BFP_OF_REAL, Int32RefSat::max_rate, Int32Rates::q, REF_RATE_FRAC, and AttRefQuatInt::saturation.
Referenced by attitude_ref_quat_int_init().
void attitude_ref_quat_int_set_max_qdot | ( | struct AttRefQuatInt * | ref, |
float | max_qdot | ||
) |
Definition at line 274 of file stabilization_attitude_ref_quat_int.c.
References BFP_OF_REAL, Int32RefSat::max_accel, Int32Rates::q, REF_ACCEL_FRAC, and AttRefQuatInt::saturation.
Referenced by attitude_ref_quat_int_init().
void attitude_ref_quat_int_set_max_r | ( | struct AttRefQuatInt * | ref, |
float | max_r | ||
) |
Definition at line 264 of file stabilization_attitude_ref_quat_int.c.
References BFP_OF_REAL, Int32RefSat::max_rate, Int32Rates::r, REF_RATE_FRAC, and AttRefQuatInt::saturation.
Referenced by attitude_ref_quat_int_init().
void attitude_ref_quat_int_set_max_rdot | ( | struct AttRefQuatInt * | ref, |
float | max_rdot | ||
) |
Definition at line 279 of file stabilization_attitude_ref_quat_int.c.
References BFP_OF_REAL, Int32RefSat::max_accel, Int32Rates::r, REF_ACCEL_FRAC, and AttRefQuatInt::saturation.
Referenced by attitude_ref_quat_int_init().
void attitude_ref_quat_int_set_omega | ( | struct AttRefQuatInt * | ref, |
struct FloatRates * | omega | ||
) |
Definition at line 222 of file stabilization_attitude_ref_quat_int.c.
References attitude_ref_quat_int_set_omega_p(), attitude_ref_quat_int_set_omega_q(), attitude_ref_quat_int_set_omega_r(), FloatRates::p, FloatRates::q, and FloatRates::r.
Referenced by attitude_ref_quat_int_init().
void attitude_ref_quat_int_set_omega_p | ( | struct AttRefQuatInt * | ref, |
float | omega_p | ||
) |
Definition at line 204 of file stabilization_attitude_ref_quat_int.c.
References AttRefQuatInt::model, IntRefModel::omega, omega_p, FloatRates::p, and update_ref_model_p().
Referenced by attitude_ref_quat_int_set_omega().
void attitude_ref_quat_int_set_omega_q | ( | struct AttRefQuatInt * | ref, |
float | omega_q | ||
) |
Definition at line 210 of file stabilization_attitude_ref_quat_int.c.
References AttRefQuatInt::model, IntRefModel::omega, omega_q, FloatRates::q, and update_ref_model_q().
Referenced by attitude_ref_quat_int_set_omega().
void attitude_ref_quat_int_set_omega_r | ( | struct AttRefQuatInt * | ref, |
float | omega_r | ||
) |
Definition at line 216 of file stabilization_attitude_ref_quat_int.c.
References AttRefQuatInt::model, IntRefModel::omega, omega_r, FloatRates::r, and update_ref_model_r().
Referenced by attitude_ref_quat_int_set_omega().
void attitude_ref_quat_int_set_zeta | ( | struct AttRefQuatInt * | ref, |
struct FloatRates * | zeta | ||
) |
Definition at line 247 of file stabilization_attitude_ref_quat_int.c.
References attitude_ref_quat_int_set_zeta_p(), attitude_ref_quat_int_set_zeta_q(), attitude_ref_quat_int_set_zeta_r(), FloatRates::p, FloatRates::q, and FloatRates::r.
Referenced by attitude_ref_quat_int_init().
void attitude_ref_quat_int_set_zeta_p | ( | struct AttRefQuatInt * | ref, |
float | zeta_p | ||
) |
Definition at line 229 of file stabilization_attitude_ref_quat_int.c.
References AttRefQuatInt::model, FloatRates::p, update_ref_model_p(), IntRefModel::zeta, and zeta_p.
Referenced by attitude_ref_quat_int_set_zeta().
void attitude_ref_quat_int_set_zeta_q | ( | struct AttRefQuatInt * | ref, |
float | zeta_q | ||
) |
Definition at line 235 of file stabilization_attitude_ref_quat_int.c.
References AttRefQuatInt::model, FloatRates::q, update_ref_model_q(), IntRefModel::zeta, and zeta_q.
Referenced by attitude_ref_quat_int_set_zeta().
void attitude_ref_quat_int_set_zeta_r | ( | struct AttRefQuatInt * | ref, |
float | zeta_r | ||
) |
Definition at line 241 of file stabilization_attitude_ref_quat_int.c.
References AttRefQuatInt::model, FloatRates::r, update_ref_model_r(), IntRefModel::zeta, and zeta_r.
Referenced by attitude_ref_quat_int_set_zeta().
void attitude_ref_quat_int_update | ( | struct AttRefQuatInt * | ref, |
struct Int32Quat * | sp_quat, | ||
float | dt | ||
) |
Propagate reference.
CAUTION! Periodic frequency is assumed to be 512 Hz. FIXME: use dt instead of hardcoded 512Hz via F_UPDATE_RES
Definition at line 100 of file stabilization_attitude_ref_quat_int.c.
References AttRefQuatInt::accel, attitude_ref_int_saturate_naive(), AttRefQuatInt::euler, F_UPDATE_RES, int32_eulers_of_quat(), int32_quat_derivative(), INT32_QUAT_FRAC, int32_quat_inv_comp(), int32_quat_normalize(), int32_quat_wrap_shortest(), INT32_RATE_FRAC, AttRefQuatInt::model, OFFSET_AND_ROUND, Int32Rates::p, Int32Rates::q, Int32Quat::qi, AttRefQuatInt::quat, QUAT_ADD, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, Int32Rates::r, AttRefQuatInt::rate, RATES_ADD, RATES_SUM, REF_ACCEL_FRAC, REF_RATE_FRAC, AttRefQuatInt::saturation, IntRefModel::two_omega2, TWO_OMEGA_2_RES, IntRefModel::two_zeta_omega, and TWO_ZETA_OMEGA_RES.
Referenced by stabilization_attitude_run().
|
inlinestatic |
Definition at line 162 of file stabilization_attitude_ref_quat_int.c.
References AttRefQuatInt::accel, AttRefQuatInt::euler, Int32Eulers::psi, Int32Rates::r, and AttRefQuatInt::rate.
Referenced by attitude_ref_quat_int_enter().
|
static |
Definition at line 192 of file stabilization_attitude_ref_quat_int.c.
References update_ref_model_p(), update_ref_model_q(), and update_ref_model_r().
Referenced by attitude_ref_quat_int_init().
|
static |
Definition at line 174 of file stabilization_attitude_ref_quat_int.c.
References BFP_OF_REAL, AttRefQuatInt::model, IntRefModel::omega, FloatRates::p, Int32Rates::p, IntRefModel::two_omega2, TWO_OMEGA_2_RES, IntRefModel::two_zeta_omega, TWO_ZETA_OMEGA_RES, and IntRefModel::zeta.
Referenced by attitude_ref_quat_int_set_omega_p(), attitude_ref_quat_int_set_zeta_p(), and update_ref_model().
|
static |
Definition at line 180 of file stabilization_attitude_ref_quat_int.c.
References BFP_OF_REAL, AttRefQuatInt::model, IntRefModel::omega, FloatRates::q, Int32Rates::q, IntRefModel::two_omega2, TWO_OMEGA_2_RES, IntRefModel::two_zeta_omega, TWO_ZETA_OMEGA_RES, and IntRefModel::zeta.
Referenced by attitude_ref_quat_int_set_omega_q(), attitude_ref_quat_int_set_zeta_q(), and update_ref_model().
|
static |
Definition at line 186 of file stabilization_attitude_ref_quat_int.c.
References BFP_OF_REAL, AttRefQuatInt::model, IntRefModel::omega, FloatRates::r, Int32Rates::r, IntRefModel::two_omega2, TWO_OMEGA_2_RES, IntRefModel::two_zeta_omega, TWO_ZETA_OMEGA_RES, and IntRefModel::zeta.
Referenced by attitude_ref_quat_int_set_omega_r(), attitude_ref_quat_int_set_zeta_r(), and update_ref_model().