Diff: BluetoothComm.cpp

Revision:

2:be605799793f

Child:

3:14050370593a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BluetoothComm.cpp	Wed Dec 17 21:46:45 2014 +0000
@@ -0,0 +1,499 @@
+#include "mbed.h"
+#include "MODSERIAL.h"
+#include "BluetoothComm.h"
+#include "init.h"
+//#include "SDFile.cpp"
+#include <string>
+#include <map>
+//Variable indices for setValues: setValues should take an array of length NUMVARS + NUMREADONLYPARAMS = 34. 
+//Map of variables to array indices is as follows:
+//0 = Torso Angle, 1 = Left Knee Angle, 2 = Right Knee Angle, 3 = Left Hip angle, 4 = Right Hip angle, 5 = Left Hip Torque, 6 = Right Hip Torque
+//8 = Exo State/Left Knee State/Right Knee State 9 = Torso Ref. Angle, 10 = Left Hip Ref. Angle, 11 = Right Hip Ref. Angle
+//12 = KP Stance, 13 = KP Swing, 14 = KP Standing, 15 = KP Sitting, 16 = KP Standing up, 17 = KP Sitting down
+//18 = KD Stance, 19 = KD Swing, 20 = KD Standing, 21 = KD Sitting, 22 = KD Standing up, 23 = KD sitting down
+//24 = Standing angle, 25 = Sitting angle, 26 = Bent Forward Angle, 27 = Forward Angle, 28 = Rear Angle, 29 = IMU Angle
+//30 = Knee Full Retract, 31 = Knee Full Extend, 32 = Lock Time, 33 = Rate
+BluetoothComm::BluetoothComm(PinName tx, PinName rx): rn42(tx, rx), len(0), counter(0), inMsg(false), numVars(22), numReadOnlyParams(12), escapesNeeded(8)
+{
+    rn42.baud(9600);
+    printf("Started BTComm init \r\n");
+  //  rn42.baud(115200);
+    int temp1[] = {0,1,2,3,4,8,9,10, -1};
+  
+    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", "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];
+    }
+    
+   //Fill the parameter map with data from SD card
+   readDataFromSD();
+  
+   readParamsFromSD();
+ 
+
+}
+
+//Reads read-only data from SD card, and fills in the parameter map
+void BluetoothComm::readDataFromSD() {
+    /
+    int *arr = readData.read(arr, numVars);
+    for (int i = 0; i < numReadOnlyParams; i += 1) {
+        paramMap[indexMap[i + numVars]] = arr[i];
+    }
+}
+
+//Reads editable parameters from SD card, and fills in the parameter map
+void BluetoothComm::readParamsFromSD() {
+    int *arr = param.read(arr, numVars);
+    for (int i = 0; i < numVars; i += 1) {
+        paramMap[indexMap[i]] = arr[i];
+    }
+}   
+
+
+//Calculates parity--0 if c even, 1 if odd
+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
+char* BluetoothComm::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);
+    }
+    //sum = (short) (-1*sum);
+    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
+void BluetoothComm::sendError(char errCode)
+{
+    rn42.putc(START);
+    rn42.putc(errCode);
+    rn42.putc(END);
+    rn42.putc(0);
+    rn42.putc(0);
+    rn42.putc(0);
+}
+
+//Sets charge level
+void BluetoothComm::setCharge(short level) {
+   // readOnlyParams[11] = level;
+   paramMap["Charge"] = level;
+}
+
+//Sends the speified char array
+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
+void BluetoothComm::setValues(std::map<string, short> newValues) {
+
+    for (std::map<string, short>::iterator it = newValues.begin(); it != newValues.end(); ++it) {
+        paramMap[it->first] = it->second;
+    }
+}
+
+//Writes the editable params. stored in paramMap to the SD card
+void BluetoothComm::writeParamsToSDCard() {
+  
+    int paramValues[numVars];
+    for (int i = 0; i < numVars; i += 1) {
+        paramValues[i] = (int) paramMap[indexMap[i]];
+    }
+    param.write(paramValues, numVars);
+    
+}
+
+//Write the read-only values stored in paramMap to the SD card
+void BluetoothComm::writeDataToSDCard() {
+    int dataValues[numReadOnlyParams];
+    for (int i = 0; i < numReadOnlyParams; i += 1) {
+        dataValues[i] = (int) paramMap[indexMap[i + numVars]];
+    }
+    param.write(dataValues, numReadOnlyParams);
+}
+
+//Sends the paramList with START/END, parity bits, and a checksum
+void BluetoothComm::sendValues(char* paramList)
+{
+    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 (i == 21) {
+            //printf("On final loop \r\n");
+        }
+        if (paramList[i] != 0xff) {
+            short s = (short)((i << 8) | paramList[i]);
+            //printf("In sendValues, calculating parity of %x\r\n", s);
+            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;
+    char* checksum = checkSum(msg, len);
+    msg[len] = checksum[0];
+    msg[len+1] = checksum[1];
+    msg[len+2] = checksum[2];
+    len += 3;
+    for (int j = 0; j < len; j++) {
+        //printf("Sending char %x \r\n", msg[j]);
+        rn42.putc(msg[j]);
+    }
+    memcpy(lastCmd, msg, 50);
+    free(checksum);
+    return ;
+}
+
+//Sends readONly Parameters, with START/END and checksum
+void BluetoothComm::sendReadOnlyValues()
+{
+    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;
+    //printf("%d readonly parameters", numReadOnlyParams);
+    for (int i = 0; i < numReadOnlyParams; i++) {
+        if (i == escapeNeeded[escapes]) {
+            //printf("Escape char. needed at index %d \r\n", i);
+            //char conflict = (char)(readOnlyParams[i] & 0xff);
+            char conflict = (char)(paramMap[indexMap[i+numVars]] & 0xff);
+            //printf("%x possibly has a conflict in %x \r\n", readOnlyParams[i], conflict);
+            escapes += 1;
+            //message[msgInd+1] = (char) (readOnlyParams[i] >> 8);
+            message[msgInd+1] = (char) (paramMap[indexMap[i+numVars]] >> 8);
+            //printf("Set msgInd+1 to %x \r\n", message[msgInd+1]);
+            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) (readOnlyParams[i] >> 8);
+        //    message[msgInd+1] = (char) (readOnlyParams[i] & 0xff);
+            message[msgInd] = (char) (paramMap[indexMap[i+numVars]] >> 8);
+            message[msgInd+1] = (char) (paramMap[indexMap[i+numVars]] & 0xff);
+            msgInd += 2;
+        }
+    }
+    message[msgLen-4] = END;
+    char* checksum = checkSum(message, msgLen-3);
+    message[msgLen-3] = checksum[0];
+    message[msgLen-2] = checksum[1];
+    message[msgLen-1] = checksum[2];
+    //printf("Sending the following readONly values: \r\n");
+    for (int j=0; j < msgLen; j++) {
+        //printf("%x \r\n", message[j]);
+        rn42.putc(message[j]);
+    }
+    memcpy(lastCmd, message, 50);
+    free(checksum);
+    //printf("Finished sending readOnly values \r\n");
+
+}
+
+//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
+bool BluetoothComm::msgCheck(char* message, int len)
+{
+    if (message[0] != START) {
+        //printf("Improper START or END \r\n");
+        sendError(START_ERR);
+        return false;
+    }
+    if (message[len-4] != END) {
+        sendError(END_ERR);
+        return false;
+    }
+    bool write = message[2] & 0x80;
+    for (int i=2; i < len-5; i+=2) {
+        if (i == 2 && message[i] == READONLY_IND) {
+            break;
+        }
+        if (i == (len-5) && message[i] == READONLY_IND) {
+            break;
+        }
+        if (((message[i] & 0x80) !=0) && !write) {
+            //printf("Does not match READ format \r\n");
+            sendError((char)(((message[i] & 0x3f) << 3) | RW_ERR));
+            return false;
+        }
+        if (((message[i] & 0x80) != 0x80) && write) {
+            //printf("char %x Does not match WRITE format \r\n", message[i]);
+            sendError((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;
+
+        if (parity1 != parity(s & 0x4000)) {
+            //printf("Parity error in VAR char \r\n");
+            sendError((char) (((message[i] & 0xbf) << 3) | PARITY_ERR));
+            return false;
+        }
+
+        char c = message[i] & 0x3f;
+        char c2 = message[i+1];
+        if ((int) c < 0 || (int) c > numVars) {
+            //printf("VAR char out of range \r\n");
+            sendError((char) (((message[i] & 0xbf) << 3) | VAR_ERR));
+            return false;
+        }
+        if ((int) c2 < 0 || (int) c2 > 100) {
+            //printf("DATA char out of range");
+            sendError((char) (((message[i] & 0xbf) << 3) | DATA_ERR));
+            return false;
+        }
+    }
+    char* checksum = checkSum(message, len-3);
+    if (checksum[0] != message[len-3]) {
+        //printf("Checksum error in char 0, expected %x but got %x \r\n", checksum[0], message[len-3]);
+        free(checksum);
+        sendError(CHECKSUM_ERR);
+        return false;
+    }
+    if (checksum[1] != message[len-2]) {
+        //printf("Checksum error in char 1, expected %x but got %x \r\n", checksum[1], message[len-2]);
+        free(checksum);
+        sendError(CHECKSUM_ERR);
+        return false;
+    }
+    if (checksum[2] != message[len-1]) {
+        //printf("Checksum error in char 2, expected %x but got %x \r\n", checksum[2], message[len-1]);
+        free(checksum);
+        sendError(CHECKSUM_ERR);
+        return false;
+    }
+    free(checksum);
+
+    return true;
+}
+
+//Checks a received readOnly message
+void BluetoothComm::processReadOnly(char* message, int len)
+{
+    //printf("Message is a ReadOnly \r\n");
+    if (!msgCheck(message, len)) {
+        //printf("MSGCHECK failed on read! \r\n");
+        return;
+    }
+    failures = 0;
+
+    //printf("Sending readOnly values \r\n");
+    sendReadOnlyValues();
+}
+
+//Checks received READ message, and places requested data into an array
+void BluetoothComm::processRead(char* message, int len)
+{
+    //If the received message is an error message, resend the last command
+    if (message[2] == START) {
+        failures += 1;
+        if (failures < 5) {
+            send(lastCmd);
+        } else {
+            failures = 0;
+        }
+        return;
+    }
+    if (!msgCheck(message, len)) {
+        //printf("MSGCHECK failed on read! \r\n");
+        return;
+    }
+    failures = 0;
+    //printf("Message is a read \r\n");
+    char paramList[numVars];
+    memset(paramList, 0xff, numVars);
+
+    for (int i=2; i < len-5; i++) {
+        char msg = message[i] & 0xbf;
+        if ((msg & 0x80) != 0) {
+            //printf("Got a non-read char %x...exiting \r\n", msg);
+            return;
+        }
+
+        int index = msg & 0xff;
+        //printf("Value at index %d requested \r\n", index);
+        paramList[index] = paramMap[indexMap[index]];
+
+    }
+    if (message[len-5] == READONLY_IND) {
+        sendReadOnlyValues();
+    }
+    sendValues(paramList);
+}
+
+//Checks received WRITE message and writes to localValues
+void BluetoothComm::processWrite(char* message, int len)
+{
+    if (message[2] == START) {
+        failures += 1;
+        if (failures < 5) {
+            send(lastCmd);
+        } else {
+            failures = 0;
+        }
+        return;
+    }
+    if (!msgCheck(message, len)) {
+        //printf("MSGCHECK failed on write! \r\n");
+        return;
+    }
+    char paramList[numVars];
+    memset(paramList, 0xff, numVars);
+    //printf("Message is a write \r\n");
+    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);
+
+    }
+    sendValues(paramList);
+    writeParamsToSDCard();
+    //SD_Card.write(localValues)
+    //sendToControlBed(localValues+1, len-2)
+}
+
+//Checks if received message is a read, write or readonly
+void BluetoothComm::process (char* message, int len)
+{
+    char c = message[2];
+    for (int i =0; i < len; i++) {
+        printf("Message character: %x \r\n", message[i]);
+    }
+    if (c == READONLY_IND) {
+        processReadOnly(message, len);
+        return;
+    }
+    if ((c & 0x80) == 0) {
+        processRead(message, len);
+        return;
+    } else {
+        processWrite(message, len);
+        return;
+    }
+}
+
+//Warning: do not put print statements in the function attachment(); it will interfere with receiving messages
+void BluetoothComm::attachment()
+{
+if (rn42.readable()) {
+            //   //printf("rn42 is readable \r\n");
+            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) {
+            //    printf("Msg START received \r\n");
+                inMsg = true;
+                counter = 3;
+                curMsg[len] = b;
+                len += 1;
+            } else if (inMsg == true and b == START) {
+           //     printf("Second start received, terminating\r\n");
+                inMsg = false;
+                counter = 0;
+                memset(curMsg, 0, 50);
+                rn42.rxBufferFlush();
+                process(msg, len);
+                len = 0;
+            } else if (inMsg || counter > 0 ) {
+             //   printf("inMsg or counter > 0 \r\n");
+                curMsg[len] = b;
+                len += 1;
+                if (!inMsg) {
+                    counter -= 1;
+                }
+                //Marks end of message, and starts processing
+                if (counter <= 0) {
+           //         printf("End of message \r\n");
+                    memset(msg, 0, 50);
+                    memcpy(msg, curMsg, 50);
+                    memset(curMsg, 0, 50);
+                    rn42.rxBufferFlush();
+                    process(msg, len);
+                    len = 0;
+                }
+            }
+            if (b == END) {
+                inMsg = false;
+
+                // rn42.putc(msg);
+            }
+            //     }
+            //rn42.putc(data);
+          
+        }
+}
+
+
+