source: pacpussensors/trunk/Dualshock/controller.cpp@ 21

Last change on this file since 21 was 21, checked in by phudelai, 11 years ago

New component for the wifibot and the dualshock 3
File size: 7.3 KB
Line 
1#include "controller.h"
2
3using namespace pacpus;
4
5DualshockAPI::DualshockAPI()
6{
7 uid_t uid = getuid();
8 if(uid)
9 throw "Run as root and you will be happy :)";
10 bdaddr_t any = {{0, 0, 0, 0, 0, 0}};
11 csk = l2cap_listen(&any, L2CAP_PSM_HIDP_CTRL);
12 isk = l2cap_listen(&any, L2CAP_PSM_HIDP_INTR);
13 if(csk < 0 || isk < 0)
14 throw "Unable to listen on l2cap socket";
15 quit = 0;
16 controller.paired = 0;
17 shmem = new ShMem("dualshock", sizeof(struct dualshockButtons_s));
18 struct dualshockButtons_s d;
19 d.available = 0;
20 d.time = road_time();
21 shmem->write((void*)&d, sizeof(struct dualshockButtons_s), 0);
22}
23
24int DualshockAPI::l2cap_listen(const bdaddr_t *bdaddr, unsigned short psm)
25{
26 int sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
27 if(sk < 0)
28 throw "Unable to open a socket";
29 struct sockaddr_l2 addr;
30 memset(&addr, 0, sizeof(addr));
31 addr.l2_family = AF_BLUETOOTH;
32 addr.l2_bdaddr = *bdaddr;//(bdaddr_t) {{0, 0, 0, 0, 0, 0}};
33 addr.l2_psm = htobs(psm);
34 if (bind(sk, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
35 close(sk);
36 throw "Bind error";
37 }
38 if (listen(sk, 5) < 0)
39 throw "Listen error";
40 return sk;
41}
42
43void DualshockAPI::update_shared_memory()
44{
45 struct dualshockButtons_s d;
46 d.available = controller.paired;
47 d.buttons = controller.buttons;
48 d.time = road_time();
49 shmem->write((void*)&d, sizeof(struct dualshockButtons_s), 0);
50 dbtFile.writeRecord(d.time, 0, (const char*)&d,
51 sizeof(struct dualshockButtons_s));
52}
53
54void DualshockAPI::main_loop()
55{
56 //std::cerr << "Main loop\n";
57 while(!quit) {
58 fd_set fds;
59 FD_ZERO(&fds);
60 FD_SET(STDIN_FILENO, &fds);
61 int fdmax = 0;
62 FD_SET(csk, &fds);
63 FD_SET(isk, &fds);
64 fdmax = csk > isk ? csk : isk;
65 if(controller.paired) {
66 FD_SET(controller.csk, &fds);
67 if (controller.csk > fdmax)
68 fdmax = controller.csk;
69 FD_SET(controller.isk, &fds);
70 if (controller.isk > fdmax)
71 fdmax = controller.isk;
72 }
73 //std::cerr << " elem\n";
74 //std::cerr << "Select\n";
75 struct timeval tv = {2, 0}; //timeout = 2s
76 if (select(fdmax + 1, &fds, NULL, NULL, &tv) < 0)
77 throw "Select";
78
79 if (FD_ISSET(csk, &fds))
80 handle_connection();
81 if (controller.paired && FD_ISSET(controller.isk, &fds)) {
82 unsigned char report[256];
83 int nr = recv(controller.isk, report, sizeof(report), 0);
84 if (nr <= 0)
85 handle_disconnection();
86 else
87 handle_report(report, nr);
88 }
89 }
90 quit = 2;
91}
92
93void DualshockAPI::stop()
94{
95 quit = 1;
96 while(quit != 2) //avoid a race condition
97 ;
98 dbtFile.close();
99}
100
101void DualshockAPI::handle_connection()
102{
103 std::cerr << "Connection !\n";
104 static int currentIndex = 0;
105 int cs, is;
106 bdaddr_t baddr;
107
108 if((cs = accept(csk, NULL, NULL)) < 0)
109 throw "Accept";
110 if((is = accept(isk, NULL, NULL)) < 0)
111 throw "Accept";
112
113 struct sockaddr_l2 addr;
114 socklen_t addrlen = sizeof(addr);
115 if (getpeername(is, (struct sockaddr *)&addr, &addrlen) < 0)
116 throw "Getpeername";
117 baddr = addr.l2_bdaddr;
118 unsigned char resp[64];
119 char get03f2[] = { HIDP_TRANS_GET_REPORT | HIDP_DATA_RTYPE_FEATURE | 8,
120 '\xf2', sizeof(resp), sizeof(resp)>>8 };
121 (void)send(cs, get03f2, sizeof(get03f2), 0);
122 (void)recv(cs, resp, sizeof(resp), 0);
123
124 if(controller.paired == 1) {
125 close(controller.csk);
126 close(controller.isk);
127 }
128 controller.index = currentIndex++;
129 controller.addr = baddr;
130 controller.paired = 1;
131 controller.csk = cs;
132 controller.isk = is;
133 setup_device(controller.csk);
134}
135
136void DualshockAPI::setup_device(int sk)
137{
138 int index = 1; //FIXME
139 char set03f4[] = { HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_FEATURE,
140 '\xf4', '\x42', '\x03', '\00', '\00' };
141 send(sk, set03f4, sizeof(set03f4), 0);
142 unsigned char ack;
143 int nr = recv(sk, &ack, sizeof(ack), 0);
144 if (nr != 1 || ack != 0)
145 throw "Device is not responding";
146 static const char ledmask[10] = { 1, 2, 4, 8, 6, 7, 11, 13, 14, 15 };
147#define LED_PERMANENT '\xff', '\x27', '\00', '\00', '\x32'
148 char set0201[] = {
149 HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_OUTPUT, 0x01,
150 '\00', '\00', '\00', '\00', '\00', '\00', '\00', '\00',
151 '\00', static_cast<char>(ledmask[index % 10] << 1),
152 LED_PERMANENT,
153 LED_PERMANENT,
154 LED_PERMANENT,
155 LED_PERMANENT,
156 '\00', '\00', '\00', '\00', '\00', '\00', '\00', '\00', '\00', '\00'
157 };
158 set0201[3] = set0201[5] = 4;
159 set0201[5] = 0;
160 set0201[6] = 0x70;
161 send(sk, set0201, sizeof(set0201), 0);
162 nr = recv(sk, &ack, sizeof(ack), 0);
163 if (nr != 1 || ack != 0)
164 throw "Device is not responding";
165}
166
167void DualshockAPI::handle_disconnection()
168{
169 std::cerr << "Disconnection\n";
170 controller.paired = 0;
171}
172
173void DualshockAPI::handle_report(unsigned char buf[], int len)
174{
175 //std::cerr << "Data\n";
176 (void)len;
177 if (buf[0] != 0xa1)
178 return;
179
180 /* Digital */
181 controller.buttons.digital.select = (buf[3] & 0x01) >> 0;
182 controller.buttons.digital.l3 = (buf[3] & 0x02) >> 1;
183 controller.buttons.digital.r3 = (buf[3] & 0x04) >> 2;
184 controller.buttons.digital.start = (buf[3] & 0x08) >> 3;
185 controller.buttons.digital.up = (buf[3] & 0x10) >> 4;
186 controller.buttons.digital.right = (buf[3] & 0x20) >> 5;
187 controller.buttons.digital.down = (buf[3] & 0x40) >> 6;
188 controller.buttons.digital.left = (buf[3] & 0x80) >> 7;
189 controller.buttons.digital.l2 = (buf[4] & 0x01) >> 1;
190 controller.buttons.digital.r2 = (buf[4] & 0x02) >> 2;
191 controller.buttons.digital.l1 = (buf[4] & 0x04) >> 3;
192 controller.buttons.digital.r1 = (buf[4] & 0x08) >> 4;
193 controller.buttons.digital.triangle = (buf[4] & 0x10) >> 5;
194 controller.buttons.digital.circle = (buf[4] & 0x20) >> 6;
195 controller.buttons.digital.cross = (buf[4] & 0x40) >> 7;
196 controller.buttons.digital.square = (buf[4] & 0x80) >> 8;
197
198 /* Sticks */
199 controller.buttons.stick.leftStick_x = buf[7];
200 controller.buttons.stick.leftStick_y = buf[8];
201 controller.buttons.stick.rightStick_x = buf[9];
202 controller.buttons.stick.rightStick_y = buf[10];
203
204 /* Analog */
205 controller.buttons.analog.l2 = buf[19];
206 controller.buttons.analog.r2 = buf[20];
207 controller.buttons.analog.l1 = buf[21];
208 controller.buttons.analog.r1 = buf[22];
209 controller.buttons.analog.triangle = buf[23];
210 controller.buttons.analog.circle = buf[24];
211 controller.buttons.analog.cross = buf[25];
212 controller.buttons.analog.square = buf[26];
213
214 /* Axis */
215 controller.buttons.axis.x = buf[43];
216 controller.buttons.axis.y = buf[45];
217 controller.buttons.axis.z = buf[47];
218 controller.buttons.axis.gZ = buf[47];
219
220 update_shared_memory();
221}
222
223void DualshockAPI::run()
224{
225 dbtFile.open("dualshock.dbt", WriteMode, FILE_DBT_UNKNOWN,
226 sizeof(struct dualshockButtons_s));
227 quit = 0;
228 main_loop();
229}
230
231DualshockAPI::~DualshockAPI()
232{
233 delete(shmem);
234}
Note: See TracBrowser for help on using the repository browser.