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
|
#include <fcntl.h>
#include <poll.h>
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/stat.h>
#include <sys/types.h>
#define GEARVR_MAX_FAILURE 5
#define GEARVR_CDEV "/dev/hidraw0"
struct gearvr_sample_data {
uint8_t n;
uint16_t t;
uint16_t cmd_id;
uint16_t temperature;
struct stat stat_buf;
int32_t ax, ay, az, gx, gy, gz;
};
int gearvr_read(int fd)
{
int ret;
int err = 0;
struct pollfd fds;
uint8_t buf[100];
fds.fd = fd;
fds.events = POLLIN | POLLHUP | POLLERR;
while (1) {
int i;
struct gearvr_sample_data odata;
if (err >= GEARVR_MAX_FAILURE) {
fprintf(stderr, "reached maximum attempts\n");
return -1;
}
ret = poll(&fds, 1, 100);
if (ret < 0)
fprintf(stderr, "failed to poll\n");
if (ret == 0)
fprintf(stderr, "timed out\n");
if (!(fds.revents & POLLIN))
fprintf(stderr, "poll exited with error\n");
if ((ret <= 0) || !(fds.revents & POLLIN))
goto fail;
ret = read(fd, buf, 100);
if (ret < 0) {
fprintf(stderr, "read() failed\n");
goto fail;
}
odata.n = buf[0];
odata.t = (buf[3] << 8) | buf[2];
odata.cmd_id = (buf[5] << 8) | buf[4];
odata.temperature = (buf[7] << 8) | buf[6];
/*
printf("[%u] Read %d bytes: %u data, cmd_id = %u, tempreature = %u\n",
odata.t, ret, odata.n, odata.cmd_id, odata.temperature);
*/
for (i = 0; i < odata.n && i < 2; i++) {
odata.ax = (buf[0 + 8 + 16 * i] << 13) | (buf[1 + 8 + 16 * i] << 5)
| ((buf[2 + 8 + 16 * i] & 0xF8) >> 3);
odata.ay = ((buf[2 + 8 + 16 * i] & 0x07) << 18) | (buf[3 + 8 + 16 * i] << 10)
| (buf[4 + 8 + 16 * i] << 2) | ((buf[5 + 8 + 16 * i] & 0xC0) >> 6);
odata.az = ((buf[5 + 8 + 16 * i] & 0x3F) << 15) | (buf[6 + 8 + 16 * i] << 7)
| (buf[7 + 8 + 16 * i] >> 1);
odata.gx = (buf[0 + 16 + 16 * i] << 13) | (buf[1 + 16 + 16 * i] << 5)
| ((buf[2 + 16 + 16 * i] & 0xF8) >> 3);
odata.gy = ((buf[2 + 16 + 16 * i] & 0x07) << 18) | (buf[3 + 16 + 16 * i] << 10)
| (buf[4 + 16 + 16 * i] << 2) | ((buf[5 + 16 + 16 * i] & 0xC0) >> 6);
odata.gz = ((buf[5 + 16 + 16 * i] & 0x3F) << 15) | (buf[6 + 16 + 16 * i] << 7)
| (buf[7 + 16 + 16 * i] >> 1);
//printf("acc [%u]: %8d %8d %8d\n", odata.n, odata.ax, odata.ay, odata.az);
printf("gyr: %8d %8d %8d\n", odata.gx, odata.gy, odata.gz);
}
err = 0;
continue;
fail:
err++;
perror("failed to read from device");
sleep(1);
}
}
int main (int argc, char *argv[])
{
int ret;
int fd;
fd = open(GEARVR_CDEV, O_RDONLY);
if (fd < 0) {
perror("cannot open file");
return -1;
}
ret = gearvr_read(fd);
close(fd);
return ret;
}
|
