This program is for an autonomous robot for the competition at the Hochschule Luzern. http://cruisingcrepe.wordpress.com/ We are one of the 32 teams. http://cruisingcrepe.wordpress.com/ The postition control is based on this Documentation: Control of Wheeled Mobile Robots: An Experimental Overview from Alessandro De Luca, Giuseppe Oriolo, Marilena Vendittelli. For more information see here: http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf

Dependencies:   mbed

Fork of autonomous Robot Android by Christian Burri

MicroBridge/ADB/Adb.cpp

Committer:
chrigelburri
Date:
2013-05-03
Revision:
18:306d362d692b

File content as of revision 18:306d362d692b:

/*
    Copyright 2011 Niels Brouwers

    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.
*/

/* Changed by Junichi Katsu */

#include <string.h>
#include <Adb.h>

//#define DEBUG

#define MAX_BUF_SIZE 128

#ifdef DEBUG
#define  log(...)       printf(__VA_ARGS__)
#else
#define  log(...)       do {} while(0)
#endif

int input_ep;
int output_ep;
int _device;
int _configuration;
int _interfaceNumber;

static Connection * firstConnection;
static boolean connected;
static int connectionLocalId = 1;
unsigned char readbuff[MAX_BUF_SIZE];

// Event handler callback function.
adb_eventHandler * eventHandler;

ADB* _adb;
PacketBuffer recv_packet_buf(100,MAX_BUF_SIZE);
Ticker timer;

int time_ms = 0;


void attime(void)
{
    time_ms++;
}

int millis()
{
    return(time_ms);
}

char *strdup(const char *src)
{
    char *p;
    
    if(src == NULL){
        return NULL;
    }
    
    p = (char *)malloc(strlen(src) + 1);
    if(p != NULL) strcpy(p, src);
    return p;

}

/**
 * Initialises the ADB protocol. This function initialises the USB layer underneath so no further setup is required.
 */
void ADB::init()
{   
    recv_packet_buf.clear();
    
    // Signal that we are not connected.
    _device = 0;
    connected = false;
    

    // Initialise Usb host.
    USBInit();
    
    timer.attach_us(&attime, 1000);
}

/**
 * Sets the ADB event handler function. This function will be called by the ADB layer
 * when interesting events occur, such as ADB connect/disconnect, connection open/close, and
 * connection writes from the ADB device.
 *
 * @param handler event handler function.
 */
void ADB::setEventHandler(adb_eventHandler * handler)
{
    eventHandler = handler;
}

/**
 * Fires an ADB event.
 * @param connection ADB connection. May be NULL in case of global connect/disconnect events.
 * @param type event type.
 * @param length payload length or zero if no payload.
 * @param data payload data if relevant or NULL otherwise.
 */
void ADB::fireEvent(Connection * connection, adb_eventType type, uint16_t length, uint8_t * data)
{
    // Fire the global event handler, if set.
    if (eventHandler!=NULL)
        eventHandler(connection, type, length, data);

    // Fire the event handler of the connection in question, if relevant
    if (connection!=NULL && connection->eventHandler!=NULL)
        connection->eventHandler(connection, type, length, data);
}

/**
 * Adds a new ADB connection. The connection string is per ADB specs, for example "tcp:1234" opens a
 * connection to tcp port 1234, and "shell:ls" outputs a listing of the phone root filesystem. Connections
 * can be made persistent by setting reconnect to true. Persistent connections will be automatically
 * reconnected when the USB cable is re-plugged in. Non-persistent connections will connect only once,
 * and should never be used after they are closed.
 *
 * The connection string is copied into the Connection record and may not exceed ADB_CONNECTIONSTRING_LENGTH-1
 * characters.
 *
 * @param connectionString ADB connectionstring. I.e. "tcp:1234" or "shell:ls".
 * @param reconnect true for automatic reconnect (persistent connections).
 * @param handler event handler.
 * @return an ADB connection record or NULL on failure (not enough slots or connection string too long).
 */
Connection * ADB::addConnection(const char * connectionString, boolean reconnect, adb_eventHandler * handler)
{

    // Allocate a new ADB connection object
    Connection * connection = (Connection*)malloc(sizeof(Connection));
    if (connection == NULL) return NULL;

    // Allocate memory for the connection string
    connection->connectionString = (char*)strdup(connectionString);
    if (connection->connectionString==NULL)
    {
        // Free the connection object and return null
        free(connection);
        return NULL;
    }

    // Initialise the newly created object.
    connection->localID = connectionLocalId ++;
    connection->status = ADB_CLOSED;
    connection->lastConnectionAttempt = 0;
    connection->reconnect = reconnect;
    connection->eventHandler = handler;

    // Add the connection to the linked list. Note that it's easier to just insert
    // at position 0 because you don't have to traverse the list :)
    connection->next = firstConnection;
    firstConnection = connection;

    // Unable to find an empty spot, all connection slots in use.
    return connection;
}

