File content as of revision 20:be8dcc2344bc:

#include "mbed.h"
#include "MODSERIAL.h"
#include "BluetoothComm.h"
#include "initDatabed.h"
#include <string>
#include <map>

BluetoothComm::BluetoothComm(PinName tx, PinName rx): _rn42(tx, rx), _len(0), _counter(0), _inMsg(false), _numVars(34), _numReadOnlyParams(12), _escapesNeeded(8)
{
    
    /* Make sure baud rate is correct--rn42 cannot be read if code and MBED have different baud rates! */
    //_rn42.baud(9600);
    
    _rn42.baud(115200);
    
    //Readonly indices where an escape character may be needed, terminated by a -1
    int temp1[] = {0,1,2,3,4,8,9,10, -1};
  
    //Sets the _escapeNeeded array with the above indices
    for (int i = 0; i < _escapesNeeded+1; i++) {
        _escapeNeeded[i] = temp1[i];
    }
    
  
    std::string temp2[] = {"KPStance", "KPSwing", "KPStanding", "KPSitting", "KPStandUp", "KPSitdown", "KDStance", "KDSwing",
        "KDStanding", "KDSitting", "KDStandUp", "KDSitDown", "StandingAngle", "SittingAngle", "BentForwardAngle", "ForwardAngle", "RearAngle",
        "IMUAngle", "KneeFullRetract", "KneeFullExtend", "LockTime", "Rate", "StandupAsst", "StandupTime", "SitdownAsst", "SitdownTime", "WalkAngle",
        "StepLength", "StepTime", "HipFlex", "PhaseShift", "MaxAmplitude", "StanceStart", "StanceEnd", 
        "TorsoAng", "LKneeAng", "RKneeAng", "LHipAng", "RHipAng", "LHipTorque", "RHipTorque", "ExoAndKneeStates", "TorsoRefAngle", "LHipRefAngle",
        "RHipRefAngle", "Charge"};
    //Populate the map of indices to param names
    for (int j = 0; j < (_numVars + _numReadOnlyParams); j += 1) {
        _indexMap[j] = temp2[j];
    }
    
    
    //Will overwrite the parameter files with all 0's--REMOVE when we figure out how to retain SDCard data
    write_params_to_sd_card();
    
    //Dummy data to initialize readonly values, remove later...
    int temp4[] = {0x01fe, 0x02ac, 0x02ff, 0x0180, 0x0012, 0x0010, 0x0020, 0x00bf, 0x023f, 0x0123, 0x03a2, 0x10};
    //readData.write(_numReadOnlyParams, temp4);
    for (int k = 0; k < _numReadOnlyParams; k += 1) {
        _paramMap[_indexMap[_numVars + k]] = temp4[k];
    }
    //pc.printf("Starting sdcard stuff\r\n");
    write_data_to_sd_card();
   
   
    //Fill the parameter map with data from SD card
    read_data_from_sd();
  
    read_params_from_sd();
    pc.printf("Finished BTComm init\r\n");

}

/**
* Reads read-only data from SD card, and fills in the parameter map.
* @author Michael Ling
* @date 2/4/2015
*/


void BluetoothComm::read_data_from_sd()
{
    pc.printf("Beginning to read\r\n");
    int arr[_numReadOnlyParams];
    readData.read(_numReadOnlyParams, arr);
    pc.printf("Finished read\r\n");
    for (int i = 0; i < _numReadOnlyParams; i += 1) {
        //pc.printf("arr[%d] is %x\r\n", i, arr[i]);
        _paramMap[_indexMap[i + _numVars]] = arr[i];
       
    }
    
  
}

/**
* Reads editable parameters from SD card, and fills in the parameter map
* @author Michael Ling
* @date 2/4/2015
*/

void BluetoothComm::read_params_from_sd()
{
    int arr[_numVars];
    param.read(_numVars, arr);
    for (int i = 0; i < _numVars; i += 1) {
        _paramMap[_indexMap[i]] = arr[i];
    }
    
}

/**
* Calculates parity--0 if c even, 1 if odd
* @param c    The short that we want to calculate parity of
* @author Michael Ling
* @date 2/4/2015
*/
bool BluetoothComm::parity(short c)
{
    bool p = false;
    while (c != 0) {
        p = !p;
        c = (short) (c & (c-1));
    }
    return p;
}

