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.c
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, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
41 
42 
43 /******************************************************************************
44  * *
45  * Transformation functions for the ORIENTATION representations *
46  * *
47  *****************************************************************************/
48 
49 void orientationCalcQuat_i(struct OrientationReps* orientation) {
50  if (bit_is_set(orientation->status, ORREP_QUAT_I))
51  return;
52 
53  if (bit_is_set(orientation->status, ORREP_QUAT_F)) {
54  QUAT_BFP_OF_REAL(orientation->quat_i, orientation->quat_f);
55  }
56  else if (bit_is_set(orientation->status, ORREP_RMAT_I)) {
57  INT32_QUAT_OF_RMAT(orientation->quat_i, orientation->rmat_i);
58  }
59  else if (bit_is_set(orientation->status, ORREP_EULER_I)) {
60  INT32_QUAT_OF_EULERS(orientation->quat_i, orientation->eulers_i);
61  }
62  else if (bit_is_set(orientation->status, ORREP_RMAT_F)) {
63  RMAT_BFP_OF_REAL(orientation->rmat_i, orientation->rmat_f);
64  SetBit(orientation->status, ORREP_RMAT_I);
65  INT32_QUAT_OF_RMAT(orientation->quat_i, orientation->rmat_i);
66  }
67  else if (bit_is_set(orientation->status, ORREP_EULER_F)) {
68  EULERS_BFP_OF_REAL(orientation->eulers_i, orientation->eulers_f);
69  SetBit(orientation->status, ORREP_EULER_I);
70  INT32_QUAT_OF_EULERS(orientation->quat_i, orientation->eulers_i);
71  }
72  /* set bit to indicate this representation is computed */
73  SetBit(orientation->status, ORREP_QUAT_I);
74 }
75 
76 void orientationCalcRMat_i(struct OrientationReps* orientation) {
77  if (bit_is_set(orientation->status, ORREP_RMAT_I))
78  return;
79 
80  if (bit_is_set(orientation->status, ORREP_RMAT_F)) {
81  RMAT_BFP_OF_REAL(orientation->rmat_i, orientation->rmat_f);
82  }
83  else if (bit_is_set(orientation->status, ORREP_QUAT_I)) {
84  INT32_RMAT_OF_QUAT(orientation->rmat_i, orientation->quat_i);
85  }
86  else if (bit_is_set(orientation->status, ORREP_EULER_I)) {
87  INT32_RMAT_OF_EULERS(orientation->rmat_i, orientation->eulers_i);
88  }
89  else if (bit_is_set(orientation->status, ORREP_QUAT_F)) {
90  QUAT_BFP_OF_REAL(orientation->quat_i, orientation->quat_f);
91  SetBit(orientation->status, ORREP_QUAT_I);
92  INT32_RMAT_OF_QUAT(orientation->rmat_i, orientation->quat_i);
93  }
94  else if (bit_is_set(orientation->status, ORREP_EULER_F)) {
95  EULERS_BFP_OF_REAL(orientation->eulers_i, orientation->eulers_f);
96  SetBit(orientation->status, ORREP_EULER_I);
97  INT32_RMAT_OF_EULERS(orientation->rmat_i, orientation->eulers_i);
98  }
99  /* set bit to indicate this representation is computed */
100  SetBit(orientation->status, ORREP_RMAT_I);
101 }
102 
103 void orientationCalcEulers_i(struct OrientationReps* orientation) {
104  if (bit_is_set(orientation->status, ORREP_EULER_I))
105  return;
106 
107  if (bit_is_set(orientation->status, ORREP_EULER_F)) {
108  EULERS_BFP_OF_REAL(orientation->eulers_i, orientation->eulers_f);
109  }
110  else if (bit_is_set(orientation->status, ORREP_RMAT_I)) {
111  INT32_EULERS_OF_RMAT(orientation->eulers_i, orientation->rmat_i);
112  }
113  else if (bit_is_set(orientation->status, ORREP_QUAT_I)) {
114  INT32_EULERS_OF_QUAT(orientation->eulers_i, orientation->quat_i);
115  }
116  else if (bit_is_set(orientation->status, ORREP_RMAT_F)) {
117  RMAT_BFP_OF_REAL(orientation->rmat_i, orientation->rmat_f);
118  SetBit(orientation->status, ORREP_RMAT_I);
119  INT32_EULERS_OF_RMAT(orientation->eulers_i, orientation->rmat_i);
120  }
121  else if (bit_is_set(orientation->status, ORREP_QUAT_F)) {
122  QUAT_BFP_OF_REAL(orientation->quat_i, orientation->quat_f);
123  SetBit(orientation->status, ORREP_QUAT_I);
124  INT32_EULERS_OF_QUAT(orientation->eulers_i, orientation->quat_i);
125  }
126  /* set bit to indicate this representation is computed */
127  SetBit(orientation->status, ORREP_EULER_I);
128 }
129 
130 void orientationCalcQuat_f(struct OrientationReps* orientation) {
131  if (bit_is_set(orientation->status, ORREP_QUAT_F))
132  return;
133 
134  if (bit_is_set(orientation->status, ORREP_QUAT_I)) {
135  QUAT_FLOAT_OF_BFP(orientation->quat_f, orientation->quat_i);
136  }
137  else if (bit_is_set(orientation->status, ORREP_RMAT_F)) {
138  FLOAT_QUAT_OF_RMAT(orientation->quat_f, orientation->rmat_f);
139  }
140  else if (bit_is_set(orientation->status, ORREP_EULER_F)) {
141  FLOAT_QUAT_OF_EULERS(orientation->quat_f, orientation->eulers_f);
142  }
143  else if (bit_is_set(orientation->status, ORREP_RMAT_I)) {
144  RMAT_FLOAT_OF_BFP(orientation->rmat_f, orientation->rmat_i);
145  SetBit(orientation->status, ORREP_RMAT_F);
146  FLOAT_QUAT_OF_RMAT(orientation->quat_f, orientation->rmat_f);
147  }
148  else if (bit_is_set(orientation->status, ORREP_EULER_I)) {
149  EULERS_FLOAT_OF_BFP(orientation->eulers_f, orientation->eulers_i);
150  SetBit(orientation->status, ORREP_EULER_F);
151  FLOAT_QUAT_OF_EULERS(orientation->quat_f, orientation->eulers_f);
152  }
153  /* set bit to indicate this representation is computed */
154  SetBit(orientation->status, ORREP_QUAT_F);
155 }
156 
157 void orientationCalcRMat_f(struct OrientationReps* orientation) {
158  if (bit_is_set(orientation->status, ORREP_RMAT_F))
159  return;
160 
161  if (bit_is_set(orientation->status, ORREP_RMAT_I)) {
162  RMAT_FLOAT_OF_BFP(orientation->rmat_f, orientation->rmat_i);
163  }
164  else if (bit_is_set(orientation->status, ORREP_QUAT_F)) {
165  FLOAT_RMAT_OF_QUAT(orientation->rmat_f, orientation->quat_f);
166  }
167  else if (bit_is_set(orientation->status, ORREP_EULER_F)) {
168  FLOAT_RMAT_OF_EULERS(orientation->rmat_f, orientation->eulers_f);
169  }
170  else if (bit_is_set(orientation->status, ORREP_QUAT_I)) {
171  QUAT_FLOAT_OF_BFP(orientation->quat_f, orientation->quat_i);
172  SetBit(orientation->status, ORREP_QUAT_F);
173  FLOAT_RMAT_OF_QUAT(orientation->rmat_f, orientation->quat_f);
174  }
175  else if (bit_is_set(orientation->status, ORREP_EULER_I)) {
176  EULERS_FLOAT_OF_BFP(orientation->eulers_f, orientation->eulers_i);
177  SetBit(orientation->status, ORREP_EULER_F);
178  FLOAT_RMAT_OF_EULERS(orientation->rmat_f, orientation->eulers_f);
179  }
180  /* set bit to indicate this representation is computed */
181  SetBit(orientation->status, ORREP_RMAT_F);
182 }
183 
184 void orientationCalcEulers_f(struct OrientationReps* orientation) {
185  if (bit_is_set(orientation->status, ORREP_EULER_F))
186  return;
187 
188  if (bit_is_set(orientation->status, ORREP_EULER_I)) {
189  EULERS_FLOAT_OF_BFP(orientation->eulers_f, orientation->eulers_i);
190  }
191  else if (bit_is_set(orientation->status, ORREP_RMAT_F)) {
192  FLOAT_EULERS_OF_RMAT(orientation->eulers_f, orientation->rmat_f);
193  }
194  else if (bit_is_set(orientation->status, ORREP_QUAT_F)) {
195  FLOAT_EULERS_OF_QUAT(orientation->eulers_f, orientation->quat_f);
196  }
197  else if (bit_is_set(orientation->status, ORREP_RMAT_I)) {
198  RMAT_FLOAT_OF_BFP(orientation->rmat_f, orientation->rmat_i);
199  SetBit(orientation->status, ORREP_RMAT_F);
200  FLOAT_EULERS_OF_RMAT(orientation->eulers_f, orientation->rmat_f);
201  }
202  else if (bit_is_set(orientation->status, ORREP_QUAT_I)) {
203  QUAT_FLOAT_OF_BFP(orientation->quat_f, orientation->quat_i);
204  SetBit(orientation->status, ORREP_QUAT_F);
205  FLOAT_EULERS_OF_QUAT(orientation->eulers_f, orientation->quat_f);
206  }
207  /* set bit to indicate this representation is computed */
208  SetBit(orientation->status, ORREP_EULER_F);
209 }
#define RMAT_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:646
#define INT32_QUAT_OF_RMAT(_q, _r)
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:665
#define ORREP_EULER_F
zyx Euler (float)
#define RMAT_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:634
struct FloatRMat rmat_f
Orientation rotation matrix.
void orientationCalcQuat_i(struct OrientationReps *orientation)
void orientationCalcRMat_f(struct OrientationReps *orientation)
#define FLOAT_EULERS_OF_RMAT(_e, _rm)
#define FLOAT_QUAT_OF_EULERS(_q, _e)
uint8_t status
Holds the status bits for all orientation representations.
#define ORREP_QUAT_F
Quaternion (float)
#define INT32_QUAT_OF_EULERS(_q, _e)
#define ORREP_QUAT_I
Quaternion (BFP int)
#define ORREP_EULER_I
zyx Euler (BFP int)
struct FloatEulers eulers_f
Orienation in zyx euler angles.
struct Int32Quat quat_i
Orientation quaternion.
#define ORREP_RMAT_F
Rotation Matrix (float)
#define INT32_EULERS_OF_RMAT(_e, _rm)
void orientationCalcEulers_i(struct OrientationReps *orientation)
#define QUAT_FLOAT_OF_BFP(_qf, _qi)
Definition: pprz_algebra.h:658
struct Int32RMat rmat_i
Orientation rotation matrix.
struct FloatQuat quat_f
Orientation as quaternion.
#define INT32_RMAT_OF_QUAT(_rm, _q)
void orientationCalcQuat_f(struct OrientationReps *orientation)
#define FLOAT_EULERS_OF_QUAT(_e, _q)
#define FLOAT_QUAT_OF_RMAT(_q, _r)
#define ORREP_RMAT_I
Rotation Matrix (BFP int)
void orientationCalcRMat_i(struct OrientationReps *orientation)
#define FLOAT_RMAT_OF_QUAT(_rm, _q)
#define FLOAT_RMAT_OF_EULERS(_rm, _e)
#define EULERS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:622
#define EULERS_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:628
Generic orientation representation and conversion.
struct Int32Eulers eulers_i
Orientation in zyx euler angles.
void orientationCalcEulers_f(struct OrientationReps *orientation)
#define INT32_EULERS_OF_QUAT(_e, _q)
#define INT32_RMAT_OF_EULERS(_rm, _e)