37#if (STABILIZATION_FILTER_COMMANDS_ROLL_PITCH || STABILIZATION_FILTER_COMMANDS_YAW)
44#if STABILIZATION_FILTER_CMD_ROLL_PITCH
45#ifndef STABILIZATION_FILTER_CMD_ROLL_CUTOFF
46#define STABILIZATION_FILTER_CMD_ROLL_CUTOFF 20.0
49#ifndef STABILIZATION_FILTER_CMD_PITCH_CUTOFF
50#define STABILIZATION_FILTER_CMD_PITCH_CUTOFF 20.0
57#if STABILIZATION_FILTER_CMD_YAW
58#ifndef STABILIZATION_FILTER_CMD_YAW_CUTOFF
59#define STABILIZATION_FILTER_CMD_YAW_CUTOFF 20.0
65#ifndef STABILIZATION_RC_ID
66#define STABILIZATION_RC_ID ABI_BROADCAST
77#if defined(COMMAND_ROLL) && defined(COMMAND_PITCH) && defined(COMMAND_YAW)
105#if STABILIZATION_FILTER_CMD_ROLL_PITCH
112#if STABILIZATION_FILTER_CMD_YAW
119#if PERIODIC_TELEMETRY
124#if USE_STABILIZATION_RATE
151#if USE_STABILIZATION_RATE
174#if USE_EARTH_BOUND_RC_SETPOINT
191#if USE_STABILIZATION_RATE
220#define TRANSITION_TO_HOVER false
221#define TRANSITION_TO_FORWARD true
223#ifndef TRANSITION_TIME
224#define TRANSITION_TIME 3.f
237#ifdef TRANSITION_MAX_OFFSET
249#if USE_STABILIZATION_RATE
261#if (STABILIZATION_FILTER_CMD_ROLL_PITCH || STABILIZATION_FILTER_CMD_YAW)
317#if STABILIZATION_FILTER_CMD_ROLL_PITCH
324#if STABILIZATION_FILTER_CMD_YAW
333 if ((sp->type == STAB_SP_QUAT) || (sp->type == STAB_SP_QUAT_FF_RATE)) {
334 if (sp->format == STAB_SP_INT) {
335 return sp->sp.quat_i;
341 }
else if (sp->type == STAB_SP_EULERS) {
342 if (sp->format == STAB_SP_INT) {
353 }
else if (sp->type == STAB_SP_LTP) {
354 if (sp->format == STAB_SP_INT) {
375 if ((sp->type == STAB_SP_QUAT) || (sp->type == STAB_SP_QUAT_FF_RATE)) {
376 if (sp->format == STAB_SP_FLOAT) {
377 return sp->sp.quat_f;
383 }
else if (sp->type == STAB_SP_EULERS) {
384 if (sp->format == STAB_SP_FLOAT) {
395 }
else if (sp->type == STAB_SP_LTP) {
396 if (sp->format == STAB_SP_FLOAT) {
417 if (sp->type == STAB_SP_EULERS) {
418 if (sp->format == STAB_SP_INT) {
419 return sp->sp.eulers_i;
425 }
else if ((sp->type == STAB_SP_QUAT) || (sp->type == STAB_SP_QUAT_FF_RATE)) {
426 if (sp->format == STAB_SP_INT) {
437 }
else if (sp->type == STAB_SP_LTP) {
438 if (sp->format == STAB_SP_INT) {
456 if (sp->type == STAB_SP_EULERS) {
457 if (sp->format == STAB_SP_FLOAT) {
458 return sp->sp.eulers_f;
464 }
else if ((sp->type == STAB_SP_QUAT) || (sp->type == STAB_SP_QUAT_FF_RATE)) {
465 if (sp->format == STAB_SP_FLOAT) {
476 }
else if (sp->type == STAB_SP_LTP) {
477 if (sp->format == STAB_SP_FLOAT) {
495 if ((sp->type == STAB_SP_RATES) || (sp->type == STAB_SP_QUAT_FF_RATE)) {
496 if (sp->format == STAB_SP_INT) {
497 return sp->r_sp.rates_i;
512 if ((sp->type == STAB_SP_RATES) || (sp->type == STAB_SP_QUAT_FF_RATE)) {
513 if (sp->format == STAB_SP_FLOAT) {
514 return sp->r_sp.rates_f;
529 if (
th->type == THRUST_SP) {
530 if (
th->format == THRUST_SP_INT) {
531 return th->sp.thrust_i[
axis];
536 if (
th->format == THRUST_SP_INT) {
537 return thrust +
th->sp.th_incr_i[
axis];
547 if (
th->type == THRUST_SP) {
548 if (
th->format == THRUST_SP_FLOAT) {
549 return th->sp.thrust_f[
axis];
554 if (
th->format == THRUST_SP_FLOAT) {
564 if (
th->type == THRUST_INCR_SP) {
565 if (
th->format == THRUST_SP_INT) {
566 return th->sp.th_incr_i[
axis];
571 if (
th->format == THRUST_SP_INT) {
572 return th->sp.thrust_i[
axis] - thrust;
582 if (
th->type == THRUST_INCR_SP) {
583 if (
th->format == THRUST_SP_FLOAT) {
584 return th->sp.th_incr_f[
axis];
589 if (
th->format == THRUST_SP_FLOAT) {
623 .
r_sp.rates_f = *rates
633 .
sp.eulers_i = *eulers
643 .
sp.eulers_f = *eulers
653 .
sp.ltp_i.vect = *vect,
664 .
sp.ltp_f.vect = *vect,
675 .
r_sp.rates_i = *rates
685 .
r_sp.rates_f = *rates
736 sp.
sp.thrust_i[0] = thrust[0];
737 sp.
sp.thrust_i[1] = thrust[1];
738 sp.
sp.thrust_i[2] = thrust[2];
748 sp.
sp.thrust_f[0] = thrust[0];
749 sp.
sp.thrust_f[1] = thrust[1];
750 sp.
sp.thrust_f[2] = thrust[2];
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
bool autopilot_in_flight(void)
get in_flight flag
Core autopilot interface common to all firmwares.
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e)
quat of euler roation 'ZYX'
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
#define EULERS_BFP_OF_REAL(_ei, _ef)
#define QUAT_BFP_OF_REAL(_qi, _qf)
#define RATES_BFP_OF_REAL(_ri, _rf)
#define QUAT_FLOAT_OF_BFP(_qf, _qi)
#define RATES_FLOAT_OF_BFP(_rf, _ri)
#define EULERS_FLOAT_OF_BFP(_ef, _ei)
int32_t phi
in rad with INT32_ANGLE_FRAC
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t theta
in rad with INT32_ANGLE_FRAC
void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q)
static void int32_quat_identity(struct Int32Quat *q)
initialises a quaternion to identity
void int32_quat_of_eulers(struct Int32Quat *q, struct Int32Eulers *e)
Quaternion from Euler angles.
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (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).
Simple first order low pass filter with bilinear transform.
static void init_second_order_low_pass_int(struct SecondOrderLowPass_int *filter, float cut_off, float Q, float sample_time, int32_t value)
Init second order low pass filter(fixed point version).
static int32_t update_second_order_low_pass_int(struct SecondOrderLowPass_int *filter, int32_t value)
Update second order low pass filter state with a new value(fixed point version).
int32_t i[2]
input history
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
#define PPRZ_ITRIG_SIN(_s, _a)
#define PPRZ_ITRIG_COS(_c, _a)
struct RadioControl radio_control
Generic interface for radio control modules.
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
#define RADIO_ROLL
Redefining RADIO_* Do not use with radio.h (ppm rc)
General attitude stabilization interface for rotorcrafts.
void stabilization_attitude_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
Attitude control run function.
void stabilization_attitude_enter(void)
Attitude control enter function.
struct ThrustSetpoint th_sp_from_thrust_f(float thrust, uint8_t axis)
struct Stabilization stabilization
struct StabilizationSetpoint stab_sp_from_ltp_i(struct Int32Vect2 *vect, int32_t heading)
static struct Int32Eulers stab_sp_rotate_i(struct Int32Vect2 *vect, int32_t heading)
struct ThrustSetpoint th_sp_from_incr_i(int32_t th_increment, uint8_t axis)
struct FloatEulers stab_sp_to_eulers_f(struct StabilizationSetpoint *sp)
void stabilization_mode_changed(uint8_t new_mode, uint8_t submode)
Check mode change.
struct StabilizationSetpoint stab_sp_from_ltp_f(struct FloatVect2 *vect, float heading)
static void stabilization_attitude_reset_rc(void)
void stabilization_filter_commands(void)
Command filter for vibrating airframes.
struct StabilizationSetpoint stab_sp_from_rates_i(struct Int32Rates *rates)
static void rc_cb(uint8_t sender_id UNUSED, struct RadioControl *rc)
static void send_tune_hover(struct transport_tx *trans UNUSED, struct link_device *dev UNUSED)
struct Int32Rates stab_sp_to_rates_i(struct StabilizationSetpoint *sp)
struct ThrustSetpoint th_sp_from_incr_vect_f(float th_increment[3])
struct StabilizationSetpoint stab_sp_from_quat_ff_rates_f(struct FloatQuat *quat, struct FloatRates *rates)
void stabilization_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
Call default stabilization control.
struct StabilizationSetpoint stab_sp_from_quat_f(struct FloatQuat *quat)
int32_t th_sp_to_incr_i(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)
struct ThrustSetpoint th_sp_from_thrust_vect_f(float thrust[3])
struct StabilizationSetpoint stab_sp_from_quat_i(struct Int32Quat *quat)
struct ThrustSetpoint th_sp_from_thrust_vect_i(int32_t thrust[3])
static struct FloatEulers stab_sp_rotate_f(struct FloatVect2 *vect, float heading)
struct ThrustSetpoint th_sp_from_incr_vect_i(int32_t th_increment[3])
struct FloatRates stab_sp_to_rates_f(struct StabilizationSetpoint *sp)
struct StabilizationSetpoint stab_sp_from_eulers_f(struct FloatEulers *eulers)
float th_sp_to_incr_f(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)
static const float transition_increment
struct Int32Quat stab_sp_to_quat_i(struct StabilizationSetpoint *sp)
float th_sp_to_thrust_f(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)
#define STABILIZATION_RC_ID
struct StabilizationSetpoint stab_sp_from_eulers_i(struct Int32Eulers *eulers)
struct ThrustSetpoint th_sp_from_incr_f(float th_increment, uint8_t axis)
struct Int32Eulers stab_sp_to_eulers_i(struct StabilizationSetpoint *sp)
struct StabilizationSetpoint WEAK stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn, struct RadioControl *rc)
Retrun attitude setpoint from RC as euler angles.
#define TRANSITION_TO_FORWARD
struct StabilizationSetpoint stabilization_get_failsafe_sp(void)
Get stabilization setpoint for failsafe.
struct ThrustSetpoint th_sp_from_thrust_i(int32_t thrust, uint8_t axis)
#define TRANSITION_TO_HOVER
Transition from 0 to 100%, used for pitch offset of hybrids.
void stabilization_init(void)
Init function.
static void transition_run(bool to_forward)
struct StabilizationSetpoint stab_sp_from_rates_f(struct FloatRates *rates)
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)
General stabilization interface for rotorcrafts.
struct StabilizationSetpoint sp
current attitude setpoint (store for messages)
#define STABILIZATION_ATT_SUBMODE_CARE_FREE
#define STABILIZATION_MODE_ATTITUDE
#define STABILIZATION_ATT_SUBMODE_HEADING
Stabilization sub-modes for attitude.
uint8_t att_submode
current attitude sub-mode
#define STABILIZATION_ATT_SUBMODE_FORWARD
struct StabilizationSetpoint stab_sp_from_eulers_f(struct FloatEulers *eulers)
float transition_ratio
transition percentage for hybrids (0.: hover; 1.: forward)
struct StabilizationSetpoint rc_sp
Keep it ? FIXME.
int32_t cmd[COMMANDS_NB]
output command vector, range from [-MAX_PPRZ:MAX_PPRZ] (store for messages)
#define STABILIZATION_MODE_RATE
#define STAB_SP_SET_EULERS_ZERO(_sp)
#define STABILIZATION_MODE_DIRECT
#define STABILIZATION_MODE_NONE
Stabilization modes.
struct AttitudeRCInput rc_in
RC input.
void stabilization_attitude_reset_care_free_heading(struct AttitudeRCInput *rc_sp)
reset the heading for care-free mode to current heading
void stabilization_attitude_reset_rc_setpoint(struct AttitudeRCInput *rc_sp)
reset to current state
void stabilization_attitude_rc_setpoint_init(struct AttitudeRCInput *rc_sp)
Init rc input.
void stabilization_attitude_read_rc_setpoint_earth_bound(struct AttitudeRCInput *rc_sp, bool in_flight, bool in_carefree, bool coordinated_turn, struct RadioControl *rc)
Read attitude setpoint from RC as quaternion in earth bound frame.
void stabilization_attitude_read_rc_setpoint(struct AttitudeRCInput *rc_sp, bool in_flight, bool in_carefree, bool coordinated_turn, struct RadioControl *rc)
Read attitude setpoint from RC as quaternion Interprets the stick positions as axes.
Read an attitude setpoint from the RC.
struct FloatQuat rc_quat
RC input in quaternion.
float transition_theta_offset
pitch offset for hybrids, add when in forward mode
void stabilization_direct_read_rc(void)
void stabilization_direct_enter(void)
void stabilization_direct_run(bool in_flight UNUSED, struct StabilizationSetpoint *sp UNUSED, struct ThrustSetpoint *thrust UNUSED, int32_t *cmd UNUSED)
Dummy stabilization for rotorcrafts.
void stabilization_rate_enter(void)
struct StabilizationSetpoint stabilization_rate_read_rc(struct RadioControl *rc)
void stabilization_rate_run(bool in_flight, struct StabilizationSetpoint *rate_sp, struct ThrustSetpoint *thrust, int32_t *cmd)
Rate stabilization for rotorcrafts.
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
@ STAB_SP_RATES
body rates
@ STAB_SP_EULERS
LTP to Body orientation in euler angles.
@ STAB_SP_LTP
banking and heading in LTP (NED) frame
@ STAB_SP_QUAT_FF_RATE
LTP to Body orientation in unit quaternion with precomputed feedforward rates.
@ STAB_SP_QUAT
LTP to Body orientation in unit quaternion.
union StabilizationSetpoint::@278 sp
union StabilizationSetpoint::@279 r_sp
enum StabilizationSetpoint::@277 format
enum StabilizationSetpoint::@276 type
Thrust setpoint // TODO to a setpoint header Structure to store the desired thrust vector with differ...
@ THRUST_SP
absolute thrust setpoint
@ THRUST_INCR_SP
thrust increment
@ THRUST_SP_INT
int is assumed to be normalized in [0:MAX_PPRZ]
@ THRUST_SP_FLOAT
float is assumed to be normalized in [0.:1.]
enum ThrustSetpoint::@282 type
enum ThrustSetpoint::@283 format
union ThrustSetpoint::@284 sp
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.