26#include "generated/airframe.h"
75#if defined COMMAND_ROLL_SURFACE && defined COMMAND_PITCH_SURFACE && defined COMMAND_YAW_SURFACE
76#define HAS_SURFACE_COMMANDS 1
79#ifdef HAS_SURFACE_COMMANDS
97#define IERROR_SCALE 1024
107 &(body_rate->
p), &(body_rate->
q), &(body_rate->
r),
176#ifdef HAS_SURFACE_COMMANDS
188#if PERIODIC_TELEMETRY
213#ifndef GAIN_PRESCALER_FF
214#define GAIN_PRESCALER_FF 1
223#ifdef HAS_SURFACE_COMMANDS
230#ifndef GAIN_PRESCALER_P
231#define GAIN_PRESCALER_P 1
233#ifndef GAIN_PRESCALER_D
234#define GAIN_PRESCALER_D 1
236#ifndef GAIN_PRESCALER_I
237#define GAIN_PRESCALER_I 1
261#ifdef HAS_SURFACE_COMMANDS
309#define INTEGRATOR_BOUND 1.0
333#ifdef HAS_SURFACE_COMMANDS
347#ifdef HAS_SURFACE_COMMANDS
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
void float_quat_inv_comp(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
Composition (multiplication) of two quaternions.
#define FLOAT_EULERS_ZERO(_e)
static void float_quat_wrap_shortest(struct FloatQuat *q)
#define FLOAT_RATES_ZERO(_r)
#define RATES_COPY(_a, _b)
#define QUAT_BFP_OF_REAL(_qi, _qf)
#define VECT3_ASSIGN(_a, _x, _y, _z)
#define RATES_DIFF(_c, _a, _b)
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
float gains[MAX_SAMPLES_LEARNING]
Paparazzi floating point algebra.
Paparazzi fixed point algebra.
struct Stabilization stabilization
struct FloatEulers stab_sp_to_eulers_f(struct StabilizationSetpoint *sp)
struct FloatQuat stab_sp_to_quat_f(struct StabilizationSetpoint *sp)
int32_t th_sp_to_thrust_i(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)
int32_t cmd[COMMANDS_NB]
output command vector, range from [-MAX_PPRZ:MAX_PPRZ] (store for messages)
static struct FloatEulers stab_att_sp_euler
static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
struct FloatAttitudeGains stabilization_gains[STABILIZATION_ATTITUDE_GAIN_NB]
static void attitude_run_ff(float ff_commands[], struct FloatAttitudeGains *gains, struct FloatRates *ref_accel)
static const float theta_ddgain[]
static struct FloatRates last_body_rate
struct AttRefQuatFloat att_ref_quat_f
void stabilization_attitude_enter(void)
Attitude control enter function.
static const float theta_igain[]
void stabilization_attitude_run(bool enable_integrator, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
Attitude control run function.
static const float phi_dgain_d[]
static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
static const float theta_pgain[]
float stabilization_att_ff_cmd[COMMANDS_NB]
void stabilization_attitude_quat_float_init(void)
static const float psi_igain[]
static const float theta_dgain_d[]
static const float phi_ddgain[]
static const float phi_igain[]
static const float psi_pgain[]
struct FloatQuat stabilization_att_sum_err_quat
static struct FloatQuat stab_att_sp_quat
static const float phi_pgain[]
#define GAIN_PRESCALER_FF
static const float theta_dgain[]
static const float psi_ddgain[]
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)
static const float phi_dgain[]
static struct FloatRates body_rate_d
static const float psi_dgain[]
float stabilization_att_fb_cmd[COMMANDS_NB]
void stabilization_attitude_gain_schedule(uint8_t idx)
static const float psi_dgain_d[]
static void send_att(struct transport_tx *trans, struct link_device *dev)
Rotorcraft attitude stabilization in quaternion float version.
#define STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT
#define STABILIZATION_ATTITUDE_GAIN_NB
void attitude_ref_quat_float_update(struct AttRefQuatFloat *ref, struct FloatQuat *sp_quat, float dt)
void attitude_ref_quat_float_enter(struct AttRefQuatFloat *ref, struct FloatQuat *state_quat)
void attitude_ref_quat_float_init(struct AttRefQuatFloat *ref)
void attitude_ref_quat_float_schedule(struct AttRefQuatFloat *ref, uint8_t idx)
Attitude reference models and state/output (float)
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Thrust setpoint // TODO to a setpoint header Structure to store the desired thrust vector with differ...
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.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.