sixpair.c
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2007, 2008 pascal@pabr.org
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <string.h>
00031 #include <unistd.h>
00032 #include <stdio.h>
00033 #include <usb.h>
00034 
00035 #define VENDOR 0x054c
00036 #define PRODUCT_SIXAXIS 0x0268
00037 #define PRODUCT_NAVIGATION 0x042f
00038 
00039 #define USB_DIR_IN 0x80
00040 #define USB_DIR_OUT 0
00041 
00042 void fatal(char *msg) { perror(msg); exit(1); }
00043 
00044 void show_master(usb_dev_handle *devh, int itfnum) {
00045   printf("Current Bluetooth master: ");
00046   unsigned char msg[8];
00047   int res = usb_control_msg
00048     (devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
00049      0x01, 0x03f5, itfnum, (void*)msg, sizeof(msg), 5000);
00050   if ( res < 0 ) { perror("USB_REQ_GET_CONFIGURATION"); return; }
00051   printf("%02x:%02x:%02x:%02x:%02x:%02x\n",
00052          msg[2], msg[3], msg[4], msg[5], msg[6], msg[7]);
00053 }
00054 
00055 void set_master(usb_dev_handle *devh, int itfnum, int mac[6]) {
00056   printf("Setting master bd_addr to %02x:%02x:%02x:%02x:%02x:%02x\n",
00057          mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
00058   char msg[8]= { 0x01, 0x00, mac[0],mac[1],mac[2],mac[3],mac[4],mac[5] };
00059   int res = usb_control_msg
00060     (devh,
00061      USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
00062      0x09,
00063      0x03f5, itfnum, msg, sizeof(msg),
00064      5000);
00065   if ( res < 0 ) fatal("USB_REQ_SET_CONFIGURATION");
00066 }
00067 
00068 void process_device(int argc, char **argv, struct usb_device *dev,
00069                     struct usb_config_descriptor *cfg, int itfnum) {
00070   int mac[6];
00071 
00072   usb_dev_handle *devh = usb_open(dev);
00073   if ( ! devh ) fatal("usb_open");
00074 
00075   usb_detach_kernel_driver_np(devh, itfnum);
00076 
00077   int res = usb_claim_interface(devh, itfnum);
00078   if ( res < 0 ) fatal("usb_claim_interface");
00079 
00080   show_master(devh, itfnum);
00081 
00082   if ( argc >= 2 ) {
00083     if ( sscanf(argv[1], "%x:%x:%x:%x:%x:%x",
00084                 &mac[0],&mac[1],&mac[2],&mac[3],&mac[4],&mac[5]) != 6 ) {
00085 
00086       printf("usage: %s [<bd_addr of master>]\n", argv[0]);
00087       exit(1);
00088     }
00089   } else {
00090     FILE *f = popen("hcitool dev", "r");
00091     if ( !f ||
00092          fscanf(f, "%*s\n%*s %x:%x:%x:%x:%x:%x",
00093                 &mac[0],&mac[1],&mac[2],&mac[3],&mac[4],&mac[5]) != 6 ) {
00094       printf("Unable to retrieve local bd_addr from `hcitool dev`.\n");
00095       printf("Please enable Bluetooth or specify an address manually.\n");
00096       exit(1);
00097     }
00098     pclose(f);
00099   }
00100     
00101   set_master(devh, itfnum, mac);
00102 
00103   usb_close(devh);
00104 }
00105 
00106 int main(int argc, char *argv[]) {  
00107 
00108   usb_init();
00109   if ( usb_find_busses() < 0 ) fatal("usb_find_busses");
00110   if ( usb_find_devices() < 0 ) fatal("usb_find_devices");
00111   struct usb_bus *busses = usb_get_busses();
00112   if ( ! busses ) fatal("usb_get_busses");
00113 
00114   int found = 0;
00115 
00116   struct usb_bus *bus;
00117   for ( bus=busses; bus; bus=bus->next ) {
00118     struct usb_device *dev;
00119     for ( dev=bus->devices; dev; dev=dev->next) {
00120       struct usb_config_descriptor *cfg;
00121       for ( cfg = dev->config;
00122             cfg < dev->config + dev->descriptor.bNumConfigurations;
00123             ++cfg ) {
00124         int itfnum;
00125         for ( itfnum=0; itfnum<cfg->bNumInterfaces; ++itfnum ) {
00126           struct usb_interface *itf = &cfg->interface[itfnum];
00127           struct usb_interface_descriptor *alt;
00128           for ( alt = itf->altsetting;
00129                 alt < itf->altsetting + itf->num_altsetting;
00130                 ++alt ) {
00131             if ( dev->descriptor.idVendor == VENDOR &&
00132                  (dev->descriptor.idProduct == PRODUCT_SIXAXIS ||
00133                   dev->descriptor.idProduct == PRODUCT_NAVIGATION) &&
00134                  alt->bInterfaceClass == 3 ) {
00135               process_device(argc, argv, dev, cfg, itfnum);
00136               ++found;
00137             }
00138           }
00139         }
00140       }
00141     }
00142   }
00143 
00144   if ( ! found ) {
00145     printf("No controller found on USB busses. Please connect your joystick via USB.\n");
00146     return 1;
00147   }
00148 
00149   return 0;
00150 
00151 }
00152 


grizzly_teleop
Author(s): Ryan Gariepy
autogenerated on Thu Jun 6 2019 21:44:09