-
Notifications
You must be signed in to change notification settings - Fork 13
/
Calibrated_sensor_output.ino
124 lines (114 loc) · 3.18 KB
/
Calibrated_sensor_output.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
#include "FastIMU.h"
#include <Wire.h>
#define IMU_ADDRESS 0x68 //Change to the address of the IMU
#define PERFORM_CALIBRATION //Comment to disable startup calibration
MPU6050 IMU; //Change to the name of any supported IMU!
// Currently supported IMUS: MPU9255 MPU9250 MPU6886 MPU6500 MPU6050 ICM20689 ICM20690 BMI055 BMX055 BMI160 LSM6DS3 LSM6DSL QMI8658
calData calib = { 0 }; //Calibration data
AccelData accelData; //Sensor data
GyroData gyroData;
MagData magData;
void setup() {
Wire.begin();
Wire.setClock(400000); //400khz clock
Serial.begin(115200);
while (!Serial) {
;
}
int err = IMU.init(calib, IMU_ADDRESS);
if (err != 0) {
Serial.print("Error initializing IMU: ");
Serial.println(err);
while (true) {
;
}
}
#ifdef PERFORM_CALIBRATION
Serial.println("FastIMU calibration & data example");
if (IMU.hasMagnetometer()) {
delay(1000);
Serial.println("Move IMU in figure 8 pattern until done.");
delay(3000);
IMU.calibrateMag(&calib);
Serial.println("Magnetic calibration done!");
}
else {
delay(5000);
}
delay(5000);
Serial.println("Keep IMU level.");
delay(5000);
IMU.calibrateAccelGyro(&calib);
Serial.println("Calibration done!");
Serial.println("Accel biases X/Y/Z: ");
Serial.print(calib.accelBias[0]);
Serial.print(", ");
Serial.print(calib.accelBias[1]);
Serial.print(", ");
Serial.println(calib.accelBias[2]);
Serial.println("Gyro biases X/Y/Z: ");
Serial.print(calib.gyroBias[0]);
Serial.print(", ");
Serial.print(calib.gyroBias[1]);
Serial.print(", ");
Serial.println(calib.gyroBias[2]);
if (IMU.hasMagnetometer()) {
Serial.println("Mag biases X/Y/Z: ");
Serial.print(calib.magBias[0]);
Serial.print(", ");
Serial.print(calib.magBias[1]);
Serial.print(", ");
Serial.println(calib.magBias[2]);
Serial.println("Mag Scale X/Y/Z: ");
Serial.print(calib.magScale[0]);
Serial.print(", ");
Serial.print(calib.magScale[1]);
Serial.print(", ");
Serial.println(calib.magScale[2]);
}
delay(5000);
IMU.init(calib, IMU_ADDRESS);
#endif
//err = IMU.setGyroRange(500); //USE THESE TO SET THE RANGE, IF AN INVALID RANGE IS SET IT WILL RETURN -1
//err = IMU.setAccelRange(2); //THESE TWO SET THE GYRO RANGE TO ±500 DPS AND THE ACCELEROMETER RANGE TO ±2g
if (err != 0) {
Serial.print("Error Setting range: ");
Serial.println(err);
while (true) {
;
}
}
}
void loop() {
IMU.update();
IMU.getAccel(&accelData);
Serial.print(accelData.accelX);
Serial.print("\t");
Serial.print(accelData.accelY);
Serial.print("\t");
Serial.print(accelData.accelZ);
Serial.print("\t");
IMU.getGyro(&gyroData);
Serial.print(gyroData.gyroX);
Serial.print("\t");
Serial.print(gyroData.gyroY);
Serial.print("\t");
Serial.print(gyroData.gyroZ);
if (IMU.hasMagnetometer()) {
IMU.getMag(&magData);
Serial.print("\t");
Serial.print(magData.magX);
Serial.print("\t");
Serial.print(magData.magY);
Serial.print("\t");
Serial.print(magData.magZ);
}
if (IMU.hasTemperature()) {
Serial.print("\t");
Serial.println(IMU.getTemp());
}
else {
Serial.println();
}
delay(50);
}