Thread: [Personalrobots-commit] SF.net SVN: personalrobots:[19709] pkg/trunk/stacks/joystick_drivers/ps3joy

SourceForge Headquarters 225 Broadway Suite 1600 San Diego, CA 92101 +1 (858) 422-6466

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.




Thread: [Personalrobots-commit] SF.net SVN: personalrobots:[19709] pkg/trunk/stacks/joystick_drivers/ps3joy

personalrobots-commit