/**
* Calculates checksum of char array, sum of all chars in array
* @param b    The char array to be summed up
* @param len    Length of the array b
* @author Michael Ling
* @date 2/4/2015
*/
char* BluetoothComm::get_checksum(char* b, int len)
{
    char* checksum = (char*)malloc(3*sizeof(char));
    short sum = 0;
    for (int i = 0; i < len; i += 1) {
        sum += ((short) b[i] & 0xff);
    }
    checksum[1] = (char) ((sum >> 8) & 0xff);
    checksum[2] = (char) (sum & 0xff);
    char check = (char) (sum & 0xff);
    //Sets escape character if the checksum contains START or END
    if (check == START || check == END) {
        checksum[0] = 1;
        checksum[2] = (char) (checksum[2] & 0x1);
    } else {
        checksum[0] = 0;
    }
    return checksum;
}

/**
* Sends error message.
* @param errCode    character representation of the failure-causing error
* @author Michael Ling
* @data 2/4/2015
*/
void BluetoothComm::send_error(char errCode)
{
    _rn42.putc(START);
    _rn42.putc(errCode);
    _rn42.putc(END);
    _rn42.putc(0);
    _rn42.putc(0);
    _rn42.putc(0);
}


/**
* Sends the specified char array through the _rn42 Bluetooth connection.
* @param cmd   The char array to be sent
* @author Michael Ling
* @date 2/4/2015
*/
void BluetoothComm::send(char *cmd)
{
    for (int i = 0; i < 50; i++) {
        _rn42.putc(cmd[i]);
    }
}                 

/**
* Sets the parameter map, based on the input map NEWVALUES
* @param newValues    A map of strings to shorts to be copied to _paramMap
* @author Michael Ling
* @date 2/4/2015
*/
void BluetoothComm::set_values(std::map<string, short> newValues)
{

    for (std::map<string, short>::iterator it = newValues.begin(); it != newValues.end(); ++it) {
        _paramMap[it->first] = it->second;
    }
}



/**
* Changes the value of a single param in the SD card
* @param index    Index to be set
* @param value    New value for that param
* @author Michael Ling
* @date 5/11/2015
*/
void BluetoothComm::set_data(int index, short value) {
    //pc.printf("New value for %d\r\n", index);
    readData.write_to_index(index, (int)value);
    
}

/**
* Writes the editable params. stored in _paramMap to the SD card
* @author Michael Ling
* @date 2/4/2015
*/

void BluetoothComm::write_params_to_sd_card()
{
  
    int paramValues[_numVars];
    for (int i = 0; i < _numVars; i += 1) {
        paramValues[i] = (int) _paramMap[_indexMap[i]];
    }
    param.write(_numVars, paramValues);
    
}

/**
* Write the read-only values stored in _paramMap to the SD card
* @author Michael Ling
* @date 2/4/2015
*/

void BluetoothComm::write_data_to_sd_card()
{
    int dataValues[_numReadOnlyParams];
    for (int i = 0; i < _numReadOnlyParams; i += 1) {
        dataValues[i] = (int) _paramMap[_indexMap[i + _numVars]];
        //pc.printf("Index %d of dataValues set to %x\r\n", i, dataValues[i]);
    }
    readData.write(_numReadOnlyParams, dataValues);
}