/**
 * Prints an ADB_message, for debugging purposes.
 * @param message ADB message to print.
 */
#ifdef DEBUG
static void adb_printMessage(adb_message * message)
{
    switch(message->command)
    {
    case A_OKAY:
        printf("OKAY message [%lx] %ld %ld\r\n", message->command, message->arg0, message->arg1);
        break;
    case A_CLSE:
        printf("CLSE message [%lx] %ld %ld\r\n", message->command, message->arg0, message->arg1);
        break;
    case A_WRTE:
        printf("WRTE message [%lx] %ld %ld, %ld bytes\r\n", message->command, message->arg0, message->arg1, message->data_length);
        break;
    case A_CNXN:
        printf("CNXN message [%lx] %ld %ld\r\n", message->command, message->arg0, message->arg1);
        break;
    case A_SYNC:
        printf("SYNC message [%lx] %ld %ld\r\n", message->command, message->arg0, message->arg1);
        break;
    case A_OPEN:
        printf("OPEN message [%lx] %ld %ld\r\n", message->command, message->arg0, message->arg1);
        break;
    default:
        printf("WTF message [%lx] %ld %ld\r\n", message->command, message->arg0, message->arg1);
        break;
    }
}
#endif

/**
 * Writes an empty message (without payload) to the ADB device.
 *
 * @param device USB device handle.
 * @param command ADB command.
 * @param arg0 first ADB argument (command dependent).
 * @param arg0 second ADB argument (command dependent).
 * @return error code or 0 for success.
 */
int ADB::writeEmptyMessage(int device, uint32_t command, uint32_t arg0, uint32_t arg1)
{
    adb_message message;

    message.command = command;
    message.arg0 = arg0;
    message.arg1 = arg1;
    message.data_length = 0;
    message.data_check = 0;
    message.magic = command ^ 0xffffffff;

#ifdef DEBUG
    printf("OUT << "); adb_printMessage(&message);
#endif

    int r = USBBulkTransfer( device , output_ep , (uint8_t*)&message , sizeof(adb_message) );

#ifdef DEBUG
    log("[writeMessage1] size:%d\r\n",r);
    
    int ii,jj;
    uint8_t* buf = (uint8_t*)&message;
    for(ii = 0 ; ii < r ; ii+=16)
    {
        for(jj = 0 ; jj < 16 ; jj+=2 )
        {
            log("%02X%02X ",buf[ii+jj],buf[ii+jj+1]);
            if((ii+jj) > r) break;
        }
        log(" : ");
        for(jj = 0 ; jj < 16 ; jj+=2 )
        {
            log("%c%c",buf[ii+jj],buf[ii+jj+1]);
            if((ii+jj) > r) break;
        }
        log("\r\n");
        if((ii+jj) > r) break;
    }
#endif

    return r;
}

/**
 * Writes an ADB message with payload to the ADB device.
 *
 * @param device USB device handle.
 * @param command ADB command.
 * @param arg0 first ADB argument (command dependent).
 * @param arg0 second ADB argument (command dependent).
 * @param length payload length.
 * @param data command payload.
 * @return error code or 0 for success.
 */
