blob: f1e968ed9084b1fc67b5c8988085a40598e3fcf3 (
plain)
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
|
#include "Arduino.h"
#include "Wire.h"
#include "calibration.hpp"
#include "sensor.hpp"
#define LINE_CLEAR_LENGTH 30
#define SENSOR_THROTTLE_TIME 50 // milliseconds
Sensor sensor(0x68, &Wire, SENSOR_THROTTLE_TIME);
char line_clear[LINE_CLEAR_LENGTH + 1];
/**
* Stops code execution.
*/
void stop()
{
while (true) {}
}
void setup()
{
Serial.begin(9600);
// Wait for Serial because the Arduino Leonardo is weird
while (!Serial) {}
while (!sensor.begin())
{
Serial.println("Error: Could not connect to the sensor. Retrying after 2000 "
"milliseconds...");
delay(2000);
}
if (!sensor.setAccelSensitivity(2)) // 8g
{
Serial.print(
"Error: Failed to set the sensor's accelerometer sensitivity. Status: ");
Serial.println(static_cast<int>(sensor.getStatus()));
stop();
}
if (!sensor.setGyroSensitivity(1)) // 500 degrees/s
{
Serial.print("Error: Failed to set the sensor's gyro sensitivity. Status: ");
Serial.println(static_cast<int>(sensor.getStatus()));
stop();
}
Serial.println("Calibrating sensor...");
SensorCalibrator sensor_calibrator(&sensor);
if (!sensor_calibrator.calibrate(SENSOR_THROTTLE_TIME))
{
Serial.print("Error: Sensor calibration timed out after ");
Serial.print(CALIBRATION_TIMEOUT);
Serial.println(" milliseconds");
stop();
}
Serial.println("Finished calibrating sensor");
Serial.println("Calibration values:");
Serial.print("Accelerometer X: ");
Serial.println(sensor.accel_cal_x);
Serial.print("Accelerometer Y: ");
Serial.println(sensor.accel_cal_y);
Serial.print("Accelerometer Z: ");
Serial.println(sensor.accel_cal_z);
Serial.print("Gyro X: ");
Serial.println(sensor.gyro_cal_x);
Serial.print("Gyro Y: ");
Serial.println(sensor.gyro_cal_y);
Serial.print("Gyro Z: ");
Serial.println(sensor.gyro_cal_z);
Serial.println("\nStarting...");
size_t prev_len = strlen(line_clear);
memset(line_clear + prev_len, ' ', LINE_CLEAR_LENGTH - prev_len);
}
void loop()
{
delay(SENSOR_THROTTLE_TIME);
if (!sensor.read())
{
SensorStatus status = sensor.getStatus();
if (status == SensorStatus::THROTTLED)
{
Serial.println("Warning: The sensor was read too frequently and throttled");
return;
}
Serial.print("Error: Failed to read sensor. Status: ");
Serial.println(static_cast<int>(status));
stop();
}
float pitch = sensor.getPitch();
float roll = sensor.getRoll();
Serial.print(line_clear);
Serial.print("\r");
Serial.print("Pitch: ");
Serial.print(pitch, 3);
Serial.print(" Roll: ");
Serial.print(roll, 3);
Serial.print("\r");
Serial.flush();
}
|
