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

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

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