/**
* Sends the paramList with START/END, parity bits, and a checksum
* @param paramList    List of parameters to be sent over Bluetooth, represented as a char array
* @param message      The message we want to send to Ctrlbed
* @param dataOut      Circular buffer that allow communication with Ctrlbed     
* @author Michael Ling
* @date 2/4/2015
*/
void BluetoothComm::send_values(char* paramList, char *message, short *dataOut)
{
    char msg[2*_numVars+6];
    int len=2;
    //printf("Sending values \r\n");
    msg[0] = START;
    msg[1] = 0;
    for (int i=0; i < _numVars; i++) {
        //If paramList[i] is not 0xff, that means it was set by the last write
        if (paramList[i] != 0xff) {
            short s = (short)((i << 8) | paramList[i]);
            //printf("In send_values, calculating parity of %x\r\n", s);
            //This block adds parity bit, if needed
            if (parity(s)) {
                //printf("%x requires TRUE parity bit\r\n", s);
                msg[len] = (char)(i | 0x40);
                len += 1;
            } else {
                //printf("%x requires FALSE parity bit\r\n", s);
                msg[len] = (char)i;
                len += 1;
            }
            msg[len] = paramList[i];
            len += 1;
        }
    }
    msg[len] = END;
    len += 1;
    //Computes checksum and appends it to the message
    char* checksum = get_checksum(msg, len);
    msg[len] = checksum[0];
    msg[len+1] = checksum[1];
    msg[len+2] = checksum[2];
    len += 3;
    //This loop sends a reply to the phone
    for (int j = 0; j < len; j++) {
        printf("Sending char %x \r\n", msg[j]);
        _rn42.putc(msg[j]);
        
    }
    //This loop passes the original write message to the ctrlbed
    //Each pair of characters is converted to a short, with orders reversed. 
    //This is so that casting the short* back to char* restores the original message.
    if (dataOut != NULL) {
        int ind = 2;
        int j;
        for (j = 0; j < len-1; j+=2) {
            dataOut[ind] = (message[j+1]<<8)|message[j];
            
            ind += 1;
        }
        //If the message has an odd number of characters
        if (j < len) {
            dataOut[ind] = message[j];
            
        } 
        for (int k=0; k<len;k++) {
            printf("Index %d: %x\r\n", k, dataOut[k]);
        }
        
    }
    memcpy(_lastCmd, msg, 50);
    free(checksum);
    return ;
}

/**
* Sends readOnly Parameters, with START/END and checksum
* @author Michael Ling
* @date 2/4/2015
*/
void BluetoothComm::send_read_only_values()
{
    pc.printf("Send read only\r\n");
    read_data_from_sd();
    pc.printf("Updated readonly map\r\n");
    int msgLen = 2*_numReadOnlyParams+_escapesNeeded+7;
    char message[msgLen];
    message[0] = START;
    message[1] = 0;
    message[2] = READONLY_IND;
    int msgInd = 3;
    int escapes = 0;
    for (int i = 0; i < _numReadOnlyParams; i++) {
        if (i == _escapeNeeded[escapes]) {
            //Format of the message is one escape byte (if in escapeNeeded) plus two data bytes
            //Before putting a readonly value into the message, check if its bytes match 0xfe or 0xff
            //If a conflict is detected, replace the offending byte with 0xfc and 0xfd, respectively...
            //...and set either the rightmost or 2nd-rightmost bit of the escape byte, depending on that byte's position
            char conflict = (char)(_paramMap[_indexMap[i+_numVars]] & 0xff);
            escapes += 1;
            //message[msgInd+1] = (char) (_paramMap[_indexMap[i+_numVars]] >> 8);
            char conflict2 = (char)(_paramMap[_indexMap[i+_numVars]] >> 8);
            if (conflict2 == (char) 0xfe) {
                message[msgInd] = 2;
                message[msgInd+1] = 0xfc;
            } else if (conflict2 == (char) 0xff) {
                message[msgInd] = 2;
                message[msgInd+1] = 0xfd;
            } else {
                message[msgInd] = 0;
                message[msgInd+1] = conflict2;
            }
            if (conflict == (char) 0xfe) {
                message[msgInd] |= 1;
                message[msgInd+2] = 0xfc;
            } else if (conflict == (char) 0xff) {
                message[msgInd] |= 1;
                message[msgInd+2] = 0xfd;
            } else {
                //message[msgInd] = 0;
                message[msgInd+2] = conflict;
            }
            msgInd += 3;
        } else {
            message[msgInd] = (char) (_paramMap[_indexMap[i+_numVars]] >> 8);
            message[msgInd+1] = (char) (_paramMap[_indexMap[i+_numVars]] & 0xff);
            msgInd += 2;
        }
    }
    //Readonly message need a checksum as well
    message[msgLen-4] = END;
    char* checksum = get_checksum(message, msgLen-3);
    message[msgLen-3] = checksum[0];
    message[msgLen-2] = checksum[1];
    message[msgLen-1] = checksum[2];
    for (int j=0; j < msgLen; j++) {
        _rn42.putc(message[j]);
    }
    memcpy(_lastCmd, message, 50);
    free(checksum);

}

