1 | // %flair:license{
|
---|
2 | // This file is part of the Flair framework distributed under the
|
---|
3 | // CECILL-C License, Version 1.0.
|
---|
4 | // %flair:license}
|
---|
5 | // created: 2015/03/30
|
---|
6 | // filename: DualShock3.h
|
---|
7 | //
|
---|
8 | // author: Gildas Bayard
|
---|
9 | // Copyright Heudiasyc UMR UTC/CNRS 7253
|
---|
10 | //
|
---|
11 | // version: $Id: $
|
---|
12 | //
|
---|
13 | // purpose: Base class for host side remote controls that talks to target
|
---|
14 | // side through ethernet connection
|
---|
15 | //
|
---|
16 | //
|
---|
17 | /*********************************************************************/
|
---|
18 | #include "DualShock3.h"
|
---|
19 | #include <Controller.h>
|
---|
20 | #include <Matrix.h>
|
---|
21 | #include <Tab.h>
|
---|
22 | #include <TabWidget.h>
|
---|
23 | #include <CheckBox.h>
|
---|
24 | #include <Label.h>
|
---|
25 | #include <DataPlot1D.h>
|
---|
26 | #include <SpinBox.h>
|
---|
27 | #include <GroupBox.h>
|
---|
28 | #include <FrameworkManager.h>
|
---|
29 | #include <sstream>
|
---|
30 |
|
---|
31 | #include <stdlib.h>
|
---|
32 | #include <unistd.h>
|
---|
33 | #include <errno.h>
|
---|
34 | #include <fcntl.h>
|
---|
35 | #include <sstream>
|
---|
36 | #include <linux/input.h>
|
---|
37 | #include <linux/hidraw.h>
|
---|
38 | #include <bluetooth/hci.h>
|
---|
39 | #include <bluetooth/hci_lib.h>
|
---|
40 | #include <sys/file.h>
|
---|
41 | #include <linux/input.h>
|
---|
42 |
|
---|
43 | #define USB_DIR_IN 0x80
|
---|
44 | #define USB_DIR_OUT 0
|
---|
45 | #define USB_GET_REPORT 0x01
|
---|
46 | #define USB_SET_REPORT 0x09
|
---|
47 | #define VENDOR_SONY 0x054c
|
---|
48 | #define PRODUCT_SIXAXIS_DS3 0x0268
|
---|
49 |
|
---|
50 | #define L2CAP_PSM_HIDP_CTRL 0x11
|
---|
51 | #define L2CAP_PSM_HIDP_INTR 0x13
|
---|
52 |
|
---|
53 | #define HIDP_TRANS_GET_REPORT 0x40
|
---|
54 | #define HIDP_TRANS_SET_REPORT 0x50
|
---|
55 | #define HIDP_DATA_RTYPE_OUTPUT 0x02
|
---|
56 | #define HIDP_DATA_RTYPE_FEATURE 0x03
|
---|
57 |
|
---|
58 | #define SIXAXIS 1
|
---|
59 | #define DUALSHOCK3 2
|
---|
60 |
|
---|
61 | #define X_AXIS_RANGE 127
|
---|
62 | #define Y_AXIS_RANGE 127
|
---|
63 |
|
---|
64 | using namespace flair::core;
|
---|
65 | using namespace flair::gui;
|
---|
66 | using namespace std;
|
---|
67 |
|
---|
68 | typedef struct motion_dev {
|
---|
69 | int index;
|
---|
70 | bdaddr_t addr;
|
---|
71 | char type;
|
---|
72 | int csk;
|
---|
73 | int isk;
|
---|
74 | struct motion_dev *next;
|
---|
75 | } motion_dev_t;
|
---|
76 |
|
---|
77 | bdaddr_t bdaddr_any = {{0, 0, 0, 0, 0, 0}};
|
---|
78 |
|
---|
79 | namespace flair {
|
---|
80 | namespace sensor {
|
---|
81 |
|
---|
82 | DualShock3::DualShock3(string name,
|
---|
83 | string receiverAddress, int receiverPort,
|
---|
84 | ConnectionType_t _connectionType, uint32_t period,
|
---|
85 | uint32_t bitsPerAxis, uint8_t priority)
|
---|
86 | : HostEthController(name, receiverAddress, receiverPort, period,
|
---|
87 | bitsPerAxis, priority),
|
---|
88 | connectionType(_connectionType) {
|
---|
89 | controllerName = "DualShock3";
|
---|
90 | last_voltage_time = 0;
|
---|
91 |
|
---|
92 | // axis stuff
|
---|
93 | axisNumber = 4;
|
---|
94 | nativeBitsPerAxis = 8;
|
---|
95 | MatrixDescriptor *axisDescriptor = new MatrixDescriptor(axisNumber, 1);
|
---|
96 | for (unsigned int i = 0; i < axisNumber; i++) {
|
---|
97 | axisDescriptor->SetElementName(i, 0, GetAxisDescription(i));
|
---|
98 | }
|
---|
99 | axis = new Matrix((IODevice *)this, axisDescriptor, Int16Type);
|
---|
100 | delete axisDescriptor;
|
---|
101 | AddDataToLog(axis);
|
---|
102 |
|
---|
103 | // buttons stuff
|
---|
104 | buttonNumber = 16;
|
---|
105 | MatrixDescriptor *buttonDescriptor =
|
---|
106 | new MatrixDescriptor(buttonNumber, 1);
|
---|
107 | for (unsigned int i = 0; i < buttonNumber; i++) {
|
---|
108 | buttonDescriptor->SetElementName(i, 0, GetButtonDescription(i));
|
---|
109 | }
|
---|
110 | button = new Matrix((IODevice *)this, buttonDescriptor, Int8Type);
|
---|
111 | AddDataToLog(button);
|
---|
112 |
|
---|
113 | Tab *settingsTab = new Tab(tabWidget, "Settings");
|
---|
114 | dataSize = 6;
|
---|
115 | datas = new int8_t[dataSize];
|
---|
116 | for(int i=0;i<dataSize;i++) datas[i]=0;
|
---|
117 |
|
---|
118 | GroupBox *settingsGroupBox =
|
---|
119 | new GroupBox(settingsTab->NewRow(), controllerName);
|
---|
120 | deadZone =
|
---|
121 | new SpinBox(settingsGroupBox->NewRow(), "dead zone:", -130, 130, 1);
|
---|
122 | batteryChargeLevel =
|
---|
123 | new Label(settingsGroupBox->LastRowLastCol(), "battery charge level");
|
---|
124 | enabled = new CheckBox(settingsGroupBox->LastRowLastCol(), "enabled");
|
---|
125 |
|
---|
126 | if (connectionType == Bluetooth) {
|
---|
127 | // init DS3
|
---|
128 | usb_scan();
|
---|
129 |
|
---|
130 | int csk = l2cap_listen(&bdaddr_any, L2CAP_PSM_HIDP_CTRL);
|
---|
131 | isk = l2cap_listen(&bdaddr_any, L2CAP_PSM_HIDP_INTR);
|
---|
132 |
|
---|
133 | if (csk >= 0 && isk >= 0)
|
---|
134 | Printf("Waiting for Bluetooth connections.\n");
|
---|
135 | else
|
---|
136 | Thread::Err("Unable to listen on HID PSMs.\n");
|
---|
137 |
|
---|
138 | fd_set fds;
|
---|
139 | FD_ZERO(&fds);
|
---|
140 |
|
---|
141 | if (csk >= 0)
|
---|
142 | FD_SET(csk, &fds);
|
---|
143 | if (select(csk + 1, &fds, NULL, NULL, NULL) < 0)
|
---|
144 | Thread::Err("select\n");
|
---|
145 | // Incoming connection ?
|
---|
146 |
|
---|
147 | if (csk >= 0 && FD_ISSET(csk, &fds)) {
|
---|
148 | // printf("accept\n");
|
---|
149 | dev = accept_device(csk, isk);
|
---|
150 | setup_device(dev);
|
---|
151 | }
|
---|
152 | } else if (connectionType == UsbHidRaw) {
|
---|
153 | int nr, i;
|
---|
154 | unsigned char buf[128];
|
---|
155 |
|
---|
156 | for (i = 0; i < 255; i++) {
|
---|
157 | ostringstream dev_name;
|
---|
158 | dev_name << "/dev/hidraw" << i;
|
---|
159 | if ((usb_fd = open(dev_name.str().c_str(), O_RDONLY)) >= 0) {
|
---|
160 | int res=flock(usb_fd, LOCK_EX|LOCK_NB);
|
---|
161 | if(res<0) {
|
---|
162 | Thread::Warn("%s seems to be locked by another application\n", dev_name.str().c_str());
|
---|
163 | close(usb_fd);
|
---|
164 | continue;
|
---|
165 | }
|
---|
166 |
|
---|
167 | struct hidraw_devinfo info;
|
---|
168 | res = ioctl(usb_fd, HIDIOCGRAWINFO, &info);
|
---|
169 | if (res < 0) {
|
---|
170 | Thread::Err("ioctl error (HIDIOCGRAWINFO) on %s\n",dev_name.str().c_str());
|
---|
171 | } else {
|
---|
172 | // Printf("%x %x\n", info.vendor, info.product);
|
---|
173 | if (info.vendor == 0x054c && info.product == 0x0268) {
|
---|
174 | Printf("successfully opened %s\n", dev_name.str().c_str());
|
---|
175 | Printf("Press PS button to turn the controller on\n");
|
---|
176 | break;
|
---|
177 | }
|
---|
178 | }
|
---|
179 | flock(usb_fd, LOCK_UN);
|
---|
180 | close(usb_fd);
|
---|
181 | }
|
---|
182 | }
|
---|
183 | if (i == 255) {
|
---|
184 | Thread::Err("open(hidrawX) - failed to open hidraw device\n");
|
---|
185 | return;
|
---|
186 | }
|
---|
187 |
|
---|
188 | // block until PS button is pressed
|
---|
189 | if ((nr = read(usb_fd, buf, sizeof(buf))) < 0) {
|
---|
190 | Thread::Err("read(fd) - failed to read from device\n");
|
---|
191 | }
|
---|
192 |
|
---|
193 | if (nr < 49 || nr > 50) {
|
---|
194 | Thread::Err("read(fd) - not a sixaxis (nr = %i )\n", nr);
|
---|
195 | }
|
---|
196 | } else if (connectionType == UsbEvent) {
|
---|
197 | int nr, i;
|
---|
198 | unsigned char buf[128];
|
---|
199 |
|
---|
200 | for (i = 0; i < 255; i++) {
|
---|
201 | ostringstream dev_name;
|
---|
202 | dev_name << "/dev/input/event" << i;
|
---|
203 | if ((usb_fd = open(dev_name.str().c_str(), O_RDONLY)) >= 0) {
|
---|
204 | int res=flock(usb_fd, LOCK_EX|LOCK_NB);
|
---|
205 | if(res<0) {
|
---|
206 | Thread::Warn("%s seems to be locked by another application\n", dev_name.str().c_str());
|
---|
207 | close(usb_fd);
|
---|
208 | continue;
|
---|
209 | }
|
---|
210 |
|
---|
211 | unsigned short id[4];
|
---|
212 | res = ioctl(usb_fd, EVIOCGID, id);
|
---|
213 | if (res < 0) {
|
---|
214 | Thread::Err("ioctl error (EVIOCGID) on %s\n",dev_name.str().c_str());
|
---|
215 | } else {
|
---|
216 | if (id[ID_VENDOR] == 0x054c && id[ID_PRODUCT] == 0x0268) {
|
---|
217 | Printf("successfully opened %s\n", dev_name.str().c_str());
|
---|
218 | Printf("Press PS button to turn the controller on\n");
|
---|
219 | break;
|
---|
220 | }
|
---|
221 | }
|
---|
222 | flock(usb_fd, LOCK_UN);
|
---|
223 | close(usb_fd);
|
---|
224 | }
|
---|
225 | }
|
---|
226 | if (i == 255) {
|
---|
227 | Thread::Err("open(/dev/input/eventX) - failed to open event device\n");
|
---|
228 | Thread::Err("you can try with the old usb_hidraw method\n");
|
---|
229 | return;
|
---|
230 | }
|
---|
231 |
|
---|
232 | // block until PS button is pressed
|
---|
233 | struct input_event tmp;
|
---|
234 | if ((nr = read(usb_fd, &tmp, sizeof(struct input_event))) < 0) {
|
---|
235 | Thread::Err("read(fd) - failed to read from device\n");
|
---|
236 | }
|
---|
237 | }
|
---|
238 | ledmask = 0;
|
---|
239 | }
|
---|
240 |
|
---|
241 | DualShock3::~DualShock3() {
|
---|
242 | if (connectionType == UsbHidRaw || connectionType == UsbEvent) {
|
---|
243 | flock(usb_fd, LOCK_UN);
|
---|
244 | close(usb_fd);
|
---|
245 | }
|
---|
246 | if (connectionType == Bluetooth) {
|
---|
247 | if (!popen("/etc/init.d/bluetooth restart", "r"))
|
---|
248 | Thread::Warn("Could not restart bluetooth service\n");
|
---|
249 | }
|
---|
250 | }
|
---|
251 |
|
---|
252 | string DualShock3::GetAxisDescription(unsigned int axis) {
|
---|
253 | string description;
|
---|
254 |
|
---|
255 | switch (axis) {
|
---|
256 | case 0:
|
---|
257 | description = "left stick x-axis";
|
---|
258 | break;
|
---|
259 | case 1:
|
---|
260 | description = "left stick y-axis";
|
---|
261 | break;
|
---|
262 | case 2:
|
---|
263 | description = "right stick x-axis";
|
---|
264 | break;
|
---|
265 | case 3:
|
---|
266 | description = "right stick y-axis";
|
---|
267 | break;
|
---|
268 | }
|
---|
269 | return description;
|
---|
270 | }
|
---|
271 |
|
---|
272 | string DualShock3::GetButtonDescription(unsigned int button) {
|
---|
273 | switch (button) {
|
---|
274 | case 0:
|
---|
275 | return "start";
|
---|
276 | break;
|
---|
277 | case 1:
|
---|
278 | return "select";
|
---|
279 | break;
|
---|
280 | case 2:
|
---|
281 | return "square";
|
---|
282 | break;
|
---|
283 | case 3:
|
---|
284 | return "triangle";
|
---|
285 | break;
|
---|
286 | case 4:
|
---|
287 | return "circle";
|
---|
288 | break;
|
---|
289 | case 5:
|
---|
290 | return "cross";
|
---|
291 | break;
|
---|
292 | case 6:
|
---|
293 | return "left 1";
|
---|
294 | break;
|
---|
295 | case 7:
|
---|
296 | return "left 2";
|
---|
297 | break;
|
---|
298 | case 8:
|
---|
299 | return "left 3";
|
---|
300 | break;
|
---|
301 | case 9:
|
---|
302 | return "right 1";
|
---|
303 | break;
|
---|
304 | case 10:
|
---|
305 | return "right 2";
|
---|
306 | break;
|
---|
307 | case 11:
|
---|
308 | return "right 3";
|
---|
309 | break;
|
---|
310 | case 12:
|
---|
311 | return "up";
|
---|
312 | break;
|
---|
313 | case 13:
|
---|
314 | return "down";
|
---|
315 | break;
|
---|
316 | case 14:
|
---|
317 | return "left";
|
---|
318 | break;
|
---|
319 | case 15:
|
---|
320 | return "right";
|
---|
321 | break;
|
---|
322 | }
|
---|
323 | }
|
---|
324 |
|
---|
325 | bool DualShock3::IsDataFrameReady() {
|
---|
326 | unsigned char report[256];
|
---|
327 | unsigned char tmp_report[256];
|
---|
328 |
|
---|
329 | if (!enabled->IsChecked()) {
|
---|
330 | meaningfulDataAvailable = false;
|
---|
331 | usleep(100000);
|
---|
332 | return false;
|
---|
333 | }
|
---|
334 | now = GetTime();
|
---|
335 | if (connectionType == Bluetooth) {
|
---|
336 | fd_set fds;
|
---|
337 | FD_ZERO(&fds);
|
---|
338 | int fdmax = 0; /*
|
---|
339 | if ( isk >= 0 ) FD_SET(isk, &fds);
|
---|
340 | if ( isk > fdmax ) fdmax = isk;
|
---|
341 | */
|
---|
342 | FD_SET(dev->isk, &fds);
|
---|
343 | if (dev->isk > fdmax)
|
---|
344 | fdmax = dev->isk;
|
---|
345 |
|
---|
346 | if (select(fdmax + 1, &fds, NULL, NULL, NULL) < 0)
|
---|
347 | fatal("select");
|
---|
348 |
|
---|
349 | // Incoming input report ?
|
---|
350 | if (FD_ISSET(dev->isk, &fds)) {
|
---|
351 | int nr;
|
---|
352 | int recv_result;
|
---|
353 | bool flushed = false;
|
---|
354 | while (!flushed) {
|
---|
355 | recv_result = recv(dev->isk, tmp_report, sizeof(report), MSG_DONTWAIT);
|
---|
356 | if (recv_result <= 0) {
|
---|
357 | if ((errno != EAGAIN) && (errno != EWOULDBLOCK)) {
|
---|
358 | fprintf(stderr, "%d disconnected\n", dev->index);
|
---|
359 | close(dev->csk);
|
---|
360 | close(dev->isk);
|
---|
361 | free(dev);
|
---|
362 | return false;
|
---|
363 | } else {
|
---|
364 | flushed = true;
|
---|
365 | // fprintf(stderr, "\n");
|
---|
366 | continue;
|
---|
367 | }
|
---|
368 | } else {
|
---|
369 | // fprintf(stderr, ".");
|
---|
370 | nr = recv_result;
|
---|
371 | memcpy(report, tmp_report, nr);
|
---|
372 | }
|
---|
373 | }
|
---|
374 | if (report[0] == 0xa1) {
|
---|
375 | return parse_report_sixaxis_ds3(report + 1, nr - 1);
|
---|
376 | }
|
---|
377 | }
|
---|
378 | return false;
|
---|
379 |
|
---|
380 | } else if (connectionType == UsbHidRaw) {
|
---|
381 | int nr = read(usb_fd, report, sizeof(report));
|
---|
382 | return parse_report_sixaxis_ds3(report, nr);
|
---|
383 | } else if (connectionType == UsbEvent) {
|
---|
384 | return parse_input_event(100);
|
---|
385 | }
|
---|
386 | return false;
|
---|
387 | }
|
---|
388 |
|
---|
389 | uint8_t DualShock3::SetBitValue(int8_t ®,uint8_t mask,uint8_t value) {
|
---|
390 | reg=(reg&~mask) | value*mask;
|
---|
391 | }
|
---|
392 |
|
---|
393 | bool DualShock3::parse_input_event(int timeout_ms) {
|
---|
394 | struct input_event ev[64];
|
---|
395 | int i, rd;
|
---|
396 | fd_set set;
|
---|
397 | struct timeval timeout;
|
---|
398 |
|
---|
399 | FD_ZERO(&set); // clear the set
|
---|
400 | FD_SET(usb_fd, &set); // add our file descriptor to the set
|
---|
401 |
|
---|
402 | timeout.tv_sec = 0;
|
---|
403 | timeout.tv_usec = timeout_ms * 1000;
|
---|
404 |
|
---|
405 | int rv = select(FD_SETSIZE, &set, NULL, NULL, &timeout);
|
---|
406 |
|
---|
407 | if (rv <=0) {
|
---|
408 | return false;
|
---|
409 | }
|
---|
410 |
|
---|
411 | rd = read(usb_fd, ev, sizeof(struct input_event) * 64);
|
---|
412 |
|
---|
413 | if (rd < (int) sizeof(struct input_event)) {
|
---|
414 | printf("expected %d bytes, got %d\n", (int) sizeof(struct input_event), rd);
|
---|
415 | return false;
|
---|
416 | }
|
---|
417 |
|
---|
418 | for (i = 0; i < rd / sizeof(struct input_event); i++) {
|
---|
419 | if (ev[i].type == EV_ABS) {
|
---|
420 | //code 2 and 5 are z/rz (l2 and r2)
|
---|
421 | if(ev[i].code==0) datas[2] = compute_dead_zone(0, ev[i].value);//x
|
---|
422 | if(ev[i].code==1) datas[3] = compute_dead_zone(1, ev[i].value);//y
|
---|
423 | if(ev[i].code==3) datas[4] = compute_dead_zone(2, ev[i].value);//rx
|
---|
424 | if(ev[i].code==4) datas[5] = compute_dead_zone(3, ev[i].value);//ry
|
---|
425 | }
|
---|
426 | if (ev[i].type == EV_KEY) {
|
---|
427 | if(ev[i].code==BTN_START) SetBitValue(datas[0],0x08,ev[i].value);
|
---|
428 | if(ev[i].code==BTN_SELECT) SetBitValue(datas[0],0x01,ev[i].value);
|
---|
429 | if(ev[i].code==BTN_WEST) SetBitValue(datas[1],0x80,ev[i].value);// square
|
---|
430 | if(ev[i].code==BTN_NORTH) SetBitValue(datas[1],0x10,ev[i].value);// triangle
|
---|
431 | if(ev[i].code==BTN_EAST) SetBitValue(datas[1],0x20,ev[i].value);// circle
|
---|
432 | if(ev[i].code==BTN_SOUTH) SetBitValue(datas[1],0x40,ev[i].value);// cross
|
---|
433 | if(ev[i].code==BTN_TL) SetBitValue(datas[1],0x04,ev[i].value);
|
---|
434 | if(ev[i].code==BTN_TL2) SetBitValue(datas[1],0x01,ev[i].value);
|
---|
435 | if(ev[i].code==BTN_THUMBL) SetBitValue(datas[0],0x02,ev[i].value);
|
---|
436 | if(ev[i].code==BTN_TR) SetBitValue(datas[1],0x08,ev[i].value);
|
---|
437 | if(ev[i].code==BTN_TR2) SetBitValue(datas[1],0x02,ev[i].value);
|
---|
438 | if(ev[i].code==BTN_THUMBR) SetBitValue(datas[0],0x04,ev[i].value);
|
---|
439 | if(ev[i].code==BTN_DPAD_UP) SetBitValue(datas[0],0x10,ev[i].value);
|
---|
440 | if(ev[i].code==BTN_DPAD_DOWN) SetBitValue(datas[0],0x40,ev[i].value);
|
---|
441 | if(ev[i].code==BTN_DPAD_LEFT) SetBitValue(datas[0],0x80,ev[i].value);
|
---|
442 | if(ev[i].code==BTN_DPAD_RIGHT) SetBitValue(datas[0],0x20,ev[i].value);
|
---|
443 | }
|
---|
444 |
|
---|
445 | if (GetTime() > (last_voltage_time + 5 * (Time)1000000000)) {
|
---|
446 | // toute les 5 secondes
|
---|
447 | // report battery charge level
|
---|
448 | batteryChargeLevel->SetText("battery: usb connected");
|
---|
449 | last_voltage_time = GetTime();
|
---|
450 | }
|
---|
451 | }
|
---|
452 | return true;
|
---|
453 | }
|
---|
454 |
|
---|
455 | bool DualShock3::parse_report_sixaxis_ds3(unsigned char *r, int len) {
|
---|
456 | if (r[0] == 0x01 && r[1] == 0 && len >= 49) {
|
---|
457 | datas[0] = r[2];
|
---|
458 | datas[1] = r[3];
|
---|
459 | datas[2] = compute_dead_zone(0, r[6]);
|
---|
460 | datas[3] = compute_dead_zone(1, r[7]);
|
---|
461 | datas[4] = compute_dead_zone(2, r[8]);
|
---|
462 | datas[5] = compute_dead_zone(3, r[9]);
|
---|
463 |
|
---|
464 | if (GetTime() > (last_voltage_time + 5 * (Time)1000000000)) {
|
---|
465 | // toute les 5 secondes
|
---|
466 | // report battery charge level
|
---|
467 | if (connectionType == Bluetooth) {
|
---|
468 | batteryChargeLevel->SetText("battery: %i/5", r[30]);
|
---|
469 | }
|
---|
470 | if (connectionType == UsbHidRaw) {
|
---|
471 | batteryChargeLevel->SetText("battery: usb connected");
|
---|
472 | }
|
---|
473 | last_voltage_time = GetTime();
|
---|
474 | }
|
---|
475 |
|
---|
476 | return true;
|
---|
477 | }
|
---|
478 | return false;
|
---|
479 | }
|
---|
480 |
|
---|
481 | void DualShock3::GetAxisData() {
|
---|
482 |
|
---|
483 | axis->GetMutex();
|
---|
484 | // axis->SetValueNoMutex(0, 0,datas[2]/(float)X_AXIS_RANGE); //left stick
|
---|
485 | // x-axis
|
---|
486 | // axis->SetValueNoMutex(1, 0,datas[3]/(float)Y_AXIS_RANGE); //left stick
|
---|
487 | // y-axis
|
---|
488 | // axis->SetValueNoMutex(2, 0,datas[4]/(float)X_AXIS_RANGE); //right stick
|
---|
489 | // x-axis
|
---|
490 | // axis->SetValueNoMutex(3, 0,datas[5]/(float)Y_AXIS_RANGE); //right stick
|
---|
491 | // y-axis
|
---|
492 | axis->SetValueNoMutex(0, 0, datas[2]); // left stick x-axis
|
---|
493 | axis->SetValueNoMutex(1, 0, datas[3]); // left stick y-axis
|
---|
494 | axis->SetValueNoMutex(2, 0, datas[4]); // right stick x-axis
|
---|
495 | axis->SetValueNoMutex(3, 0, datas[5]); // right stick y-axis
|
---|
496 | axis->ReleaseMutex();
|
---|
497 | axis->SetDataTime(now);
|
---|
498 | }
|
---|
499 |
|
---|
500 | void DualShock3::GetButtonData() {
|
---|
501 | button->GetMutex();
|
---|
502 | button->SetValueNoMutex(0, 0, (datas[0] & 0x08) == 0 ? 0 : 1); // start
|
---|
503 | button->SetValueNoMutex(1, 0, (datas[0] & 0x01) == 0 ? 0 : 1); // select
|
---|
504 | button->SetValueNoMutex(2, 0, (datas[1] & 0x80) == 0 ? 0 : 1); // square
|
---|
505 | button->SetValueNoMutex(3, 0, (datas[1] & 0x10) == 0 ? 0 : 1); // triangle
|
---|
506 | button->SetValueNoMutex(4, 0, (datas[1] & 0x20) == 0 ? 0 : 1); // circle
|
---|
507 | button->SetValueNoMutex(5, 0, (datas[1] & 0x40) == 0 ? 0 : 1); // cross
|
---|
508 | button->SetValueNoMutex(6, 0, (datas[1] & 0x04) == 0 ? 0 : 1); // left 1
|
---|
509 | button->SetValueNoMutex(7, 0, (datas[1] & 0x01) == 0 ? 0 : 1); // left 2
|
---|
510 | button->SetValueNoMutex(8, 0, (datas[0] & 0x02) == 0 ? 0 : 1); // left 3
|
---|
511 | button->SetValueNoMutex(9, 0, (datas[1] & 0x08) == 0 ? 0 : 1); // right 1
|
---|
512 | button->SetValueNoMutex(10, 0, (datas[1] & 0x02) == 0 ? 0 : 1); // right 2
|
---|
513 | button->SetValueNoMutex(11, 0, (datas[0] & 0x04) == 0 ? 0 : 1); // right 3
|
---|
514 | button->SetValueNoMutex(12, 0, (datas[0] & 0x10) == 0 ? 0 : 1); // up
|
---|
515 | button->SetValueNoMutex(13, 0, (datas[0] & 0x40) == 0 ? 0 : 1); // down
|
---|
516 | button->SetValueNoMutex(14, 0, (datas[0] & 0x80) == 0 ? 0 : 1); // left
|
---|
517 | button->SetValueNoMutex(15, 0, (datas[0] & 0x20) == 0 ? 0 : 1); // right
|
---|
518 | button->ReleaseMutex();
|
---|
519 | button->SetDataTime(now);
|
---|
520 | }
|
---|
521 |
|
---|
522 | void DualShock3::ProcessMessage(core::Message *controllerAction) {
|
---|
523 | ControllerAction action;
|
---|
524 | memcpy(&action, controllerAction->buffer, sizeof(ControllerAction));
|
---|
525 | if (action == ControllerAction::SetLedOn) {
|
---|
526 | Thread::Info("LedOn action request\n");
|
---|
527 | } else if (action == ControllerAction::SetLedOff) {
|
---|
528 | Thread::Info("LedOff action request\n");
|
---|
529 | } else if (action == ControllerAction::Rumble) {
|
---|
530 | Thread::Info("Rumble action request\n");
|
---|
531 | } else if (action == ControllerAction::FlashLed) {
|
---|
532 | Thread::Info("FlashLed action request\n");
|
---|
533 | }
|
---|
534 | // (char *msg, int msgSize)
|
---|
535 | /* for (unsigned int i=0; i<4; i++) {
|
---|
536 | if(msg[4+2*i]!=0 || msg[5+2*i]!=0) set_led(i+1,msg[4+2*i],msg[5+2*i]);
|
---|
537 |
|
---|
538 | }
|
---|
539 | if(msg[0]!=0 || msg[2]!=0) rumble(msg[0],msg[1],msg[2],msg[3]);
|
---|
540 | */
|
---|
541 | }
|
---|
542 |
|
---|
543 | // ----------------------------------------------------------------------
|
---|
544 | // Replacement for libbluetooth
|
---|
545 |
|
---|
546 | int DualShock3::mystr2ba(const char *s, bdaddr_t *ba) {
|
---|
547 | if (strlen(s) != 17)
|
---|
548 | return 1;
|
---|
549 | for (int i = 0; i < 6; ++i) {
|
---|
550 | int d = strtol(s + 15 - 3 * i, NULL, 16);
|
---|
551 | if (d < 0 || d > 255)
|
---|
552 | return 1;
|
---|
553 | ba->b[i] = d;
|
---|
554 | }
|
---|
555 | return 0;
|
---|
556 | }
|
---|
557 |
|
---|
558 | char *DualShock3::myba2str(const bdaddr_t *ba) {
|
---|
559 | static char buf[2][18]; // Static buffer valid for two invocations.
|
---|
560 | static int index = 0;
|
---|
561 | index = (index + 1) % 2;
|
---|
562 | sprintf(buf[index], "%02x:%02x:%02x:%02x:%02x:%02x", ba->b[5], ba->b[4],
|
---|
563 | ba->b[3], ba->b[2], ba->b[1], ba->b[0]);
|
---|
564 | return buf[index];
|
---|
565 | }
|
---|
566 |
|
---|
567 | void DualShock3::fatal(const char *msg) {
|
---|
568 | if (errno)
|
---|
569 | perror(msg);
|
---|
570 | else
|
---|
571 | fprintf(stderr, "%s\n", msg);
|
---|
572 | exit(1);
|
---|
573 | }
|
---|
574 |
|
---|
575 | /**********************************************************************/
|
---|
576 | // Bluetooth HID devices
|
---|
577 | // Incoming connections.
|
---|
578 |
|
---|
579 | int DualShock3::l2cap_listen(const bdaddr_t *bdaddr, unsigned short psm) {
|
---|
580 | int sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
|
---|
581 | if (sk < 0)
|
---|
582 | fatal("socket");
|
---|
583 |
|
---|
584 | struct sockaddr_l2 addr;
|
---|
585 | addr.l2_family = AF_BLUETOOTH;
|
---|
586 | addr.l2_bdaddr = *BDADDR_ANY;
|
---|
587 | addr.l2_psm = htobs(psm);
|
---|
588 | addr.l2_cid = 0;
|
---|
589 |
|
---|
590 | if (bind(sk, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
|
---|
591 | close(sk);
|
---|
592 | char errorMsg[256];
|
---|
593 | Thread::Err("bth bind, %s\n", strerror_r(errno, errorMsg, sizeof(errorMsg)));
|
---|
594 | return -1;
|
---|
595 | }
|
---|
596 |
|
---|
597 | if (listen(sk, 5) < 0)
|
---|
598 | fatal("listen");
|
---|
599 | return sk;
|
---|
600 | }
|
---|
601 |
|
---|
602 | struct motion_dev *DualShock3::accept_device(int csk, int isk) {
|
---|
603 | Printf("Incoming connection...\n");
|
---|
604 | struct motion_dev *dev = (motion_dev *)malloc(sizeof(struct motion_dev));
|
---|
605 | if (!dev)
|
---|
606 | fatal("malloc");
|
---|
607 |
|
---|
608 | dev->csk = accept(csk, NULL, NULL);
|
---|
609 | if (dev->csk < 0)
|
---|
610 | fatal("accept(CTRL)");
|
---|
611 | dev->isk = accept(isk, NULL, NULL);
|
---|
612 | if (dev->isk < 0)
|
---|
613 | fatal("accept(INTR)");
|
---|
614 |
|
---|
615 | struct sockaddr_l2 addr;
|
---|
616 | socklen_t addrlen = sizeof(addr);
|
---|
617 | if (getpeername(dev->isk, (struct sockaddr *)&addr, &addrlen) < 0)
|
---|
618 | fatal("getpeername");
|
---|
619 | dev->addr = addr.l2_bdaddr;
|
---|
620 |
|
---|
621 | // Distinguish SIXAXIS / DS3
|
---|
622 | unsigned char resp[64];
|
---|
623 | char get03f2[] = {HIDP_TRANS_GET_REPORT | HIDP_DATA_RTYPE_FEATURE | 8,
|
---|
624 | (char)0xf2, sizeof(resp), sizeof(resp) >> 8};
|
---|
625 | send(dev->csk, get03f2, sizeof(get03f2), 0); // 0301 is interesting too.
|
---|
626 | int nr = recv(dev->csk, resp, sizeof(resp), 0);
|
---|
627 |
|
---|
628 | dev->type = (resp[13] == 0x40) ? SIXAXIS : DUALSHOCK3; // My guess.
|
---|
629 |
|
---|
630 | return dev;
|
---|
631 | }
|
---|
632 |
|
---|
633 | /**********************************************************************/
|
---|
634 | // Device setup
|
---|
635 |
|
---|
636 | #define IR0 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, 0x00, 0x41
|
---|
637 | #define IR1 0x40, 0x00
|
---|
638 |
|
---|
639 | #define BIT1 2
|
---|
640 |
|
---|
641 | void DualShock3::hidp_trans(int csk, char *buf, int len) {
|
---|
642 | if (send(csk, buf, len, 0) != len)
|
---|
643 | fatal("send(CTRL)");
|
---|
644 | char ack;
|
---|
645 | int nr = recv(csk, &ack, sizeof(ack), 0);
|
---|
646 | if (nr != 1 || ack != 0)
|
---|
647 | fatal("ack");
|
---|
648 | }
|
---|
649 |
|
---|
650 | void DualShock3::setup_device(struct motion_dev *dev) {
|
---|
651 |
|
---|
652 | switch (dev->type) {
|
---|
653 | case SIXAXIS:
|
---|
654 | case DUALSHOCK3:
|
---|
655 | // Enable reporting
|
---|
656 | char set03f4[] = {HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_FEATURE,
|
---|
657 | (char)0xf4, 0x42, 0x03, 0x00, 0x00};
|
---|
658 | hidp_trans(dev->csk, set03f4, sizeof(set03f4));
|
---|
659 |
|
---|
660 | break;
|
---|
661 | }
|
---|
662 | }
|
---|
663 |
|
---|
664 | /**********************************************************************/
|
---|
665 | // Reports
|
---|
666 |
|
---|
667 | // faire un reglage station sol pour dead zone
|
---|
668 | int8_t DualShock3::compute_dead_zone(int axis, unsigned char value) {
|
---|
669 | float tmp;
|
---|
670 |
|
---|
671 | if (value > 128 + deadZone->Value()) {
|
---|
672 | tmp =
|
---|
673 | (value - deadZone->Value() - 128.) * 128. / (128. - deadZone->Value());
|
---|
674 | } else if (value < 128 - deadZone->Value()) {
|
---|
675 | // return value+DEAD_ZONE-128;
|
---|
676 | tmp =
|
---|
677 | (value + deadZone->Value() - 128.) * 127. / (128. - deadZone->Value());
|
---|
678 | } else {
|
---|
679 | return 0;
|
---|
680 | }
|
---|
681 |
|
---|
682 | if (tmp > 127)
|
---|
683 | return 127;
|
---|
684 | if (tmp < -127)
|
---|
685 | return -127;
|
---|
686 | if (tmp > ((int8_t)tmp + .5) && tmp > 0)
|
---|
687 | return (int8_t)(tmp + 1);
|
---|
688 | if (tmp < ((int8_t)tmp - .5) && tmp < 0)
|
---|
689 | return (int8_t)(tmp - 1);
|
---|
690 |
|
---|
691 | return (int8_t)tmp;
|
---|
692 | }
|
---|
693 |
|
---|
694 | /**********************************************************************/
|
---|
695 | // USB functions
|
---|
696 |
|
---|
697 | void DualShock3::usb_pair_device(struct usb_device *dev, int itfnum) {
|
---|
698 |
|
---|
699 | usb_dev_handle *devh = usb_open(dev);
|
---|
700 | if (!devh)
|
---|
701 | fatal("usb_open");
|
---|
702 | usb_detach_kernel_driver_np(devh, itfnum);
|
---|
703 | int res = usb_claim_interface(devh, itfnum);
|
---|
704 | if (res < 0)
|
---|
705 | fatal("usb_claim_interface");
|
---|
706 |
|
---|
707 | bdaddr_t current_ba; // Current pairing address.
|
---|
708 |
|
---|
709 | switch (dev->descriptor.idProduct) {
|
---|
710 | case PRODUCT_SIXAXIS_DS3: {
|
---|
711 | // remote_printf("USB: SIXAXIS/DS3\n");
|
---|
712 | char msg[8];
|
---|
713 | res =
|
---|
714 | usb_control_msg(devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
|
---|
715 | USB_GET_REPORT, 0x03f5, itfnum, msg, sizeof(msg), 5000);
|
---|
716 | /*
|
---|
717 | unsigned char msg[8];
|
---|
718 | res = usb_control_msg
|
---|
719 | (devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
|
---|
720 | USB_GET_REPORT, 0x03f5, itfnum, (void*)msg, sizeof(msg),
|
---|
721 | 5000);*/
|
---|
722 | if (res < 0)
|
---|
723 | fatal("usb_control_msg(read master)");
|
---|
724 | for (int i = 0; i < 6; ++i)
|
---|
725 | current_ba.b[i] = (uint8_t)msg[7 - i];
|
---|
726 | break;
|
---|
727 | }
|
---|
728 | }
|
---|
729 |
|
---|
730 | // New pairing address
|
---|
731 | int dev_id;
|
---|
732 | dev_id = hci_get_route(NULL);
|
---|
733 | struct hci_dev_info di;
|
---|
734 | hci_devinfo(dev_id, &di);
|
---|
735 |
|
---|
736 | // Perform pairing.
|
---|
737 | if (!bacmp(¤t_ba, &di.bdaddr)) {
|
---|
738 | printf(" Already paired to %s\n", myba2str(&di.bdaddr));
|
---|
739 | } else {
|
---|
740 | printf(" Changing master from %s to %s\n", myba2str(¤t_ba),
|
---|
741 | myba2str(&di.bdaddr));
|
---|
742 | switch (dev->descriptor.idProduct) {
|
---|
743 | case PRODUCT_SIXAXIS_DS3: {
|
---|
744 | char msg[8] = {0x01, 0x00, (char)di.bdaddr.b[5], (char)di.bdaddr.b[4],
|
---|
745 | (char)di.bdaddr.b[3], (char)di.bdaddr.b[2],
|
---|
746 | (char)di.bdaddr.b[1], (char)di.bdaddr.b[0]};
|
---|
747 | res = usb_control_msg(
|
---|
748 | devh, USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
|
---|
749 | USB_SET_REPORT, 0x03f5, itfnum, msg, sizeof(msg), 5000);
|
---|
750 | if (res < 0)
|
---|
751 | fatal("usb_control_msg(write master)");
|
---|
752 | break;
|
---|
753 | }
|
---|
754 | }
|
---|
755 | }
|
---|
756 |
|
---|
757 | if (dev->descriptor.idProduct == PRODUCT_SIXAXIS_DS3)
|
---|
758 | printf(" Now unplug the USB cable and press the PS button.\n");
|
---|
759 | else
|
---|
760 | printf(" Now press the PS button.\n");
|
---|
761 | }
|
---|
762 |
|
---|
763 | void DualShock3::usb_scan() {
|
---|
764 | usb_init();
|
---|
765 | if (usb_find_busses() < 0)
|
---|
766 | fatal("usb_find_busses");
|
---|
767 | if (usb_find_devices() < 0)
|
---|
768 | fatal("usb_find_devices");
|
---|
769 | struct usb_bus *busses = usb_get_busses();
|
---|
770 | if (!busses)
|
---|
771 | fatal("usb_get_busses");
|
---|
772 |
|
---|
773 | struct usb_bus *bus;
|
---|
774 | for (bus = busses; bus; bus = bus->next) {
|
---|
775 | struct usb_device *dev;
|
---|
776 | for (dev = bus->devices; dev; dev = dev->next) {
|
---|
777 | struct usb_config_descriptor *cfg;
|
---|
778 | for (cfg = dev->config;
|
---|
779 | cfg < dev->config + dev->descriptor.bNumConfigurations; ++cfg) {
|
---|
780 | int itfnum;
|
---|
781 | for (itfnum = 0; itfnum < cfg->bNumInterfaces; ++itfnum) {
|
---|
782 | struct usb_interface *itf = &cfg->interface[itfnum];
|
---|
783 | struct usb_interface_descriptor *alt;
|
---|
784 | for (alt = itf->altsetting;
|
---|
785 | alt < itf->altsetting + itf->num_altsetting; ++alt) {
|
---|
786 | if (dev->descriptor.idVendor == VENDOR_SONY &&
|
---|
787 | (dev->descriptor.idProduct == PRODUCT_SIXAXIS_DS3) &&
|
---|
788 | alt->bInterfaceClass == 3)
|
---|
789 | usb_pair_device(dev, itfnum);
|
---|
790 | }
|
---|
791 | }
|
---|
792 | }
|
---|
793 | }
|
---|
794 | }
|
---|
795 | }
|
---|
796 |
|
---|
797 | void DualShock3::rumble(uint8_t left_force, uint8_t left_timeout,
|
---|
798 | uint8_t right_force, uint8_t right_timeout) {
|
---|
799 | // printf("rumble\n");
|
---|
800 |
|
---|
801 | unsigned char datas[] = {
|
---|
802 | 0x52 /* HIDP_TRANS_SET_REPORT|HIDP_DATA_RTYPE_OUPUT */, 0x01, 0x00, 0x00,
|
---|
803 | 0x00, 0x00, 0x00, // rumble values
|
---|
804 | 0x00, 0x00, 0x00, 0x00, (unsigned char)ledmask, // 0x10=LED1 .. 0x02=LED4
|
---|
805 |
|
---|
806 | 0xff, 0x27, led4_on, led4_off, 0x32, 0xff, 0x27, led3_on, led3_off, 0x32,
|
---|
807 | 0xff, 0x27, led2_on, led2_off, 0x32, 0xff, 0x27, led1_on, led1_off, 0x32,
|
---|
808 | 0x00, 0x00, 0x00, 0x00, 0x00,
|
---|
809 | };
|
---|
810 |
|
---|
811 | datas[5] = left_timeout; // left timeout
|
---|
812 | datas[6] = left_force; // left force
|
---|
813 | datas[3] = right_timeout; // right timeout
|
---|
814 | datas[4] = right_force; // right force
|
---|
815 |
|
---|
816 | if (connectionType == Bluetooth) {
|
---|
817 | hidp_trans(dev->csk, (char *)datas, sizeof(datas));
|
---|
818 | }
|
---|
819 | }
|
---|
820 |
|
---|
821 | void DualShock3::set_led(uint8_t led, uint8_t on_timeout, uint8_t off_timeout) {
|
---|
822 | uint8_t mask;
|
---|
823 |
|
---|
824 | switch (led) {
|
---|
825 | case 1:
|
---|
826 | led1_on = on_timeout;
|
---|
827 | led1_off = off_timeout;
|
---|
828 | mask = 2;
|
---|
829 | break;
|
---|
830 | case 2:
|
---|
831 | led2_on = on_timeout;
|
---|
832 | led2_off = off_timeout;
|
---|
833 | mask = 4;
|
---|
834 | break;
|
---|
835 | case 3:
|
---|
836 | led3_on = on_timeout;
|
---|
837 | led3_off = off_timeout;
|
---|
838 | mask = 8;
|
---|
839 | break;
|
---|
840 | case 4:
|
---|
841 | led4_on = on_timeout;
|
---|
842 | led4_off = off_timeout;
|
---|
843 | mask = 16;
|
---|
844 | break;
|
---|
845 | }
|
---|
846 |
|
---|
847 | if (on_timeout != 0) {
|
---|
848 | ledmask |= mask;
|
---|
849 | } else {
|
---|
850 | ledmask &= ~mask;
|
---|
851 | }
|
---|
852 | /*
|
---|
853 | printf("led %x\n",ledmask);
|
---|
854 | printf("1:%i %i\n",led1_on,led1_off);
|
---|
855 | printf("2:%i %i\n",led2_on,led2_off);
|
---|
856 | printf("3:%i %i\n",led3_on,led3_off);
|
---|
857 | printf("4:%i %i\n",led4_on,led4_off);*/
|
---|
858 |
|
---|
859 | unsigned char datas[] = {
|
---|
860 | 0x52 /* HIDP_TRANS_SET_REPORT|HIDP_DATA_RTYPE_OUPUT */, 0x01, 0x00, 0x00,
|
---|
861 | 0x00, 0x00, 0x00, // rumble values
|
---|
862 | 0x00, 0x00, 0x00, 0x00, (unsigned char)ledmask, // 0x10=LED1 .. 0x02=LED4
|
---|
863 |
|
---|
864 | 0xff, 0x27, led4_on, led4_off, 0x32, 0xff, 0x27, led3_on, led3_off, 0x32,
|
---|
865 | 0xff, 0x27, led2_on, led2_off, 0x32, 0xff, 0x27, led1_on, led1_off, 0x32,
|
---|
866 | 0x00, 0x00, 0x00, 0x00, 0x00,
|
---|
867 | };
|
---|
868 |
|
---|
869 | if (connectionType == Bluetooth) {
|
---|
870 | hidp_trans(dev->csk, (char *)datas, sizeof(datas));
|
---|
871 | }
|
---|
872 | }
|
---|
873 |
|
---|
874 | } // end namespace sensor
|
---|
875 | } // end namespace flair
|
---|