int ADB::writeMessage(int device, uint32_t command, uint32_t arg0, uint32_t arg1, uint32_t length, uint8_t * data)
{
    adb_message message;
    uint8_t msg[256];
    uint32_t count, sum = 0;
    uint8_t * x;

    // Calculate data checksum
    count = length;
    x = data;
    while(count-- > 0) sum += *x++;

    // Fill out the message record.
    message.command = command;
    message.arg0 = arg0;
    message.arg1 = arg1;
    message.data_length = length;
    message.data_check = (sum);
    message.magic = command ^ 0xffffffff;
    
#ifdef DEBUG
    printf("OUT << "); adb_printMessage(&message);
#endif

    int r = USBBulkTransfer( device , output_ep , (uint8_t*)&message , sizeof(adb_message) );
    
    if (r<0) return r;

#ifdef DEBUG
    log("[writeMessage1] size:%d\r\n",r);
    
    int ii,jj;
    uint8_t* buf = (uint8_t*)&message;
    for(ii = 0 ; ii < r ; ii+=16)
    {
        for(jj = 0 ; jj < 16 ; jj+=2 )
        {
            log("%02X%02X ",buf[ii+jj],buf[ii+jj+1]);
            if((ii+jj) > r) break;
        }
        log(" : ");
        for(jj = 0 ; jj < 16 ; jj+=2 )
        {
            log("%c%c",buf[ii+jj],buf[ii+jj+1]);
            if((ii+jj) > r) break;
        }
        log("\r\n");
        if((ii+jj) > r) break;
    }
#endif
    
    memcpy( msg , data , length );
    
    r = USBBulkTransfer( device , output_ep , msg , length );
    log("USB SEND RET2:%d\r\n",r);
    
    if (r<0) return r;
    
#ifdef DEBUG
    log("[writeMessage2] size:%d\r\n",r);
    
    buf = msg;
    for(ii = 0 ; ii < r ; ii+=16)
    {
        for(jj = 0 ; jj < 16 ; jj+=2 )
        {
            log("%02X%02X ",buf[ii+jj],buf[ii+jj+1]);
            if((ii+jj) > r) break;
        }
        log(" : ");
        for(jj = 0 ; jj < 16 ; jj+=2 )
        {
            log("%c%c",buf[ii+jj],buf[ii+jj+1]);
            if((ii+jj) > r) break;
        }
        log("\r\n");
        if((ii+jj) > r) break;
    }
#endif
    
    r = 0;
    
    return r;
}

/**
 * Writes an ADB command with a string as payload.
 *
 * @param device USB device handle.
 * @param command ADB command.
 * @param arg0 first ADB argument (command dependent).
 * @param arg0 second ADB argument (command dependent).
 * @param str payload string.
 * @return error code or 0 for success.
 */
int ADB::writeStringMessage(int device, uint32_t command, uint32_t arg0, uint32_t arg1, char * str)
{
    return ADB::writeMessage(device, command, arg0, arg1, strlen(str) + 1, (uint8_t*)str);
}

/**
 * Poll an ADB message.
 * @param message on success, the ADB message will be returned in this struct.
 * @param poll true to poll for a packet on the input endpoint, false to wait for a packet. Use false here when a packet is expected (i.e. OKAY in response to WRTE)
 * @return true iff a packet was successfully received, false otherwise.
 */
boolean ADB::pollMessage(adb_message * message, boolean poll)
{
    int bytesRead = 0;
    uint8_t buf[ADB_USB_PACKETSIZE];

    // Poll a packet from the USB
    bytesRead = recv_packet_buf.GetPacket((char*)buf);

    // Check if the USB in transfer was successful.
    if (bytesRead<=0) return false;

    log("[pollMessage] byteRead size:%d\r\n",bytesRead);
    
    // Check if the buffer contains a valid message
    memcpy((void*)message, (void*)buf, sizeof(adb_message));

    // If the message is corrupt, return.
#if 1
    if (message->magic != (message->command ^ 0xffffffff))
    {
#ifdef DEBUG
        printf("Broken message, magic mismatch, %d bytes\r\n", bytesRead);
        return false;
#endif
    }
#endif
    // Check if the received number of bytes matches our expected 24 bytes of ADB message header.
    if (bytesRead != sizeof(adb_message)) return false;

    return true;
}

/**
 * Sends an ADB OPEN message for any connections that are currently in the CLOSED state.
 */
void ADB::openClosedConnections()
{
    uint32_t timeSinceLastConnect;
    Connection * connection;

    // Iterate over the connection list and send "OPEN" for the ones that are currently closed.
    for (connection = firstConnection; connection!=NULL; connection = connection->next)
    {
        timeSinceLastConnect = millis() - connection->lastConnectionAttempt;
        if (connection->status==ADB_CLOSED && timeSinceLastConnect>ADB_CONNECTION_RETRY_TIME)
        {
            // Issue open command.
            ADB::writeStringMessage(_device, A_OPEN, connection->localID, 0, connection->connectionString);

            // Record the last attempt time
            connection->lastConnectionAttempt = millis();
            connection->status = ADB_OPENING;

        }
    }

}

/**
 * Handles and ADB OKAY message, which represents a transition in the connection state machine.
 *
 * @param connection ADB connection
 * @param message ADB message struct.
 */