/**
* Checks the message with length len. Checks for START and END in correct spots, parity, all data and variable byte in proper range, and correct checksum
* @param message    The received message to check
* @param len    Length of the message
* @author Michael Ling
* @date 2/4/2015
*/
bool BluetoothComm::msg_check(char* message, int len)
{
    //Message must have a START and an END
    if (message[0] != START) {
        send_error(START_ERR);
        return false;
    }
    if (message[len-4] != END) {
        send_error(END_ERR);
        return false;
    }
    bool write = message[2] & 0x80;
    for (int i=2; i < len-5; i+=2) {
        //This block indicates a purely readonly message
        if (i == 2 && message[i] == READONLY_IND) {
            break;
        }
        if (i == (len-5) && message[i] == READONLY_IND) {
            break;
        }
        //Error if the write bit is set in a read message, or not set in a write message
        if (((message[i] & 0x80) !=0) && !write) {
            send_error((char)(((message[i] & 0x3f) << 3) | RW_ERR));
            return false;
        }
        if (((message[i] & 0x80) != 0x80) && write) {
            send_error((char)(((message[i] & 0x3f) << 3) | RW_ERR));
            return false;
        }
        short s;
        if (write) {
            s = (short) ((message[i] << 8) | message[i+1]);
        } else {
            s = (short) (message[i] << 8);
        }
        bool parity1 = (s & 0x4000) != 0;

        //Error if the two-byte chunk has wrong parity
        if (parity1 != parity(s & 0x4000)) {
            send_error((char) (((message[i] & 0xbf) << 3) | PARITY_ERR));
            return false;
        }

        char c = message[i] & 0x3f;
        char c2 = message[i+1];
        //Error if the param index is out of range
        if ((int) c < 0 || (int) c > _numVars) {
            send_error((char) (((message[i] & 0xbf) << 3) | VAR_ERR));
            return false;
        }
        //Error if the new param value is out of range
        if ((int) c2 < 0 || (int) c2 > 100) {
            send_error((char) (((message[i] & 0xbf) << 3) | DATA_ERR));
            return false;
        }
    }
    //Error if checksum calculated incorrectly
    char* checksum = get_checksum(message, len-3);
    if (checksum[0] != message[len-3]) {
        free(checksum);
        send_error(CHECKSUM_ERR);
        return false;
    }
    if (checksum[1] != message[len-2]) {
        free(checksum);
        send_error(CHECKSUM_ERR);
        return false;
    }
    if (checksum[2] != message[len-1]) {
        free(checksum);
        send_error(CHECKSUM_ERR);
        return false;
    }
    free(checksum);

    return true;
}

/**
* Checks a received readOnly message
* @param message    The readonly message received
* @param len    Length of the message
* @author Michael Ling
* @date 2/4/2015
*/
void BluetoothComm::process_read_only(char* message, int len)
{
   
    if (!msg_check(message, len)) {
        printf("msg_check failed on readonly! \r\n");
        return;
    }
    _failures = 0;

    send_read_only_values();
}

/**
* Checks received READ message, and places requested data into an array
* @param message    The received READ request as a char array
* @param len    Length of the READ message
* @author Michael Ling
* @date 2/4/2015
*/
void BluetoothComm::process_read(char* message, int len)
{
    //If the received message is an error message, resend the last command
    read_params_from_sd();
    if (message[2] == END) {
        _failures += 1;
        if (_failures < 5) {
            send(_lastCmd);
        } else {
            _failures = 0;
        }
        return;
    }
    if (!msg_check(message, len)) {
        return;
    }
    
    _failures = 0;
    char paramList[_numVars];
    memset(paramList, 0xff, _numVars);

    for (int i=2; i < len-5; i++) {
        char msg = message[i] & 0xbf;
        if ((msg & 0x80) != 0) {
            return;
        }

        int index = msg & 0xff;
        paramList[index] = _paramMap[_indexMap[index]];

    }
    //presence of READONLY_IND at the end of the indices to read indicates that we want to read readonly values as well as editable params
    if (message[len-5] == READONLY_IND) {
        send_read_only_values();
    }
    //read_params_from_sd();
    send_values(paramList, NULL, NULL);
}

