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
118
119
120
121
122
123
124
125
126
127
128
|
/*
* Copyright (C) 2011 by Daniel Friesel <derf@finalrewind.org>
* License: WTFPL <http://sam.zoy.org/wtfpl>
* 0. You just DO WHAT THE FUCK YOU WANT TO.
*/
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <time.h>
#include <math.h>
#include <bluetooth/bluetooth.h>
#include <cwiid.h>
#define NUM_VALUES 1000
cwiid_mesg_callback_t cwiid_callback;
volatile unsigned short values[3][NUM_VALUES];
int main()
{
cwiid_wiimote_t *wiimote = NULL;
fputs("Press 1+2 to connect wiimote\n", stdout);
fputs("Put it onto a flat surface and do not move it\n", stdout);
if ((wiimote = cwiid_open(BDADDR_ANY, 0)) == NULL) {
fputs("Unable to connect\n", stderr);
return EXIT_FAILURE;
}
sleep(2);
cwiid_set_led(wiimote, 1);
if (cwiid_set_mesg_callback(wiimote, cwiid_callback))
fputs("cannot set callback\n", stderr);
cwiid_enable(wiimote, CWIID_FLAG_MOTIONPLUS);
if (cwiid_enable(wiimote, CWIID_FLAG_MESG_IFC))
fputs("cannot enable callback\n", stderr);
if (cwiid_set_rpt_mode(wiimote,
CWIID_RPT_EXT))
fputs("cannot set report mode\n", stderr);
while (1) {
/* nothing to do here */
sleep(1);
}
return EXIT_SUCCESS;
}
void show_calibration()
{
long unsigned int total[3] = {0, 0, 0};
unsigned short min[3], max[3];
min[0] = max[0] = values[0][0];
min[1] = max[1] = values[1][0];
min[2] = max[2] = values[2][0];
for (int step = 0; step < NUM_VALUES; step++) {
for (int i = 0; i < 3; i++) {
total[i] += values[i][step];
if (values[i][step] < min[i])
min[i] = values[i][step];
if (values[i][step] > max[i])
max[i] = values[i][step];
}
}
printf("\r\033[2K avg ( min .. max )\n");
printf("Pitch: %5lu (%5d .. %5d)\n",
total[0] / NUM_VALUES, min[0], max[0]);
printf("Roll : %5lu (%5d .. %5d)\n",
total[1] / NUM_VALUES, min[1], max[1]);
printf("Yaw : %5lu (%5d .. %5d)\n",
total[2] / NUM_VALUES, min[2], max[2]);
exit(EXIT_SUCCESS);
}
void cwiid_callback(cwiid_wiimote_t *wiimote, int mesg_count,
union cwiid_mesg mesg[], struct timespec *ts)
{
static int cnt = 0;
static int skip = 100;
if (skip > 0) {
skip--;
return;
}
if (!(cnt % 100)) {
printf("\r\033[2K%4d / %d", cnt, NUM_VALUES);
fflush(stdout);
}
if (cnt == (NUM_VALUES/4))
cwiid_set_led(wiimote, 0x3);
else if (cnt == (NUM_VALUES/2))
cwiid_set_led(wiimote, 0x7);
else if (cnt == (NUM_VALUES*3/4))
cwiid_set_led(wiimote, 0xf);
for (int i = 0; i < mesg_count; i++) {
if (mesg[i].type == CWIID_MESG_MOTIONPLUS) {
values[0][cnt] = mesg[i].motionplus_mesg.angle_rate[0];
values[1][cnt] = mesg[i].motionplus_mesg.angle_rate[1];
values[2][cnt] = mesg[i].motionplus_mesg.angle_rate[2];
cnt++;
if (cnt >= NUM_VALUES)
show_calibration();
}
}
}
|
