-
Notifications
You must be signed in to change notification settings - Fork 51
/
Copy pathNXPMotionSense.h
188 lines (176 loc) · 7.63 KB
/
NXPMotionSense.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
#ifndef _NXP_Motion_Sensors_
#define _NXP_Motion_Sensors_
#include <Arduino.h>
#include <Wire.h>
#include <EEPROM.h>
// TODO: move these inside class namespace
#define G_PER_COUNT 0.0001220703125f // = 1/8192
#define DEG_PER_SEC_PER_COUNT 0.0625f // = 1/16
#define UT_PER_COUNT 0.1f
class NXPMotionSense {
public:
bool begin();
bool available() {
update();
if (newdata) return true;
return false;
}
void readMotionSensor(int& ax, int& ay, int& az, int& gx, int& gy, int& gz) {
if (!newdata) update();
newdata = 0;
ax = accel_mag_raw[0];
ay = accel_mag_raw[1];
az = accel_mag_raw[2];
gx = gyro_raw[0];
gy = gyro_raw[1];
gz = gyro_raw[2];
}
void readMotionSensor(int& ax, int& ay, int& az, int& gx, int& gy, int& gz, int& mx, int& my, int& mz) {
if (!newdata) update();
newdata = 0;
ax = accel_mag_raw[0];
ay = accel_mag_raw[1];
az = accel_mag_raw[2];
mx = accel_mag_raw[3];
my = accel_mag_raw[4];
mz = accel_mag_raw[5];
gx = gyro_raw[0];
gy = gyro_raw[1];
gz = gyro_raw[2];
}
void readMotionSensor(float& ax, float& ay, float& az, float& gx, float& gy, float& gz) {
if (!newdata) update();
newdata = 0;
ax = (float)accel_mag_raw[0] * G_PER_COUNT;
ay = (float)accel_mag_raw[1] * G_PER_COUNT;
az = (float)accel_mag_raw[2] * G_PER_COUNT;
gx = (float)gyro_raw[0] * DEG_PER_SEC_PER_COUNT;
gy = (float)gyro_raw[1] * DEG_PER_SEC_PER_COUNT;
gz = (float)gyro_raw[2] * DEG_PER_SEC_PER_COUNT;
}
void readMotionSensor(float& ax, float& ay, float& az, float& gx, float& gy, float& gz, float& mx, float& my, float& mz) {
if (!newdata) update();
newdata = 0;
ax = (float)accel_mag_raw[0] * G_PER_COUNT - cal[0];
ay = (float)accel_mag_raw[1] * G_PER_COUNT - cal[1];
az = (float)accel_mag_raw[2] * G_PER_COUNT - cal[2];
gx = (float)gyro_raw[0] * DEG_PER_SEC_PER_COUNT - cal[3];
gy = (float)gyro_raw[1] * DEG_PER_SEC_PER_COUNT - cal[4];
gz = (float)gyro_raw[2] * DEG_PER_SEC_PER_COUNT - cal[5];
float x = (float)accel_mag_raw[3] * UT_PER_COUNT - cal[6];
float y = (float)accel_mag_raw[4] * UT_PER_COUNT - cal[7];
float z = (float)accel_mag_raw[5] * UT_PER_COUNT - cal[8];
mx = x * cal[10] + y * cal[13] + z * cal[14];
my = x * cal[13] + y * cal[11] + z * cal[15];
mz = x * cal[14] + y * cal[15] + z * cal[12];
}
bool writeCalibration(const void *data);
void getCalibration(float *offsets, float *softiron=NULL, float *fieldstrength=NULL) {
if (offsets != NULL) {
memcpy(offsets, cal, 36);
}
if (softiron != NULL) {
*softiron++ = cal[10];
*softiron++ = cal[13];
*softiron++ = cal[14];
*softiron++ = cal[13];
*softiron++ = cal[11];
*softiron++ = cal[15];
*softiron++ = cal[14];
*softiron++ = cal[15];
*softiron++ = cal[12];
}
if (fieldstrength != NULL) *fieldstrength = cal[9];
}
private:
void update();
bool FXOS8700_begin();
bool FXAS21002_begin();
bool MPL3115_begin();
bool FXOS8700_read(int16_t *data);
bool FXAS21002_read(int16_t *data);
bool MPL3115_read(int32_t *altitude, int16_t *temperature);
float cal[16]; // 0-8=offsets, 9=field strength, 10-15=soft iron map
int16_t accel_mag_raw[6];
int16_t gyro_raw[3];
int16_t temperature_raw;
uint8_t newdata;
};
class NXPSensorFusion {
public:
void begin(float sampleRate=100.0f);
void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
// TODO: these aren't the same ranges as Madgwick & Mahony... or are they??
float getRoll() { return PhiPl; }
float getPitch() { return ThePl; }
float getYaw() { return PsiPl; }
typedef struct {
float q0; // w
float q1; // x
float q2; // y
float q3; // z
} Quaternion_t;
void getQuaternion(float quat[4]) {
quat[0] = qPl.q0;
quat[1] = qPl.q1;
quat[2] = qPl.q2;
quat[3] = qPl.q3;
}
// These are Madgwick & Mahony - extrinsic rotation reference (wrong!)
//float getPitch() {return atan2f(2.0f * qPl.q2 * qPl.q3 - 2.0f * qPl.q0 * qPl.q1, 2.0f * qPl.q0 * qPl.q0 + 2.0f * qPl.q3 * qPl.q3 - 1.0f);};
//float getRoll() {return -1.0f * asinf(2.0f * qPl.q1 * qPl.q3 + 2.0f * qPl.q0 * qPl.q2);};
//float getYaw() {return atan2f(2.0f * qPl.q1 * qPl.q2 - 2.0f * qPl.q0 * qPl.q3, 2.0f * qPl.q0 * qPl.q0 + 2.0f * qPl.q1 * qPl.q1 - 1.0f);};
private:
float PhiPl; // roll (deg)
float ThePl; // pitch (deg)
float PsiPl; // yaw (deg)
float RhoPl; // compass (deg)
float ChiPl; // tilt from vertical (deg)
// orientation matrix, quaternion and rotation vector
float RPl[3][3]; // a posteriori orientation matrix
Quaternion_t qPl; // a posteriori orientation quaternion
float RVecPl[3]; // rotation vector
// angular velocity
float Omega[3]; // angular velocity (deg/s)
// systick timer for benchmarking
int32_t systick; // systick timer;
// end: elements common to all motion state vectors
// elements transmitted over bluetooth in kalman packet
float bPl[3]; // gyro offset (deg/s)
float ThErrPl[3]; // orientation error (deg)
float bErrPl[3]; // gyro offset error (deg/s)
// end elements transmitted in kalman packet
float dErrGlPl[3]; // magnetic disturbance error (uT, global frame)
float dErrSePl[3]; // magnetic disturbance error (uT, sensor frame)
float aErrSePl[3]; // linear acceleration error (g, sensor frame)
float aSeMi[3]; // linear acceleration (g, sensor frame)
float DeltaPl; // inclination angle (deg)
float aSePl[3]; // linear acceleration (g, sensor frame)
float aGlPl[3]; // linear acceleration (g, global frame)
float gErrSeMi[3]; // difference (g, sensor frame) of gravity vector (accel) and gravity vector (gyro)
float mErrSeMi[3]; // difference (uT, sensor frame) of geomagnetic vector (magnetometer) and geomagnetic vector (gyro)
float gSeGyMi[3]; // gravity vector (g, sensor frame) measurement from gyro
float mSeGyMi[3]; // geomagnetic vector (uT, sensor frame) measurement from gyro
float mGl[3]; // geomagnetic vector (uT, global frame)
float QvAA; // accelerometer terms of Qv
float QvMM; // magnetometer terms of Qv
float PPlus12x12[12][12]; // covariance matrix P+
float K12x6[12][6]; // kalman filter gain matrix K
float Qw12x12[12][12]; // covariance matrix Qw
float C6x12[6][12]; // measurement matrix C
float RMi[3][3]; // a priori orientation matrix
Quaternion_t Deltaq; // delta quaternion
Quaternion_t qMi; // a priori orientation quaternion
float casq; // FCA * FCA;
float cdsq; // FCD * FCD;
float Fastdeltat; // sensor sampling interval (s) = 1 / SENSORFS
float deltat; // kalman filter sampling interval (s) = OVERSAMPLE_RATIO / SENSORFS
float deltatsq; // fdeltat * fdeltat
float QwbplusQvG; // FQWB + FQVG
int8_t FirstOrientationLock; // denotes that 9DOF orientation has locked to 6DOF
int8_t resetflag; // flag to request re-initialization on next pass
};
#undef G_PER_COUNT
#undef DEG_PER_SEC_PER_COUNT
#undef UT_PER_COUNT
#endif