-
Notifications
You must be signed in to change notification settings - Fork 3
/
orientation_and_position_v0.2_GITHUB.ino
286 lines (214 loc) · 8.72 KB
/
orientation_and_position_v0.2_GITHUB.ino
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
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
//Special thanks to the guy of https://www.thepoorengineer.com/en/quaternion/
//ALL ANGLES IN RADIANS
#include <Wire.h>
#include <BasicLinearAlgebra.h> //will be ported to Eigen in the future
using namespace BLA;
//CALIBRATION
double gyro_cal[]={0,0,0}; //Fill with your calibration values and set calflag to false
double acc_cal[]={0,0,0};
bool calflag=true;
long accelX, accelY, accelZ;
double acc[]={0,0,0}, acc_total_vector;
long gyroY,gyroX,gyroZ;
double gyro[]={0,0,0};
double orientation[]={0,0,0}; //ROLL, PITCH, YAW GLOBAL
double orientation_local[]={0,0,0}; //ROLL, PITCH, YAW LOCAL
//QUATERNIONS
double c11, c12, c13, c21, c22, c23, c31, c32, c33;
BLA::Matrix<4,4> Sw = {0, 0, 0, 0,
0, 0, 0, 0,
0, 0, 0, 0,
0, 0, 0, 0};
BLA::Matrix<4,3> Sq = {0, 0, 0,
0, 0, 0,
0, 0, 0,
0, 0, 0};
BLA::Matrix<3,1> w = {0,
0,
0};
BLA::Matrix<3,1> orientation_x = {0,
0,
0};
BLA::Matrix<3,1> orientation_y = {0,
0,
0};
BLA::Matrix<3,1> orientation_z = {0,
0,
0};
BLA::Matrix<3,1> unit_x = {1,
0,
0};
BLA::Matrix<3,1> unit_y = {0,
1,
0};
BLA::Matrix<3,1> unit_z = {0,
0,
1};
BLA::Matrix<4,1> q = {1,
0,
0,
0};
BLA::Matrix<4,1> q_local = {1,
0,
0,
0};
BLA::Matrix<3,3> C = {0, 0, 0,
0, 0, 0,
0, 0, 0};
double dot;
//TIMES
double T=0.01,dt; //T is the sample time of your program, change it accordingly
unsigned long currentTime,previousTime;
unsigned long timer_run;
void setup() {
Serial.begin(250000);
Wire.begin();
setupMPU();
Serial.print("Gyro calibration");
recordAccelRegisters();
recordGyroRegisters();
if(calflag==true){
for (int cal_int = 0; cal_int < 1000 ; cal_int ++){ //Run this code 1000 times
recordGyroRegisters();
gyro_cal[0] += gyroX; //Add the gyro x-axis offset to the gyro_x_cal variable
gyro_cal[1] += gyroY; //Add the gyro y-axis offset to the gyro_y_cal variable
gyro_cal[2] += gyroZ; //Add the gyro z-axis offset to the gyro_z_cal variable
delay(3); //Delay 3us to simulate the 250Hz program loop
}
gyro_cal[0] /= (1000); //65.5 RAW TO DEG
gyro_cal[1] /= (1000); //Divide the gyro_y_cal variable by 1000 to get the avarage offset
gyro_cal[2] /= (1000); }
}
void loop() {
if(micros()>=timer_run+T*1000000UL){ // T is going to be replaced by dt in the future
dt=(micros()-previousTime)/1000000UL;
previousTime=micros();
timer_run=micros();
recordAccelRegisters();
recordGyroRegisters();
//printData();
calculate_orientation();
printData2();
}}
void setupMPU(){
Wire.beginTransmission(0b1101000); //This is the I2C address of the MPU (b1101000/b1101001 for AC0 low/high datasheet sec. 9.2)
Wire.write(0x6B); //Accessing the register 6B - Power Management (Sec. 4.28)
Wire.write(0b00000000); //Setting SLEEP register to 0. (Required; see Note on p. 9)
Wire.endTransmission();
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x1B); //Accessing the register 1B - Gyroscope Configuration (Sec. 4.4)
Wire.write(0b00011000); //Setting the gyro to full scale +/- 2000deg./s
Wire.endTransmission();
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x1C); //Accessing the register 1C - Acccelerometer Configuration (Sec. 4.5)
Wire.write(0b00011000); //Setting the accel to +/- 16g
Wire.endTransmission();
}
void recordAccelRegisters() {
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x3B); //Starting register for Accel Readings
Wire.endTransmission();
Wire.requestFrom(0b1101000,6); //Request Accel Registers (3B - 40)
while(Wire.available() < 6);
accelX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
accelY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
accelZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
processAccelData();
}
void processAccelData(){
acc[1] = (accelX / 2048.0);
acc[0] = (accelY / 2048.0);
acc[2] = (-accelZ / 2048.0)-0.07;
}
void recordGyroRegisters() {
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x43); //Starting register for Gyro Readings
Wire.endTransmission();
Wire.requestFrom(0b1101000,6); //Request Gyro Registers (43 - 48)
while(Wire.available() < 6);
gyroX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
gyroY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
gyroZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
processGyroData();
}
void processGyroData() {
gyro[1] = ((gyroX-gyro_cal[0]) / (16.375*57.295));
gyro[0]= (gyroY-gyro_cal[1]) / (16.375*57.295);
gyro[2]= -(gyroZ-gyro_cal[2]) / (16.375*57.295);
}
void printData() {
Serial.print("Gyro (deg)");
Serial.print(" X=");
Serial.print(gyro[0]*57.3,6);
Serial.print(" Y=");
Serial.print(gyro[1]*57.3,6);
Serial.print(" Z=");
Serial.print(gyro[2]*57.3,6);
Serial.print(" Accel (g)");
Serial.print(" X=");
Serial.print(acc[0],6);
Serial.print(" Y=");
Serial.print(acc[1],6);
Serial.print(" Z=");
Serial.println(acc[2],6);
}
void printData2() {
// Serial.print(orientation[0]*57.3,6);
// Serial.print(" , ");
// Serial.print(orientation[1]*57.3,6);
// Serial.print(" , ");
// Serial.println(orientation[2]*57.3,6);
Serial.print(orientation[0]*57.3);
Serial.print(" , ");
Serial.print(orientation_local[1]*57.3);
Serial.print(" , ");
Serial.println(orientation_local[2]*57.3);
//
// Serial.print(sqrt(sq(orientation_x(0,0))+sq(orientation_x(1,0))+sq(orientation_x(2,0))),6);
// Serial.print(" , ");
// Serial << "orientation_matrix: " << orientation_x << '\n';
//
// Serial.print(" , ");
//Serial.println(dt,6);
}
//ORIENTATION
void calculate_orientation(){
w<< gyro[0], //LOCAL ROLL RATE
gyro[1], //LOCAL PITCH RATE
gyro[2]; //LOCAL YAW RATE
Sq<< -q(1,0),-q(2,0),-q(3,0),
q(0,0),-q(3,0),q(2,0),
q(3,0),q(0,0),-q(1,0),
-q(2,0),q(1,0),q(0,0);
q_local=Sq*w; //BLA DOESN'T WANT TO DO ALL THE OPERATIONS IN ONE LINE
q_local(0,0)=0.5*T*q_local(0,0); //Euler integration, will be changed to trapezoidal in the future
q_local(1,0)=0.5*T*q_local(1,0);
q_local(2,0)=0.5*T*q_local(2,0);
q_local(3,0)=0.5*T*q_local(3,0);
q=q_local+q; //Euler integration, will be changed to trapezoidal in the future
double quat_mod=sqrt((sq(q(0,0))+sq(q(1,0))+sq(q(2,0))+sq(q(3,0)))); //NORMALIZATION OF QUATERNION
q(0,0)=q(0,0)/quat_mod;
q(1,0)=q(1,0)/quat_mod;
q(2,0)=q(2,0)/quat_mod;
q(3,0)=q(3,0)/quat_mod;
c12=2*(q(1,0)*q(2,0)-q(3,0)*q(0,0));
c13=2*(q(1,0)*q(3,0)+q(2,0)*q(0,0));
c21=2*(q(1,0)*q(2,0)+q(3,0)*q(0,0));
c23=2*(q(2,0)*q(3,0)-q(1,0)*q(0,0));
c31=2*(q(1,0)*q(3,0)-q(2,0)*q(0,0));
c32=2*(q(2,0)*q(3,0)+q(1,0)*q(0,0));
C<< (1-2*(sq(q(2,0))+sq(q(3,0)))), (c12), (c13), //CREATES THE ROTATION MATRIX
(c21), (1-2*(sq(q(1,0))+sq(q(3,0)))), (c23),
(c31), (c32), (1-2*(sq(q(1,0))+sq(q(2,0))));
orientation_x=C*unit_x; //Computes the new base
orientation_y=C*unit_y;
orientation_z=C*unit_z;
//COMPUTES GLOBAL ORIENTATION //STILL EULER ANGLES, READINGS ARE NOT GOOD FOR HIGH PITCH ANGLES
orientation[0]=-atan2(C(1,2),C(2,2)); //ROLL
orientation[1]=-asin((orientation_x(2,0))/(1)); //GLOBAL PITCH
orientation[2]=asin((orientation_x(1,0))/(sqrt(sq(orientation_x(1,0))+sq(orientation_x(0,0))))); //GLOBAL YAW
//LOCAL ANGLES, THIS ARE THE ONES YOU SHOULD FEED TO THE CONTROL SYSTEM
orientation_local[0]=-atan2(C(1,2),C(2,2)); //ROLL
orientation_local[1]=asin(C(0,2)); //PITCH
orientation_local[2]=-atan2(C(0,1),C(0,0)); //YAW
}