Takashi Ogura / Mbed 2 deprecated myBlueUSB_ros

Based on myBlueUSB and rosserial_mbed

Dependencies:   mbed myUSBHost AvailableMemory myBlueUSB

TestShell.cpp@1:18139954944b, 2011-09-17 (annotated)

Committer:
OTL
Date:
Sat Sep 17 14:24:13 2011 +0000
Revision:
1:18139954944b
Parent:
0:7684b95768c7 
remove m3pi and main

Who changed what in which revision?

UserRevisionLine numberNew contents of line
OTL 0:7684b95768c7 1
OTL 0:7684b95768c7 2 /*
OTL 0:7684b95768c7 3 Copyright (c) 2010 Peter Barrett
OTL 0:7684b95768c7 4
OTL 0:7684b95768c7 5 Permission is hereby granted, free of charge, to any person obtaining a copy
OTL 0:7684b95768c7 6 of this software and associated documentation files (the "Software"), to deal
OTL 0:7684b95768c7 7 in the Software without restriction, including without limitation the rights
OTL 0:7684b95768c7 8 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
OTL 0:7684b95768c7 9 copies of the Software, and to permit persons to whom the Software is
OTL 0:7684b95768c7 10 furnished to do so, subject to the following conditions:
OTL 0:7684b95768c7 11
OTL 0:7684b95768c7 12 The above copyright notice and this permission notice shall be included in
OTL 0:7684b95768c7 13 all copies or substantial portions of the Software.
OTL 0:7684b95768c7 14
OTL 0:7684b95768c7 15 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
OTL 0:7684b95768c7 16 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
OTL 0:7684b95768c7 17 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
OTL 0:7684b95768c7 18 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
OTL 0:7684b95768c7 19 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OTL 0:7684b95768c7 20 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
OTL 0:7684b95768c7 21 THE SOFTWARE.
OTL 0:7684b95768c7 22 */
OTL 0:7684b95768c7 23 #include "mbed.h"
OTL 0:7684b95768c7 24 #include <vector>
OTL 0:7684b95768c7 25 #include "Utils.h"
OTL 0:7684b95768c7 26 #include "USBHost.h"
OTL 0:7684b95768c7 27 #include "hci.h"
OTL 0:7684b95768c7 28 #include "HCITransportUSB.h"
OTL 0:7684b95768c7 29 #include "RFCOMM.h"
OTL 0:7684b95768c7 30 #include "ftclasslibusbdevbt.h"
OTL 0:7684b95768c7 31 #include "sdp_data.h"
OTL 0:7684b95768c7 32 #include "sdp.h"
OTL 0:7684b95768c7 33 #include "btserial.h"
OTL 0:7684b95768c7 34 #include "neighbourhood.h"
OTL 0:7684b95768c7 35
OTL 0:7684b95768c7 36 /************************************************
OTL 0:7684b95768c7 37 TODO:
OTL 0:7684b95768c7 38 mtu and credits are completely unhandled - in progress
OTL 0:7684b95768c7 39 multiple rfcomm sessions should be possible - done
OTL 0:7684b95768c7 40 SDP would be nice - beta
OTL 0:7684b95768c7 41 multiple rfcomm channels are untested
OTL 0:7684b95768c7 42 decoupling of rfcomm and application - much better
OTL 0:7684b95768c7 43 packets are not reassembled - some are (HCI and ft application level)
OTL 0:7684b95768c7 44 disconnect and timeouts
OTL 0:7684b95768c7 45 ************************************************/
OTL 0:7684b95768c7 46 #define DEBUG 1
OTL 0:7684b95768c7 47 int state = 0;
OTL 0:7684b95768c7 48
OTL 0:7684b95768c7 49 //int bulk = 0;
OTL 0:7684b95768c7 50 void printf(const BD_ADDR* addr) {
OTL 0:7684b95768c7 51 const u8* a = addr->addr;
OTL 0:7684b95768c7 52 printf("%02X:%02X:%02X:%02X:%02X:%02X",a[5],a[4],a[3],a[2],a[1],a[0]);
OTL 0:7684b95768c7 53 }
OTL 0:7684b95768c7 54
OTL 0:7684b95768c7 55 const char FtDevClass[3] = {0x00, 0x1F, 0x82 };
OTL 0:7684b95768c7 56 const char SerDevClass[3] = {4, 1, 0x00};
OTL 0:7684b95768c7 57 // Instance
OTL 0:7684b95768c7 58 //RFCOMMManager rfcomm_manager;
OTL 0:7684b95768c7 59
OTL 0:7684b95768c7 60 class application : public HCI {
OTL 0:7684b95768c7 61 BTDevice* devs[8];
OTL 0:7684b95768c7 62 int count, i, pending;
OTL 0:7684b95768c7 63 public:
OTL 0:7684b95768c7 64 // We have connected to a device
OTL 0:7684b95768c7 65 void ConnectionComplete(connection_info* info) {
OTL 0:7684b95768c7 66 printf("ConnectionComplete ");
OTL 0:7684b95768c7 67 BD_ADDR* a = &info->bdaddr;
OTL 0:7684b95768c7 68 printf(a);
OTL 0:7684b95768c7 69 printf("\n");
OTL 0:7684b95768c7 70 RemoteNameRequest(a);
OTL 0:7684b95768c7 71 for (i++; i < count; i++) {//find the next device to open
OTL 0:7684b95768c7 72 printfBytes("DEVICE CLASS",devs[i]->_info.dev_class,3);
OTL 0:7684b95768c7 73 // if (devs[i]->_handle == 0 && memcmp(devs[i]->_info.dev_class, FtDevClass, 3)==0) {//or some other way to connect to RFCOMM devices
OTL 0:7684b95768c7 74 if (devs[i]->_handle == 0) {
OTL 0:7684b95768c7 75 BD_ADDR* bd = &devs[i]->_info.bdaddr;
OTL 0:7684b95768c7 76 printf("Connecting to ");
OTL 0:7684b95768c7 77 printf(bd);
OTL 0:7684b95768c7 78 printf("\n");
OTL 0:7684b95768c7 79 pending++;
OTL 0:7684b95768c7 80 CreateConnection(bd); //some low level connect, just let it happen for now (sets pin, mtu etc.)
OTL 0:7684b95768c7 81 printf("connection cmd was sent\n");
OTL 0:7684b95768c7 82 return;
OTL 0:7684b95768c7 83 }
OTL 0:7684b95768c7 84 }
OTL 0:7684b95768c7 85 //state = 1; //start the real application
OTL 0:7684b95768c7 86 }
OTL 0:7684b95768c7 87
OTL 0:7684b95768c7 88 void ConnectDevices() {
OTL 0:7684b95768c7 89 count = GetDevices(devs,8);//get pointers to all bluetooth devices
OTL 0:7684b95768c7 90 pending = 0;
OTL 0:7684b95768c7 91 for (i = 0; i < count; i++) {
OTL 0:7684b95768c7 92 printfBytes("DEVICE CLASS",devs[i]->_info.dev_class,3);
OTL 0:7684b95768c7 93 if (devs[i]->_handle == 0 /*&& memcmp(devs[i]->_info.dev_class, FtDevClass, 3)==0*/) {//or some other way to connect to RFCOMM devices
OTL 0:7684b95768c7 94 BD_ADDR* bd = &devs[i]->_info.bdaddr;
OTL 0:7684b95768c7 95 printf("Connecting to ");
OTL 0:7684b95768c7 96 printf(bd);
OTL 0:7684b95768c7 97 printf("\n");
OTL 0:7684b95768c7 98 pending++;
OTL 0:7684b95768c7 99 CreateConnection(bd); //some low level connect, just let it happen for now (sets pin, mtu etc.)
OTL 0:7684b95768c7 100 printf("connection cmd was sent\n");
OTL 0:7684b95768c7 101 return;
OTL 0:7684b95768c7 102 }
OTL 0:7684b95768c7 103 }
OTL 0:7684b95768c7 104 if (pending == 0) state = 1;//for the case when there are no ft devices
OTL 0:7684b95768c7 105 }
OTL 0:7684b95768c7 106 virtual void Callback(HCI_CALLBACK_EVENT c, const u8* data, int len);
OTL 0:7684b95768c7 107 int csr_write_bd_addr(BD_ADDR *bdaddr, bool transient=true);
OTL 0:7684b95768c7 108 int csr_reset_device(bool transient=true);
OTL 0:7684b95768c7 109 } App; //application instance
OTL 0:7684b95768c7 110
OTL 0:7684b95768c7 111 extern "C" void mbed_reset();
OTL 0:7684b95768c7 112
OTL 0:7684b95768c7 113
OTL 0:7684b95768c7 114 void application::Callback(HCI_CALLBACK_EVENT evt, const u8* data, int len) {//these events are forwarded (in)directly from HCIRecv
OTL 0:7684b95768c7 115 unsigned char pin[] = "1234";
OTL 0:7684b95768c7 116 unsigned char newaddr[] = {0x2c, 0x07, 0x54, 0x7b, 0x13, 0x00};//possible ft TX address (ROBO TX-277)
OTL 0:7684b95768c7 117 // unsigned char newaddr[] = {0x57, 0x0a, 0x3d, 0x83, 0x15, 0x00};//original address of the cheap round BT dongle
OTL 0:7684b95768c7 118 printf("\x1b[%dm", 33);
OTL 0:7684b95768c7 119 switch (evt) {
OTL 0:7684b95768c7 120 case CALLBACK_READY:
OTL 0:7684b95768c7 121 printf("CALLBACK_READY\n");
OTL 0:7684b95768c7 122 printf("my address = ");
OTL 0:7684b95768c7 123 printf((BD_ADDR*)data);
OTL 0:7684b95768c7 124 if (memcmp(newaddr, data, 6) != 0) { //csr_write_bd_addr((BD_ADDR*)newaddr, false);
OTL 0:7684b95768c7 125 }
OTL 0:7684b95768c7 126 Inquiry();//start the second phase of the discovery
OTL 0:7684b95768c7 127 break;
OTL 0:7684b95768c7 128
OTL 0:7684b95768c7 129 case CALLBACK_INQUIRY_RESULT: //optionally build the list of FT devices here
OTL 0:7684b95768c7 130 printf("CALLBACK_INQUIRY_RESULT ");
OTL 0:7684b95768c7 131 printf((BD_ADDR*)data);
OTL 0:7684b95768c7 132 printf("\n");//data points to inquiry_info struct
OTL 0:7684b95768c7 133 // RemoteNameRequest((BD_ADDR*)data);
OTL 0:7684b95768c7 134 break;
OTL 0:7684b95768c7 135
OTL 0:7684b95768c7 136 case CALLBACK_INQUIRY_DONE:
OTL 0:7684b95768c7 137 printf("CALLBACK_INQUIRY_DONE\n");
OTL 0:7684b95768c7 138 neighbors = new neighbourhood(&App);
OTL 0:7684b95768c7 139 neighbors->read();
OTL 0:7684b95768c7 140 ConnectDevices();
OTL 0:7684b95768c7 141 break;
OTL 0:7684b95768c7 142
OTL 0:7684b95768c7 143 case CALLBACK_REMOTE_NAME: {
OTL 0:7684b95768c7 144 BD_ADDR* addr = (BD_ADDR*)data;
OTL 0:7684b95768c7 145 const char* name = (const char*)(data + 6);
OTL 0:7684b95768c7 146 printf(addr);
OTL 0:7684b95768c7 147 printf(" = % s\n",name);
OTL 0:7684b95768c7 148 pending--;
OTL 0:7684b95768c7 149 if (pending == 0) state = 1;
OTL 0:7684b95768c7 150 }
OTL 0:7684b95768c7 151 break;
OTL 0:7684b95768c7 152
OTL 0:7684b95768c7 153 case CALLBACK_CONNECTION_COMPLETE: {
OTL 0:7684b95768c7 154 connection_info *ci = (connection_info*)data;
OTL 0:7684b95768c7 155 if (ci->status>0) {
OTL 0:7684b95768c7 156 printf("Connection failed, status=0x%02X\n", ci->status);
OTL 0:7684b95768c7 157 break;
OTL 0:7684b95768c7 158 }
OTL 0:7684b95768c7 159 ConnectionComplete(ci);
OTL 0:7684b95768c7 160 printf("Going to open sdp socket\n");
OTL 0:7684b95768c7 161 L2CAPAddr addr;
OTL 0:7684b95768c7 162 memcpy(&addr.bdaddr, &ci->bdaddr, 6);
OTL 0:7684b95768c7 163 //int s = SDP.Open(&addr.hdr);
OTL 0:7684b95768c7 164 }
OTL 0:7684b95768c7 165 break;
OTL 0:7684b95768c7 166 case CALLBACK_PIN_REQ:
OTL 0:7684b95768c7 167 printf("Enter PIN for ");
OTL 0:7684b95768c7 168 printf((BD_ADDR*)data);
OTL 0:7684b95768c7 169 printf(" : submitting %s\n", pin);
OTL 0:7684b95768c7 170 //USBLoop(); wait(1.0); USBLoop();
OTL 0:7684b95768c7 171 //for(int k=0; k<2000000;k++) USBLoop();
OTL 0:7684b95768c7 172 PinCodeReply(data, pin);
OTL 0:7684b95768c7 173 break;
OTL 0:7684b95768c7 174 case CALLBACK_VENDOR:
OTL 0:7684b95768c7 175 printfBytes("Vendor Reply:", data, len);
OTL 0:7684b95768c7 176 //mbed_reset();
OTL 0:7684b95768c7 177 if (data[0] == 0xc2)
OTL 0:7684b95768c7 178 csr_reset_device(false);
OTL 0:7684b95768c7 179 break;
OTL 0:7684b95768c7 180 default:
OTL 0:7684b95768c7 181 printf("Unhandled HCI Callback %d\n", evt);
OTL 0:7684b95768c7 182 };
OTL 0:7684b95768c7 183 printf("\x1b[%dm", 0);
OTL 0:7684b95768c7 184 }
OTL 0:7684b95768c7 185
OTL 0:7684b95768c7 186 #define CSR_WRITE 0xFC00
OTL 0:7684b95768c7 187
OTL 0:7684b95768c7 188 int application::csr_write_bd_addr(BD_ADDR *bdaddr, bool transient) {
OTL 0:7684b95768c7 189 unsigned char cmd[] = { 0xc2,
OTL 0:7684b95768c7 190 0x02, 0x00, 0x0c, 0x00, 0x11, 0x47, 0x03, 0x70,
OTL 0:7684b95768c7 191 0x00, 0x00, 0x01, 0x00, 0x04, 0x00, 0x00, 0x00,
OTL 0:7684b95768c7 192 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
OTL 0:7684b95768c7 193 };
OTL 0:7684b95768c7 194
OTL 0:7684b95768c7 195 if (transient)
OTL 0:7684b95768c7 196 cmd[15] = 0x08;
OTL 0:7684b95768c7 197
OTL 0:7684b95768c7 198 cmd[17] = bdaddr->addr[2];
OTL 0:7684b95768c7 199 cmd[18] = 0x00;
OTL 0:7684b95768c7 200 cmd[19] = bdaddr->addr[0];
OTL 0:7684b95768c7 201 cmd[20] = bdaddr->addr[1];
OTL 0:7684b95768c7 202 cmd[21] = bdaddr->addr[3];
OTL 0:7684b95768c7 203 cmd[22] = 0x00;
OTL 0:7684b95768c7 204 cmd[23] = bdaddr->addr[4];
OTL 0:7684b95768c7 205 cmd[24] = bdaddr->addr[5];
OTL 0:7684b95768c7 206
OTL 0:7684b95768c7 207 return SendCmd(CSR_WRITE, cmd, sizeof(cmd));
OTL 0:7684b95768c7 208 }
OTL 0:7684b95768c7 209
OTL 0:7684b95768c7 210 int application::csr_reset_device(bool transient) {
OTL 0:7684b95768c7 211 unsigned char cmd[] = { 0xc2, 0x02, 0x00, 0x09, 0x00,
OTL 0:7684b95768c7 212 0x00, 0x00, 0x01, 0x40, 0x00, 0x00,
OTL 0:7684b95768c7 213 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
OTL 0:7684b95768c7 214 };
OTL 0:7684b95768c7 215
OTL 0:7684b95768c7 216 if (transient)
OTL 0:7684b95768c7 217 cmd[7] = 0x02;
OTL 0:7684b95768c7 218
OTL 0:7684b95768c7 219 return SendCmd(CSR_WRITE, cmd, sizeof(cmd));
OTL 0:7684b95768c7 220 }
OTL 0:7684b95768c7 221
OTL 0:7684b95768c7 222
OTL 0:7684b95768c7 223 // these should be placed in the DMA SRAM
OTL 0:7684b95768c7 224 typedef struct {
OTL 0:7684b95768c7 225 u8 _hciBuffer[MAX_HCL_SIZE];
OTL 0:7684b95768c7 226 u8 _aclBuffer[MAX_ACL_SIZE];
OTL 0:7684b95768c7 227 } SRAMPlacement;
OTL 0:7684b95768c7 228
OTL 0:7684b95768c7 229 HCITransportUSB _HCITransportUSB; //use USB as the transport to the radio
OTL 0:7684b95768c7 230
OTL 0:7684b95768c7 231 int OnBluetoothInsert(int device) {//install the HCI and start discovery, user callbacks are made to HciCalback
OTL 0:7684b95768c7 232 printf("Bluetooth inserted of %d\n",device);
OTL 0:7684b95768c7 233 u32 sramLen;
OTL 0:7684b95768c7 234 u8* sram = USBGetBuffer(&sramLen);
OTL 0:7684b95768c7 235 sram = (u8*)(((u32)sram + 1023) & ~1023);
OTL 0:7684b95768c7 236 SRAMPlacement* s = (SRAMPlacement*)sram;
OTL 0:7684b95768c7 237 _HCITransportUSB.Open(device,s->_hciBuffer,s->_aclBuffer);//setup buffers for USB host, incoming data goes first to HCIRecv and ACLRecv
OTL 0:7684b95768c7 238 RegisterSocketHandler(SOCKET_L2CAP,&App); //register the application::hci as handler for L2CAP events
OTL 0:7684b95768c7 239 RegisterSocketHandler(SOCKET_RFCOM, &rfcomm_manager);//set the RFCOMMManager as the RFCOM socket handler
OTL 0:7684b95768c7 240 if (RegisterSocketHandler(SOCKET_SDP, &SDP))
OTL 0:7684b95768c7 241 printf("Could not register SDP socket type\n");
OTL 0:7684b95768c7 242 App.Open(&_HCITransportUSB);//the callback is virtual
OTL 0:7684b95768c7 243 App.Inquiry();//start discovery of BT devices phase 0
OTL 0:7684b95768c7 244 return 0;
OTL 0:7684b95768c7 245 }
OTL 0:7684b95768c7 246
OTL 0:7684b95768c7 247 DigitalOut led(LED1), loop(LED2);
OTL 0:7684b95768c7 248
OTL 0:7684b95768c7 249 int comm = 0;
OTL 0:7684b95768c7 250 btserial *incoming, *outgoing;
OTL 0:7684b95768c7 251
OTL 0:7684b95768c7 252 void echo(int socket, SocketState state, const u8* data, int len, void* userData) {
OTL 0:7684b95768c7 253 const u8 connack[] = { 0xbe, 0xef, 8, 'C','O','N','N','_','A','C','K', 0x11};
OTL 0:7684b95768c7 254 printf("Echo: socket %d, state %d, len=%d\n", socket, state, len);
OTL 0:7684b95768c7 255 if (state==SocketState_Open) {
OTL 0:7684b95768c7 256 if (len == 0) {
OTL 0:7684b95768c7 257 printf("Sending CONN_ACK\n");
OTL 0:7684b95768c7 258 Socket_Send(socket, connack, sizeof(connack));
OTL 0:7684b95768c7 259 } else {
OTL 0:7684b95768c7 260 Socket_Send(socket, data, len);
OTL 0:7684b95768c7 261 printfBytes("echo:", data, len);
OTL 0:7684b95768c7 262 }
OTL 0:7684b95768c7 263 }
OTL 0:7684b95768c7 264 }
OTL 0:7684b95768c7 265
OTL 0:7684b95768c7 266 void TestShell() {
OTL 0:7684b95768c7 267 int n=0;
OTL 0:7684b95768c7 268 USBInit();
OTL 0:7684b95768c7 269 for (;;) {
OTL 0:7684b95768c7 270 switch (state) {
OTL 0:7684b95768c7 271 case 0: //inquiry and low-level connection
OTL 0:7684b95768c7 272 break;
OTL 0:7684b95768c7 273 case 1: {//initialisation
OTL 0:7684b95768c7 274 printf("Ready to open ports!!!!!!!!!!!!!\n");
OTL 0:7684b95768c7 275 InitFtBtDeviceList();
OTL 0:7684b95768c7 276 int n = GetNrOfFtBtDevices();
OTL 0:7684b95768c7 277 printf("%d ft BT devices have been found\n", n);
OTL 0:7684b95768c7 278 if (n > 0) {
OTL 0:7684b95768c7 279 ftbtdev *d = GetFtUsbDeviceHandle(0);
OTL 0:7684b95768c7 280 if (d==0) printf("could not get device handle\n");
OTL 0:7684b95768c7 281 int sock = OpenFtBtDevice(d);
OTL 0:7684b95768c7 282 }
OTL 0:7684b95768c7 283 state = 2;
OTL 0:7684b95768c7 284 //comm = Socket_Listen(SOCKET_RFCOM, 1, echo, 0);
OTL 0:7684b95768c7 285 incoming = new btserial(1);
OTL 0:7684b95768c7 286 //incoming->baud(3);
OTL 0:7684b95768c7 287 }
OTL 0:7684b95768c7 288 break;
OTL 0:7684b95768c7 289 case 2://main loop
OTL 0:7684b95768c7 290 if (n%10000==0) {printf("*");}
OTL 0:7684b95768c7 291 if (incoming->readable()>0){
OTL 0:7684b95768c7 292 int c= incoming->getc();
OTL 0:7684b95768c7 293 printf("!!!!!!!!!!!!!!!!!!!!!!!!GOT%d!!!!!!!!!!!!!!!!!!!!!!!!!!", c);
OTL 0:7684b95768c7 294 if (c > 0) {
OTL 0:7684b95768c7 295 printf("sending");
OTL 0:7684b95768c7 296 //Socket_Send(SOCKET_RFCOM, &data , 1);
OTL 0:7684b95768c7 297 if ( incoming->putc(c) == -1) {
OTL 0:7684b95768c7 298 printf("FAIL");
OTL 0:7684b95768c7 299 }
OTL 0:7684b95768c7 300 // if (incoming->putc('\n')==-1) {
OTL 0:7684b95768c7 301 // printf("FAIL2");
OTL 0:7684b95768c7 302 // }
OTL 0:7684b95768c7 303 }
OTL 0:7684b95768c7 304 }
OTL 0:7684b95768c7 305 else if (incoming->readable()<0){
OTL 0:7684b95768c7 306 state = 3;
OTL 0:7684b95768c7 307 printf("end of session");
OTL 0:7684b95768c7 308 delete incoming;
OTL 0:7684b95768c7 309 }
OTL 0:7684b95768c7 310 break;
OTL 0:7684b95768c7 311 default:
OTL 0:7684b95768c7 312 break;
OTL 0:7684b95768c7 313 }
OTL 0:7684b95768c7 314 loop=1;
OTL 0:7684b95768c7 315 USBLoop();
OTL 0:7684b95768c7 316 loop=0;
OTL 0:7684b95768c7 317 n++;
OTL 0:7684b95768c7 318 if (n>=500000) {
OTL 0:7684b95768c7 319 u8 hoge = 1;
OTL 0:7684b95768c7 320 Socket_Send(SOCKET_RFCOM, &hoge, 1);
OTL 0:7684b95768c7 321
OTL 0:7684b95768c7 322 n=0;
OTL 0:7684b95768c7 323 led = !led;
OTL 0:7684b95768c7 324 }
OTL 0:7684b95768c7 325 }
OTL 0:7684b95768c7 326 //printf("Dropped out of main loop!\n");
OTL 0:7684b95768c7 327 }
OTL 0:7684b95768c7 328
OTL 0:7684b95768c7 329 //********************************************************************************************************************************