4#include <linux/i2c-dev.h>
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)
25#define LSM9DS0_OUT_X_L_A (0x28)
26#define LSM9DS0_OUT_X_L_G (0x28)
27#define LSM9DS0_OUT_X_L_M (0x08)
30#define GYRO_ADDRESS (0x6a)
31#define ACC_ADDRESS (0x1e)
32#define MAG_ADDRESS (0x1e)
37static bool select_device(
int file,
int addr)
39 if (ioctl(file, I2C_SLAVE, addr) < 0) {
45static bool write_acc_register(
int file, vrpn_uint8 reg, vrpn_uint8 value)
47 if (!select_device(file, ACC_ADDRESS)) {
48 fprintf(stderr,
"write_acc_register(): Cannot select device\n");
51 return vrpn_i2c_smbus_write_byte_data(file, reg, value) >= 0;
54static bool write_gyro_register(
int file, vrpn_uint8 reg, vrpn_uint8 value)
56 if (!select_device(file, GYRO_ADDRESS)) {
57 fprintf(stderr,
"write_gyro_register(): Cannot select device\n");
60 return vrpn_i2c_smbus_write_byte_data(file, reg, value) >= 0;
63static bool write_mag_register(
int file, vrpn_uint8 reg, vrpn_uint8 value)
65 if (!select_device(file, MAG_ADDRESS)) {
66 fprintf(stderr,
"write_mag_register(): Cannot select device\n");
69 return vrpn_i2c_smbus_write_byte_data(file, reg, value) >= 0;
72vrpn_OzzMaker_BerryIMU::vrpn_OzzMaker_BerryIMU(
74 std::string
const &device,
75 double read_interval_seconds)
79 d_read_interval_seconds = read_interval_seconds;
81 for (vrpn_int32 i = 0; i < num_channel; i++) {
88 d_i2c_dev = open(device.c_str(), O_RDWR);
91 "vrpn_OzzMaker_BerryIMU::vrpn_OzzMaker_BerryIMU(): "
92 "Cannot open %s\n", device.c_str());
102 if (!write_acc_register(d_i2c_dev, LSM9DS0_CTRL_REG1_XM, 0b01100111)) {
104 "vrpn_OzzMaker_BerryIMU::vrpn_OzzMaker_BerryIMU(): "
105 "Cannot configure accelerometer on %s\n", device.c_str());
111 if (!write_acc_register(d_i2c_dev, LSM9DS0_CTRL_REG2_XM, 0b00100000)) {
113 "vrpn_OzzMaker_BerryIMU::vrpn_OzzMaker_BerryIMU(): "
114 "Cannot configure accelerometer range on %s\n", device.c_str());
121 if (!write_gyro_register(d_i2c_dev, LSM9DS0_CTRL_REG1_G, 0b00001111)) {
123 "vrpn_OzzMaker_BerryIMU::vrpn_OzzMaker_BerryIMU(): "
124 "Cannot configure gyro on %s\n", device.c_str());
130 if (!write_gyro_register(d_i2c_dev, LSM9DS0_CTRL_REG4_G, 0b00110000)) {
132 "vrpn_OzzMaker_BerryIMU::vrpn_OzzMaker_BerryIMU(): "
133 "Cannot configure gyro range on %s\n", device.c_str());
142 if (!write_mag_register(d_i2c_dev, LSM9DS0_CTRL_REG5_XM, 0b11110000)) {
144 "vrpn_OzzMaker_BerryIMU::vrpn_OzzMaker_BerryIMU(): "
145 "Cannot configure magnetometer on %s\n", device.c_str());
151 if (!write_mag_register(d_i2c_dev, LSM9DS0_CTRL_REG6_G, 0b01100000)) {
153 "vrpn_OzzMaker_BerryIMU::vrpn_OzzMaker_BerryIMU(): "
154 "Cannot configure magnetometer range on %s\n", device.c_str());
160 if (!write_mag_register(d_i2c_dev, LSM9DS0_CTRL_REG7_G, 0b00000000)) {
162 "vrpn_OzzMaker_BerryIMU::vrpn_OzzMaker_BerryIMU(): "
163 "Cannot configure magnetometer mode on %s\n", device.c_str());
178vrpn_OzzMaker_BerryIMU::~vrpn_OzzMaker_BerryIMU()
180 if (d_i2c_dev >= 0) {
185void vrpn_OzzMaker_BerryIMU::mainloop()
190 if (d_i2c_dev < 0) {
return; }
197 if (duration < d_read_interval_seconds) {
return; }
201 if (!select_device(d_i2c_dev, ACC_ADDRESS)) {
202 fprintf(stderr,
"vrpn_OzzMaker_BerryIMU::mainloop: Cannot select accelerometer\n");
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.");
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));
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;
226 if (!select_device(d_i2c_dev, GYRO_ADDRESS)) {
227 fprintf(stderr,
"vrpn_OzzMaker_BerryIMU::mainloop: Cannot select gyroscope\n");
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.");
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));
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;
251 if (!select_device(d_i2c_dev, MAG_ADDRESS)) {
252 fprintf(stderr,
"vrpn_OzzMaker_BerryIMU::mainloop: Cannot select magnetometer\n");
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.");
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));
270 const double mag_gain = 0.48e-3;
271 channel[6] *= mag_gain;
272 channel[7] *= mag_gain;
273 channel[8] *= mag_gain;
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.
#define vrpn_gettimeofday