Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
40 
41 
42 /******************************************************************************
43  * *
44  * Transformation functions for the ORIENTATION representations *
45  * *
46  *****************************************************************************/
47 
48 void orientationCalcQuat_i(struct OrientationReps *orientation)
49 {
50  if (bit_is_set(orientation->status, ORREP_QUAT_I)) {
51  return;
52  }
53 
54  if (bit_is_set(orientation->status, ORREP_QUAT_F)) {
55  QUAT_BFP_OF_REAL(orientation->quat_i, orientation->quat_f);
56  } else if (bit_is_set(orientation->status, ORREP_RMAT_I)) {
57  int32_quat_of_rmat(&(orientation->quat_i), &(orientation->rmat_i));
58  } else if (bit_is_set(orientation->status, ORREP_EULER_I)) {
59  int32_quat_of_eulers(&(orientation->quat_i), &(orientation->eulers_i));
60  } else if (bit_is_set(orientation->status, ORREP_RMAT_F)) {
61  RMAT_BFP_OF_REAL(orientation->rmat_i, orientation->rmat_f);
62  SetBit(orientation->status, ORREP_RMAT_I);
63  int32_quat_of_rmat(&(orientation->quat_i), &(orientation->rmat_i));
64  } else if (bit_is_set(orientation->status, ORREP_EULER_F)) {
65  EULERS_BFP_OF_REAL(orientation->eulers_i, orientation->eulers_f);
66  SetBit(orientation->status, ORREP_EULER_I);
67  int32_quat_of_eulers(&(orientation->quat_i), &(orientation->eulers_i));
68  }
69  /* set bit to indicate this representation is computed */
70  SetBit(orientation->status, ORREP_QUAT_I);
71 }
72 
73 void orientationCalcRMat_i(struct OrientationReps *orientation)
74 {
75  if (bit_is_set(orientation->status, ORREP_RMAT_I)) {
76  return;
77  }
78 
79  if (bit_is_set(orientation->status, ORREP_RMAT_F)) {
80  RMAT_BFP_OF_REAL(orientation->rmat_i, orientation->rmat_f);
81  } else if (bit_is_set(orientation->status, ORREP_QUAT_I)) {
82  int32_rmat_of_quat(&(orientation->rmat_i), &(orientation->quat_i));
83  } else if (bit_is_set(orientation->status, ORREP_EULER_I)) {
84  int32_rmat_of_eulers(&(orientation->rmat_i), &(orientation->eulers_i));
85  } else if (bit_is_set(orientation->status, ORREP_QUAT_F)) {
86  QUAT_BFP_OF_REAL(orientation->quat_i, orientation->quat_f);
87  SetBit(orientation->status, ORREP_QUAT_I);
88  int32_rmat_of_quat(&(orientation->rmat_i), &(orientation->quat_i));
89  } else if (bit_is_set(orientation->status, ORREP_EULER_F)) {
90  EULERS_BFP_OF_REAL(orientation->eulers_i, orientation->eulers_f);
91  SetBit(orientation->status, ORREP_EULER_I);
92  int32_rmat_of_eulers(&(orientation->rmat_i), &(orientation->eulers_i));
93  }
94  /* set bit to indicate this representation is computed */
95  SetBit(orientation->status, ORREP_RMAT_I);
96 }
97 
98 void orientationCalcEulers_i(struct OrientationReps *orientation)
99 {
100  if (bit_is_set(orientation->status, ORREP_EULER_I)) {
101  return;
102  }
103 
104  if (bit_is_set(orientation->status, ORREP_EULER_F)) {
105  EULERS_BFP_OF_REAL(orientation->eulers_i, orientation->eulers_f);
106  } else if (bit_is_set(orientation->status, ORREP_RMAT_I)) {
107  int32_eulers_of_rmat(&(orientation->eulers_i), &(orientation->rmat_i));
108  } else if (bit_is_set(orientation->status, ORREP_QUAT_I)) {
109  int32_eulers_of_quat(&(orientation->eulers_i), &(orientation->quat_i));
110  } else if (bit_is_set(orientation->status, ORREP_RMAT_F)) {
111  RMAT_BFP_OF_REAL(orientation->rmat_i, orientation->rmat_f);
112  SetBit(orientation->status, ORREP_RMAT_I);
113  int32_eulers_of_rmat(&(orientation->eulers_i), &(orientation->rmat_i));
114  } else if (bit_is_set(orientation->status, ORREP_QUAT_F)) {
115  QUAT_BFP_OF_REAL(orientation->quat_i, orientation->quat_f);
116  SetBit(orientation->status, ORREP_QUAT_I);
117  int32_eulers_of_quat(&(orientation->eulers_i), &(orientation->quat_i));
118  }
119  /* set bit to indicate this representation is computed */
120  SetBit(orientation->status, ORREP_EULER_I);
121 }
122 
123 void orientationCalcQuat_f(struct OrientationReps *orientation)
124 {
125  if (bit_is_set(orientation->status, ORREP_QUAT_F)) {
126  return;
127  }
128 
129  if (bit_is_set(orientation->status, ORREP_QUAT_I)) {
130  QUAT_FLOAT_OF_BFP(orientation->quat_f, orientation->quat_i);
131  } else if (bit_is_set(orientation->status, ORREP_RMAT_F)) {
132  float_quat_of_rmat(&(orientation->quat_f), &(orientation->rmat_f));
133  } else if (bit_is_set(orientation->status, ORREP_EULER_F)) {
134  float_quat_of_eulers(&(orientation->quat_f), &(orientation->eulers_f));
135  } else if (bit_is_set(orientation->status, ORREP_RMAT_I)) {
136  RMAT_FLOAT_OF_BFP(orientation->rmat_f, orientation->rmat_i);
137  SetBit(orientation->status, ORREP_RMAT_F);
138  float_quat_of_rmat(&(orientation->quat_f), &(orientation->rmat_f));
139  } else if (bit_is_set(orientation->status, ORREP_EULER_I)) {
140  EULERS_FLOAT_OF_BFP(orientation->eulers_f, orientation->eulers_i);
141  SetBit(orientation->status, ORREP_EULER_F);
142  float_quat_of_eulers(&(orientation->quat_f), &(orientation->eulers_f));
143  }
144  /* set bit to indicate this representation is computed */
145  SetBit(orientation->status, ORREP_QUAT_F);
146 }
147 
148 void orientationCalcRMat_f(struct OrientationReps *orientation)
149 {
150  if (bit_is_set(orientation->status, ORREP_RMAT_F)) {
151  return;
152  }
153 
154  if (bit_is_set(orientation->status, ORREP_RMAT_I)) {
155  RMAT_FLOAT_OF_BFP(orientation->rmat_f, orientation->rmat_i);
156  } else if (bit_is_set(orientation->status, ORREP_QUAT_F)) {
157  float_rmat_of_quat(&(orientation->rmat_f), &(orientation->quat_f));
158  } else if (bit_is_set(orientation->status, ORREP_EULER_F)) {
159  float_rmat_of_eulers(&(orientation->rmat_f), &(orientation->eulers_f));
160  } else if (bit_is_set(orientation->status, ORREP_QUAT_I)) {
161  QUAT_FLOAT_OF_BFP(orientation->quat_f, orientation->quat_i);
162  SetBit(orientation->status, ORREP_QUAT_F);
163  float_rmat_of_quat(&(orientation->rmat_f), &(orientation->quat_f));
164  } else if (bit_is_set(orientation->status, ORREP_EULER_I)) {
165  EULERS_FLOAT_OF_BFP(orientation->eulers_f, orientation->eulers_i);
166  SetBit(orientation->status, ORREP_EULER_F);
167  float_rmat_of_eulers(&(orientation->rmat_f), &(orientation->eulers_f));
168  }
169  /* set bit to indicate this representation is computed */
170  SetBit(orientation->status, ORREP_RMAT_F);
171 }
172 
173 void orientationCalcEulers_f(struct OrientationReps *orientation)
174 {
175  if (bit_is_set(orientation->status, ORREP_EULER_F)) {
176  return;
177  }
178 
179  if (bit_is_set(orientation->status, ORREP_EULER_I)) {
180  EULERS_FLOAT_OF_BFP(orientation->eulers_f, orientation->eulers_i);
181  } else if (bit_is_set(orientation->status, ORREP_RMAT_F)) {
182  float_eulers_of_rmat(&(orientation->eulers_f), &(orientation->rmat_f));
183  } else if (bit_is_set(orientation->status, ORREP_QUAT_F)) {
184  float_eulers_of_quat(&(orientation->eulers_f), &(orientation->quat_f));
185  } else if (bit_is_set(orientation->status, ORREP_RMAT_I)) {
186  RMAT_FLOAT_OF_BFP(orientation->rmat_f, orientation->rmat_i);
187  SetBit(orientation->status, ORREP_RMAT_F);
188  float_eulers_of_rmat(&(orientation->eulers_f), &(orientation->rmat_f));
189  } else if (bit_is_set(orientation->status, ORREP_QUAT_I)) {
190  QUAT_FLOAT_OF_BFP(orientation->quat_f, orientation->quat_i);
191  SetBit(orientation->status, ORREP_QUAT_F);
192  float_eulers_of_quat(&(orientation->eulers_f), &(orientation->quat_f));
193  }
194  /* set bit to indicate this representation is computed */
195  SetBit(orientation->status, ORREP_EULER_F);
196 }
int32_quat_of_eulers
void int32_quat_of_eulers(struct Int32Quat *q, struct Int32Eulers *e)
Quaternion from Euler angles.
Definition: pprz_algebra_int.c:375
OrientationReps
Definition: pprz_orientation_conversion.h:79
float_rmat_of_eulers
#define float_rmat_of_eulers
Definition: pprz_algebra_float.h:333
ORREP_RMAT_I
#define ORREP_RMAT_I
Rotation Matrix (BFP int)
Definition: pprz_orientation_conversion.h:71
float_eulers_of_rmat
void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm)
Definition: pprz_algebra_float.c:628
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
int32_eulers_of_rmat
void int32_eulers_of_rmat(struct Int32Eulers *e, struct Int32RMat *rm)
Definition: pprz_algebra_int.c:504
RMAT_BFP_OF_REAL
#define RMAT_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:721
OrientationReps::quat_i
struct Int32Quat quat_i
Orientation quaternion.
Definition: pprz_orientation_conversion.h:91
orientationCalcRMat_i
void orientationCalcRMat_i(struct OrientationReps *orientation)
Definition: pprz_orientation_conversion.c:73
orientationCalcEulers_i
void orientationCalcEulers_i(struct OrientationReps *orientation)
Definition: pprz_orientation_conversion.c:98
int32_eulers_of_quat
void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q)
Definition: pprz_algebra_int.c:523
int32_quat_of_rmat
void int32_quat_of_rmat(struct Int32Quat *q, struct Int32RMat *r)
Quaternion from rotation matrix.
Definition: pprz_algebra_int.c:421
OrientationReps::eulers_f
struct FloatEulers eulers_f
Orienation in zyx euler angles.
Definition: pprz_orientation_conversion.h:115
OrientationReps::quat_f
struct FloatQuat quat_f
Orientation as quaternion.
Definition: pprz_orientation_conversion.h:109
orientationCalcQuat_f
void orientationCalcQuat_f(struct OrientationReps *orientation)
Definition: pprz_orientation_conversion.c:123
orientationCalcRMat_f
void orientationCalcRMat_f(struct OrientationReps *orientation)
Definition: pprz_orientation_conversion.c:148
ORREP_QUAT_I
#define ORREP_QUAT_I
Quaternion (BFP int)
Definition: pprz_orientation_conversion.h:69
float_rmat_of_quat
void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q)
Definition: pprz_algebra_float.c:236
RMAT_FLOAT_OF_BFP
#define RMAT_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:733
float_quat_of_rmat
void float_quat_of_rmat(struct FloatQuat *q, struct FloatRMat *rm)
Quaternion from rotation matrix.
Definition: pprz_algebra_float.c:577
QUAT_BFP_OF_REAL
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:752
EULERS_FLOAT_OF_BFP
#define EULERS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:709
int32_rmat_of_eulers
#define int32_rmat_of_eulers
Rotation matrix from Euler angles.
Definition: pprz_algebra_int.h:408
orientationCalcQuat_i
void orientationCalcQuat_i(struct OrientationReps *orientation)
Definition: pprz_orientation_conversion.c:48
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
ORREP_EULER_F
#define ORREP_EULER_F
zyx Euler (float)
Definition: pprz_orientation_conversion.h:73
pprz_orientation_conversion.h
ORREP_RMAT_F
#define ORREP_RMAT_F
Rotation Matrix (float)
Definition: pprz_orientation_conversion.h:74
int32_rmat_of_quat
void int32_rmat_of_quat(struct Int32RMat *rm, struct Int32Quat *q)
Convert unit quaternion to rotation matrix.
Definition: pprz_algebra_int.c:148
QUAT_FLOAT_OF_BFP
#define QUAT_FLOAT_OF_BFP(_qf, _qi)
Definition: pprz_algebra.h:745
ORREP_EULER_I
#define ORREP_EULER_I
zyx Euler (BFP int)
Definition: pprz_orientation_conversion.h:70
float_eulers_of_quat
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
Definition: pprz_algebra_float.c:650
EULERS_BFP_OF_REAL
#define EULERS_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:715
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
float_quat_of_eulers
void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e)
quat of euler roation 'ZYX'
Definition: pprz_algebra_float.c:477