00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 from bluetooth import *
00037 import select
00038 import fcntl
00039 import os
00040 import time
00041 import sys
00042 import traceback
00043 import subprocess
00044
00045 L2CAP_PSM_HIDP_CTRL = 17
00046 L2CAP_PSM_HIDP_INTR = 19
00047
00048 class uinput:
00049 EV_KEY = 1
00050 EV_REL = 2
00051 EV_ABS = 3
00052 BUS_USB = 3
00053 ABS_MAX = 0x3f
00054
00055 class uinputjoy:
00056 def open_uinput(self):
00057 for name in ["/dev/input/uinput", "/dev/misc/uinput", "/dev/uinput"]:
00058 try:
00059 return os.open(name, os.O_WRONLY)
00060 break
00061 except Exception, e:
00062
00063 pass
00064 return None
00065
00066 def __init__(self, buttons, axes, axmin, axmax, axfuzz, axflat):
00067 self.file = self.open_uinput()
00068 if self.file == None:
00069 print >> sys.stderr, "Trying to modprobe uinput."
00070 os.system("modprobe uinput > /dev/null 2>&1")
00071 time.sleep(1)
00072 self.file = self.open_uinput()
00073 if self.file == None:
00074 print >> sys.stderr, "Can't open uinput device. Is it accessible by this user? Did you mean to run as root?"
00075 raise IOError
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085 UI_SET_EVBIT = 0x40045564
00086 UI_SET_KEYBIT = 0x40045565
00087 UI_SET_RELBIT = 0x40045566
00088 UI_DEV_CREATE = 0x5501
00089 UI_SET_RELBIT = 0x40045566
00090 UI_SET_ABSBIT = 0x40045567
00091 uinput_user_dev = "80sHHHHi" + (uinput.ABS_MAX+1)*4*'i'
00092
00093 if len(axes) != len(axmin) or len(axes) != len(axmax):
00094 raise Exception("uinputjoy.__init__: axes, axmin and axmax should have same length")
00095 absmin = [0] * (uinput.ABS_MAX+1)
00096 absmax = [0] * (uinput.ABS_MAX+1)
00097 absfuzz = [2] * (uinput.ABS_MAX+1)
00098 absflat = [4] * (uinput.ABS_MAX+1)
00099 for i in range(0, len(axes)):
00100 absmin[axes[i]] = axmin[i]
00101 absmax[axes[i]] = axmax[i]
00102 absfuzz[axes[i]] = axfuzz[i]
00103 absflat[axes[i]] = axflat[i]
00104
00105 os.write(self.file, struct.pack(uinput_user_dev, "Sony Playstation SixAxis/DS3",
00106 uinput.BUS_USB, 0x054C, 0x0268, 0, 0, *(absmax + absmin + absfuzz + absflat)))
00107
00108 fcntl.ioctl(self.file, UI_SET_EVBIT, uinput.EV_KEY)
00109
00110 for b in buttons:
00111 fcntl.ioctl(self.file, UI_SET_KEYBIT, b)
00112
00113 for a in axes:
00114 fcntl.ioctl(self.file, UI_SET_EVBIT, uinput.EV_ABS)
00115 fcntl.ioctl(self.file, UI_SET_ABSBIT, a)
00116
00117 fcntl.ioctl(self.file, UI_DEV_CREATE)
00118
00119 self.value = [None] * (len(buttons) + len(axes))
00120 self.type = [uinput.EV_KEY] * len(buttons) + [uinput.EV_ABS] * len(axes)
00121 self.code = buttons + axes
00122
00123 def update(self, value):
00124 input_event = "LLHHi"
00125 t = time.time()
00126 th = int(t)
00127 tl = int((t - th) * 1000000)
00128 if len(value) != len(self.value):
00129 print >> sys.stderr, "Unexpected length for value in update (%i instead of %i). This is a bug."%(len(value), len(self.value))
00130 for i in range(0, len(value)):
00131 if value[i] != self.value[i]:
00132 os.write(self.file, struct.pack(input_event, th, tl, self.type[i], self.code[i], value[i]))
00133 self.value = list(value)
00134
00135 class BadJoystickException(Exception):
00136 def __init__(self):
00137 Exception.__init__(self, "Unsupported joystick.")
00138
00139 class decoder:
00140 def __init__(self, inactivity_timeout = float(1e3000), continuous_motion_output = False):
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152 buttons = range(0x100,0x111)
00153 axes = range(0, 20)
00154 axmin = [0] * 20
00155 axmax = [255] * 20
00156 axfuzz = [2] * 20
00157 axflat = [4] * 20
00158 for i in range(-4,0):
00159 axmax[i] = 1023
00160 axfuzz[i] = 4
00161 axflat[i] = 4
00162 if continuous_motion_output:
00163 axfuzz[i] = 0
00164 axflat[i] = 0
00165 for i in range(4,len(axmin)-4):
00166 axmin[i] = -axmax[i]
00167 self.joy = uinputjoy(buttons, axes, axmin, axmax, axfuzz, axflat)
00168 self.axmid = [sum(pair)/2 for pair in zip(axmin, axmax)]
00169 self.fullstop()
00170 self.outlen = len(buttons) + len(axes)
00171 self.inactivity_timeout = inactivity_timeout
00172
00173 step_active = 1
00174 step_idle = 2
00175 step_error = 3
00176
00177 def step(self, rawdata):
00178 if len(rawdata) == 50:
00179 joy_coding = "!1B2x3B1x4B4x12B15x4H"
00180 data = list(struct.unpack(joy_coding, rawdata))
00181 prefix = data.pop(0)
00182 if prefix != 161:
00183 print >> sys.stderr, "Unexpected prefix (%i). Is this a PS3 Dual Shock or Six Axis?"%prefix
00184 return self.step_error
00185 out = []
00186 for j in range(0,2):
00187 curbyte = data.pop(0)
00188 for k in range(0,8):
00189 out.append(int((curbyte & (1 << k)) != 0))
00190 out = out + data
00191 self.joy.update(out)
00192 axis_motion = [abs(out[17:][i] - self.axmid[i]) > 20 for i in range(0,len(out)-17-4)]
00193
00194 if any(out[0:17]) or any(axis_motion):
00195 return self.step_active
00196 return self.step_idle
00197 elif len(rawdata) == 13:
00198
00199 print >> sys.stderr, "Your bluetooth adapter is not supported. Does it support Bluetooth 2.0? Please report its model to blaise@willowgarage.com"
00200 raise BadJoystickException()
00201 else:
00202 print >> sys.stderr, "Unexpected packet length (%i). Is this a PS3 Dual Shock or Six Axis?"%len(rawdata)
00203 return self.step_error
00204
00205 def fullstop(self):
00206 self.joy.update([0] * 17 + self.axmid)
00207
00208 def run(self, intr, ctrl):
00209 activated = False
00210 try:
00211 self.fullstop()
00212 lastactivitytime = lastvalidtime = time.time()
00213 while True:
00214 (rd, wr, err) = select.select([intr], [], [], 0.1)
00215 curtime = time.time()
00216 if len(rd) + len(wr) + len(err) == 0:
00217
00218 ctrl.send("\x53\xf4\x42\x03\x00\x00")
00219 else:
00220
00221 if not activated:
00222 print "Connection activated"
00223 activated = True
00224 try:
00225 rawdata = intr.recv(128)
00226 except BluetoothError, s:
00227 print "Got Bluetooth error %s. Disconnecting."%s
00228 return
00229 if len(rawdata) == 0:
00230 print "Joystick shut down the connection, battery may be discharged."
00231 return
00232 stepout = self.step(rawdata)
00233 if stepout != self.step_error:
00234 lastvalidtime = curtime
00235 if stepout == self.step_active:
00236 lastactivitytime = curtime
00237 if curtime - lastactivitytime > self.inactivity_timeout:
00238 print "Joystick inactive for %.0f seconds. Disconnecting to save battery."%self.inactivity_timeout
00239 return
00240 if curtime - lastvalidtime >= 0.1:
00241 self.fullstop()
00242 if curtime - lastvalidtime >= 5:
00243 print "No valid data for 5 seconds. Disconnecting. This should not happen, please report it."
00244 return
00245 time.sleep(0.005)
00246 finally:
00247 self.fullstop()
00248
00249 class Quit(Exception):
00250 def __init__(self, errorcode):
00251 Exception.__init__(self)
00252 self.errorcode = errorcode
00253
00254 def check_hci_status():
00255
00256 proc = subprocess.Popen(['hciconfig'], stdout=subprocess.PIPE, stderr=subprocess.PIPE)
00257 (out, err) = proc.communicate()
00258 if out.find('UP') == -1:
00259 os.system("hciconfig hci0 up > /dev/null 2>&1")
00260 if out.find('PSCAN') == -1:
00261 os.system("hciconfig hci0 pscan > /dev/null 2>&1")
00262
00263 class connection_manager:
00264 def __init__(self, decoder):
00265 self.decoder = decoder
00266 self.shutdown = False
00267
00268 def prepare_bluetooth_socket(self, port):
00269 sock = BluetoothSocket(L2CAP)
00270 return self.prepare_socket(sock, port)
00271
00272 def prepare_net_socket(self, port):
00273 sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
00274 return self.prepare_socket(sock, port)
00275
00276 def prepare_socket(self, sock, port):
00277 first_loop = True
00278 while True:
00279 try:
00280 sock.bind(("", port))
00281 except Exception, e:
00282 print repr(e)
00283 if first_loop:
00284 print >> sys.stderr, "Error binding to socket, will retry every 5 seconds. Do you have another ps3joy.py running? This error occurs on some distributions (such as Ubuntu Karmic). Please read http://www.ros.org/wiki/ps3joy/Troubleshooting for solutions."
00285 first_loop = False
00286 time.sleep(0.5)
00287 continue
00288 sock.listen(1)
00289 return sock
00290
00291 def listen_net(self,intr_port, ctrl_port):
00292 intr_sock = self.prepare_net_socket(intr_port)
00293 ctrl_sock = self.prepare_net_socket(ctrl_port)
00294 self.listen(intr_sock, ctrl_sock)
00295
00296 def listen_bluetooth(self):
00297 intr_sock = self.prepare_bluetooth_socket(L2CAP_PSM_HIDP_INTR)
00298 ctrl_sock = self.prepare_bluetooth_socket(L2CAP_PSM_HIDP_CTRL)
00299 self.listen(intr_sock, ctrl_sock)
00300
00301 def listen(self, intr_sock, ctrl_sock):
00302 self.n = 0
00303 while not self.shutdown:
00304 print "Waiting for connection. Disconnect your PS3 joystick from USB and press the pairing button."
00305 try:
00306 intr_sock.settimeout(5)
00307 ctrl_sock.settimeout(1)
00308 while True:
00309 try:
00310 (intr, (idev, iport)) = intr_sock.accept();
00311 break
00312 except Exception, e:
00313 if str(e) == 'timed out':
00314 check_hci_status()
00315 else:
00316 raise
00317
00318 try:
00319 try:
00320 (ctrl, (cdev, cport)) = ctrl_sock.accept();
00321 except Exception, e:
00322 print >> sys.stderr, "Got interrupt connection without control connection. Giving up on it."
00323 continue
00324 try:
00325 if idev == cdev:
00326 self.decoder.run(intr, ctrl)
00327 print "Connection terminated."
00328 else:
00329 print >> sys.stderr, "Simultaneous connection from two different devices. Ignoring both."
00330 finally:
00331 ctrl.close()
00332 finally:
00333 intr.close()
00334 except BadJoystickException:
00335 pass
00336 except KeyboardInterrupt:
00337 print "CTRL+C detected. Exiting."
00338 quit(0)
00339 except Exception, e:
00340 traceback.print_exc()
00341 print >> sys.stderr, "Caught exception: %s"%str(e)
00342 time.sleep(1)
00343 print
00344
00345 inactivity_timout_string = "--inactivity-timeout"
00346 no_disable_bluetoothd_string = "--no-disable-bluetoothd"
00347 redirect_output_string = "--redirect-output"
00348 continuous_motion_output_string = "--continuous-output"
00349
00350 def usage(errcode):
00351 print "usage: ps3joy.py ["+inactivity_timout_string+"=<n>] ["+no_disable_bluetoothd_string+"] ["+redirect_output_string+"] ["+continuous_motion_output_string+"]=<f>"
00352 print "<n>: inactivity timeout in seconds (saves battery life)."
00353 print "<f>: file name to redirect output to."
00354 print "Unless "+no_disable_bluetoothd_string+" is specified, bluetoothd will be stopped."
00355 raise Quit(errcode)
00356
00357 def is_arg_with_param(arg, prefix):
00358 if not arg.startswith(prefix):
00359 return False
00360 if not arg.startswith(prefix+"="):
00361 print "Expected '=' after "+prefix
00362 print
00363 usage(1)
00364 return True
00365
00366 if __name__ == "__main__":
00367 errorcode = 0
00368 try:
00369 inactivity_timeout = float(1e3000)
00370 disable_bluetoothd = True
00371 continuous_output = False
00372 for arg in sys.argv[1:]:
00373 if arg == "--help":
00374 usage(0)
00375 elif is_arg_with_param(arg, inactivity_timout_string):
00376 str_value = arg[len(inactivity_timout_string)+1:]
00377 try:
00378 inactivity_timeout = float(str_value)
00379 if inactivity_timeout < 0:
00380 print "Inactivity timeout must be positive."
00381 print
00382 usage(1)
00383 except ValueError:
00384 print "Error parsing inactivity timeout: "+str_value
00385 print
00386 usage(1)
00387 elif arg == no_disable_bluetoothd_string:
00388 disable_bluetoothd = False
00389 elif arg == continuous_motion_output_string:
00390 continuous_output = True
00391 elif is_arg_with_param(arg, redirect_output_string):
00392 str_value = arg[len(redirect_output_string)+1:]
00393 try:
00394 print "Redirecting output to:", str_value
00395 sys.stdout = open(str_value, "a", 1)
00396 except IOError, e:
00397 print "Error opening file to redirect output:", str_value
00398 raise Quit(1)
00399 sys.stderr = sys.stdout
00400 else:
00401 print "Ignoring parameter: '%s'"%arg
00402 if os.getuid() != 0:
00403 print >> sys.stderr, "ps3joy.py must be run as root."
00404 quit(1)
00405 if disable_bluetoothd:
00406 os.system("/etc/init.d/bluetooth stop > /dev/null 2>&1")
00407 time.sleep(1)
00408 try:
00409 while os.system("hciconfig hci0 > /dev/null 2>&1") != 0:
00410 print >> sys.stderr, "No bluetooth dongle found or bluez rosdep not installed. Will retry in 5 seconds."
00411 time.sleep(5)
00412 if inactivity_timeout == float(1e3000):
00413 print "No inactivity timeout was set. (Run with --help for details.)"
00414 else:
00415 print "Inactivity timeout set to %.0f seconds."%inactivity_timeout
00416 cm = connection_manager(decoder(inactivity_timeout = inactivity_timeout, continuous_motion_output = continuous_output))
00417 cm.listen_bluetooth()
00418 finally:
00419 if disable_bluetoothd:
00420 os.system("/etc/init.d/bluetooth start > /dev/null 2>&1")
00421 except Quit, e:
00422 errorcode = e.errorcode
00423 except KeyboardInterrupt:
00424 print "CTRL+C detected. Exiting."
00425 exit(errorcode)