Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[19709] pkg/trunk/stacks/joystick_drivers/ps3joy
|
From: <bla...@us...> - 2009-07-27 09:16:23
|
Revision: 19709
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19709&view=rev
Author: blaisegassend
Date: 2009-07-27 09:16:09 +0000 (Mon, 27 Jul 2009)
Log Message:
-----------
Robustified and simplified the driver. Removed dependence on swiged kernel
headers.
Modified Paths:
--------------
pkg/trunk/stacks/joystick_drivers/ps3joy/ps3joy.py
Removed Paths:
-------------
pkg/trunk/stacks/joystick_drivers/ps3joy/uinput.i
Modified: pkg/trunk/stacks/joystick_drivers/ps3joy/ps3joy.py
===================================================================
--- pkg/trunk/stacks/joystick_drivers/ps3joy/ps3joy.py 2009-07-27 08:27:45 UTC (rev 19708)
+++ pkg/trunk/stacks/joystick_drivers/ps3joy/ps3joy.py 2009-07-27 09:16:09 UTC (rev 19709)
@@ -1,14 +1,22 @@
#!/usr/bin/python
from bluetooth import *
import select
-import uinput
import fcntl
import os
import time
+import sys
+import traceback
L2CAP_PSM_HIDP_CTRL = 17
L2CAP_PSM_HIDP_INTR = 19
+class uinput:
+ EV_KEY = 1
+ EV_REL = 2
+ EV_ABS = 3
+ BUS_USB = 3
+ ABS_MAX = 0x3f
+
class uinputjoy:
def __init__(self, buttons, axes):
self.file = None
@@ -72,95 +80,76 @@
class decoder:
def __init__(self):
- buttons=[uinput.BTN_SELECT, uinput.BTN_THUMBL, uinput.BTN_THUMBR, uinput.BTN_START,
- uinput.BTN_FORWARD, uinput.BTN_RIGHT, uinput.BTN_BACK, uinput.BTN_LEFT,
- uinput.BTN_TL, uinput.BTN_TR, uinput.BTN_TL2, uinput.BTN_TR2,
- uinput.BTN_X, uinput.BTN_A, uinput.BTN_B, uinput.BTN_Y,
- uinput.BTN_MODE]
- axes=[uinput.ABS_X, uinput.ABS_Y, uinput.ABS_Z, uinput.ABS_RX,
- uinput.ABS_RX, uinput.ABS_RY, uinput.ABS_PRESSURE, uinput.ABS_DISTANCE,
- uinput.ABS_THROTTLE, uinput.ABS_RUDDER, uinput.ABS_WHEEL, uinput.ABS_GAS,
- uinput.ABS_HAT0Y, uinput.ABS_HAT1Y, uinput.ABS_HAT2Y, uinput.ABS_HAT3Y,
- uinput.ABS_TILT_X, uinput.ABS_TILT_Y, uinput.ABS_MISC, uinput.ABS_RZ,
- ]
+ #buttons=[uinput.BTN_SELECT, uinput.BTN_THUMBL, uinput.BTN_THUMBR, uinput.BTN_START,
+ # uinput.BTN_FORWARD, uinput.BTN_RIGHT, uinput.BTN_BACK, uinput.BTN_LEFT,
+ # uinput.BTN_TL, uinput.BTN_TR, uinput.BTN_TL2, uinput.BTN_TR2,
+ # uinput.BTN_X, uinput.BTN_A, uinput.BTN_B, uinput.BTN_Y,
+ # uinput.BTN_MODE]
+ #axes=[uinput.ABS_X, uinput.ABS_Y, uinput.ABS_Z, uinput.ABS_RX,
+ # uinput.ABS_RX, uinput.ABS_RY, uinput.ABS_PRESSURE, uinput.ABS_DISTANCE,
+ # uinput.ABS_THROTTLE, uinput.ABS_RUDDER, uinput.ABS_WHEEL, uinput.ABS_GAS,
+ # uinput.ABS_HAT0Y, uinput.ABS_HAT1Y, uinput.ABS_HAT2Y, uinput.ABS_HAT3Y,
+ # uinput.ABS_TILT_X, uinput.ABS_TILT_Y, uinput.ABS_MISC, uinput.ABS_RZ,
+ # ]
+ buttons = range(0x100,0x111)
+ axes = range(0, 20)
self.joy = uinputjoy(buttons, axes)
+ self.outlen = len(buttons) + len(axes)
- def step(self, sock):
- joy_coding = "!3x3B1x4B4x12B15x4H"
- data = struct.unpack(joy_coding, sock.recv(128))
- #print data,
- out = [0] * (17 + 20)
- i = 0
+ def step(self, sock): # Returns true if the packet was legal
+ joy_coding = "!1B2x3B1x4B4x12B15x4H"
+ rawdata = sock.recv(128)
+ if len(rawdata) != 50:
+ print "Unexpected packet length:", len(data)
+ return False
+ data = list(struct.unpack(joy_coding, rawdata))
+ prefix = data.pop(0)
+ if prefix != 161:
+ print "Unexpected prefix:", prefix
+ return False
+ out = []
for j in range(0,2):
+ curbyte = data.pop(0)
for k in range(0,8):
- out[i] = int((data[j] & (1 << k)) != 0)
- i = i + 1
- out[i] =data[2]
- i = i + 1
+ out.append(int((curbyte & (1 << k)) != 0))
+ out.append(data.pop(0))
for j in range(3,7):
- out[i] = data[j] - 0x80
- i = i + 1
+ out.append(data.pop(0) - 0x80)
for j in range(7,19):
- out[i] = data[j]
- i = i + 1
+ out.append(data.pop(0))
for j in range(19,23):
- out[i] = data[j] - 0x200
- i = i + 1
+ out.append(data.pop(0) - 0x200)
#print out
self.joy.update(out)
+ return True
+ def fullstop(self):
+ self.joy.update([0] * self.outlen)
+
def run(self, intr, ctrl):
- start = time.time()
- frames = 0
- while True:
- (rd, wr, err) = select.select([intr], [], [], 0.1)
- if len(rd) + len(wr) + len(err) == 0: # Timeout
- print "Activating connection."
- ctrl.send("\x53\xf4\x42\x03\x00\x00") # Try activating the stream.
- else: # Got a frame.
- frames = frames + 1
+ try:
+ lastvalidtime = 0
+ while True:
+ (rd, wr, err) = select.select([intr], [], [], 0.1)
curtime = time.time()
- print "Got a frame at ", curtime, frames / (curtime - start)
- self.step(intr)
+ if len(rd) + len(wr) + len(err) == 0: # Timeout
+ print "Activating connection."
+ ctrl.send("\x53\xf4\x42\x03\x00\x00") # Try activating the stream.
+ if lastvalidtime - curtime >= 0.1: # Zero all outputs if we don't hear a valid frame for 0.1 to 0.2 seconds
+ self.fullstop()
+ if lastvalidtime - curtime >= 5: # Disconnect if we don't hear a valid frame for 5 seconds
+ return
+ else: # Got a frame.
+ print "Got a frame at ", curtime, 1 / (curtime - lastvalidtime)
+ if self.step(intr):
+ lastvalidtime = curtime
+ finally:
+ self.fullstop()
class connection_manager:
def __init__(self, decoder):
- self.devicesockets = {} # Stores dev to socket pair mapping
- self.intrdev = {} # Stores socket to dev mapping
- self.ctrldev = {} # Stores socket to dev mapping
- self.socknames = [ "intr", "ctrl" ]
- self.sockdev = [ self.intrdev, self.ctrldev ]
self.decoder = decoder
- def acceptsock(self, index, servsock):
- (sock, (dev, port)) = servsock.accept()
- print "Adding", self.socknames[index], "from", dev
- try:
- devsock = self.devicesockets[dev]
- except KeyError:
- devsock = [None, None]
- if devsock[index] != None:
- print "addintr: Already had an", self.socknames[index], "socket for ", dev
- devsock[index] = sock
- self.devicesockets[dev] = devsock
- self.sockdev[index][sock] = dev
- print devsock
- print self.devicesockets
- if devsock[0] != None and devsock[1] != None:
- print "Both connections are open"
- try:
- self.decoder.run(devsock[0], devsock[1])
- except: # Normal exit point upon disconnect.
- print "Joystick disconnected or errored out."
- devsock[0].close()
- devsock[1].close()
-
- def acceptintr(self, sock):
- self.acceptsock(0, sock)
-
- def acceptctrl(self, sock):
- self.acceptsock(1, sock)
-
def prepare_socket(self, port):
sock = BluetoothSocket(L2CAP)
sock.bind(("", port))
@@ -172,15 +161,18 @@
ctrl_sock = self.prepare_socket(L2CAP_PSM_HIDP_CTRL)
while True:
- (rd, wr, err) = select.select([intr_sock, ctrl_sock], [], [], 60)
- for s in rd:
- #print "Socket ", s, "is in read list."
- if s == intr_sock:
- cm.acceptintr(s)
- elif s == ctrl_sock:
- cm.acceptctrl(s)
- else:
- print "Unexpected socket after select"
+ (intr, (idev, iport)) = intr_sock.accept();
+ (ctrl, (cdev, cport)) = ctrl_sock.accept();
+ if idev == cdev:
+ try:
+ self.decoder.run(intr, ctrl)
+ except:
+ print "Connection broken or error."
+ traceback.print_exc()
+ else:
+ print "Simultaneous connection from two different devices. Ignoring both."
+ ctrl.close()
+ intr.close()
if __name__ == "__main__":
cm = connection_manager(decoder())
Deleted: pkg/trunk/stacks/joystick_drivers/ps3joy/uinput.i
===================================================================
--- pkg/trunk/stacks/joystick_drivers/ps3joy/uinput.i 2009-07-27 08:27:45 UTC (rev 19708)
+++ pkg/trunk/stacks/joystick_drivers/ps3joy/uinput.i 2009-07-27 09:16:09 UTC (rev 19709)
@@ -1,8 +0,0 @@
-%module uinput
-%{
-#include "linux/uinput.h"
-#include "linux/input.h"
-%}
-
-%include "linux/uinput.h"
-%include "linux/input.h"
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
