Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
pprz_orientation_conversion.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2011-2012 The Paparazzi Team
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, see
18  * <http://www.gnu.org/licenses/>.
19  *
20  */
21 
57 #ifndef PPRZ_ORIENTATION_CONVERSION_H
58 #define PPRZ_ORIENTATION_CONVERSION_H
59 
60 #include "math/pprz_algebra_int.h"
62 
63 #include "std.h"
64 
65 
66 #define ORREP_QUAT_I 0
67 #define ORREP_EULER_I 1
68 #define ORREP_RMAT_I 2
69 #define ORREP_QUAT_F 3
70 #define ORREP_EULER_F 4
71 #define ORREP_RMAT_F 5
72 
73 /*
74  * @brief Struct with euler/rmat/quaternion orientation representations in BFP int and float
75  */
83 
88  struct Int32Quat quat_i;
89 
95 
101 
107 
113 
119 };
120 
121 /************* declaration of transformation functions ************/
122 extern void orientationCalcQuat_i(struct OrientationReps* orientation);
123 extern void orientationCalcRMat_i(struct OrientationReps* orientation);
124 extern void orientationCalcEulers_i(struct OrientationReps* orientation);
125 extern void orientationCalcQuat_f(struct OrientationReps* orientation);
126 extern void orientationCalcRMat_f(struct OrientationReps* orientation);
127 extern void orientationCalcEulers_f(struct OrientationReps* orientation);
128 
129 
130 /*********************** validity test functions ******************/
132 static inline bool_t orienationCheckValid(struct OrientationReps* orientation) {
133  return (orientation->status);
134 }
135 
137 static inline void orientationSetQuat_i(struct OrientationReps* orientation, struct Int32Quat* quat) {
138  QUAT_COPY(orientation->quat_i, *quat);
139  /* clear bits for all attitude representations and only set the new one */
140  orientation->status = (1 << ORREP_QUAT_I);
141 }
142 
144 static inline void orientationSetRMat_i(struct OrientationReps* orientation, struct Int32RMat* rmat) {
145  RMAT_COPY(orientation->rmat_i, *rmat);
146  /* clear bits for all attitude representations and only set the new one */
147  orientation->status = (1 << ORREP_RMAT_I);
148 }
149 
151 static inline void orientationSetEulers_i(struct OrientationReps* orientation, struct Int32Eulers* eulers) {
152  EULERS_COPY(orientation->eulers_i, *eulers);
153  /* clear bits for all attitude representations and only set the new one */
154  orientation->status = (1 << ORREP_EULER_I);
155 }
156 
158 static inline void orientationSetQuat_f(struct OrientationReps* orientation, struct FloatQuat* quat) {
159  QUAT_COPY(orientation->quat_f, *quat);
160  /* clear bits for all attitude representations and only set the new one */
161  orientation->status = (1 << ORREP_QUAT_F);
162 }
163 
165 static inline void orientationSetRMat_f(struct OrientationReps* orientation, struct FloatRMat* rmat) {
166  RMAT_COPY(orientation->rmat_f, *rmat);
167  /* clear bits for all attitude representations and only set the new one */
168  orientation->status = (1 << ORREP_RMAT_F);
169 }
170 
172 static inline void orientationSetEulers_f(struct OrientationReps* orientation, struct FloatEulers* eulers) {
173  EULERS_COPY(orientation->eulers_f, *eulers);
174  /* clear bits for all attitude representations and only set the new one */
175  orientation->status = (1 << ORREP_EULER_F);
176 }
177 
178 
180 static inline struct Int32Quat* orientationGetQuat_i(struct OrientationReps* orientation) {
181  if (!bit_is_set(orientation->status, ORREP_QUAT_I))
182  orientationCalcQuat_i(orientation);
183  return &orientation->quat_i;
184 }
185 
187 static inline struct Int32RMat* orientationGetRMat_i(struct OrientationReps* orientation) {
188  if (!bit_is_set(orientation->status, ORREP_RMAT_I))
189  orientationCalcRMat_i(orientation);
190  return &orientation->rmat_i;
191 }
192 
194 static inline struct Int32Eulers* orientationGetEulers_i(struct OrientationReps* orientation) {
195  if (!bit_is_set(orientation->status, ORREP_EULER_I))
196  orientationCalcEulers_i(orientation);
197  return &orientation->eulers_i;
198 }
199 
201 static inline struct FloatQuat* orientationGetQuat_f(struct OrientationReps* orientation) {
202  if (!bit_is_set(orientation->status, ORREP_QUAT_F))
203  orientationCalcQuat_f(orientation);
204  return &orientation->quat_f;
205 }
206 
208 static inline struct FloatRMat* orientationGetRMat_f(struct OrientationReps* orientation) {
209  if (!bit_is_set(orientation->status, ORREP_RMAT_F))
210  orientationCalcRMat_f(orientation);
211  return &orientation->rmat_f;
212 }
213 
215 static inline struct FloatEulers* orientationGetEulers_f(struct OrientationReps* orientation) {
216  if (!bit_is_set(orientation->status, ORREP_EULER_F))
217  orientationCalcEulers_f(orientation);
218  return &orientation->eulers_f;
219 }
220 
221 #endif /* PPRZ_ORIENTATION_CONVERSION_H */
222 
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
rotation matrix
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
#define ORREP_EULER_F
zyx Euler (float)
Rotation quaternion.
static struct Int32Eulers * orientationGetEulers_i(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (int).
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
struct FloatRMat rmat_f
Orientation rotation matrix.
void orientationCalcQuat_i(struct OrientationReps *orientation)
void orientationCalcRMat_f(struct OrientationReps *orientation)
euler angles
static void orientationSetQuat_i(struct OrientationReps *orientation, struct Int32Quat *quat)
Set vehicle body attitude from quaternion (int).
uint8_t status
Holds the status bits for all orientation representations.
Roation quaternion.
#define ORREP_QUAT_F
Quaternion (float)
static void orientationSetEulers_i(struct OrientationReps *orientation, struct Int32Eulers *eulers)
Set vehicle body attitude from euler angles (int).
#define ORREP_QUAT_I
Quaternion (BFP int)
#define ORREP_EULER_I
zyx Euler (BFP int)
struct FloatEulers eulers_f
Orienation in zyx euler angles.
Paparazzi floating point algebra.
static void orientationSetRMat_i(struct OrientationReps *orientation, struct Int32RMat *rmat)
Set vehicle body attitude from rotation matrix (int).
#define RMAT_COPY(_o, _i)
Definition: pprz_algebra.h:617
struct Int32Quat quat_i
Orientation quaternion.
#define ORREP_RMAT_F
Rotation Matrix (float)
void orientationCalcEulers_i(struct OrientationReps *orientation)
struct Int32RMat rmat_i
Orientation rotation matrix.
struct FloatQuat quat_f
Orientation as quaternion.
unsigned char uint8_t
Definition: types.h:14
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:512
static struct Int32Quat * orientationGetQuat_i(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (int).
void orientationCalcQuat_f(struct OrientationReps *orientation)
static struct FloatEulers * orientationGetEulers_f(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (float).
#define ORREP_RMAT_I
Rotation Matrix (BFP int)
void orientationCalcRMat_i(struct OrientationReps *orientation)
static bool_t orienationCheckValid(struct OrientationReps *orientation)
Test if orientations are valid.
rotation matrix
static void orientationSetRMat_f(struct OrientationReps *orientation, struct FloatRMat *rmat)
Set vehicle body attitude from rotation matrix (float).
struct Int32Eulers eulers_i
Orientation in zyx euler angles.
void orientationCalcEulers_f(struct OrientationReps *orientation)
#define EULERS_COPY(_a, _b)
Definition: pprz_algebra.h:257
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
Paparazzi fixed point algebra.
static void orientationSetEulers_f(struct OrientationReps *orientation, struct FloatEulers *eulers)
Set vehicle body attitude from euler angles (float).
euler angles