vrpn 07.35
Virtual Reality Peripheral Network
Loading...
Searching...
No Matches
vrpn_OzzMaker.C
Go to the documentation of this file.
1#include "vrpn_OzzMaker.h"
2
3#ifdef VRPN_USE_I2CDEV
4#include <linux/i2c-dev.h>
5#include <unistd.h>
6#include <fcntl.h>
7#include <sys/stat.h>
8#include <stdio.h>
9#include <sys/ioctl.h>
10#include <cmath>
11
12// Developed using information from
13// http://ozzmaker.com/berryimu/
14
15// Constants that describe the device registers
16#define LSM9DS0_CTRL_REG1_XM (0x20)
17#define LSM9DS0_CTRL_REG2_XM (0x21)
18#define LSM9DS0_CTRL_REG5_XM (0x24)
19#define LSM9DS0_CTRL_REG1_G (0x20)
20#define LSM9DS0_CTRL_REG2_G (0x21)
21#define LSM9DS0_CTRL_REG4_G (0x23)
22#define LSM9DS0_CTRL_REG6_G (0x25)
23#define LSM9DS0_CTRL_REG7_G (0x26)
24
25#define LSM9DS0_OUT_X_L_A (0x28)
26#define LSM9DS0_OUT_X_L_G (0x28)
27#define LSM9DS0_OUT_X_L_M (0x08)
28
29// Constants that define the I2C bus addresses
30#define GYRO_ADDRESS (0x6a)
31#define ACC_ADDRESS (0x1e)
32#define MAG_ADDRESS (0x1e)
33
34#include "vrpn_i2c_helpers.h"
35
36// Helper functions
37static bool select_device(int file, int addr)
38{
39 if (ioctl(file, I2C_SLAVE, addr) < 0) {
40 return false;
41 }
42 return true;
43}
44
45static bool write_acc_register(int file, vrpn_uint8 reg, vrpn_uint8 value)
46{
47 if (!select_device(file, ACC_ADDRESS)) {
48 fprintf(stderr,"write_acc_register(): Cannot select device\n");
49 return false;
50 }
51 return vrpn_i2c_smbus_write_byte_data(file, reg, value) >= 0;
52}
53
54static bool write_gyro_register(int file, vrpn_uint8 reg, vrpn_uint8 value)
55{
56 if (!select_device(file, GYRO_ADDRESS)) {
57 fprintf(stderr,"write_gyro_register(): Cannot select device\n");
58 return false;
59 }
60 return vrpn_i2c_smbus_write_byte_data(file, reg, value) >= 0;
61}
62
63static bool write_mag_register(int file, vrpn_uint8 reg, vrpn_uint8 value)
64{
65 if (!select_device(file, MAG_ADDRESS)) {
66 fprintf(stderr,"write_mag_register(): Cannot select device\n");
67 return false;
68 }
69 return vrpn_i2c_smbus_write_byte_data(file, reg, value) >= 0;
70}
71
72vrpn_OzzMaker_BerryIMU::vrpn_OzzMaker_BerryIMU(
73 std::string const &name, vrpn_Connection *c,
74 std::string const &device,
75 double read_interval_seconds)
76 : vrpn_Analog_Server(name.c_str(), c)
77{
78 // Set device parameters.
79 d_read_interval_seconds = read_interval_seconds;
80 num_channel = 11;
81 for (vrpn_int32 i = 0; i < num_channel; i++) {
82 channel[i] = 0;
83 last[i] = channel[i];
84 }
85
86 //--------------------------------------------------------
87 // Open the file we're going to use to talk with the device.
88 d_i2c_dev = open(device.c_str(), O_RDWR);
89 if (d_i2c_dev < 0) {
90 fprintf(stderr,
91 "vrpn_OzzMaker_BerryIMU::vrpn_OzzMaker_BerryIMU(): "
92 "Cannot open %s\n", device.c_str());
93 return;
94 }
95
96 //--------------------------------------------------------
97 // Configure the sensors on the device.
98
99 // Enable the accelerometer at a rate consistent with our read rate.
100 // Enable all 3 axes for continuous update.
101 // @todo For now, 100 Hz data rate
102 if (!write_acc_register(d_i2c_dev, LSM9DS0_CTRL_REG1_XM, 0b01100111)) {
103 fprintf(stderr,
104 "vrpn_OzzMaker_BerryIMU::vrpn_OzzMaker_BerryIMU(): "
105 "Cannot configure accelerometer on %s\n", device.c_str());
106 close(d_i2c_dev);
107 d_i2c_dev = -1;
108 return;
109 }
110 // +/- 16G full scale, 773 Hz (max) anti-alias filter bandwidth
111 if (!write_acc_register(d_i2c_dev, LSM9DS0_CTRL_REG2_XM, 0b00100000)) {
112 fprintf(stderr,
113 "vrpn_OzzMaker_BerryIMU::vrpn_OzzMaker_BerryIMU(): "
114 "Cannot configure accelerometer range on %s\n", device.c_str());
115 close(d_i2c_dev);
116 d_i2c_dev = -1;
117 return;
118 }
119
120 // Enable the gyro for all axes.
121 if (!write_gyro_register(d_i2c_dev, LSM9DS0_CTRL_REG1_G, 0b00001111)) {
122 fprintf(stderr,
123 "vrpn_OzzMaker_BerryIMU::vrpn_OzzMaker_BerryIMU(): "
124 "Cannot configure gyro on %s\n", device.c_str());
125 close(d_i2c_dev);
126 d_i2c_dev = -1;
127 return;
128 }
129 // Continuous update, 2000 dps full scale
130 if (!write_gyro_register(d_i2c_dev, LSM9DS0_CTRL_REG4_G, 0b00110000)) {
131 fprintf(stderr,
132 "vrpn_OzzMaker_BerryIMU::vrpn_OzzMaker_BerryIMU(): "
133 "Cannot configure gyro range on %s\n", device.c_str());
134 close(d_i2c_dev);
135 d_i2c_dev = -1;
136 return;
137 }
138
139 // Enable the magnetometer
140 // Temperature enable, M data rate = 50Hz
141 // @todo Change data rate to match what we are planning to use
142 if (!write_mag_register(d_i2c_dev, LSM9DS0_CTRL_REG5_XM, 0b11110000)) {
143 fprintf(stderr,
144 "vrpn_OzzMaker_BerryIMU::vrpn_OzzMaker_BerryIMU(): "
145 "Cannot configure magnetometer on %s\n", device.c_str());
146 close(d_i2c_dev);
147 d_i2c_dev = -1;
148 return;
149 }
150 // +/- 12 Gauss
151 if (!write_mag_register(d_i2c_dev, LSM9DS0_CTRL_REG6_G, 0b01100000)) {
152 fprintf(stderr,
153 "vrpn_OzzMaker_BerryIMU::vrpn_OzzMaker_BerryIMU(): "
154 "Cannot configure magnetometer range on %s\n", device.c_str());
155 close(d_i2c_dev);
156 d_i2c_dev = -1;
157 return;
158 }
159 // Continuous update
160 if (!write_mag_register(d_i2c_dev, LSM9DS0_CTRL_REG7_G, 0b00000000)) {
161 fprintf(stderr,
162 "vrpn_OzzMaker_BerryIMU::vrpn_OzzMaker_BerryIMU(): "
163 "Cannot configure magnetometer mode on %s\n", device.c_str());
164 close(d_i2c_dev);
165 d_i2c_dev = -1;
166 return;
167 }
168
169 // @todo Enable the temperature and pressure unit using info from
170 // http://ozzmaker.com/wp-content/uploads/2015/01/BMP180-DS000-09.pdf
171 // The sensor is located at address 0x77.
172
173 //--------------------------------------------------------
174 // Record the time we opened the device.
175 vrpn_gettimeofday(&timestamp, NULL);
176}
177
178vrpn_OzzMaker_BerryIMU::~vrpn_OzzMaker_BerryIMU()
179{
180 if (d_i2c_dev >= 0) {
181 close(d_i2c_dev);
182 }
183}
184
185void vrpn_OzzMaker_BerryIMU::mainloop()
186{
187 server_mainloop();
188
189 // Check and see if we have an open device. If not, return.
190 if (d_i2c_dev < 0) { return; }
191
192 // Check to see if it has been long enough since our last report.
193 // if not, return. If so, reset the timestamp to now.
194 struct timeval now;
195 vrpn_gettimeofday(&now, NULL);
196 double duration = vrpn_TimevalDurationSeconds(now, timestamp);
197 if (duration < d_read_interval_seconds) { return; }
198 timestamp = now;
199
200 // Select the Accelerometer device
201 if (!select_device(d_i2c_dev, ACC_ADDRESS)) {
202 fprintf(stderr,"vrpn_OzzMaker_BerryIMU::mainloop: Cannot select accelerometer\n");
203 return;
204 }
205
206 // Read and parse the raw values from the accelerometer
207 vrpn_uint8 block[6];
208 int result = vrpn_i2c_smbus_read_i2c_block_data(d_i2c_dev,
209 0x80 | LSM9DS0_OUT_X_L_A, sizeof(block), block);
210 if (result != sizeof(block)) {
211 printf("vrpn_OzzMaker_BerryIMU::mainloop: Failed to read from accelerometer.");
212 return;
213 }
214 channel[0] = static_cast<vrpn_int16>(block[0] | (block[1] << 8));
215 channel[1] = static_cast<vrpn_int16>(block[2] | (block[3] << 8));
216 channel[2] = static_cast<vrpn_int16>(block[4] | (block[5] << 8));
217
218 // Convert to meters/second/second.
219 // For range of +/- 16g, it report 0.732 mg/count.
220 const double acc_gain = 9.80665 * 0.732e-3;
221 channel[0] *= acc_gain;
222 channel[1] *= acc_gain;
223 channel[2] *= acc_gain;
224
225 // Select the Gyroscope device
226 if (!select_device(d_i2c_dev, GYRO_ADDRESS)) {
227 fprintf(stderr,"vrpn_OzzMaker_BerryIMU::mainloop: Cannot select gyroscope\n");
228 return;
229 }
230
231 // Read and parse the raw values from the gyroscope
232 result = vrpn_i2c_smbus_read_i2c_block_data(d_i2c_dev,
233 0x80 | LSM9DS0_OUT_X_L_G, sizeof(block), block);
234 if (result != sizeof(block)) {
235 printf("vrpn_OzzMaker_BerryIMU::mainloop: Failed to read from gyro.");
236 return;
237 }
238 channel[3] = static_cast<vrpn_int16>(block[0] | (block[1] << 8));
239 channel[4] = static_cast<vrpn_int16>(block[2] | (block[3] << 8));
240 channel[5] = static_cast<vrpn_int16>(block[4] | (block[5] << 8));
241
242 // Convert to radians/second.
243 // For 2000 degree/second full range, it reports in 70 millidegrees
244 // per second for each count.
245 const double gyro_gain = (VRPN_PI/180.0) * 70e-3;
246 channel[3] *= gyro_gain;
247 channel[4] *= gyro_gain;
248 channel[5] *= gyro_gain;
249
250 // Select the Magnetometer device
251 if (!select_device(d_i2c_dev, MAG_ADDRESS)) {
252 fprintf(stderr,"vrpn_OzzMaker_BerryIMU::mainloop: Cannot select magnetometer\n");
253 return;
254 }
255
256 // Read and parse the raw values from the magnetometer
257 result = vrpn_i2c_smbus_read_i2c_block_data(d_i2c_dev,
258 0x80 | LSM9DS0_OUT_X_L_M, sizeof(block), block);
259 if (result != sizeof(block)) {
260 printf("vrpn_OzzMaker_BerryIMU::mainloop: Failed to read from magnetometer.");
261 return;
262 }
263 channel[6] = static_cast<vrpn_int16>(block[0] | (block[1] << 8));
264 channel[7] = static_cast<vrpn_int16>(block[2] | (block[3] << 8));
265 channel[8] = static_cast<vrpn_int16>(block[4] | (block[5] << 8));
266
267 // Convert to Gauss.
268 // For 12 Gauss full range, it reports in 0.48 milliGauss
269 // per second for each count.
270 const double mag_gain = 0.48e-3;
271 channel[6] *= mag_gain;
272 channel[7] *= mag_gain;
273 channel[8] *= mag_gain;
274
275 // @todo Read and convert temperature and pressure using info from
276 // http://ozzmaker.com/wp-content/uploads/2015/01/BMP180-DS000-09.pdf
277
278 report_changes();
279}
280
281#endif
282
Generic connection class not specific to the transport mechanism.
double vrpn_TimevalDurationSeconds(struct timeval endT, struct timeval startT)
Return the number of seconds between startT and endT as a floating-point value.
Definition: vrpn_Shared.C:144
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:99
#define VRPN_PI
Definition: vrpn_Shared.h:13