26 #include "generated/airframe.h"
39 #ifndef STABILIZATION_ATTITUDE_PHI_FFDGAIN
40 #define STABILIZATION_ATTITUDE_PHI_FFDGAIN 0
42 #ifndef STABILIZATION_ATTITUDE_THETA_FFDGAIN
43 #define STABILIZATION_ATTITUDE_THETA_FFDGAIN 0
45 #ifndef STABILIZATION_ATTITUDE_PSI_FFDGAIN
46 #define STABILIZATION_ATTITUDE_PSI_FFDGAIN 0
50 {STABILIZATION_ATTITUDE_PHI_PGAIN, STABILIZATION_ATTITUDE_THETA_PGAIN, STABILIZATION_ATTITUDE_PSI_PGAIN },
51 {STABILIZATION_ATTITUDE_PHI_DGAIN, STABILIZATION_ATTITUDE_THETA_DGAIN, STABILIZATION_ATTITUDE_PSI_DGAIN },
52 {STABILIZATION_ATTITUDE_PHI_DDGAIN, STABILIZATION_ATTITUDE_THETA_DDGAIN, STABILIZATION_ATTITUDE_PSI_DDGAIN },
53 {STABILIZATION_ATTITUDE_PHI_IGAIN, STABILIZATION_ATTITUDE_THETA_IGAIN, STABILIZATION_ATTITUDE_PSI_IGAIN },
58 #if (STABILIZATION_ATTITUDE_PHI_PGAIN < 0) || \
59 (STABILIZATION_ATTITUDE_THETA_PGAIN < 0) || \
60 (STABILIZATION_ATTITUDE_PSI_PGAIN < 0) || \
61 (STABILIZATION_ATTITUDE_PHI_DGAIN < 0) || \
62 (STABILIZATION_ATTITUDE_THETA_DGAIN < 0) || \
63 (STABILIZATION_ATTITUDE_PSI_DGAIN < 0) || \
64 (STABILIZATION_ATTITUDE_PHI_IGAIN < 0) || \
65 (STABILIZATION_ATTITUDE_THETA_IGAIN < 0) || \
66 (STABILIZATION_ATTITUDE_PSI_IGAIN < 0)
67 #error "ALL control gains have to be positive!!!"
80 #define IERROR_SCALE 128
81 #define GAIN_PRESCALER_FF 48
82 #define GAIN_PRESCALER_P 12
83 #define GAIN_PRESCALER_D 3
84 #define GAIN_PRESCALER_I 3
86 #if PERIODIC_TELEMETRY
89 static void send_att(
struct transport_tx *trans,
struct link_device *
dev)
93 pprz_msg_send_STAB_ATTITUDE_INT(trans,
dev, AC_ID,
94 &(body_rate->
p), &(body_rate->
q), &(body_rate->
r),
118 pprz_msg_send_STAB_ATTITUDE_REF_INT(trans,
dev, AC_ID,
136 pprz_msg_send_AHRS_REF_QUAT(trans,
dev, AC_ID,
155 #if PERIODIC_TELEMETRY
223 #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
224 #define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))
239 fb_commands[COMMAND_ROLL] =
244 fb_commands[COMMAND_PITCH] =
249 fb_commands[COMMAND_YAW] =
264 static const float dt = (1./PERIODIC_FREQUENCY);
287 RATES_DIFF(rate_err, rate_ref_scaled, (*body_rate));
289 #define INTEGRATOR_BOUND 100000
291 if (enable_integrator) {
323 #if USE_EARTH_BOUND_RC_SETPOINT
#define QUAT_BFP_OF_REAL(_qi, _qf)
#define RATES_DIFF(_c, _a, _b)
int32_t p
in rad/s with INT32_RATE_FRAC
int32_t r
in rad/s with INT32_RATE_FRAC
int32_t phi
in rad with INT32_ANGLE_FRAC
int32_t q
in rad/s with INT32_RATE_FRAC
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t theta
in rad with INT32_ANGLE_FRAC
static void int32_quat_normalize(struct Int32Quat *q)
normalize a quaternion inplace
void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q)
#define QUAT1_FLOAT_OF_BFP(_qi)
static void int32_quat_identity(struct Int32Quat *q)
initialises a quaternion to identity
#define RATE_FLOAT_OF_BFP(_ai)
static void int32_quat_wrap_shortest(struct Int32Quat *q)
void int32_quat_of_eulers(struct Int32Quat *q, struct Int32Eulers *e)
Quaternion from Euler angles.
#define INT32_EULERS_LSHIFT(_o, _i, _r)
void int32_quat_inv_comp(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
Composition (multiplication) of two quaternions.
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
static struct Int32Rates * stateGetBodyRates_i(void)
Get vehicle body angular rate (int).
float gains[MAX_SAMPLES_LEARNING]
Paparazzi floating point algebra.
Paparazzi fixed point algebra.
#define PPRZ_ITRIG_SIN(_s, _a)
#define PPRZ_ITRIG_COS(_c, _a)
General attitude stabilization interface for rotorcrafts.
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
struct Int32Quat stab_sp_to_quat_i(struct StabilizationSetpoint *sp)
struct Int32Eulers stab_sp_to_eulers_i(struct StabilizationSetpoint *sp)
void stabilization_attitude_init(void)
stabilization_attitude_init
static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
void stabilization_attitude_run(bool enable_integrator)
void stabilization_attitude_enter(void)
int32_t stabilization_att_ff_cmd[COMMANDS_NB]
void stabilization_attitude_set_quat_setpoint_i(struct Int32Quat *quat)
static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
#define STABILIZATION_ATTITUDE_PSI_FFDGAIN
#define OFFSET_AND_ROUND(_a, _b)
void stabilization_attitude_set_failsafe_setpoint(void)
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
static void attitude_run_fb(int32_t fb_commands[], struct Int32AttitudeGains *gains, struct Int32Quat *att_err, struct Int32Rates *rate_err, struct Int32Quat *sum_err)
struct Int32AttitudeGains stabilization_gains
static void attitude_run_ff(int32_t ff_commands[], struct Int32AttitudeGains *gains, struct Int32Rates *ref_accel)
int32_t stabilization_att_fb_cmd[COMMANDS_NB]
void stabilization_attitude_set_stab_sp(struct StabilizationSetpoint *sp)
#define GAIN_PRESCALER_FF
#define STABILIZATION_ATTITUDE_THETA_FFDGAIN
#define STABILIZATION_ATTITUDE_PHI_FFDGAIN
explicitly define to zero to disable feed-forward rate term by default
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
struct AttRefQuatInt att_ref_quat_i
struct Int32Quat stab_att_sp_quat
with INT32_QUAT_FRAC
void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
static void send_att(struct transport_tx *trans, struct link_device *dev)
struct Int32Quat stabilization_att_sum_err_quat
int32_t stabilization_attitude_get_heading_i(void)
void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat *q_sp, bool in_flight, bool in_carefree, bool coordinated_turn)
void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool in_flight, bool in_carefree, bool coordinated_turn)
Read attitude setpoint from RC as quaternion Interprets the stick positions as axes.
Read an attitude setpoint from the RC.
void attitude_ref_quat_int_update(struct AttRefQuatInt *ref, struct Int32Quat *sp_quat, float dt)
Propagate reference.
void attitude_ref_quat_int_enter(struct AttRefQuatInt *ref, struct Int32Quat *state_quat)
void attitude_ref_quat_int_init(struct AttRefQuatInt *ref)
struct Int32Rates rate
with REF_RATE_FRAC
struct Int32Eulers euler
with INT32_ANGLE_FRAC
struct Int32Rates accel
with REF_ACCEL_FRAC
Attitude reference models and state/output (quat int)
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
int int32_t
Typedef defining 32 bit int type.