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
|
#ifndef SENSOR_HPP
#define SENSOR_HPP
#include "Arduino.h"
#include "Wire.h"
#include "time_utils.hpp"
#define SENSOR_WAKEUP 0x00
#define RAD_TO_DEGREES (180.0 / PI)
#define RAW_TO_DPS_FACTOR (1.0 / 131.0)
#define RAW_TO_G_FACTOR (1.0 / 16384.0)
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 throttle_time A minumum time between sensor reads for the sensor to throttle
*/
Sensor(uint8_t address, TwoWire *wire, 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();
/**
* Resets the sensor.
*/
void reset();
/**
* 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).
*/
float getAccelX();
/**
* Returns the current Y axis acceleration in g:s (g-force).
*/
float getAccelY();
/**
* Returns the current Z axis acceleration in g:s (g-force).
*/
float getAccelZ();
/**
* Returns the current X angle.
*/
float getAngleX();
/**
* Returns the current Y angle.
*/
float getAngleY();
/**
* Returns the current X axis in degrees/s.
*/
float getGyroX();
/**
* Returns the current Y axis in degrees/s.
*/
float getGyroY();
/**
* Returns the current Z axis in degrees/s.
*/
float getGyroZ();
/**
* Returns the current pitch angle.
*/
float getPitch();
/**
* Returns the current roll angle.
*/
float 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
float accel_cal_x;
float accel_cal_y;
float accel_cal_z;
// Gyroscope calibration values
float gyro_cal_x;
float gyro_cal_y;
float gyro_cal_z;
private:
uint8_t _address;
bool _throttle_enabled;
unsigned int _throttle_time;
Time _last_time;
SensorStatus _status;
uint8_t _accel_sensitivity;
float _accel_to_g_force;
float _accel_raw_x, _accel_raw_y, _accel_raw_z;
float _accel_x, _accel_y;
uint8_t _gyro_sensitivity;
float _ang_rate_to_dps;
float _gyro_raw_x, _gyro_raw_y, _gyro_raw_z;
float _gyro_x, _gyro_y, _gyro_z;
float _pitch, _roll, _yaw;
int16_t _readTwoBytes();
TwoWire *_wire;
};
#endif
|