/**
* Checks received WRITE message and writes to paramMap/SDCard
* @param message    The received WRITE message
* @param len    Length of the WRITE message
* @param dataOut Buffer that contains message for Ctrlbed
* @author Michael Ling
* @date 2/4/2015
*/
void BluetoothComm::process_write(char* message, int len, short* dataOut)
{
    if (message[2] == END) {
        _failures += 1;
        if (_failures < 5) {
            send(_lastCmd);
        } else {
            _failures = 0;
        }
        return;
    }
    //dataOut = message;
    if (!msg_check(message, len)) {
        printf("msg_check failed on write! \r\n");
        return;
    }
   
    char paramList[_numVars];
    memset(paramList, 0xff, _numVars);
    //Strips received message of parity and write bits, sets paramMap to the new values, then passes the phone/ctrlbed messages to sendValues
    for (int i=2; i < len-5; i+=2) {
        char msg = message[i] & 0xbf;
        if ((msg & 0x80) != 0x80) {
            return;
        }
        int index = msg & 0x7f;
        _paramMap[_indexMap[index]] = message[i+1];
        paramList[index] = message[i+1];
        //printf("Wrote %x to index %d of localValues \r\n", localValues[index], index);

    }
    send_values(paramList, message, dataOut);
    
    write_params_to_sd_card();
    
}

/** 
* Checks if received message is a read, write or readonly
* @param message    The received message
* @param len    Length of the message
* @param dataOut Buffer for sending to ctrlbed
* @author Michael Ling
* @date 2/4/2015
*/
void BluetoothComm::process (char* message, int len, short* dataOut)
{
    char c = message[2];
    for (int i =0; i < len; i++) {
        //printf("Message character: %x \r\n", message[i]);
    }
    if (c == READONLY_IND) {
        process_read_only(message, len);
        return;
    }
    if ((c & 0x80) == 0) {
        //printf("Is a read\r\n");
        process_read(message, len);
        param.debug_print();
        return;
    } else {
        process_write(message, len, dataOut);
        return;
    }
}

//Warning: do not put print statements in the function attachment(); it will interfere with receiving messages
/**
* Scans for data received through Bluetooth, and passes it on if it detects a message-like chunk. Should be run via an interuppt.
* @param dataOut    Buffer for sending to Ctrlbed
* @author Michael Ling
* @date 2/4/2015
*/
void BluetoothComm::attachment(short* dataOut)
{
     
   // pc.printf("Entered attachment\r\n");
    if (_rn42.readable()) {
        _data=_rn42.getc();

        //    if (_data != NULL) {
        char b = _data & 0xff;
        //printf("Got char: %x \r\n", b);
        if (b != NULL or _inMsg) {
            //       printf("Got char non null: %x \r\n", b);
        }
        //This marks the START of a message
        if (_inMsg == false and b == START) {
            //boardLed4 = 1;
            _inMsg = true;
            _counter = 3;
            _curMsg[_len] = b;
            _len += 1;
        //Kill the current message if we receive a second START
        } else if (_inMsg == true and b == START) {
            _inMsg = false;
            _counter = 0;
            memset(_curMsg, 0, 50);
            _rn42.rxBufferFlush();
            process(_msg, _len, dataOut);
            _len = 0;
        //Read the received byte into the message, if we're still in a message...
        } else if (_inMsg || _counter > 0 ) {
            _curMsg[_len] = b;
            _len += 1;
            //This will happen if we're processing the checksum--we want to read 3 bytes past the END
            if (!_inMsg) {
                _counter -= 1;
            }
            //Marks end of message, and starts processing
            if (_counter <= 0) {
                memset(_msg, 0, 50);
                memcpy(_msg, _curMsg, 50);
                memset(_curMsg, 0, 50);
                _rn42.rxBufferFlush();
                process(_msg, _len, dataOut);
                _len = 0;
            }
        }
        if (b == END) {
            _inMsg = false;

           
        }
        
          
    }
}