vrpn 07.35
Virtual Reality Peripheral Network
Loading...
Searching...
No Matches
vrpn_Adafruit.C
Go to the documentation of this file.
1#include "vrpn_Adafruit.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
11// Developed using information from
12// http://ozzmaker.com/guide-to-interfacing-a-gyro-and-accelerometer-with-a-raspberry-pi
13// @todo This implementation is not yet complete, nor completely tested.
14// It does open the device and read raw values from the acceleromater. The gyro
15// read may be working as well.
16
17// Constants that describe the device
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)
24
25#define GYRO_ADDRESS (0x6b)
26#define ACC_ADDRESS (0x19)
27#define MAG_ADDRESS (0x1e)
28
29#include "vrpn_i2c_helpers.h"
30
31// Helper functions
32static bool select_device(int file, int addr)
33{
34 if (ioctl(file, I2C_SLAVE, addr) < 0) {
35 return false;
36 }
37 return true;
38}
39
40static bool write_acc_register(int file, vrpn_uint8 reg, vrpn_uint8 value)
41{
42 if (!select_device(file, ACC_ADDRESS)) {
43 fprintf(stderr,"write_acc_register(): Cannot select device\n");
44 return false;
45 }
46 return vrpn_i2c_smbus_write_byte_data(file, reg, value) >= 0;
47}
48
49static bool write_gyro_register(int file, vrpn_uint8 reg, vrpn_uint8 value)
50{
51 if (!select_device(file, GYRO_ADDRESS)) {
52 fprintf(stderr,"write_gyro_register(): Cannot select device\n");
53 return false;
54 }
55 return vrpn_i2c_smbus_write_byte_data(file, reg, value) >= 0;
56}
57
58vrpn_Adafruit_10DOF::vrpn_Adafruit_10DOF(
59 std::string const &name, vrpn_Connection *c,
60 std::string const &device,
61 double read_interval_seconds)
62 : vrpn_Analog_Server(name.c_str(), c)
63{
64 // Set device parameters.
65 d_read_interval_seconds = read_interval_seconds;
66 num_channel = 11;
67 for (vrpn_int32 i = 0; i < num_channel; i++) {
68 channel[i] = 0;
69 last[i] = channel[i];
70 }
71
72 //--------------------------------------------------------
73 // Open the file we're going to use to talk with the device.
74 d_i2c_dev = open(device.c_str(), O_RDWR);
75 if (d_i2c_dev < 0) {
76 fprintf(stderr,
77 "vrpn_Adafruit_10DOF::vrpn_Adafruit_10DOF(): "
78 "Cannot open %s\n", device.c_str());
79 return;
80 }
81
82 //--------------------------------------------------------
83 // Configure the sensors on the device.
84
85 // Enable the accelerometer at a rate consistent with our
86 // read rate.
87 // @todo For now, 100 Hz data rate
88 if (!write_acc_register(d_i2c_dev, LSM303_CTRL_REG1_A, 0b01010111)) {
89 fprintf(stderr,
90 "vrpn_Adafruit_10DOF::vrpn_Adafruit_10DOF(): "
91 "Cannot configure accelerometer on %s\n", device.c_str());
92 close(d_i2c_dev);
93 d_i2c_dev = -1;
94 return;
95 }
96 // +/- 8G full scale, FS = 10 on HLHC, high resolution output mode
97 if (!write_acc_register(d_i2c_dev, LSM303_CTRL_REG4_A, 0b00101000)) {
98 fprintf(stderr,
99 "vrpn_Adafruit_10DOF::vrpn_Adafruit_10DOF(): "
100 "Cannot configure accelerometer range on %s\n", device.c_str());
101 close(d_i2c_dev);
102 d_i2c_dev = -1;
103 return;
104 }
105
106 // Enable the gyro.
107 if (!write_gyro_register(d_i2c_dev, L3G_CTRL_REG1, 0b01010111)) {
108 fprintf(stderr,
109 "vrpn_Adafruit_10DOF::vrpn_Adafruit_10DOF(): "
110 "Cannot configure gyro on %s\n", device.c_str());
111 close(d_i2c_dev);
112 d_i2c_dev = -1;
113 return;
114 }
115 // Continuous update, 2000 dps full scale
116 if (!write_gyro_register(d_i2c_dev, L3G_CTRL_REG4, 0b00110000)) {
117 fprintf(stderr,
118 "vrpn_Adafruit_10DOF::vrpn_Adafruit_10DOF(): "
119 "Cannot configure gyro range on %s\n", device.c_str());
120 close(d_i2c_dev);
121 d_i2c_dev = -1;
122 return;
123 }
124
125 //--------------------------------------------------------
126 // Record the time we opened the device.
127 vrpn_gettimeofday(&timestamp, NULL);
128}
129
130vrpn_Adafruit_10DOF::~vrpn_Adafruit_10DOF()
131{
132 if (d_i2c_dev >= 0) {
133 close(d_i2c_dev);
134 }
135}
136
137void vrpn_Adafruit_10DOF::mainloop()
138{
139 server_mainloop();
140
141 // Check and see if we have an open device. If not, return.
142 if (d_i2c_dev < 0) { return; }
143
144 // Check to see if it has been long enough since our last report.
145 // if not, return. If so, reset the timestamp to now.
146 struct timeval now;
147 vrpn_gettimeofday(&now, NULL);
148 double duration = vrpn_TimevalDurationSeconds(now, timestamp);
149 if (duration < d_read_interval_seconds) { return; }
150 timestamp = now;
151
152 // Select the Accelerometer device
153 if (!select_device(d_i2c_dev, ACC_ADDRESS)) {
154 fprintf(stderr,"vrpn_Adafruit_10DOF::mainloop: Cannot select accelerometer\n");
155 return;
156 }
157
158 // Read and parse the raw values from the accelerometer
159 vrpn_uint8 block[6];
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.");
164 return;
165 }
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;
169
170 // Convert to meters/second/second
171 // @todo
172
173 // Select the Gyroscope device
174 if (!select_device(d_i2c_dev, GYRO_ADDRESS)) {
175 fprintf(stderr,"vrpn_Adafruit_10DOF::mainloop: Cannot select gyroscope\n");
176 return;
177 }
178
179 // Read and parse the raw values from the accelerometer
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.");
184 return;
185 }
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);
189
190 // Convert to radians/second
191 // @todo
192
193 report_changes();
194}
195
196#endif
197
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