TestShell.cpp

00001 
00002 /*
00003 Copyright (c) 2010 Peter Barrett
00004 
00005 Permission is hereby granted, free of charge, to any person obtaining a copy
00006 of this software and associated documentation files (the "Software"), to deal
00007 in the Software without restriction, including without limitation the rights
00008 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00009 copies of the Software, and to permit persons to whom the Software is
00010 furnished to do so, subject to the following conditions:
00011 
00012 The above copyright notice and this permission notice shall be included in
00013 all copies or substantial portions of the Software.
00014 
00015 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00016 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00017 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00018 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00019 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00020 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00021 THE SOFTWARE.
00022 */
00023 #include "mbed.h"
00024 #include <vector>
00025 #include "Utils.h"
00026 #include "USBHost.h"
00027 #include "hci.h"
00028 #include "HCITransportUSB.h"
00029 #include "RFCOMM.h"
00030 #include "ftclasslibusbdevbt.h"
00031 #include "sdp_data.h"
00032 #include "sdp.h"
00033 #include "btserial.h"
00034 #include "neighbourhood.h"
00035 
00036 #include "motordriver.h"
00037 
00038 /************************************************
00039 TODO:
00040 mtu and credits are completely unhandled - in progress
00041 multiple rfcomm sessions should be possible - done
00042 SDP would be nice - beta
00043 multiple rfcomm channels are untested
00044 decoupling of rfcomm and application - much better
00045 packets are not reassembled - some are (HCI and ft application level)
00046 disconnect and timeouts
00047 ************************************************/
00048 #define DEBUG   1
00049 int state = 0;
00050 
00051 //int bulk = 0;
00052 void printf(const BD_ADDR* addr) {
00053     const u8* a = addr->addr;
00054     printf("%02X:%02X:%02X:%02X:%02X:%02X",a[5],a[4],a[3],a[2],a[1],a[0]);
00055 }
00056 
00057 const char FtDevClass[3] = {0x00, 0x1F, 0x82 };
00058 const char SerDevClass[3] = {4, 1, 0x00};
00059 //  Instance
00060 extern RFCOMMManager rfcomm_manager;
00061 
00062 class application : public HCI {
00063     BTDevice* devs[8];
00064     int count, i, pending;
00065 public:
00066     //  We have connected to a device
00067     void ConnectionComplete(connection_info* info) {
00068         printf("ConnectionComplete ");
00069         BD_ADDR* a = &info->bdaddr;
00070         printf(a);
00071         printf("\n");
00072         RemoteNameRequest(a);
00073         for (i++; i < count; i++) {//find the next device to open
00074             printfBytes("DEVICE CLASS",devs[i]->_info.dev_class,3);
00075             if (devs[i]->_handle == 0 && memcmp(devs[i]->_info.dev_class, FtDevClass, 3)==0) {//or some other way to connect to RFCOMM devices
00076                 BD_ADDR* bd = &devs[i]->_info.bdaddr;
00077                 printf("Connecting to ");
00078                 printf(bd);
00079                 printf("\n");
00080                 pending++;
00081                 CreateConnection(bd); //some low level connect, just let it happen for now (sets pin, mtu etc.)
00082                 printf("connection cmd was sent\n");
00083                 return;
00084             }
00085         }
00086         //state = 1; //start the real application
00087     }
00088 
00089     void ConnectDevices() {
00090         count = GetDevices(devs,8);//get pointers to all bluetooth devices
00091         pending = 0;
00092         for (i = 0; i < count; i++) {
00093             printfBytes("DEVICE CLASS",devs[i]->_info.dev_class,3);
00094             if (devs[i]->_handle == 0 /*&& memcmp(devs[i]->_info.dev_class, FtDevClass, 3)==0*/) {//or some other way to connect to RFCOMM devices
00095                 BD_ADDR* bd = &devs[i]->_info.bdaddr;
00096                 printf("Connecting to ");
00097                 printf(bd);
00098                 printf("\n");
00099                 pending++;
00100                 CreateConnection(bd); //some low level connect, just let it happen for now (sets pin, mtu etc.)
00101                 printf("connection cmd was sent\n");
00102                 return;
00103             }
00104         }
00105         if (pending == 0) state = 1;//for the case when there are no ft devices
00106     }
00107     virtual void Callback(HCI_CALLBACK_EVENT c, const u8* data, int len);
00108     int csr_write_bd_addr(BD_ADDR *bdaddr, bool transient=true);
00109     int csr_reset_device(bool transient=true);
00110 } App; //application instance
00111 
00112 extern "C" void mbed_reset();
00113 
00114 
00115 void application::Callback(HCI_CALLBACK_EVENT evt, const u8* data, int len) {//these events are forwarded (in)directly from HCIRecv
00116     unsigned char pin[] = "1234";
00117     unsigned char newaddr[] = {0x2c, 0x07, 0x54, 0x7b, 0x13, 0x00};//possible ft TX address (ROBO TX-277)
00118 //    unsigned char newaddr[] = {0x57, 0x0a, 0x3d, 0x83, 0x15, 0x00};//original address of the cheap round BT dongle
00119     printf("\x1b[%dm", 33);
00120     switch (evt) {
00121         case CALLBACK_READY:
00122             printf("CALLBACK_READY\n");
00123             printf("my address = ");
00124             printf((BD_ADDR*)data);
00125             if (memcmp(newaddr, data, 6) != 0) { //csr_write_bd_addr((BD_ADDR*)newaddr, false);
00126             }
00127             Inquiry();//start the second phase of the discovery
00128             break;
00129 
00130         case CALLBACK_INQUIRY_RESULT: //optionally build the list of FT devices here
00131             printf("CALLBACK_INQUIRY_RESULT ");
00132             printf((BD_ADDR*)data);
00133             printf("\n");//data points to inquiry_info struct
00134 //          RemoteNameRequest((BD_ADDR*)data);
00135             break;
00136 
00137         case CALLBACK_INQUIRY_DONE:
00138             printf("CALLBACK_INQUIRY_DONE\n");
00139             neighbors = new neighbourhood(&App);
00140             neighbors->read();
00141             ConnectDevices();
00142             break;
00143 
00144         case CALLBACK_REMOTE_NAME: {
00145             BD_ADDR* addr = (BD_ADDR*)data;
00146             const char* name = (const char*)(data + 6);
00147             printf(addr);
00148             printf(" = % s\n",name);
00149             pending--;
00150             if (pending == 0) state = 1;
00151         }
00152         break;
00153 
00154         case CALLBACK_CONNECTION_COMPLETE: {
00155             connection_info *ci = (connection_info*)data;
00156             if (ci->status>0) {
00157                 printf("Connection failed, status=0x%02X\n", ci->status);
00158                 break;
00159             }
00160             ConnectionComplete(ci);
00161             printf("Going to open sdp socket\n");
00162             L2CAPAddr addr;
00163             memcpy(&addr.bdaddr, &ci->bdaddr, 6);
00164             //int s = SDP.Open(&addr.hdr);
00165         }
00166         break;
00167         case CALLBACK_PIN_REQ:
00168             printf("Enter PIN for ");
00169             printf((BD_ADDR*)data);
00170             printf(" :  submitting %s\n", pin);
00171             //USBLoop(); wait(1.0); USBLoop();
00172             //for(int k=0; k<2000000;k++) USBLoop();
00173             PinCodeReply(data, pin);
00174             break;
00175         case CALLBACK_VENDOR:
00176             printfBytes("Vendor Reply:", data, len);
00177             //mbed_reset();
00178             if (data[0] == 0xc2)
00179                 csr_reset_device(false);
00180             break;
00181         default:
00182             printf("Unhandled HCI Callback %d\n", evt);
00183     };
00184     printf("\x1b[%dm", 0);
00185 }
00186 
00187 #define CSR_WRITE        0xFC00
00188 
00189 int application::csr_write_bd_addr(BD_ADDR *bdaddr, bool transient) {
00190     unsigned char cmd[] = { 0xc2,
00191                             0x02, 0x00, 0x0c, 0x00, 0x11, 0x47, 0x03, 0x70,
00192                             0x00, 0x00, 0x01, 0x00, 0x04, 0x00, 0x00, 0x00,
00193                             0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
00194                           };
00195 
00196     if (transient)
00197         cmd[15] = 0x08;
00198 
00199     cmd[17] = bdaddr->addr[2];
00200     cmd[18] = 0x00;
00201     cmd[19] = bdaddr->addr[0];
00202     cmd[20] = bdaddr->addr[1];
00203     cmd[21] = bdaddr->addr[3];
00204     cmd[22] = 0x00;
00205     cmd[23] = bdaddr->addr[4];
00206     cmd[24] = bdaddr->addr[5];
00207 
00208     return SendCmd(CSR_WRITE, cmd, sizeof(cmd));
00209 }
00210 
00211 int application::csr_reset_device(bool transient) {
00212     unsigned char cmd[] = { 0xc2, 0x02, 0x00, 0x09, 0x00,
00213                             0x00, 0x00, 0x01, 0x40, 0x00, 0x00,
00214                             0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
00215                           };
00216 
00217     if (transient)
00218         cmd[7] = 0x02;
00219 
00220     return SendCmd(CSR_WRITE, cmd, sizeof(cmd));
00221 }
00222 
00223 
00224 //  these should be placed in the DMA SRAM
00225 typedef struct {
00226     u8 _hciBuffer[MAX_HCL_SIZE];
00227     u8 _aclBuffer[MAX_ACL_SIZE];
00228 } SRAMPlacement;
00229 
00230 HCITransportUSB _HCITransportUSB;  //use USB as the transport to the radio
00231 
00232 int OnBluetoothInsert(int device) {//install the HCI and start discovery, user callbacks are made to HciCalback
00233     printf("Bluetooth inserted of %d\n",device);
00234     u32 sramLen;
00235     u8* sram =  USBGetBuffer(&sramLen);
00236     sram = (u8*)(((u32)sram + 1023) & ~1023);
00237     SRAMPlacement* s = (SRAMPlacement*)sram;
00238     _HCITransportUSB.Open(device,s->_hciBuffer,s->_aclBuffer);//setup buffers for USB host, incoming data goes first to HCIRecv and ACLRecv
00239     RegisterSocketHandler(SOCKET_L2CAP,&App); //register the application::hci as handler for L2CAP events
00240     RegisterSocketHandler(SOCKET_RFCOM, &rfcomm_manager);//set the RFCOMMManager as the RFCOM socket handler
00241     if (RegisterSocketHandler(SOCKET_SDP, &SDP))
00242         printf("Could not register SDP socket type\n");
00243     App.Open(&_HCITransportUSB);//the callback is virtual
00244     App.Inquiry();//start discovery of BT devices phase 0
00245     return 0;
00246 }
00247 
00248 DigitalOut led(LED1), loop(LED2);
00249 
00250 int comm = 0;
00251 btserial *incoming, *outgoing;
00252 
00253 Motor motorRight(p21, p5, p6, 1); // pwm, fwd, rev, can break
00254 Motor motorLeft(p22, p7, p8, 1); // pwm, fwd, rev, can break
00255 
00256 void motorSpeed(Motor& motor, int stick)
00257 {
00258     float speed;
00259     if(100 <= stick && stick <= 155) {
00260         motor.stop(0);
00261     } else if(0 <= stick && stick <= 99) {
00262         speed = (100 - stick) * 0.01;
00263         motor.speed(speed);
00264         printf("Motor:%f", speed);
00265     } else {
00266         speed = (155 - stick) * 0.01;
00267         motor.speed(speed);
00268         printf("Motor:%f", speed);
00269     }
00270 }
00271 
00272 void echo(int socket, SocketState state, const u8* data, int len, void* userData) {
00273     const u8 connack[] = { 0xbe, 0xef, 8, 'C','O','N','N','_','A','C','K', 0x11};
00274     printf("Echo: socket %d, state %d, len=%d\n", socket, state, len);
00275     if (state==SocketState_Open) {
00276         if (len == 0) {
00277             printf("Sending CONN_ACK\n");
00278             Socket_Send(socket, connack, sizeof(connack));
00279         } else {
00280             //Socket_Send(socket, data, len);
00281             printfBytes("echo:", data, len);
00282             
00283             if(len >= 4) {
00284                 char temp[5] = {0};
00285                 memcpy(temp, data, 4);
00286                 int num = strtol(temp, NULL, 16);
00287                 
00288                 int LeftStickY = num / 256;
00289                 int RightStickY = num - (LeftStickY * 256);
00290     
00291                 motorSpeed(motorLeft, LeftStickY);
00292                 motorSpeed(motorRight, RightStickY);
00293             }
00294         }
00295     }
00296 }
00297 
00298 void TestShell() {
00299     int n=0;
00300     USBInit();
00301     for (;;) {
00302         switch (state) {
00303             case 0: //inquiry and low-level connection
00304                 break;
00305             case 1: {//initialisation
00306                 printf("Ready to open ports\n");
00307                 InitFtBtDeviceList();
00308                 int n = GetNrOfFtBtDevices();
00309                 printf("%d ft BT devices have been found\n", n);
00310                 if (n > 0) {
00311                     ftbtdev *d = GetFtUsbDeviceHandle(0);
00312                     if (d==0) printf("could not get device handle\n");
00313                     //int sock = OpenFtBtDevice(d);
00314                 }
00315                 state = 2;
00316                 comm = Socket_Listen(SOCKET_RFCOM, 1, echo, 0);
00317                 //incoming = new btserial(1);
00318             }
00319             break;
00320             case 2://main loop
00321                 /*   if (incoming->readable()>0){
00322                      int c= incoming->getc();
00323                      putc(c, stderr);
00324                      incoming->putc(c);
00325                      }
00326                      else if (incoming->readable()<0){
00327                        state = 3;
00328                        printf("end of session");
00329                        delete incoming;
00330                      }*/
00331                 break;
00332             default:
00333                 break;
00334         }
00335         loop=1;
00336         USBLoop();
00337         loop=0;
00338         n++;
00339         if (n>=500000) {
00340             n=0;
00341             led = !led;
00342         }
00343     }
00344     //printf("Dropped out of main loop!\n");
00345 }
00346 
00347 //********************************************************************************************************************************