28 #include "generated/airframe.h"
51 #if PERIODIC_TELEMETRY
54 static void send_att(
struct transport_tx *trans,
struct link_device *
dev)
59 pprz_msg_send_STAB_ATTITUDE_FLOAT(trans, dev, AC_ID,
60 &(body_rate->
p), &(body_rate->
q), &(body_rate->
r),
82 pprz_msg_send_STAB_ATTITUDE_REF_FLOAT(trans, dev, AC_ID,
104 STABILIZATION_ATTITUDE_PHI_PGAIN,
105 STABILIZATION_ATTITUDE_THETA_PGAIN,
106 STABILIZATION_ATTITUDE_PSI_PGAIN);
109 STABILIZATION_ATTITUDE_PHI_DGAIN,
110 STABILIZATION_ATTITUDE_THETA_DGAIN,
111 STABILIZATION_ATTITUDE_PSI_DGAIN);
114 STABILIZATION_ATTITUDE_PHI_IGAIN,
115 STABILIZATION_ATTITUDE_THETA_IGAIN,
116 STABILIZATION_ATTITUDE_PSI_IGAIN);
119 STABILIZATION_ATTITUDE_PHI_DDGAIN,
120 STABILIZATION_ATTITUDE_THETA_DDGAIN,
121 STABILIZATION_ATTITUDE_PSI_DDGAIN);
125 #if PERIODIC_TELEMETRY
167 float s_psi = sinf(psi);
168 float c_psi = cosf(psi);
174 #define MAX_SUM_ERR 200
180 static const float dt = (1./PERIODIC_FREQUENCY);
#define FLOAT_ANGLE_NORMALIZE(_a)
void attitude_ref_euler_float_enter(struct AttRefEulerFloat *ref, float psi)
static void send_att(struct transport_tx *trans, struct link_device *dev)
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Periodic telemetry system header (includes downlink utility and generated code).
General attitude stabilization interface for rotorcrafts.
#define FLOAT_EULERS_ZERO(_e)
void stabilization_attitude_set_failsafe_setpoint(void)
#define VECT3_ASSIGN(_a, _x, _y, _z)
Read an attitude setpoint from the RC.
#define FLOAT_RATES_ZERO(_r)
#define RATES_DIFF(_c, _a, _b)
#define EULERS_BOUND_CUBE(_v, _min, _max)
#define ANGLE_FLOAT_OF_BFP(_ai)
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
struct FloatEulers stab_att_sp_euler
with INT32_ANGLE_FRAC
void stabilization_attitude_enter(void)
Paparazzi floating point algebra.
Attitude reference state/output (euler float)
struct AttRefEulerFloat att_ref_euler_f
struct FloatAttitudeGains stabilization_gains
#define DefaultPeriodic
Set default periodic telemetry.
float stabilization_att_fb_cmd[COMMANDS_NB]
void stabilization_attitude_init(void)
void attitude_ref_euler_float_update(struct AttRefEulerFloat *ref, struct FloatEulers *sp_eulers, float dt)
void stabilization_attitude_run(bool in_flight)
void attitude_ref_euler_float_init(struct AttRefEulerFloat *ref)
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
static const struct usb_device_descriptor dev
API to get/set the generic vehicle states.
#define EULERS_DIFF(_c, _a, _b)
struct FloatEulers stabilization_att_sum_err
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
#define EULERS_COPY(_a, _b)
void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool in_flight, bool in_carefree, bool coordinated_turn)
static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
#define EULERS_ADD(_a, _b)
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
float stabilization_att_ff_cmd[COMMANDS_NB]
float stabilization_attitude_get_heading_f(void)
void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
#define EULERS_FLOAT_OF_BFP(_ef, _ei)
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.