Michael Ling / ExoController

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   Data-Management-Honka

BluetoothComm.cpp@2:be605799793f, 2014-12-17 (annotated)

Committer:
mzling
Date:
Wed Dec 17 21:46:45 2014 +0000
Revision:
2:be605799793f
Child:
3:14050370593a
Edited folder structure

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mzling 2:be605799793f 1 #include "mbed.h"
mzling 2:be605799793f 2 #include "MODSERIAL.h"
mzling 2:be605799793f 3 #include "BluetoothComm.h"
mzling 2:be605799793f 4 #include "init.h"
mzling 2:be605799793f 5 //#include "SDFile.cpp"
mzling 2:be605799793f 6 #include <string>
mzling 2:be605799793f 7 #include <map>
mzling 2:be605799793f 8 //Variable indices for setValues: setValues should take an array of length NUMVARS + NUMREADONLYPARAMS = 34.
mzling 2:be605799793f 9 //Map of variables to array indices is as follows:
mzling 2:be605799793f 10 //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
mzling 2:be605799793f 11 //8 = Exo State/Left Knee State/Right Knee State 9 = Torso Ref. Angle, 10 = Left Hip Ref. Angle, 11 = Right Hip Ref. Angle
mzling 2:be605799793f 12 //12 = KP Stance, 13 = KP Swing, 14 = KP Standing, 15 = KP Sitting, 16 = KP Standing up, 17 = KP Sitting down
mzling 2:be605799793f 13 //18 = KD Stance, 19 = KD Swing, 20 = KD Standing, 21 = KD Sitting, 22 = KD Standing up, 23 = KD sitting down
mzling 2:be605799793f 14 //24 = Standing angle, 25 = Sitting angle, 26 = Bent Forward Angle, 27 = Forward Angle, 28 = Rear Angle, 29 = IMU Angle
mzling 2:be605799793f 15 //30 = Knee Full Retract, 31 = Knee Full Extend, 32 = Lock Time, 33 = Rate
mzling 2:be605799793f 16 BluetoothComm::BluetoothComm(PinName tx, PinName rx): rn42(tx, rx), len(0), counter(0), inMsg(false), numVars(22), numReadOnlyParams(12), escapesNeeded(8)
mzling 2:be605799793f 17 {
mzling 2:be605799793f 18 rn42.baud(9600);
mzling 2:be605799793f 19 printf("Started BTComm init \r\n");
mzling 2:be605799793f 20 // rn42.baud(115200);
mzling 2:be605799793f 21 int temp1[] = {0,1,2,3,4,8,9,10, -1};
mzling 2:be605799793f 22
mzling 2:be605799793f 23 for (int i = 0; i < escapesNeeded+1; i++) {
mzling 2:be605799793f 24 escapeNeeded[i] = temp1[i];
mzling 2:be605799793f 25 }
mzling 2:be605799793f 26
mzling 2:be605799793f 27
mzling 2:be605799793f 28 std::string temp2[] = {"KPStance", "KPSwing", "KPStanding", "KPSitting", "KPStandUp", "KPSitdown", "KDStance", "KDSwing",
mzling 2:be605799793f 29 "KDStanding", "KDSitting", "KDStandUp", "KDSitDown", "StandingAngle", "SittingAngle", "BentForwardAngle", "ForwardAngle", "RearAngle",
mzling 2:be605799793f 30 "IMUAngle", "KneeFullRetract", "KneeFullExtend", "LockTime", "Rate", "TorsoAng", "LKneeAng", "RKneeAng", "LHipAng", "RHipAng", "LHipTorque",
mzling 2:be605799793f 31 "RHipTorque", "ExoAndKneeStates", "TorsoRefAngle",
mzling 2:be605799793f 32 "LHipRefAngle", "RHipRefAngle", "Charge"};
mzling 2:be605799793f 33 //Populate the map of indices to param names
mzling 2:be605799793f 34 for (int j = 0; j < (numVars + numReadOnlyParams); j += 1) {
mzling 2:be605799793f 35 indexMap[j] = temp2[j];
mzling 2:be605799793f 36 }
mzling 2:be605799793f 37
mzling 2:be605799793f 38 //Fill the parameter map with data from SD card
mzling 2:be605799793f 39 readDataFromSD();
mzling 2:be605799793f 40
mzling 2:be605799793f 41 readParamsFromSD();
mzling 2:be605799793f 42
mzling 2:be605799793f 43
mzling 2:be605799793f 44 }
mzling 2:be605799793f 45
mzling 2:be605799793f 46 //Reads read-only data from SD card, and fills in the parameter map
mzling 2:be605799793f 47 void BluetoothComm::readDataFromSD() {
mzling 2:be605799793f 48 /
mzling 2:be605799793f 49 int *arr = readData.read(arr, numVars);
mzling 2:be605799793f 50 for (int i = 0; i < numReadOnlyParams; i += 1) {
mzling 2:be605799793f 51 paramMap[indexMap[i + numVars]] = arr[i];
mzling 2:be605799793f 52 }
mzling 2:be605799793f 53 }
mzling 2:be605799793f 54
mzling 2:be605799793f 55 //Reads editable parameters from SD card, and fills in the parameter map
mzling 2:be605799793f 56 void BluetoothComm::readParamsFromSD() {
mzling 2:be605799793f 57 int *arr = param.read(arr, numVars);
mzling 2:be605799793f 58 for (int i = 0; i < numVars; i += 1) {
mzling 2:be605799793f 59 paramMap[indexMap[i]] = arr[i];
mzling 2:be605799793f 60 }
mzling 2:be605799793f 61 }
mzling 2:be605799793f 62
mzling 2:be605799793f 63
mzling 2:be605799793f 64 //Calculates parity--0 if c even, 1 if odd
mzling 2:be605799793f 65 bool BluetoothComm::parity(short c)
mzling 2:be605799793f 66 {
mzling 2:be605799793f 67 bool p = false;
mzling 2:be605799793f 68 while (c != 0) {
mzling 2:be605799793f 69 p = !p;
mzling 2:be605799793f 70 c = (short) (c & (c-1));
mzling 2:be605799793f 71 }
mzling 2:be605799793f 72 return p;
mzling 2:be605799793f 73 }
mzling 2:be605799793f 74
mzling 2:be605799793f 75 //Calculates checksum of char array, sum of all chars in array
mzling 2:be605799793f 76 char* BluetoothComm::checkSum(char* b, int len)
mzling 2:be605799793f 77 {
mzling 2:be605799793f 78 char* checksum = (char*)malloc(3*sizeof(char));
mzling 2:be605799793f 79 short sum = 0;
mzling 2:be605799793f 80 for (int i = 0; i < len; i += 1) {
mzling 2:be605799793f 81 sum += ((short) b[i] & 0xff);
mzling 2:be605799793f 82 }
mzling 2:be605799793f 83 //sum = (short) (-1*sum);
mzling 2:be605799793f 84 checksum[1] = (char) ((sum >> 8) & 0xff);
mzling 2:be605799793f 85 checksum[2] = (char) (sum & 0xff);
mzling 2:be605799793f 86 char check = (char) (sum & 0xff);
mzling 2:be605799793f 87 //Sets escape character if the checksum contains START or END
mzling 2:be605799793f 88 if (check == START || check == END) {
mzling 2:be605799793f 89 checksum[0] = 1;
mzling 2:be605799793f 90 checksum[2] = (char) (checksum[2] & 0x1);
mzling 2:be605799793f 91 } else {
mzling 2:be605799793f 92 checksum[0] = 0;
mzling 2:be605799793f 93 }
mzling 2:be605799793f 94 return checksum;
mzling 2:be605799793f 95 }
mzling 2:be605799793f 96
mzling 2:be605799793f 97 //Sends error message
mzling 2:be605799793f 98 void BluetoothComm::sendError(char errCode)
mzling 2:be605799793f 99 {
mzling 2:be605799793f 100 rn42.putc(START);
mzling 2:be605799793f 101 rn42.putc(errCode);
mzling 2:be605799793f 102 rn42.putc(END);
mzling 2:be605799793f 103 rn42.putc(0);
mzling 2:be605799793f 104 rn42.putc(0);
mzling 2:be605799793f 105 rn42.putc(0);
mzling 2:be605799793f 106 }
mzling 2:be605799793f 107
mzling 2:be605799793f 108 //Sets charge level
mzling 2:be605799793f 109 void BluetoothComm::setCharge(short level) {
mzling 2:be605799793f 110 // readOnlyParams[11] = level;
mzling 2:be605799793f 111 paramMap["Charge"] = level;
mzling 2:be605799793f 112 }
mzling 2:be605799793f 113
mzling 2:be605799793f 114 //Sends the speified char array
mzling 2:be605799793f 115 void BluetoothComm::send(char *cmd)
mzling 2:be605799793f 116 {
mzling 2:be605799793f 117 for (int i = 0; i < 50; i++) {
mzling 2:be605799793f 118 rn42.putc(cmd[i]);
mzling 2:be605799793f 119 }
mzling 2:be605799793f 120 }
mzling 2:be605799793f 121
mzling 2:be605799793f 122 //Sets the parameter map, based on the input map NEWVALUES
mzling 2:be605799793f 123 void BluetoothComm::setValues(std::map<string, short> newValues) {
mzling 2:be605799793f 124
mzling 2:be605799793f 125 for (std::map<string, short>::iterator it = newValues.begin(); it != newValues.end(); ++it) {
mzling 2:be605799793f 126 paramMap[it->first] = it->second;
mzling 2:be605799793f 127 }
mzling 2:be605799793f 128 }
mzling 2:be605799793f 129
mzling 2:be605799793f 130 //Writes the editable params. stored in paramMap to the SD card
mzling 2:be605799793f 131 void BluetoothComm::writeParamsToSDCard() {
mzling 2:be605799793f 132
mzling 2:be605799793f 133 int paramValues[numVars];
mzling 2:be605799793f 134 for (int i = 0; i < numVars; i += 1) {
mzling 2:be605799793f 135 paramValues[i] = (int) paramMap[indexMap[i]];
mzling 2:be605799793f 136 }
mzling 2:be605799793f 137 param.write(paramValues, numVars);
mzling 2:be605799793f 138
mzling 2:be605799793f 139 }
mzling 2:be605799793f 140
mzling 2:be605799793f 141 //Write the read-only values stored in paramMap to the SD card
mzling 2:be605799793f 142 void BluetoothComm::writeDataToSDCard() {
mzling 2:be605799793f 143 int dataValues[numReadOnlyParams];
mzling 2:be605799793f 144 for (int i = 0; i < numReadOnlyParams; i += 1) {
mzling 2:be605799793f 145 dataValues[i] = (int) paramMap[indexMap[i + numVars]];
mzling 2:be605799793f 146 }
mzling 2:be605799793f 147 param.write(dataValues, numReadOnlyParams);
mzling 2:be605799793f 148 }
mzling 2:be605799793f 149
mzling 2:be605799793f 150 //Sends the paramList with START/END, parity bits, and a checksum
mzling 2:be605799793f 151 void BluetoothComm::sendValues(char* paramList)
mzling 2:be605799793f 152 {
mzling 2:be605799793f 153 char msg[2*numVars+6];
mzling 2:be605799793f 154 int len=2;
mzling 2:be605799793f 155 //printf("Sending values \r\n");
mzling 2:be605799793f 156 msg[0] = START;
mzling 2:be605799793f 157 msg[1] = 0;
mzling 2:be605799793f 158 for (int i=0; i < numVars; i++) {
mzling 2:be605799793f 159 if (i == 21) {
mzling 2:be605799793f 160 //printf("On final loop \r\n");
mzling 2:be605799793f 161 }
mzling 2:be605799793f 162 if (paramList[i] != 0xff) {
mzling 2:be605799793f 163 short s = (short)((i << 8) | paramList[i]);
mzling 2:be605799793f 164 //printf("In sendValues, calculating parity of %x\r\n", s);
mzling 2:be605799793f 165 if (parity(s)) {
mzling 2:be605799793f 166 //printf("%x requires TRUE parity bit\r\n", s);
mzling 2:be605799793f 167 msg[len] = (char)(i | 0x40);
mzling 2:be605799793f 168 len += 1;
mzling 2:be605799793f 169 } else {
mzling 2:be605799793f 170 //printf("%x requires FALSE parity bit\r\n", s);
mzling 2:be605799793f 171 msg[len] = (char)i;
mzling 2:be605799793f 172 len += 1;
mzling 2:be605799793f 173 }
mzling 2:be605799793f 174 msg[len] = paramList[i];
mzling 2:be605799793f 175 len += 1;
mzling 2:be605799793f 176 }
mzling 2:be605799793f 177 }
mzling 2:be605799793f 178 msg[len] = END;
mzling 2:be605799793f 179 len += 1;
mzling 2:be605799793f 180 char* checksum = checkSum(msg, len);
mzling 2:be605799793f 181 msg[len] = checksum[0];
mzling 2:be605799793f 182 msg[len+1] = checksum[1];
mzling 2:be605799793f 183 msg[len+2] = checksum[2];
mzling 2:be605799793f 184 len += 3;
mzling 2:be605799793f 185 for (int j = 0; j < len; j++) {
mzling 2:be605799793f 186 //printf("Sending char %x \r\n", msg[j]);
mzling 2:be605799793f 187 rn42.putc(msg[j]);
mzling 2:be605799793f 188 }
mzling 2:be605799793f 189 memcpy(lastCmd, msg, 50);
mzling 2:be605799793f 190 free(checksum);
mzling 2:be605799793f 191 return ;
mzling 2:be605799793f 192 }
mzling 2:be605799793f 193
mzling 2:be605799793f 194 //Sends readONly Parameters, with START/END and checksum
mzling 2:be605799793f 195 void BluetoothComm::sendReadOnlyValues()
mzling 2:be605799793f 196 {
mzling 2:be605799793f 197 int msgLen = 2*numReadOnlyParams+escapesNeeded+7;
mzling 2:be605799793f 198 char message[msgLen];
mzling 2:be605799793f 199 message[0] = START;
mzling 2:be605799793f 200 message[1] = 0;
mzling 2:be605799793f 201 message[2] = READONLY_IND;
mzling 2:be605799793f 202 int msgInd = 3;
mzling 2:be605799793f 203 int escapes = 0;
mzling 2:be605799793f 204 //printf("%d readonly parameters", numReadOnlyParams);
mzling 2:be605799793f 205 for (int i = 0; i < numReadOnlyParams; i++) {
mzling 2:be605799793f 206 if (i == escapeNeeded[escapes]) {
mzling 2:be605799793f 207 //printf("Escape char. needed at index %d \r\n", i);
mzling 2:be605799793f 208 //char conflict = (char)(readOnlyParams[i] & 0xff);
mzling 2:be605799793f 209 char conflict = (char)(paramMap[indexMap[i+numVars]] & 0xff);
mzling 2:be605799793f 210 //printf("%x possibly has a conflict in %x \r\n", readOnlyParams[i], conflict);
mzling 2:be605799793f 211 escapes += 1;
mzling 2:be605799793f 212 //message[msgInd+1] = (char) (readOnlyParams[i] >> 8);
mzling 2:be605799793f 213 message[msgInd+1] = (char) (paramMap[indexMap[i+numVars]] >> 8);
mzling 2:be605799793f 214 //printf("Set msgInd+1 to %x \r\n", message[msgInd+1]);
mzling 2:be605799793f 215 if (conflict == (char) 0xfe) {
mzling 2:be605799793f 216 message[msgInd] = 1;
mzling 2:be605799793f 217 message[msgInd+2] = 0xfc;
mzling 2:be605799793f 218 } else if (conflict == (char) 0xff) {
mzling 2:be605799793f 219 message[msgInd] = 1;
mzling 2:be605799793f 220 message[msgInd+2] = 0xfd;
mzling 2:be605799793f 221 } else {
mzling 2:be605799793f 222 message[msgInd] = 0;
mzling 2:be605799793f 223 message[msgInd+2] = conflict;
mzling 2:be605799793f 224 }
mzling 2:be605799793f 225 msgInd += 3;
mzling 2:be605799793f 226 } else {
mzling 2:be605799793f 227 // message[msgInd] = (char) (readOnlyParams[i] >> 8);
mzling 2:be605799793f 228 // message[msgInd+1] = (char) (readOnlyParams[i] & 0xff);
mzling 2:be605799793f 229 message[msgInd] = (char) (paramMap[indexMap[i+numVars]] >> 8);
mzling 2:be605799793f 230 message[msgInd+1] = (char) (paramMap[indexMap[i+numVars]] & 0xff);
mzling 2:be605799793f 231 msgInd += 2;
mzling 2:be605799793f 232 }
mzling 2:be605799793f 233 }
mzling 2:be605799793f 234 message[msgLen-4] = END;
mzling 2:be605799793f 235 char* checksum = checkSum(message, msgLen-3);
mzling 2:be605799793f 236 message[msgLen-3] = checksum[0];
mzling 2:be605799793f 237 message[msgLen-2] = checksum[1];
mzling 2:be605799793f 238 message[msgLen-1] = checksum[2];
mzling 2:be605799793f 239 //printf("Sending the following readONly values: \r\n");
mzling 2:be605799793f 240 for (int j=0; j < msgLen; j++) {
mzling 2:be605799793f 241 //printf("%x \r\n", message[j]);
mzling 2:be605799793f 242 rn42.putc(message[j]);
mzling 2:be605799793f 243 }
mzling 2:be605799793f 244 memcpy(lastCmd, message, 50);
mzling 2:be605799793f 245 free(checksum);
mzling 2:be605799793f 246 //printf("Finished sending readOnly values \r\n");
mzling 2:be605799793f 247
mzling 2:be605799793f 248 }
mzling 2:be605799793f 249
mzling 2:be605799793f 250 //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
mzling 2:be605799793f 251 bool BluetoothComm::msgCheck(char* message, int len)
mzling 2:be605799793f 252 {
mzling 2:be605799793f 253 if (message[0] != START) {
mzling 2:be605799793f 254 //printf("Improper START or END \r\n");
mzling 2:be605799793f 255 sendError(START_ERR);
mzling 2:be605799793f 256 return false;
mzling 2:be605799793f 257 }
mzling 2:be605799793f 258 if (message[len-4] != END) {
mzling 2:be605799793f 259 sendError(END_ERR);
mzling 2:be605799793f 260 return false;
mzling 2:be605799793f 261 }
mzling 2:be605799793f 262 bool write = message[2] & 0x80;
mzling 2:be605799793f 263 for (int i=2; i < len-5; i+=2) {
mzling 2:be605799793f 264 if (i == 2 && message[i] == READONLY_IND) {
mzling 2:be605799793f 265 break;
mzling 2:be605799793f 266 }
mzling 2:be605799793f 267 if (i == (len-5) && message[i] == READONLY_IND) {
mzling 2:be605799793f 268 break;
mzling 2:be605799793f 269 }
mzling 2:be605799793f 270 if (((message[i] & 0x80) !=0) && !write) {
mzling 2:be605799793f 271 //printf("Does not match READ format \r\n");
mzling 2:be605799793f 272 sendError((char)(((message[i] & 0x3f) << 3) | RW_ERR));
mzling 2:be605799793f 273 return false;
mzling 2:be605799793f 274 }
mzling 2:be605799793f 275 if (((message[i] & 0x80) != 0x80) && write) {
mzling 2:be605799793f 276 //printf("char %x Does not match WRITE format \r\n", message[i]);
mzling 2:be605799793f 277 sendError((char)(((message[i] & 0x3f) << 3) | RW_ERR));
mzling 2:be605799793f 278 return false;
mzling 2:be605799793f 279 }
mzling 2:be605799793f 280 short s;
mzling 2:be605799793f 281 if (write) {
mzling 2:be605799793f 282 s = (short) ((message[i] << 8) | message[i+1]);
mzling 2:be605799793f 283 } else {
mzling 2:be605799793f 284 s = (short) (message[i] << 8);
mzling 2:be605799793f 285 }
mzling 2:be605799793f 286 bool parity1 = (s & 0x4000) != 0;
mzling 2:be605799793f 287
mzling 2:be605799793f 288 if (parity1 != parity(s & 0x4000)) {
mzling 2:be605799793f 289 //printf("Parity error in VAR char \r\n");
mzling 2:be605799793f 290 sendError((char) (((message[i] & 0xbf) << 3) | PARITY_ERR));
mzling 2:be605799793f 291 return false;
mzling 2:be605799793f 292 }
mzling 2:be605799793f 293
mzling 2:be605799793f 294 char c = message[i] & 0x3f;
mzling 2:be605799793f 295 char c2 = message[i+1];
mzling 2:be605799793f 296 if ((int) c < 0 || (int) c > numVars) {
mzling 2:be605799793f 297 //printf("VAR char out of range \r\n");
mzling 2:be605799793f 298 sendError((char) (((message[i] & 0xbf) << 3) | VAR_ERR));
mzling 2:be605799793f 299 return false;
mzling 2:be605799793f 300 }
mzling 2:be605799793f 301 if ((int) c2 < 0 || (int) c2 > 100) {
mzling 2:be605799793f 302 //printf("DATA char out of range");
mzling 2:be605799793f 303 sendError((char) (((message[i] & 0xbf) << 3) | DATA_ERR));
mzling 2:be605799793f 304 return false;
mzling 2:be605799793f 305 }
mzling 2:be605799793f 306 }
mzling 2:be605799793f 307 char* checksum = checkSum(message, len-3);
mzling 2:be605799793f 308 if (checksum[0] != message[len-3]) {
mzling 2:be605799793f 309 //printf("Checksum error in char 0, expected %x but got %x \r\n", checksum[0], message[len-3]);
mzling 2:be605799793f 310 free(checksum);
mzling 2:be605799793f 311 sendError(CHECKSUM_ERR);
mzling 2:be605799793f 312 return false;
mzling 2:be605799793f 313 }
mzling 2:be605799793f 314 if (checksum[1] != message[len-2]) {
mzling 2:be605799793f 315 //printf("Checksum error in char 1, expected %x but got %x \r\n", checksum[1], message[len-2]);
mzling 2:be605799793f 316 free(checksum);
mzling 2:be605799793f 317 sendError(CHECKSUM_ERR);
mzling 2:be605799793f 318 return false;
mzling 2:be605799793f 319 }
mzling 2:be605799793f 320 if (checksum[2] != message[len-1]) {
mzling 2:be605799793f 321 //printf("Checksum error in char 2, expected %x but got %x \r\n", checksum[2], message[len-1]);
mzling 2:be605799793f 322 free(checksum);
mzling 2:be605799793f 323 sendError(CHECKSUM_ERR);
mzling 2:be605799793f 324 return false;
mzling 2:be605799793f 325 }
mzling 2:be605799793f 326 free(checksum);
mzling 2:be605799793f 327
mzling 2:be605799793f 328 return true;
mzling 2:be605799793f 329 }
mzling 2:be605799793f 330
mzling 2:be605799793f 331 //Checks a received readOnly message
mzling 2:be605799793f 332 void BluetoothComm::processReadOnly(char* message, int len)
mzling 2:be605799793f 333 {
mzling 2:be605799793f 334 //printf("Message is a ReadOnly \r\n");
mzling 2:be605799793f 335 if (!msgCheck(message, len)) {
mzling 2:be605799793f 336 //printf("MSGCHECK failed on read! \r\n");
mzling 2:be605799793f 337 return;
mzling 2:be605799793f 338 }
mzling 2:be605799793f 339 failures = 0;
mzling 2:be605799793f 340
mzling 2:be605799793f 341 //printf("Sending readOnly values \r\n");
mzling 2:be605799793f 342 sendReadOnlyValues();
mzling 2:be605799793f 343 }
mzling 2:be605799793f 344
mzling 2:be605799793f 345 //Checks received READ message, and places requested data into an array
mzling 2:be605799793f 346 void BluetoothComm::processRead(char* message, int len)
mzling 2:be605799793f 347 {
mzling 2:be605799793f 348 //If the received message is an error message, resend the last command
mzling 2:be605799793f 349 if (message[2] == START) {
mzling 2:be605799793f 350 failures += 1;
mzling 2:be605799793f 351 if (failures < 5) {
mzling 2:be605799793f 352 send(lastCmd);
mzling 2:be605799793f 353 } else {
mzling 2:be605799793f 354 failures = 0;
mzling 2:be605799793f 355 }
mzling 2:be605799793f 356 return;
mzling 2:be605799793f 357 }
mzling 2:be605799793f 358 if (!msgCheck(message, len)) {
mzling 2:be605799793f 359 //printf("MSGCHECK failed on read! \r\n");
mzling 2:be605799793f 360 return;
mzling 2:be605799793f 361 }
mzling 2:be605799793f 362 failures = 0;
mzling 2:be605799793f 363 //printf("Message is a read \r\n");
mzling 2:be605799793f 364 char paramList[numVars];
mzling 2:be605799793f 365 memset(paramList, 0xff, numVars);
mzling 2:be605799793f 366
mzling 2:be605799793f 367 for (int i=2; i < len-5; i++) {
mzling 2:be605799793f 368 char msg = message[i] & 0xbf;
mzling 2:be605799793f 369 if ((msg & 0x80) != 0) {
mzling 2:be605799793f 370 //printf("Got a non-read char %x...exiting \r\n", msg);
mzling 2:be605799793f 371 return;
mzling 2:be605799793f 372 }
mzling 2:be605799793f 373
mzling 2:be605799793f 374 int index = msg & 0xff;
mzling 2:be605799793f 375 //printf("Value at index %d requested \r\n", index);
mzling 2:be605799793f 376 paramList[index] = paramMap[indexMap[index]];
mzling 2:be605799793f 377
mzling 2:be605799793f 378 }
mzling 2:be605799793f 379 if (message[len-5] == READONLY_IND) {
mzling 2:be605799793f 380 sendReadOnlyValues();
mzling 2:be605799793f 381 }
mzling 2:be605799793f 382 sendValues(paramList);
mzling 2:be605799793f 383 }
mzling 2:be605799793f 384
mzling 2:be605799793f 385 //Checks received WRITE message and writes to localValues
mzling 2:be605799793f 386 void BluetoothComm::processWrite(char* message, int len)
mzling 2:be605799793f 387 {
mzling 2:be605799793f 388 if (message[2] == START) {
mzling 2:be605799793f 389 failures += 1;
mzling 2:be605799793f 390 if (failures < 5) {
mzling 2:be605799793f 391 send(lastCmd);
mzling 2:be605799793f 392 } else {
mzling 2:be605799793f 393 failures = 0;
mzling 2:be605799793f 394 }
mzling 2:be605799793f 395 return;
mzling 2:be605799793f 396 }
mzling 2:be605799793f 397 if (!msgCheck(message, len)) {
mzling 2:be605799793f 398 //printf("MSGCHECK failed on write! \r\n");
mzling 2:be605799793f 399 return;
mzling 2:be605799793f 400 }
mzling 2:be605799793f 401 char paramList[numVars];
mzling 2:be605799793f 402 memset(paramList, 0xff, numVars);
mzling 2:be605799793f 403 //printf("Message is a write \r\n");
mzling 2:be605799793f 404 for (int i=2; i < len-5; i+=2) {
mzling 2:be605799793f 405 char msg = message[i] & 0xbf;
mzling 2:be605799793f 406 if ((msg & 0x80) != 0x80) {
mzling 2:be605799793f 407 return;
mzling 2:be605799793f 408 }
mzling 2:be605799793f 409 int index = msg & 0x7f;
mzling 2:be605799793f 410 paramMap[indexMap[index]] = message[i+1];
mzling 2:be605799793f 411 paramList[index] = message[i+1];
mzling 2:be605799793f 412 //printf("Wrote %x to index %d of localValues \r\n", localValues[index], index);
mzling 2:be605799793f 413
mzling 2:be605799793f 414 }
mzling 2:be605799793f 415 sendValues(paramList);
mzling 2:be605799793f 416 writeParamsToSDCard();
mzling 2:be605799793f 417 //SD_Card.write(localValues)
mzling 2:be605799793f 418 //sendToControlBed(localValues+1, len-2)
mzling 2:be605799793f 419 }
mzling 2:be605799793f 420
mzling 2:be605799793f 421 //Checks if received message is a read, write or readonly
mzling 2:be605799793f 422 void BluetoothComm::process (char* message, int len)
mzling 2:be605799793f 423 {
mzling 2:be605799793f 424 char c = message[2];
mzling 2:be605799793f 425 for (int i =0; i < len; i++) {
mzling 2:be605799793f 426 printf("Message character: %x \r\n", message[i]);
mzling 2:be605799793f 427 }
mzling 2:be605799793f 428 if (c == READONLY_IND) {
mzling 2:be605799793f 429 processReadOnly(message, len);
mzling 2:be605799793f 430 return;
mzling 2:be605799793f 431 }
mzling 2:be605799793f 432 if ((c & 0x80) == 0) {
mzling 2:be605799793f 433 processRead(message, len);
mzling 2:be605799793f 434 return;
mzling 2:be605799793f 435 } else {
mzling 2:be605799793f 436 processWrite(message, len);
mzling 2:be605799793f 437 return;
mzling 2:be605799793f 438 }
mzling 2:be605799793f 439 }
mzling 2:be605799793f 440
mzling 2:be605799793f 441 //Warning: do not put print statements in the function attachment(); it will interfere with receiving messages
mzling 2:be605799793f 442 void BluetoothComm::attachment()
mzling 2:be605799793f 443 {
mzling 2:be605799793f 444 if (rn42.readable()) {
mzling 2:be605799793f 445 // //printf("rn42 is readable \r\n");
mzling 2:be605799793f 446 data=rn42.getc();
mzling 2:be605799793f 447
mzling 2:be605799793f 448 // if (data != NULL) {
mzling 2:be605799793f 449 char b = data & 0xff;
mzling 2:be605799793f 450 //printf("Got char: %x \r\n", b);
mzling 2:be605799793f 451 if (b != NULL or inMsg) {
mzling 2:be605799793f 452 // printf("Got char non null: %x \r\n", b);
mzling 2:be605799793f 453 }
mzling 2:be605799793f 454 //This marks the START of a message
mzling 2:be605799793f 455 if (inMsg == false and b == START) {
mzling 2:be605799793f 456 // printf("Msg START received \r\n");
mzling 2:be605799793f 457 inMsg = true;
mzling 2:be605799793f 458 counter = 3;
mzling 2:be605799793f 459 curMsg[len] = b;
mzling 2:be605799793f 460 len += 1;
mzling 2:be605799793f 461 } else if (inMsg == true and b == START) {
mzling 2:be605799793f 462 // printf("Second start received, terminating\r\n");
mzling 2:be605799793f 463 inMsg = false;
mzling 2:be605799793f 464 counter = 0;
mzling 2:be605799793f 465 memset(curMsg, 0, 50);
mzling 2:be605799793f 466 rn42.rxBufferFlush();
mzling 2:be605799793f 467 process(msg, len);
mzling 2:be605799793f 468 len = 0;
mzling 2:be605799793f 469 } else if (inMsg || counter > 0 ) {
mzling 2:be605799793f 470 // printf("inMsg or counter > 0 \r\n");
mzling 2:be605799793f 471 curMsg[len] = b;
mzling 2:be605799793f 472 len += 1;
mzling 2:be605799793f 473 if (!inMsg) {
mzling 2:be605799793f 474 counter -= 1;
mzling 2:be605799793f 475 }
mzling 2:be605799793f 476 //Marks end of message, and starts processing
mzling 2:be605799793f 477 if (counter <= 0) {
mzling 2:be605799793f 478 // printf("End of message \r\n");
mzling 2:be605799793f 479 memset(msg, 0, 50);
mzling 2:be605799793f 480 memcpy(msg, curMsg, 50);
mzling 2:be605799793f 481 memset(curMsg, 0, 50);
mzling 2:be605799793f 482 rn42.rxBufferFlush();
mzling 2:be605799793f 483 process(msg, len);
mzling 2:be605799793f 484 len = 0;
mzling 2:be605799793f 485 }
mzling 2:be605799793f 486 }
mzling 2:be605799793f 487 if (b == END) {
mzling 2:be605799793f 488 inMsg = false;
mzling 2:be605799793f 489
mzling 2:be605799793f 490 // rn42.putc(msg);
mzling 2:be605799793f 491 }
mzling 2:be605799793f 492 // }
mzling 2:be605799793f 493 //rn42.putc(data);
mzling 2:be605799793f 494
mzling 2:be605799793f 495 }
mzling 2:be605799793f 496 }
mzling 2:be605799793f 497
mzling 2:be605799793f 498
mzling 2:be605799793f 499