void ADB::handleOkay(Connection * connection, adb_message * message)
{
    // Check if the OKAY message was a response to a CONNECT message.
    if (connection->status==ADB_OPENING)
    {
        connection->status = ADB_OPEN;
        connection->remoteID = message->arg0;

        ADB::fireEvent(connection, ADB_CONNECTION_OPEN, 0, NULL);
    }

    // Check if the OKAY message was a response to a WRITE message.
    if (connection->status == ADB_WRITING)
        connection->status = ADB_OPEN;

}

/**
 * Handles an ADB CLOSE message, and fires an ADB event accordingly.
 *
 * @param connection ADB connection
 */
void ADB::handleClose(Connection * connection)
{
    // Check if the CLOSE message was a response to a CONNECT message.
    if (connection->status==ADB_OPENING)
        ADB::fireEvent(connection, ADB_CONNECTION_FAILED, 0, NULL);
    else
        ADB::fireEvent(connection, ADB_CONNECTION_CLOSE, 0, NULL);

    // Connection failed
    if (connection->reconnect)
        connection->status = ADB_CLOSED;
    else
        connection->status = ADB_UNUSED;

}

/**
 * Handles an ADB WRITE message.
 *
 * @param connection ADB connection
 * @param message ADB message struct.
 */
void ADB::handleWrite(Connection * connection, adb_message * message)
{
    uint32_t bytesLeft = message->data_length;
    uint8_t buf[ADB_USB_PACKETSIZE];
    ConnectionStatus previousStatus;
    int bytesRead;

    previousStatus = connection->status;

    connection->status = ADB_RECEIVING;
    connection->dataRead = 0;
    connection->dataSize = message->data_length;

    while (bytesLeft>0)
    {
        int len = bytesLeft < ADB_USB_PACKETSIZE ? bytesLeft : ADB_USB_PACKETSIZE;

        // Read payload
        bytesRead = recv_packet_buf.GetPacket((char*)buf);
        

        // Poll the USB layer.
        USBLoop();
        
        log("[handleWrite] byteRead size:%d\r\n",bytesRead);

//        if (len != bytesRead)
//            printf("bytes read mismatch: %d expected, %d read, %ld left\r\n", len, bytesRead, bytesLeft);

        // Break out of the read loop if there's no data to read :(
        if (bytesRead==-1) break;
        else if(bytesRead!=0)
        {
            connection->dataRead += len;
            ADB::fireEvent(connection, ADB_CONNECTION_RECEIVE, len, buf);

            bytesLeft -= bytesRead;
        }
    }

    // Send OKAY message in reply.
    bytesRead = ADB::writeEmptyMessage(_device, A_OKAY, message->arg1, message->arg0);

    connection->status = previousStatus;
    
}

/**
 * Close all ADB connections.
 *
 * @param connection ADB connection
 * @param message ADB message struct.
 */
void ADB::closeAll()
{
    Connection * connection;

    // Iterate over all connections and close the ones that are currently open.
    for (connection = firstConnection; connection != NULL; connection = connection->next)
        if (!(connection->status==ADB_UNUSED || connection->status==ADB_CLOSED))
            ADB::handleClose(connection);

}

/**
 * Handles an ADB connect message. This is a response to a connect message sent from our side.
 * @param message ADB message.
 */
void ADB::handleConnect(adb_message * message)
{
    unsigned int bytesRead;
    uint8_t buf[MAX_BUF_SIZE];
    uint16_t len;

    // Read payload (remote ADB device ID)
    len = message->data_length < MAX_BUF_SIZE ? message->data_length : MAX_BUF_SIZE;
    bytesRead = recv_packet_buf.GetPacket((char*)buf);
    
    log("[handleConnect] byteRead size:%d\r\n",bytesRead);

    // Signal that we are now connected to an Android device (yay!)
    connected = true;

    // Fire event.
    ADB::fireEvent(NULL, ADB_CONNECT, len, buf);

}

/**
 * This method is called periodically to check for new messages on the USB bus and process them.
 */
