Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
stabilization.h
Go to the documentation of this file.
1/*
2 * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
3 *
4 * This file is part of paparazzi.
5 *
6 * paparazzi is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2, or (at your option)
9 * any later version.
10 *
11 * paparazzi is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with paparazzi; see the file COPYING. If not, write to
18 * the Free Software Foundation, 59 Temple Place - Suite 330,
19 * Boston, MA 02111-1307, USA.
20 */
21
25
26#ifndef STABILIZATION_H
27#define STABILIZATION_H
28
29#include "std.h"
30
31#include "generated/airframe.h"
35
38#define STABILIZATION_MODE_NONE 0
39#define STABILIZATION_MODE_DIRECT 1
40#define STABILIZATION_MODE_RATE 2
41#define STABILIZATION_MODE_ATTITUDE 3
42
45#define STABILIZATION_ATT_SUBMODE_HEADING 0 // direct heading control
46#define STABILIZATION_ATT_SUBMODE_CARE_FREE 1 // care free heading mode
47#define STABILIZATION_ATT_SUBMODE_FORWARD 2 // forward flight for hybrid-like
48
54 enum {
61 enum {
65 union {
66 struct Int32Quat quat_i;
67 struct FloatQuat quat_f;
68 struct Int32Eulers eulers_i;
69 struct FloatEulers eulers_f;
70 struct { struct Int32Vect2 vect; int32_t heading; } ltp_i;
71 struct { struct FloatVect2 vect; float heading; } ltp_f;
72 } sp;
73 union {
74 struct Int32Rates rates_i;
75 struct FloatRates rates_f;
77};
78
83 enum {
87 enum {
91 union {
93 float thrust_f[3];
95 float th_incr_f[3];
96 } sp;
97};
98
99
111
112extern struct Stabilization stabilization;
113
116extern void stabilization_init(void);
117
120extern void stabilization_mode_changed(uint8_t new_mode, uint8_t submode);
121
128extern void stabilization_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd);
129
133
136extern void stabilization_filter_commands(void);
137
138// helper convert functions
139extern struct Int32Quat stab_sp_to_quat_i(struct StabilizationSetpoint *sp);
140extern struct FloatQuat stab_sp_to_quat_f(struct StabilizationSetpoint *sp);
143extern struct Int32Rates stab_sp_to_rates_i(struct StabilizationSetpoint *sp);
144extern struct FloatRates stab_sp_to_rates_f(struct StabilizationSetpoint *sp);
145// thrust setpoint helper functions
146// - first param is the thrust setpoint structure
147// - second param is the current thrust (expected in [0:MAX_PPRZ])
148extern int32_t th_sp_to_thrust_i(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis);
149extern float th_sp_to_thrust_f(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis);
150extern int32_t th_sp_to_incr_i(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis);
151extern float th_sp_to_incr_f(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis);
152
153// helper make functions
154extern struct StabilizationSetpoint stab_sp_from_quat_i(struct Int32Quat *quat);
155extern struct StabilizationSetpoint stab_sp_from_quat_f(struct FloatQuat *quat);
156extern struct StabilizationSetpoint stab_sp_from_quat_ff_rates_f(struct FloatQuat *quat, struct FloatRates *rates);
157extern struct StabilizationSetpoint stab_sp_from_eulers_i(struct Int32Eulers *eulers);
158extern struct StabilizationSetpoint stab_sp_from_eulers_f(struct FloatEulers *eulers);
160extern struct StabilizationSetpoint stab_sp_from_ltp_f(struct FloatVect2 *vect, float heading);
161extern struct StabilizationSetpoint stab_sp_from_rates_i(struct Int32Rates *rates);
162extern struct StabilizationSetpoint stab_sp_from_rates_f(struct FloatRates *rates);
163extern struct ThrustSetpoint th_sp_from_thrust_i(int32_t thrust, uint8_t axis);
164extern struct ThrustSetpoint th_sp_from_thrust_f(float thrust, uint8_t axis);
165extern struct ThrustSetpoint th_sp_from_incr_i(int32_t th_increment, uint8_t axis);
166extern struct ThrustSetpoint th_sp_from_incr_f(float th_increment, uint8_t axis);
167extern struct ThrustSetpoint th_sp_from_thrust_vect_i(int32_t thrust[3]);
168extern struct ThrustSetpoint th_sp_from_thrust_vect_f(float thrust[3]);
169extern struct ThrustSetpoint th_sp_from_incr_vect_i(int32_t th_increment[3]);
170extern struct ThrustSetpoint th_sp_from_incr_vect_f(float th_increment[3]);
171
172#define THRUST_AXIS_X 0
173#define THRUST_AXIS_Y 1
174#define THRUST_AXIS_Z 2
175
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; \
182}
183
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; \
190}
191
192#endif /* STABILIZATION_H */
euler angles
Roation quaternion.
angular rates
euler angles
Rotation quaternion.
angular rates
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.
uint8_t mode
current mode
void stabilization_filter_commands(void)
Command filter for vibrating airframes.
struct FloatRates rates_f
struct FloatEulers eulers_f
int32_t th_incr_i[3]
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.
struct Int32Quat quat_i
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
struct FloatQuat quat_f
int32_t thrust_i[3]
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)
Stabilization structure.
Read an attitude setpoint from the RC.
Attitude (and Rate) Remote Control input.
Stabilization setpoint.
@ 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.
float heading
Definition wedgebug.c:258