26#ifndef STABILIZATION_H
27#define STABILIZATION_H
31#include "generated/airframe.h"
38#define STABILIZATION_MODE_NONE 0
39#define STABILIZATION_MODE_DIRECT 1
40#define STABILIZATION_MODE_RATE 2
41#define STABILIZATION_MODE_ATTITUDE 3
45#define STABILIZATION_ATT_SUBMODE_HEADING 0
46#define STABILIZATION_ATT_SUBMODE_CARE_FREE 1
47#define STABILIZATION_ATT_SUBMODE_FORWARD 2
172#define THRUST_AXIS_X 0
173#define THRUST_AXIS_Y 1
174#define THRUST_AXIS_Z 2
176#define STAB_SP_SET_EULERS_ZERO(_sp) { \
177 _sp.type = STAB_SP_EULERS; \
178 _sp.format = STAB_SP_INT; \
179 _sp.sp.eulers_i.phi = 0; \
180 _sp.sp.eulers_i.theta = 0; \
181 _sp.sp.eulers_i.psi = 0; \
184#define THRUST_SP_SET_ZERO(_sp) { \
185 _sp.type = THRUST_SP; \
186 _sp.format = THRUST_SP_INT; \
187 _sp.sp.thrust_i[0] = 0; \
188 _sp.sp.thrust_i[1] = 0; \
189 _sp.sp.thrust_i[2] = 0; \
Paparazzi floating point algebra.
Paparazzi fixed point algebra.
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)
struct ThrustSetpoint th_sp_from_incr_i(int32_t th_increment, uint8_t axis)
struct FloatEulers stab_sp_to_eulers_f(struct StabilizationSetpoint *sp)
struct StabilizationSetpoint stab_sp_from_ltp_f(struct FloatVect2 *vect, float heading)
struct StabilizationSetpoint stab_sp_from_rates_i(struct Int32Rates *rates)
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)
struct StabilizationSetpoint stab_sp_from_quat_f(struct FloatQuat *quat)
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])
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)
struct Int32Quat stab_sp_to_quat_i(struct StabilizationSetpoint *sp)
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 stabilization_get_failsafe_sp(void)
Get stabilization setpoint for failsafe.
struct ThrustSetpoint th_sp_from_thrust_i(int32_t thrust, uint8_t axis)
struct StabilizationSetpoint stab_sp_from_rates_f(struct FloatRates *rates)
struct FloatQuat stab_sp_to_quat_f(struct StabilizationSetpoint *sp)
struct StabilizationSetpoint sp
current attitude setpoint (store for messages)
void stabilization_mode_changed(uint8_t new_mode, uint8_t submode)
Check mode change.
void stabilization_filter_commands(void)
Command filter for vibrating airframes.
struct FloatRates rates_f
struct FloatEulers eulers_f
void stabilization_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
Call default stabilization control.
int32_t th_sp_to_incr_i(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)
struct Int32Eulers eulers_i
uint8_t att_submode
current attitude sub-mode
float th_sp_to_incr_f(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)
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)
float th_sp_to_thrust_f(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)
struct Int32Rates rates_i
void stabilization_init(void)
Init function.
struct AttitudeRCInput rc_in
RC input.
int32_t th_sp_to_thrust_i(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)
Read an attitude setpoint from the RC.
@ 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::@354356152067340015216250042204214023005305374154 sp
union StabilizationSetpoint::@100131152155304140146025333156303202065204130301 r_sp
enum StabilizationSetpoint::@326217302154325144200324100161107342073132054326 type
enum StabilizationSetpoint::@222144255207145214030046254223270046116212372345 format
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
union ThrustSetpoint::@302344065342070214103253314124156277107357236133 sp
enum ThrustSetpoint::@227227372251370165225315201346301141210232313211 type
enum ThrustSetpoint::@231163145046014344011223177110357203006342352115 format
@ 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.]
int int32_t
Typedef defining 32 bit int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.