void ADB::poll()
{
    Connection * connection;
    adb_message message;

    // Poll the USB layer.
    USBLoop();

    // If no USB device, there's no work for us to be done, so just return.
    if (_device==0) return;

    // If not connected, send a connection string to the device.
    if (!connected)
    {
        ADB::writeStringMessage(_device, A_CNXN, 0x01000000, 4096, (char*)"host::microbridge");
        for(int ii=0;ii<400;ii++)
        {
            USBLoop();
            wait_ms(1);
        }
        //wait_ms(500); // Give the device some time to respond.
    }

    // If we are connected, check if there are connections that need to be opened
    if (connected)
        ADB::openClosedConnections();

    // Check for an incoming ADB message.
    if (!ADB::pollMessage(&message, true))
        return;

    // Handle a response from the ADB device to our CONNECT message.
    if (message.command == A_CNXN)
        ADB::handleConnect(&message);

    // Handle messages for specific connections
    for (connection = firstConnection; connection != NULL; connection = connection->next)
    {
        if(connection->status!=ADB_UNUSED && connection->localID==message.arg1)
        {
            switch(message.command)
            {
            case A_OKAY:
                //printf("HANDLE OKEY\r\n");
                ADB::handleOkay(connection, &message);
                break;
            case A_CLSE:
                printf("HANDLE CLOSE\r\n");
                ADB::handleClose(connection);
                break;
            case A_WRTE:
                //printf("HANDLE WRITE\r\n");
                ADB::handleWrite(connection, &message);
                break;
            default:
                break;
            }
        }
    }

}

void ADB::AdbreadCallback(int device, int endpoint, int status, u8* buf, int len, void* userData) {
    
    recv_packet_buf.PutPacket((char*)buf,len);
    
#ifdef DEBUG
    log("[AdbreadCallback] size:%d\r\n",len);
    
    int ii,jj;
    for(ii = 0 ; ii < len ; ii+=16)
    {
        for(jj = 0 ; jj < 16 ; jj+=2 )
        {
            log("%02X%02X ",buf[ii+jj],buf[ii+jj+1]);
            if((ii+jj) > len) break;
        }
        log(" : ");
        for(jj = 0 ; jj < 16 ; jj+=2 )
        {
            log("%c%c",buf[ii+jj],buf[ii+jj+1]);
            if((ii+jj) > len) break;
        }
        log("\r\n");
        if((ii+jj) > len) break;
    }
#endif
    USBBulkTransfer(device, endpoint ,readbuff,sizeof(readbuff), AdbreadCallback, userData);
  //  wait_ms(4);
}

/**
 * Checks whether the a connected USB device is an ADB device and populates a configuration record if it is.
 *
 * @param device USB device.
 * @param handle pointer to a configuration record. The endpoint device address, configuration, and endpoint information will be stored here.
 * @return true iff the device is an ADB device.
 */
boolean ADB::isAdbDevice(int device, int configuration, int interfaceNumber)
{
    boolean ret = false;
    
    log("connecting Android \r\n");
    
    _device = device;
    _configuration = configuration;
    _interfaceNumber = interfaceNumber;
    
    log("device = %d configuration = %d interfaceNumber = %d\r\n", device, configuration, interfaceNumber);
    
    int err;
    
    u8 buffer[255];
    err = GetDescriptor(_device,DESCRIPTOR_TYPE_CONFIGURATION,0,buffer,4);
    
    if (err < 0) {
        log("Failed to get descriptor\r\n");
        return(ret);
    }
    
    int len = buffer[2] | (buffer[3] << 8);
    if (len > sizeof(buffer)) {
        log("config descriptor too large\r\n");
        /* might want to truncate here */
        return(ret);
    }
    err = GetDescriptor(_device,DESCRIPTOR_TYPE_CONFIGURATION,0,buffer,len);
    u8* p = buffer;
    input_ep=0;
    output_ep=0;
    EndpointDescriptor *epDesc;
    
    log("Descriptor size:%d\r\n",len);
    int ii,jj;
    for(ii = 0 ; ii < len ; ii+=16)
    {
        for(jj = 0 ; jj < 16 ; jj+=2 )
        {
            log("%02X%02X ",buffer[ii+jj],buffer[ii+jj+1]);
            if((ii+jj) > len) break;
        }
        log("\r\n");
        if((ii+jj) > len) break;
    }
    u8 interface_num = 0;
    
    while (p<(buffer+len)) {
        u8 descLen  = p[0];
        u8 descType = p[1];
        log("descLen=%d,descType=%d\r\n",descLen,descType);
        switch (descType) {
            case DESCRIPTOR_TYPE_CONFIGURATION:
                log("config desc\r\n");
                break;
            case DESCRIPTOR_TYPE_INTERFACE:
                interface_num = p[2];
                log("interface desc num[%d]\r\n",interface_num);
                break;
            case DESCRIPTOR_TYPE_ENDPOINT:
                epDesc=(EndpointDescriptor*)p;
                if( interface_num == 1 )
                {
                    if (!input_ep && (epDesc->bEndpointAddress& 0x80)) {
                        input_ep=epDesc->bEndpointAddress& 0x7f;
                        //PacketSize drop
                        log("input Endpoint address=%d,wMaxPacketSize=%d,bmAttributes=%d\r\n",input_ep,epDesc->wMaxPacketSize,epDesc->bmAttributes);

                    } else if (!output_ep) {
                        output_ep=epDesc->bEndpointAddress& 0x7f;
                        //PacketSize drop
                        log("output Endpoint address=%d,wMaxPacketSize=%d,bmAttributes=%d\r\n",output_ep,epDesc->wMaxPacketSize,epDesc->bmAttributes);
                    } else {
                        //other
                        log("non input,output Endpoint address=%d,wMaxPacketSize=%d,bmAttributes=%d\r\n",input_ep,epDesc->wMaxPacketSize,epDesc->bmAttributes);
                    }
                }
                break;
            default:
                log("unkown desc type(%d) \r\n",descType);
        }
        p+=descLen;
    }
    
    if (!(input_ep && output_ep)) {
        log("can't find accessory endpoints\r\n");
        return(false);
    }
    
    log("SetConfiguration\r\n");
    err = SetConfiguration(device,configuration);
    if (err < 0) {
        log("SetConfiguration error\r\n");
        return(false);
    }
    
    log("interrupt setup\r\n");
    //interrupt setup
    if (IO_PENDING!=USBBulkTransfer(_device,input_ep|0x80,readbuff,sizeof(readbuff),AdbreadCallback,NULL))    return(ret);
    
    log("ADB Standby\r\n");
    ret = true;
    
    return(ret);
}


