Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 
56 #ifndef PPRZ_ORIENTATION_CONVERSION_H
57 #define PPRZ_ORIENTATION_CONVERSION_H
58 
59 #ifdef __cplusplus
60 extern "C" {
61 #endif
62 
63 #include "math/pprz_algebra_int.h"
65 
66 #include "std.h"
67 
68 
69 #define ORREP_QUAT_I 0
70 #define ORREP_EULER_I 1
71 #define ORREP_RMAT_I 2
72 #define ORREP_QUAT_F 3
73 #define ORREP_EULER_F 4
74 #define ORREP_RMAT_F 5
75 
76 /*
77  * @brief Struct with euler/rmat/quaternion orientation representations in BFP int and float
78  */
86 
91  struct Int32Quat quat_i;
92 
98 
104 
110 
116 
122 };
123 
124 /************* declaration of transformation functions ************/
125 extern void orientationCalcQuat_i(struct OrientationReps *orientation);
126 extern void orientationCalcRMat_i(struct OrientationReps *orientation);
127 extern void orientationCalcEulers_i(struct OrientationReps *orientation);
128 extern void orientationCalcQuat_f(struct OrientationReps *orientation);
129 extern void orientationCalcRMat_f(struct OrientationReps *orientation);
130 extern void orientationCalcEulers_f(struct OrientationReps *orientation);
131 
132 
133 /*********************** validity test functions ******************/
135 static inline bool orienationCheckValid(struct OrientationReps *orientation)
136 {
137  return (orientation->status);
138 }
139 
141 static inline void orientationSetIdentity(struct OrientationReps *orientation)
142 {
143  int32_quat_identity(&orientation->quat_i);
144  /* clear bits for all attitude representations and only set the new one */
145  orientation->status = (1 << ORREP_QUAT_I);
146 }
147 
149 static inline void orientationSetQuat_i(struct OrientationReps *orientation, struct Int32Quat *quat)
150 {
151  QUAT_COPY(orientation->quat_i, *quat);
152  /* clear bits for all attitude representations and only set the new one */
153  orientation->status = (1 << ORREP_QUAT_I);
154 }
155 
157 static inline void orientationSetRMat_i(struct OrientationReps *orientation, struct Int32RMat *rmat)
158 {
159  RMAT_COPY(orientation->rmat_i, *rmat);
160  /* clear bits for all attitude representations and only set the new one */
161  orientation->status = (1 << ORREP_RMAT_I);
162 }
163 
165 static inline void orientationSetEulers_i(struct OrientationReps *orientation, struct Int32Eulers *eulers)
166 {
167  EULERS_COPY(orientation->eulers_i, *eulers);
168  /* clear bits for all attitude representations and only set the new one */
169  orientation->status = (1 << ORREP_EULER_I);
170 }
171 
173 static inline void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
174 {
175  QUAT_COPY(orientation->quat_f, *quat);
176  /* clear bits for all attitude representations and only set the new one */
177  orientation->status = (1 << ORREP_QUAT_F);
178 }
179 
181 static inline void orientationSetRMat_f(struct OrientationReps *orientation, struct FloatRMat *rmat)
182 {
183  RMAT_COPY(orientation->rmat_f, *rmat);
184  /* clear bits for all attitude representations and only set the new one */
185  orientation->status = (1 << ORREP_RMAT_F);
186 }
187 
189 static inline void orientationSetEulers_f(struct OrientationReps *orientation, struct FloatEulers *eulers)
190 {
191  EULERS_COPY(orientation->eulers_f, *eulers);
192  /* clear bits for all attitude representations and only set the new one */
193  orientation->status = (1 << ORREP_EULER_F);
194 }
195 
196 
198 static inline struct Int32Quat *orientationGetQuat_i(struct OrientationReps *orientation)
199 {
200  if (!bit_is_set(orientation->status, ORREP_QUAT_I)) {
201  orientationCalcQuat_i(orientation);
202  }
203  return &orientation->quat_i;
204 }
205 
207 static inline struct Int32RMat *orientationGetRMat_i(struct OrientationReps *orientation)
208 {
209  if (!bit_is_set(orientation->status, ORREP_RMAT_I)) {
210  orientationCalcRMat_i(orientation);
211  }
212  return &orientation->rmat_i;
213 }
214 
216 static inline struct Int32Eulers *orientationGetEulers_i(struct OrientationReps *orientation)
217 {
218  if (!bit_is_set(orientation->status, ORREP_EULER_I)) {
219  orientationCalcEulers_i(orientation);
220  }
221  return &orientation->eulers_i;
222 }
223 
225 static inline struct FloatQuat *orientationGetQuat_f(struct OrientationReps *orientation)
226 {
227  if (!bit_is_set(orientation->status, ORREP_QUAT_F)) {
228  orientationCalcQuat_f(orientation);
229  }
230  return &orientation->quat_f;
231 }
232 
234 static inline struct FloatRMat *orientationGetRMat_f(struct OrientationReps *orientation)
235 {
236  if (!bit_is_set(orientation->status, ORREP_RMAT_F)) {
237  orientationCalcRMat_f(orientation);
238  }
239  return &orientation->rmat_f;
240 }
241 
243 static inline struct FloatEulers *orientationGetEulers_f(struct OrientationReps *orientation)
244 {
245  if (!bit_is_set(orientation->status, ORREP_EULER_F)) {
246  orientationCalcEulers_f(orientation);
247  }
248  return &orientation->eulers_f;
249 }
250 
251 #ifdef __cplusplus
252 } /* extern "C" */
253 #endif
254 
255 #endif /* PPRZ_ORIENTATION_CONVERSION_H */
256 
OrientationReps
Definition: pprz_orientation_conversion.h:79
orientationSetEulers_i
static void orientationSetEulers_i(struct OrientationReps *orientation, struct Int32Eulers *eulers)
Set vehicle body attitude from euler angles (int).
Definition: pprz_orientation_conversion.h:165
ORREP_RMAT_I
#define ORREP_RMAT_I
Rotation Matrix (BFP int)
Definition: pprz_orientation_conversion.h:71
Int32RMat
rotation matrix
Definition: pprz_algebra_int.h:159
orientationSetRMat_f
static void orientationSetRMat_f(struct OrientationReps *orientation, struct FloatRMat *rmat)
Set vehicle body attitude from rotation matrix (float).
Definition: pprz_orientation_conversion.h:181
OrientationReps::rmat_f
struct FloatRMat rmat_f
Orientation rotation matrix.
Definition: pprz_orientation_conversion.h:121
ORREP_QUAT_F
#define ORREP_QUAT_F
Quaternion (float)
Definition: pprz_orientation_conversion.h:72
Int32Quat
Rotation quaternion.
Definition: pprz_algebra_int.h:99
OrientationReps::quat_i
struct Int32Quat quat_i
Orientation quaternion.
Definition: pprz_orientation_conversion.h:91
orientationGetQuat_i
static struct Int32Quat * orientationGetQuat_i(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (int).
Definition: pprz_orientation_conversion.h:198
orientationCalcRMat_i
void orientationCalcRMat_i(struct OrientationReps *orientation)
Definition: pprz_orientation_conversion.c:73
pprz_algebra_float.h
Paparazzi floating point algebra.
orientationCalcEulers_i
void orientationCalcEulers_i(struct OrientationReps *orientation)
Definition: pprz_orientation_conversion.c:98
orientationGetRMat_f
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
Definition: pprz_orientation_conversion.h:234
OrientationReps::eulers_f
struct FloatEulers eulers_f
Orienation in zyx euler angles.
Definition: pprz_orientation_conversion.h:115
pprz_algebra_int.h
Paparazzi fixed point algebra.
OrientationReps::quat_f
struct FloatQuat quat_f
Orientation as quaternion.
Definition: pprz_orientation_conversion.h:109
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
orientationCalcQuat_f
void orientationCalcQuat_f(struct OrientationReps *orientation)
Definition: pprz_orientation_conversion.c:123
std.h
orientationCalcRMat_f
void orientationCalcRMat_f(struct OrientationReps *orientation)
Definition: pprz_orientation_conversion.c:148
RMAT_COPY
#define RMAT_COPY(_o, _i)
Definition: pprz_algebra.h:704
orientationGetQuat_f
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
Definition: pprz_orientation_conversion.h:225
orientationSetQuat_f
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
Definition: pprz_orientation_conversion.h:173
orientationSetIdentity
static void orientationSetIdentity(struct OrientationReps *orientation)
Set to identity orientation.
Definition: pprz_orientation_conversion.h:141
ORREP_QUAT_I
#define ORREP_QUAT_I
Quaternion (BFP int)
Definition: pprz_orientation_conversion.h:69
EULERS_COPY
#define EULERS_COPY(_a, _b)
Definition: pprz_algebra.h:268
uint8_t
unsigned char uint8_t
Definition: types.h:14
Int32Eulers
euler angles
Definition: pprz_algebra_int.h:146
orientationSetEulers_f
static void orientationSetEulers_f(struct OrientationReps *orientation, struct FloatEulers *eulers)
Set vehicle body attitude from euler angles (float).
Definition: pprz_orientation_conversion.h:189
orientationCalcQuat_i
void orientationCalcQuat_i(struct OrientationReps *orientation)
Definition: pprz_orientation_conversion.c:48
QUAT_COPY
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:596
OrientationReps::rmat_i
struct Int32RMat rmat_i
Orientation rotation matrix.
Definition: pprz_orientation_conversion.h:103
OrientationReps::status
uint8_t status
Holds the status bits for all orientation representations.
Definition: pprz_orientation_conversion.h:85
orientationSetRMat_i
static void orientationSetRMat_i(struct OrientationReps *orientation, struct Int32RMat *rmat)
Set vehicle body attitude from rotation matrix (int).
Definition: pprz_orientation_conversion.h:157
ORREP_EULER_F
#define ORREP_EULER_F
zyx Euler (float)
Definition: pprz_orientation_conversion.h:73
ORREP_RMAT_F
#define ORREP_RMAT_F
Rotation Matrix (float)
Definition: pprz_orientation_conversion.h:74
orientationGetRMat_i
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
Definition: pprz_orientation_conversion.h:207
orientationGetEulers_i
static struct Int32Eulers * orientationGetEulers_i(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (int).
Definition: pprz_orientation_conversion.h:216
FloatRMat
rotation matrix
Definition: pprz_algebra_float.h:77
orientationSetQuat_i
static void orientationSetQuat_i(struct OrientationReps *orientation, struct Int32Quat *quat)
Set vehicle body attitude from quaternion (int).
Definition: pprz_orientation_conversion.h:149
FloatEulers
euler angles
Definition: pprz_algebra_float.h:84
orienationCheckValid
static bool orienationCheckValid(struct OrientationReps *orientation)
Test if orientations are valid.
Definition: pprz_orientation_conversion.h:135
ORREP_EULER_I
#define ORREP_EULER_I
zyx Euler (BFP int)
Definition: pprz_orientation_conversion.h:70
int32_quat_identity
static void int32_quat_identity(struct Int32Quat *q)
initialises a quaternion to identity
Definition: pprz_algebra_int.h:431
orientationGetEulers_f
static struct FloatEulers * orientationGetEulers_f(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (float).
Definition: pprz_orientation_conversion.h:243
orientationCalcEulers_f
void orientationCalcEulers_f(struct OrientationReps *orientation)
Definition: pprz_orientation_conversion.c:173
OrientationReps::eulers_i
struct Int32Eulers eulers_i
Orientation in zyx euler angles.
Definition: pprz_orientation_conversion.h:97