ISF  2.2 rev 5
Intelligent Sensing Framework for Kinetis with Processor Expert
fusion_types.h
Go to the documentation of this file.
1 // Copyright (c) 2015, Freescale Semiconductor, Inc.
2 // All rights reserved.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright
7 // notice, this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of Freescale Semiconductor, Inc. nor the
12 // names of its contributors may be used to endorse or promote products
13 // derived from this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 // DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR ANY
19 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 
26 #ifndef FUSION_TYPES_H_
27 #define FUSION_TYPES_H_
28 
29 #include "matrix.h"
30 // coordinate system for the build
31 #define NED 0 // identifier for NED angle output
32 #define ANDROID 1 // identifier for Andro
33 #define WIN8 2 // identifier for Windows 8 angle output
34 
35 // useful multiplicative conversion constants
36 #define PI 3.141592654F // Pi
37 #define FPIOVER180 0.01745329251994F // degrees to radians conversion = pi / 180
38 #define F180OVERPI 57.2957795130823F // radians to degrees conversion = 180 / pi
39 #define F180OVERPISQ 3282.8063500117F // square of F180OVERPI
40 #define ONETHIRD 0.33333333F // one third
41 #define ONESIXTH 0.166666667F // one sixth
42 #define ONEOVER48 0.02083333333F // 1 / 48
43 #define ONEOVER120 0.0083333333F // 1 / 120
44 #define ONEOVER3840 0.0002604166667F // 1 / 3840
45 #define ONEOVERSQRT2 0.707106781F // 1/sqrt(2)
46 #define GTOMSEC2 9.80665 // standard gravity in m/s2
47 
48 // type definitions
49 
50 // the quaternion type to be transmitted
52 
53 
54 // structure definitions
55 
56 
57 // quaternion structure definition
59 {
60  float q0; // scalar component
61  float q1; // x vector component
62  float q2; // y vector component
63  float q3; // z vector component
64 };
65 
66 
67 // 1DOF pressure state vector structure
69 {
70  float fLPH; // low pass filtered height (m)
71  float fLPT; // low pass filtered temperature (C)
72  float fdeltat; // time interval (s)
73  float flpf; // low pass filter coefficient
74  int32 systick; // systick timer
75  int8 resetflag; // flag to request re-initialization on next pass
76 };
77 
78 // 3DOF basic accelerometer state vector structure
80 {
81  // start: elements common to all motion state vectors
82  // Euler angles
83  float fLPPhi; // low pass roll (deg)
84  float fLPThe; // low pass pitch (deg)
85  float fLPPsi; // low pass yaw (deg)
86  float fLPRho; // low pass compass (deg)
87  float fLPChi; // low pass tilt from vertical (deg)
88  // orientation matrix, quaternion and rotation vector
89  float fLPR[3][3]; // low pass filtered orientation matrix
90  struct fquaternion fLPq; // low pass filtered orientation quaternion
91  float fLPRVec[3]; // rotation vector
92  // angular velocity
93  float fOmega[3]; // angular velocity (deg/s)
94  // systick timer for benchmarking
95  int32 systick; // systick timer
96  // end: elements common to all motion state vectors
97  float fR[3][3]; // unfiltered orientation matrix
98  struct fquaternion fq; // unfiltered orientation quaternion
99  float fdeltat; // time interval (s)
100  float flpf; // low pass filter coefficient
101  int8 resetflag; // flag to request re-initialization on next pass
102 };
103 
104 // 3DOF basic magnetometer state vector structure
106 {
107  // start: elements common to all motion state vectors
108  // Euler angles
109  float fLPPhi; // low pass roll (deg)
110  float fLPThe; // low pass pitch (deg)
111  float fLPPsi; // low pass yaw (deg)
112  float fLPRho; // low pass compass (deg)
113  float fLPChi; // low pass tilt from vertical (deg)
114  // orientation matrix, quaternion and rotation vector
115  float fLPR[3][3]; // low pass filtered orientation matrix
116  struct fquaternion fLPq; // low pass filtered orientation quaternion
117  float fLPRVec[3]; // rotation vector
118  // angular velocity
119  float fOmega[3]; // angular velocity (deg/s)
120  // systick timer for benchmarking
121  int32 systick; // systick timer
122  // end: elements common to all motion state vectors
123  float fR[3][3]; // unfiltered orientation matrix
124  struct fquaternion fq; // unfiltered orientation quaternion
125  float fdeltat; // time interval (s)
126  float flpf; // low pass filter coefficient
127  int8 resetflag; // flag to request re-initialization on next pass
128 };
129 
130 // 3DOF basic gyroscope state vector structure
132 {
133  // start: elements common to all motion state vectors
134  // Euler angles
135  float fPhi; // roll (deg)
136  float fThe; // pitch (deg)
137  float fPsi; // yaw (deg)
138  float fRho; // compass (deg)
139  float fChi; // tilt from vertical (deg)
140  // orientation matrices and quaternions
141  float fR[3][3]; // unfiltered orientation matrix
142  struct fquaternion fq; // unfiltered orientation quaternion
143  float fRVec[3]; // rotation vector
144  // angular velocity
145  float fOmega[3]; // angular velocity (deg/s)
146  // systick timer for benchmarking
147  int32 systick; // systick timer
148  // end: elements common to all motion state vectors
149  float fGyrodeltat; // gyro sensor sampling interval (s) = 1 / SENSORFS
150  float fdeltat; // kalman filter sampling interval (s) = OVERSAMPLE_RATIO / SENSORFS
151  int8 resetflag; // flag to request re-initialization on next pass
152 };
153 
154 // 6DOF basic accelerometer and magnetometer state vector structure
156 {
157  // start: elements common to all motion state vectors
158  // Euler angles
159  float fLPPhi; // low pass roll (deg)
160  float fLPThe; // low pass pitch (deg)
161  float fLPPsi; // low pass yaw (deg)
162  float fLPRho; // low pass compass (deg)
163  float fLPChi; // low pass tilt from vertical (deg)
164  // orientation matrix, quaternion and rotation vector
165  float fLPR[3][3]; // low pass filtered orientation matrix
166  struct fquaternion fLPq; // low pass filtered orientation quaternion
167  float fLPRVec[3]; // rotation vector
168  // angular velocity
169  float fOmega[3]; // virtual gyro angular velocity (deg/s)
170  // systick timer for benchmarking
171  int32 systick; // systick timer
172  // end: elements common to all motion state vectors
173  float fR[3][3]; // unfiltered orientation matrix
174  struct fquaternion fq; // unfiltered orientation quaternion
175  float fDelta; // unfiltered inclination angle (deg)
176  float fLPDelta; // low pass filtered inclination angle (deg)
177  float fdeltat; // time interval (s)
178  float flpf; // low pass filter coefficient
179  int8 resetflag; // flag to request re-initialization on next pass
180 };
181 
182 // 6DOF Kalman filter accelerometer and gyroscope state vector structure
184 {
185  // start: elements common to all motion state vectors
186  // Euler angles
187  float fPhiPl; // roll (deg)
188  float fThePl; // pitch (deg)
189  float fPsiPl; // yaw (deg)
190  float fRhoPl; // compass (deg)
191  float fChiPl; // tilt from vertical (deg)
192  // orientation matrix, quaternion and rotation vector
193  float fRPl[3][3]; // a posteriori orientation matrix
194  struct fquaternion fqPl; // a posteriori orientation quaternion
195  float fRVecPl[3]; // rotation vector
196  float fOmega[3]; // average angular velocity (deg/s)
197  int32 systick; // systick timer;
198  // end: elements common to all motion state vectors
199  float fQw6x6[6][6]; // covariance matrix Qw
200  float fK6x3[6][3]; // kalman filter gain matrix K
201  float fQwCT6x3[6][3]; // Qw.C^T matrix
202  float fQv; // measurement noise covariance matrix leading diagonal
203  float fZErr[3]; // measurement error vector
204  float fqgErrPl[3]; // gravity vector tilt orientation quaternion error (dimensionless)
205  float fbPl[3]; // gyro offset (deg/s)
206  float fbErrPl[3]; // gyro offset error (deg/s)
207  float fAccGl[3]; // linear acceleration (g) in global frame
208  float fGyrodeltat; // gyro sampling interval (s) = 1 / SENSORFS
209  float fAlphaOver2; // PI/360 * fKalmandeltat
210  float fAlphaOver2Sq; // (PI/360 * fKalmandeltat)^2
211  float fAlphaOver2SqQvYQwb; // (PI/360 * fKalmandeltat)^2 * (QvY + Qwb)
212  float fAlphaOver2Qwb; // PI/360 * fKalmandeltat * FQWB_9DOF_GBY_KALMAN;
213  int16 iFirstOrientationLock; // denotes that 6DOF orientation has locked to 3DOF
214  int8 resetflag; // flag to request re-initialization on next pass
215 };
216 
217 // 9DOF Kalman filter accelerometer, magnetometer and gyroscope state vector structure
219 {
220  // start: elements common to all motion state vectors
221  // Euler angles
222  float fPhiPl; // roll (deg)
223  float fThePl; // pitch (deg)
224  float fPsiPl; // yaw (deg)
225  float fRhoPl; // compass (deg)
226  float fChiPl; // tilt from vertical (deg)
227  // orientation matrix, quaternion and rotation vector
228  float fRPl[3][3]; // a posteriori orientation matrix
229  struct fquaternion fqPl; // a posteriori orientation quaternion
230  float fRVecPl[3]; // rotation vector
231  // angular velocity
232  float fOmega[3]; // average angular velocity (deg/s)
233  // systick timer for benchmarking
234  int32 systick; // systick timer;
235  // end: elements common to all motion state vectors
236  float fQw10x10[10][10]; // covariance matrix Qw
237  float fK10x7[10][7]; // kalman filter gain matrix K
238  float fQwCT10x7[10][7]; // Qw.C^T matrix
239  float fZErr[7]; // measurement error vector
240  float fQv7x1[7]; // measurement noise covariance matrix leading diagonal
241  float fDeltaPl; // a posteriori inclination angle from Kalman filter (deg)
242  float fsinDeltaPl; // sin(fDeltaPl)
243  float fcosDeltaPl; // cos(fDeltaPl)
244  float fqgErrPl[3]; // gravity vector tilt orientation quaternion error (dimensionless)
245  float fqmErrPl[3]; // geomagnetic vector tilt orientation quaternion error (dimensionless)
246  float fbPl[3]; // gyro offset (deg/s)
247  float fbErrPl[3]; // gyro offset error (deg/s)
248  float fDeltaErrPl; // a priori inclination angle error correction
249  float fAccGl[3]; // linear acceleration (g) in global frame
250  float fVelGl[3]; // velocity (m/s) in global frame
251  float fDisGl[3]; // displacement (m) in global frame
252  float fGyrodeltat; // gyro sampling interval (s) = 1 / SENSORFS
253  float fKalmandeltat; // Kalman filter interval (s) = OVERSAMPLE_RATIO / SENSORFS
254  float fgKalmandeltat; // g (m/s2) * Kalman filter interval (s) = 9.80665 * OVERSAMPLE_RATIO / SENSORFS
255  float fAlphaOver2; // PI/360 * fKalmandeltat
256  float fAlphaOver2Sq; // (PI/360 * fKalmandeltat)^2
257  float fAlphaOver2SqQvYQwb; // (PI/360 * fKalmandeltat)^2 * (QvY + Qwb)
258  float fAlphaOver2Qwb; // PI/360 * fKalmandeltat * FQWB_9DOF_GBY_KALMAN;
259  int8 iFirstAccelMagLock; // denotes that 9DOF orientation has locked to 6DOF eCompass
260  int16 iFirstOrientationLock; // denotes that 9DOF orientation has locked to 6DOF
261  int8 resetflag; // flag to request re-initialization on next pass
262 };
263 
264 typedef struct {
265  uint32 outputDataRate; /* time in usecs between orientation output samples */
266  uint8 coordinateSystem; /* use CoordinateSystem_t enums */
267  uint8 accelOversampleRatio; /* number of accel samples to take per orientation output */
268  uint8 magOversampleRatio; /* number of mag samples to take per orientation output */
269  uint8 gyroOversampleRatio; /* number of gyro samples to take per orientation output */
270  uint8 pressureOversampleRatio; /* number of gyro samples to take per orientation output */
272 
273 #endif /* FUSION_TYPES_H_ */
int16 iFirstOrientationLock
Definition: fusion_types.h:213
float fLPRVec[3]
Definition: fusion_types.h:117
unsigned char uint8
Definition: isf_types.h:76
struct fquaternion fLPq
Definition: fusion_types.h:90
float fQw6x6[6][6]
Definition: fusion_types.h:199
float fR[3][3]
Definition: fusion_types.h:173
float fK6x3[6][3]
Definition: fusion_types.h:200
quaternion
Definition: fusion_types.h:51
float fRPl[3][3]
Definition: fusion_types.h:193
float fRPl[3][3]
Definition: fusion_types.h:228
struct fquaternion fqPl
Definition: fusion_types.h:229
float fLPRVec[3]
Definition: fusion_types.h:91
struct fquaternion fq
Definition: fusion_types.h:124
struct fquaternion fq
Definition: fusion_types.h:98
struct fquaternion fLPq
Definition: fusion_types.h:116
float fQwCT6x3[6][3]
Definition: fusion_types.h:201
float fQwCT10x7[10][7]
Definition: fusion_types.h:238
enum quaternion quaternion_type
float fK10x7[10][7]
Definition: fusion_types.h:237
float fLPR[3][3]
Definition: fusion_types.h:115
struct fquaternion fqPl
Definition: fusion_types.h:194
float fR[3][3]
Definition: fusion_types.h:141
float fR[3][3]
Definition: fusion_types.h:97
float fLPR[3][3]
Definition: fusion_types.h:165
signed short int int16
Definition: isf_types.h:73
float fR[3][3]
Definition: fusion_types.h:123
struct fquaternion fq
Definition: fusion_types.h:174
float fLPR[3][3]
Definition: fusion_types.h:89
struct fquaternion fq
Definition: fusion_types.h:142
float fOmega[3]
Definition: fusion_types.h:93
signed long int int32
Definition: isf_types.h:74
unsigned long int uint32
Definition: isf_types.h:78
struct fquaternion fLPq
Definition: fusion_types.h:166
float fQw10x10[10][10]
Definition: fusion_types.h:236
signed char int8
Definition: isf_types.h:72