void desconnect(void)
{
    ADB::closeAll();
    _device = 0;
    connected = false;
}

/**
 * Write a set of bytes to an open ADB connection.
 *
 * @param connection ADB connection to write the data to.
 * @param length number of bytes to transmit.
 * @param data data to send.
 * @return number of transmitted bytes, or -1 on failure.
 */
int ADB::write(Connection * connection, uint16_t length, uint8_t * data)
{
    int ret;

    // First check if we have a working ADB connection
    if (_device==0 || !connected) return -1;

    // Check if the connection is open for writing.
    if (connection->status != ADB_OPEN) return -2;

    // Write payload
    ret = ADB::writeMessage(_device, A_WRTE, connection->localID, connection->remoteID, length, data);
    if (ret==0)
        connection->status = ADB_WRITING;

    return ret;
}

/**
 * Write a string to an open ADB connection. The trailing zero is not transmitted.
 *
 * @param connection ADB connection to write the data to.
 * @param length number of bytes to transmit.
 * @param data data to send.
 * @return number of transmitted bytes, or -1 on failure.
 */
int ADB::writeString(Connection * connection, char * str)
{
    int ret;

    // First check if we have a working ADB connection
    if (_device==0 || !connected) return -1;

    // Check if the connection is open for writing.
    if (connection->status != ADB_OPEN) return -2;

    // Write payload
    ret = ADB::writeStringMessage(_device, A_WRTE, connection->localID, connection->remoteID, str);
    if (ret==0)
        connection->status = ADB_WRITING;

    return ret;
}

/**
 * Write a set of bytes to this ADB connection.
 *
 * @param length number of bytes to transmit.
 * @param data data to send.
 * @return number of transmitted bytes, or -1 on failure.
 */
int Connection::write(uint16_t length, uint8_t * data)
{
    return ADB::write(this, length, data);
}

/**
 * Write a string to this connection.
 *
 * @param length number of bytes to transmit.
 * @param data data to send.
 * @return number of transmitted bytes, or -1 on failure.
 */
int Connection::writeString(char * str)
{
    return ADB::writeString(this, str);
}

/**
 * Checks if the connection is open for writing.
 * @return true iff the connection is open and ready to accept write commands.
 */
bool Connection::isOpen()
{
    return this->status == ADB_OPEN;
}


/** from USBHost load function. initialize Android device**/
void OnLoadDevice(int device, DeviceDescriptor* deviceDesc, InterfaceDescriptor* interfaceDesc) {
    char s[128];
    
    log("LoadDevice %d %02X:%02X:%02X\r\n",device,interfaceDesc->bInterfaceClass,interfaceDesc->bInterfaceSubClass,interfaceDesc->bInterfaceProtocol);
    
    for (int i = 1; i < 4; i++) {
        if (GetString(device,i,s,sizeof(s)) < 0)
            break;
        printf("%d: %s\r\n",i,s);
    }
    
    // Adb?
    if(1)
    {
        ADB::isAdbDevice(device,1,2);
    }
    
}