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
|
#include "calibration.hpp"
#include "sensor/sensor.hpp"
#include "utils/memory.hpp"
#include "utils/serial.hpp"
#include <Arduino.h>
#include <Wire.h>
// constexpr unsigned int LINE_CLEAR_LENGTH = 30U;
constexpr unsigned int SENSOR_ADDRESS = 0x68U;
constexpr unsigned int SENSOR_THROTTLE_TIME = 50U; // milliseconds
constexpr unsigned int BAUD_RATE = 9600U;
constexpr unsigned int SENSOR_RETRY_TIME = 2000U;
UniquePtr<SerialStream> sout_ptr;
UniquePtr<Sensor> sensor;
void setup()
{
sout_ptr = make_unique<SerialStream>(Serial, BAUD_RATE);
auto sout = *sout_ptr;
sensor = make_unique<Sensor>(SENSOR_ADDRESS, Wire, sout, SENSOR_THROTTLE_TIME);
while (!sensor->begin())
{
sout << "Error: Could not connect to the sensor. Retrying after 2000 "
"milliseconds..."
<< endl;
delay(SENSOR_RETRY_TIME);
}
if (!sensor->setAccelSensitivity(2)) // 8g
{
sout << "Error: Failed to set the sensor's accelerometer sensitivity. Status: "
<< static_cast<int>(sensor->getStatus()) << endl;
stop();
}
if (!sensor->setGyroSensitivity(1)) // 500 degrees/s
{
sout << "Error: Failed to set the sensor's gyro sensitivity. Status: "
<< static_cast<int>(sensor->getStatus()) << endl;
stop();
}
sout << "Calibrating sensor..." << endl;
SensorCalibrator sensor_calibrator(sensor, sout);
if (!sensor_calibrator.calibrate(SENSOR_THROTTLE_TIME))
{
sout << "Error: Sensor calibration timed out after " << CALIBRATION_TIMEOUT
<< " milliseconds" << endl;
stop();
}
sout << "Finished calibrating sensor\n";
sout << "Calibration values:\n"
<< "Accelerometer X: " << sensor->accel_cal_x << "\n"
<< "Accelerometer Y: " << sensor->accel_cal_y << "\n"
<< "Accelerometer Z: " << sensor->accel_cal_z << "\n"
<< "Gyro X: " << sensor->gyro_cal_x << "\n"
<< "Gyro Y: " << sensor->gyro_cal_y << "\n"
<< "Gyro Z: " << sensor->gyro_cal_z << "\n";
sout << "Starting..." << endl;
sout << "Pitch: " << sensor->getPitch() << " "
<< "Roll: " << sensor->getRoll() << endl;
}
void loop()
{
/*
delay(SENSOR_THROTTLE_TIME);
if (!sensor.read())
{
SensorStatus status = sensor.getStatus();
if (status == SensorStatus::THROTTLED)
{
printSerial("Warning: The sensor was read too frequently and throttled");
endl();
return;
}
printSerial("Error: Failed to read sensor. Status: %d", static_cast<int>(status));
endl();
stop();
}
char *pitch = floatToStr(sensor.getPitch(), FLOAT_WIDTH, FLOAT_PRECISION);
char *roll = floatToStr(sensor.getRoll(), FLOAT_WIDTH, FLOAT_PRECISION);
printSerial("\rPitch: %s Roll: %s", pitch, roll);
Serial.flush();
free(pitch);
free(roll);
*/
}
|
