4#include <linux/i2c-dev.h>
18#define LSM303_CTRL_REG1_A (0x20)
19#define LSM303_CTRL_REG4_A (0x23)
20#define LSM303_OUT_X_L_A (0x28)
21#define L3G_CTRL_REG1 (0x20)
22#define L3G_CTRL_REG4 (0x23)
23#define L3G_OUT_X_L (0x28)
25#define GYRO_ADDRESS (0x6b)
26#define ACC_ADDRESS (0x19)
27#define MAG_ADDRESS (0x1e)
32static bool select_device(
int file,
int addr)
34 if (ioctl(file, I2C_SLAVE, addr) < 0) {
40static bool write_acc_register(
int file, vrpn_uint8 reg, vrpn_uint8 value)
42 if (!select_device(file, ACC_ADDRESS)) {
43 fprintf(stderr,
"write_acc_register(): Cannot select device\n");
46 return vrpn_i2c_smbus_write_byte_data(file, reg, value) >= 0;
49static bool write_gyro_register(
int file, vrpn_uint8 reg, vrpn_uint8 value)
51 if (!select_device(file, GYRO_ADDRESS)) {
52 fprintf(stderr,
"write_gyro_register(): Cannot select device\n");
55 return vrpn_i2c_smbus_write_byte_data(file, reg, value) >= 0;
58vrpn_Adafruit_10DOF::vrpn_Adafruit_10DOF(
60 std::string
const &device,
61 double read_interval_seconds)
65 d_read_interval_seconds = read_interval_seconds;
67 for (vrpn_int32 i = 0; i < num_channel; i++) {
74 d_i2c_dev = open(device.c_str(), O_RDWR);
77 "vrpn_Adafruit_10DOF::vrpn_Adafruit_10DOF(): "
78 "Cannot open %s\n", device.c_str());
88 if (!write_acc_register(d_i2c_dev, LSM303_CTRL_REG1_A, 0b01010111)) {
90 "vrpn_Adafruit_10DOF::vrpn_Adafruit_10DOF(): "
91 "Cannot configure accelerometer on %s\n", device.c_str());
97 if (!write_acc_register(d_i2c_dev, LSM303_CTRL_REG4_A, 0b00101000)) {
99 "vrpn_Adafruit_10DOF::vrpn_Adafruit_10DOF(): "
100 "Cannot configure accelerometer range on %s\n", device.c_str());
107 if (!write_gyro_register(d_i2c_dev, L3G_CTRL_REG1, 0b01010111)) {
109 "vrpn_Adafruit_10DOF::vrpn_Adafruit_10DOF(): "
110 "Cannot configure gyro on %s\n", device.c_str());
116 if (!write_gyro_register(d_i2c_dev, L3G_CTRL_REG4, 0b00110000)) {
118 "vrpn_Adafruit_10DOF::vrpn_Adafruit_10DOF(): "
119 "Cannot configure gyro range on %s\n", device.c_str());
130vrpn_Adafruit_10DOF::~vrpn_Adafruit_10DOF()
132 if (d_i2c_dev >= 0) {
137void vrpn_Adafruit_10DOF::mainloop()
142 if (d_i2c_dev < 0) {
return; }
149 if (duration < d_read_interval_seconds) {
return; }
153 if (!select_device(d_i2c_dev, ACC_ADDRESS)) {
154 fprintf(stderr,
"vrpn_Adafruit_10DOF::mainloop: Cannot select accelerometer\n");
160 int result = vrpn_i2c_smbus_read_i2c_block_data(d_i2c_dev,
161 0x80 | LSM303_OUT_X_L_A,
sizeof(block), block);
162 if (result !=
sizeof(block)) {
163 printf(
"vrpn_Adafruit_10DOF::mainloop: Failed to read from accelerometer.");
166 channel[0] = (block[0] |
static_cast<vrpn_int16
>(block[1]) << 8) >> 4;
167 channel[1] = (block[2] |
static_cast<vrpn_int16
>(block[3]) << 8) >> 4;
168 channel[2] = (block[4] |
static_cast<vrpn_int16
>(block[5]) << 8) >> 4;
174 if (!select_device(d_i2c_dev, GYRO_ADDRESS)) {
175 fprintf(stderr,
"vrpn_Adafruit_10DOF::mainloop: Cannot select gyroscope\n");
180 result = vrpn_i2c_smbus_read_i2c_block_data(d_i2c_dev,
181 0x80 | L3G_OUT_X_L,
sizeof(block), block);
182 if (result !=
sizeof(block)) {
183 printf(
"vrpn_Adafruit_10DOF::mainloop: Failed to read from gyro.");
186 channel[4] = (block[0] |
static_cast<vrpn_int16
>(block[1]) << 8);
187 channel[5] = (block[2] |
static_cast<vrpn_int16
>(block[3]) << 8);
188 channel[6] = (block[4] |
static_cast<vrpn_int16
>(block[5]) << 8);
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