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
|
#pragma once
#include "utils/serial.hpp"
#include "utils/time.hpp"
#include <Arduino.h>
#include <Wire.h>
constexpr uint8_t SENSOR_WAKEUP = 0x00U;
constexpr float RAW_TO_DPS_FACTOR = 1.0 / 131.0;
constexpr float RAW_TO_G_FACTOR = 1.0 / 16384.0;
constexpr unsigned int ONE_EIGHTY = 180;
enum class SensorStatus
{
OK = 0,
THROTTLED = 1,
ERR_READ = -1,
ERR_WRITE = -2,
ERR_NOT_CONNECTED = -3
};
/**
* A GY521 gyro and accelerometer sensor.
*/
class Sensor
{
public:
/**
* A GY521 gyro and accelerometer sensor.
*
* @param address The address of the sensor
* @param wire A Wire instance
* @param sout A serial output stream
* @param throttle_time A minumum time between sensor reads for the sensor to throttle
*/
Sensor(uint8_t address, TwoWire wire, SerialStream sout, unsigned int throttle_time);
/**
* Initializes communication with the sensor.
*
* @returns Whether or not the sensor can be communicated with.
*/
bool begin();
/**
* Returns whether or not the sensor is connected.
*/
bool isConnected();
/**
* Reads from the sensor.
*
* @returns Whether or not it succeeded.
*/
bool read();
/**
* Sets the accelerometer sensitivity.
*
* @param sensitivity Accelerometer sensitivity. 0, 1, 2, 3 => 2g 4g 8g 16g
* @returns Whether or not it succeeded.
*/
bool setAccelSensitivity(uint8_t sensitivity);
/**
* Sets the gyro sensitivity.
*
* @param sensitivity Gyro sensitivity.
* 0, 1, 2, 3 => 250, 500, 1000, 2000 degrees/s
* @returns Whether or not it succeeded.
*/
bool setGyroSensitivity(uint8_t sensitivity);
/**
* Returns the current X axis acceleration in g:s (g-force).
*/
double getAccelX();
/**
* Returns the current Y axis acceleration in g:s (g-force).
*/
double getAccelY();
/**
* Returns the current Z axis acceleration in g:s (g-force).
*/
double getAccelZ();
/**
* Returns the current X angle.
*/
double getAngleX();
/**
* Returns the current Y angle.
*/
double getAngleY();
/**
* Returns the current X axis in degrees/s.
*/
double getGyroX();
/**
* Returns the current Y axis in degrees/s.
*/
double getGyroY();
/**
* Returns the current Z axis in degrees/s.
*/
double getGyroZ();
/**
* Returns the current pitch angle.
*/
double getPitch();
/**
* Returns the current roll angle.
*/
double getRoll();
/**
* Sets the value of a key in the sensor's register.
*
* @param reg The address of a registry key
* @param value A new value
* @returns Whether or not it succeeded.
*/
bool setRegister(uint8_t reg, uint8_t value);
/**
* Returns the value of a key in the sensor's registry.
*
* @param reg The address of a registry key
*/
uint8_t getRegister(uint8_t reg);
/**
* Returns the last sensor status.
*/
SensorStatus getStatus();
// Accelerometer calibration values
double accel_cal_x = 0.0F;
double accel_cal_y = 0.0F;
double accel_cal_z = 0.0F;
// Gyroscope calibration values
double gyro_cal_x = 0.0F;
double gyro_cal_y = 0.0F;
double gyro_cal_z = 0.0F;
private:
TwoWire _wire;
uint8_t _address;
bool _throttle_enabled;
unsigned int _throttle_time;
Time _last_time;
SensorStatus _status;
uint8_t _accel_sensitivity = 0U;
float _accel_to_g_force;
double _accel_raw_x = 0;
double _accel_raw_y = 0;
double _accel_raw_z = 0;
double _accel_x = 0;
double _accel_y = 0;
uint8_t _gyro_sensitivity = 0U;
float _ang_rate_to_dps;
double _gyro_raw_x = 0;
double _gyro_raw_y = 0;
double _gyro_raw_z = 0;
double _gyro_x = 0;
double _gyro_y = 0;
double _gyro_z = 0;
double _pitch = 0;
double _roll = 0;
double _yaw = 0;
int16_t _readHighLow();
SerialStream _sout;
};
|
