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@11:56bd3affbbfc, 2015-03-05 (annotated)

Committer:
mzling
Date:
Thu Mar 05 23:48:54 2015 +0000
Revision:
11:56bd3affbbfc
Parent:
10:235c0b81c4c7
Parent:
9:8024861fccac
Child:
12:50f6f9ed5cc6
Added dataOut arguments.

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 5:92659a4c2f89 4 #include "initDatabed.h"
mzling 2:be605799793f 5 #include <string>
mzling 2:be605799793f 6 #include <map>
mzling 3:14050370593a 7 //Variable indices for set_values: set_values should take an array of length NUMVARS + NUMREADONLYPARAMS = 34.
mzling 2:be605799793f 8 //Map of variables to array indices is as follows:
mzling 2:be605799793f 9 //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 10 //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 11 //12 = KP Stance, 13 = KP Swing, 14 = KP Standing, 15 = KP Sitting, 16 = KP Standing up, 17 = KP Sitting down
mzling 2:be605799793f 12 //18 = KD Stance, 19 = KD Swing, 20 = KD Standing, 21 = KD Sitting, 22 = KD Standing up, 23 = KD sitting down
mzling 2:be605799793f 13 //24 = Standing angle, 25 = Sitting angle, 26 = Bent Forward Angle, 27 = Forward Angle, 28 = Rear Angle, 29 = IMU Angle
mzling 2:be605799793f 14 //30 = Knee Full Retract, 31 = Knee Full Extend, 32 = Lock Time, 33 = Rate
mzling 9:8024861fccac 15 BluetoothComm::BluetoothComm(PinName tx, PinName rx): _rn42(tx, rx), _len(0), _counter(0), _inMsg(false), _numVars(27), _numReadOnlyParams(12), _escapesNeeded(8)
mzling 2:be605799793f 16 {
mzling 5:92659a4c2f89 17
mzling 6:721f5e8a794d 18 /* Make sure baud rate is correct--rn42 cannot be read if code and MBED have different baud rates! */
mzling 6:721f5e8a794d 19 //_rn42.baud(9600);
mzling 6:721f5e8a794d 20 //pc.printf("Started BTComm init \r\n");
mzling 6:721f5e8a794d 21 _rn42.baud(115200);
mzling 2:be605799793f 22 int temp1[] = {0,1,2,3,4,8,9,10, -1};
mzling 2:be605799793f 23
mzling 3:14050370593a 24 for (int i = 0; i < _escapesNeeded+1; i++) {
mzling 3:14050370593a 25 _escapeNeeded[i] = temp1[i];
mzling 2:be605799793f 26 }
mzling 2:be605799793f 27
mzling 2:be605799793f 28
mzling 2:be605799793f 29 std::string temp2[] = {"KPStance", "KPSwing", "KPStanding", "KPSitting", "KPStandUp", "KPSitdown", "KDStance", "KDSwing",
mzling 2:be605799793f 30 "KDStanding", "KDSitting", "KDStandUp", "KDSitDown", "StandingAngle", "SittingAngle", "BentForwardAngle", "ForwardAngle", "RearAngle",
mzling 9:8024861fccac 31 "IMUAngle", "KneeFullRetract", "KneeFullExtend", "LockTime", "Rate", "StandupAsst", "StandupTime", "SitdownAsst", "SitdownTime", "WalkAngle",
mzling 9:8024861fccac 32 "TorsoAng", "LKneeAng", "RKneeAng", "LHipAng", "RHipAng", "LHipTorque", "RHipTorque", "ExoAndKneeStates", "TorsoRefAngle", "LHipRefAngle",
mzling 9:8024861fccac 33 "RHipRefAngle", "Charge"};
mzling 2:be605799793f 34 //Populate the map of indices to param names
mzling 3:14050370593a 35 for (int j = 0; j < (_numVars + _numReadOnlyParams); j += 1) {
mzling 3:14050370593a 36 _indexMap[j] = temp2[j];
mzling 2:be605799793f 37 }
mzling 2:be605799793f 38
mzling 3:14050370593a 39
mzling 3:14050370593a 40 write_params_to_sd_card();
mzling 6:721f5e8a794d 41 // pc.printf("Initialized PARAM \r\n");
mzling 4:7e3bbf896e78 42 int temp4[] = {0x01fe, 0x02ac, 0x02ff, 0x0180, 0x0012, 0x0010, 0x0020, 0x00bf, 0x023f, 0x0123, 0x03a2, 0x10};
mzling 4:7e3bbf896e78 43 readData.write(_numReadOnlyParams, temp4);
mzling 4:7e3bbf896e78 44 for (int k = 0; k < _numReadOnlyParams; k += 1) {
mzling 4:7e3bbf896e78 45 _paramMap[_indexMap[_numVars + k]] = temp4[k];
mzling 4:7e3bbf896e78 46 }
mzling 3:14050370593a 47 //printf("Test: %x\r\n", _paramMap["TorsoAng"]);
mzling 3:14050370593a 48 write_data_to_sd_card();
mzling 3:14050370593a 49 //printf("Initialized data\r\n");
mzling 3:14050370593a 50 //Fill the parameter map with data from SD card
mzling 3:14050370593a 51 read_data_from_sd();
mzling 6:721f5e8a794d 52 //pc.printf("Test: %x\r\n", _paramMap["TorsoAng"]);
mzling 3:14050370593a 53 read_params_from_sd();
mzling 2:be605799793f 54
mzling 2:be605799793f 55
mzling 2:be605799793f 56 }
mzling 2:be605799793f 57
mzling 3:14050370593a 58 /**
mzling 3:14050370593a 59 * Reads read-only data from SD card, and fills in the parameter map.
mzling 3:14050370593a 60 * @author Michael Ling
mzling 3:14050370593a 61 * @date 2/4/2015
mzling 3:14050370593a 62 */
mzling 3:14050370593a 63 void BluetoothComm::read_data_from_sd()
mzling 3:14050370593a 64 {
mzling 3:14050370593a 65
mzling 3:14050370593a 66 int *arr = readData.read(_numReadOnlyParams, arr);
mzling 3:14050370593a 67 for (int i = 0; i < _numReadOnlyParams; i += 1) {
mzling 3:14050370593a 68 _paramMap[_indexMap[i + _numVars]] = arr[i];
mzling 5:92659a4c2f89 69 // printf("Read: %x\r\n", arr[i]);
mzling 2:be605799793f 70 }
mzling 5:92659a4c2f89 71 // printf("Finished reading data\r\n");
mzling 2:be605799793f 72 }
mzling 2:be605799793f 73
mzling 3:14050370593a 74 /**
mzling 3:14050370593a 75 * Reads editable parameters from SD card, and fills in the parameter map
mzling 3:14050370593a 76 * @author Michael Ling
mzling 3:14050370593a 77 * @date 2/4/2015
mzling 3:14050370593a 78 */
mzling 3:14050370593a 79 void BluetoothComm::read_params_from_sd()
mzling 3:14050370593a 80 {
mzling 3:14050370593a 81 int *arr = param.read(_numVars, arr);
mzling 3:14050370593a 82 for (int i = 0; i < _numVars; i += 1) {
mzling 3:14050370593a 83 _paramMap[_indexMap[i]] = arr[i];
mzling 2:be605799793f 84 }
mzling 6:721f5e8a794d 85 //printf("Finished reading params\r\n");
mzling 2:be605799793f 86 }
mzling 2:be605799793f 87
mzling 3:14050370593a 88 /**
mzling 3:14050370593a 89 * Calculates parity--0 if c even, 1 if odd
mzling 3:14050370593a 90 * @param c The short that we want to calculate parity of
mzling 3:14050370593a 91 * @author Michael Ling
mzling 3:14050370593a 92 * @date 2/4/2015
mzling 3:14050370593a 93 */
mzling 2:be605799793f 94 bool BluetoothComm::parity(short c)
mzling 2:be605799793f 95 {
mzling 2:be605799793f 96 bool p = false;
mzling 2:be605799793f 97 while (c != 0) {
mzling 2:be605799793f 98 p = !p;
mzling 2:be605799793f 99 c = (short) (c & (c-1));
mzling 2:be605799793f 100 }
mzling 2:be605799793f 101 return p;
mzling 2:be605799793f 102 }
mzling 2:be605799793f 103
mzling 3:14050370593a 104 /**
mzling 3:14050370593a 105 * Calculates checksum of char array, sum of all chars in array
mzling 3:14050370593a 106 * @param b The char array to be summed up
mzling 3:14050370593a 107 * @param len Length of the array b
mzling 3:14050370593a 108 * @author Michael Ling
mzling 3:14050370593a 109 * @date 2/4/2015
mzling 3:14050370593a 110 */
mzling 3:14050370593a 111 char* BluetoothComm::get_checksum(char* b, int len)
mzling 2:be605799793f 112 {
mzling 2:be605799793f 113 char* checksum = (char*)malloc(3*sizeof(char));
mzling 2:be605799793f 114 short sum = 0;
mzling 2:be605799793f 115 for (int i = 0; i < len; i += 1) {
mzling 2:be605799793f 116 sum += ((short) b[i] & 0xff);
mzling 2:be605799793f 117 }
mzling 2:be605799793f 118 checksum[1] = (char) ((sum >> 8) & 0xff);
mzling 2:be605799793f 119 checksum[2] = (char) (sum & 0xff);
mzling 2:be605799793f 120 char check = (char) (sum & 0xff);
mzling 2:be605799793f 121 //Sets escape character if the checksum contains START or END
mzling 2:be605799793f 122 if (check == START || check == END) {
mzling 2:be605799793f 123 checksum[0] = 1;
mzling 2:be605799793f 124 checksum[2] = (char) (checksum[2] & 0x1);
mzling 2:be605799793f 125 } else {
mzling 2:be605799793f 126 checksum[0] = 0;
mzling 2:be605799793f 127 }
mzling 2:be605799793f 128 return checksum;
mzling 2:be605799793f 129 }
mzling 2:be605799793f 130
mzling 3:14050370593a 131 /**
mzling 3:14050370593a 132 * Sends error message.
mzling 3:14050370593a 133 * @param errCode character representation of the failure-causing error
mzling 3:14050370593a 134 * @author Michael Ling
mzling 3:14050370593a 135 * @data 2/4/2015
mzling 3:14050370593a 136 */
mzling 3:14050370593a 137 void BluetoothComm::send_error(char errCode)
mzling 2:be605799793f 138 {
mzling 3:14050370593a 139 _rn42.putc(START);
mzling 3:14050370593a 140 _rn42.putc(errCode);
mzling 3:14050370593a 141 _rn42.putc(END);
mzling 3:14050370593a 142 _rn42.putc(0);
mzling 3:14050370593a 143 _rn42.putc(0);
mzling 3:14050370593a 144 _rn42.putc(0);
mzling 2:be605799793f 145 }
mzling 3:14050370593a 146 /**
mzling 3:14050370593a 147 //Sets charge level. DEPRECATED
mzling 3:14050370593a 148 void BluetoothComm::setCharge(short level) {
mzling 3:14050370593a 149 // _readOnlyParams[11] = level;
mzling 3:14050370593a 150 _paramMap["Charge"] = level;
mzling 3:14050370593a 151 }
mzling 3:14050370593a 152 */
mzling 2:be605799793f 153
mzling 3:14050370593a 154 /**
mzling 3:14050370593a 155 * Sends the specified char array through the _rn42 Bluetooth connection.
mzling 3:14050370593a 156 * @param cmd The char array to be sent
mzling 3:14050370593a 157 * @author Michael Ling
mzling 3:14050370593a 158 * @date 2/4/2015
mzling 3:14050370593a 159 */
mzling 2:be605799793f 160 void BluetoothComm::send(char *cmd)
mzling 2:be605799793f 161 {
mzling 2:be605799793f 162 for (int i = 0; i < 50; i++) {
mzling 3:14050370593a 163 _rn42.putc(cmd[i]);
mzling 2:be605799793f 164 }
mzling 2:be605799793f 165 }
mzling 2:be605799793f 166
mzling 3:14050370593a 167 /**
mzling 3:14050370593a 168 * Sets the parameter map, based on the input map NEWVALUES
mzling 3:14050370593a 169 * @param newValues A map of strings to shorts to be copied to _paramMap
mzling 3:14050370593a 170 * @author Michael Ling
mzling 3:14050370593a 171 * @date 2/4/2015
mzling 3:14050370593a 172 */
mzling 3:14050370593a 173 void BluetoothComm::set_values(std::map<string, short> newValues)
mzling 3:14050370593a 174 {
mzling 2:be605799793f 175
mzling 2:be605799793f 176 for (std::map<string, short>::iterator it = newValues.begin(); it != newValues.end(); ++it) {
mzling 3:14050370593a 177 _paramMap[it->first] = it->second;
mzling 2:be605799793f 178 }
mzling 2:be605799793f 179 }
mzling 2:be605799793f 180
mzling 3:14050370593a 181 /**
mzling 3:14050370593a 182 * Writes the editable params. stored in _paramMap to the SD card
mzling 3:14050370593a 183 * @author Michael Ling
mzling 3:14050370593a 184 * @date 2/4/2015
mzling 3:14050370593a 185 */
mzling 3:14050370593a 186 void BluetoothComm::write_params_to_sd_card()
mzling 3:14050370593a 187 {
mzling 2:be605799793f 188
mzling 3:14050370593a 189 int paramValues[_numVars];
mzling 3:14050370593a 190 for (int i = 0; i < _numVars; i += 1) {
mzling 3:14050370593a 191 paramValues[i] = (int) _paramMap[_indexMap[i]];
mzling 2:be605799793f 192 }
mzling 3:14050370593a 193 param.write(_numVars, paramValues);
mzling 2:be605799793f 194
mzling 2:be605799793f 195 }
mzling 2:be605799793f 196
mzling 3:14050370593a 197 /**
mzling 3:14050370593a 198 * Write the read-only values stored in _paramMap to the SD card
mzling 3:14050370593a 199 * @author Michael Ling
mzling 3:14050370593a 200 * @date 2/4/2015
mzling 3:14050370593a 201 */
mzling 3:14050370593a 202 void BluetoothComm::write_data_to_sd_card()
mzling 3:14050370593a 203 {
mzling 3:14050370593a 204 int dataValues[_numReadOnlyParams];
mzling 3:14050370593a 205 for (int i = 0; i < _numReadOnlyParams; i += 1) {
mzling 3:14050370593a 206 dataValues[i] = (int) _paramMap[_indexMap[i + _numVars]];
mzling 3:14050370593a 207 // printf("Index %d of dataValues set to %x\r\n", i, dataValues[i]);
mzling 2:be605799793f 208 }
mzling 3:14050370593a 209 readData.write(_numReadOnlyParams, dataValues);
mzling 2:be605799793f 210 }
mzling 2:be605799793f 211
mzling 3:14050370593a 212 /**
mzling 3:14050370593a 213 * Sends the paramList with START/END, parity bits, and a checksum
mzling 3:14050370593a 214 * @param paramList List of parameters to be sent over Bluetooth, represented as a char array
mzling 3:14050370593a 215 * @author Michael Ling
mzling 3:14050370593a 216 * @date 2/4/2015
mzling 3:14050370593a 217 */
mzling 10:235c0b81c4c7 218 void BluetoothComm::send_values(char* paramList, short *dataOut)
mzling 2:be605799793f 219 {
mzling 3:14050370593a 220 char msg[2*_numVars+6];
mzling 2:be605799793f 221 int len=2;
mzling 2:be605799793f 222 //printf("Sending values \r\n");
mzling 2:be605799793f 223 msg[0] = START;
mzling 2:be605799793f 224 msg[1] = 0;
mzling 3:14050370593a 225 for (int i=0; i < _numVars; i++) {
mzling 2:be605799793f 226 if (i == 21) {
mzling 2:be605799793f 227 //printf("On final loop \r\n");
mzling 2:be605799793f 228 }
mzling 2:be605799793f 229 if (paramList[i] != 0xff) {
mzling 2:be605799793f 230 short s = (short)((i << 8) | paramList[i]);
mzling 3:14050370593a 231 //printf("In send_values, calculating parity of %x\r\n", s);
mzling 2:be605799793f 232 if (parity(s)) {
mzling 2:be605799793f 233 //printf("%x requires TRUE parity bit\r\n", s);
mzling 2:be605799793f 234 msg[len] = (char)(i | 0x40);
mzling 2:be605799793f 235 len += 1;
mzling 2:be605799793f 236 } else {
mzling 2:be605799793f 237 //printf("%x requires FALSE parity bit\r\n", s);
mzling 2:be605799793f 238 msg[len] = (char)i;
mzling 2:be605799793f 239 len += 1;
mzling 2:be605799793f 240 }
mzling 2:be605799793f 241 msg[len] = paramList[i];
mzling 2:be605799793f 242 len += 1;
mzling 2:be605799793f 243 }
mzling 2:be605799793f 244 }
mzling 2:be605799793f 245 msg[len] = END;
mzling 2:be605799793f 246 len += 1;
mzling 3:14050370593a 247 char* checksum = get_checksum(msg, len);
mzling 2:be605799793f 248 msg[len] = checksum[0];
mzling 2:be605799793f 249 msg[len+1] = checksum[1];
mzling 2:be605799793f 250 msg[len+2] = checksum[2];
mzling 2:be605799793f 251 len += 3;
mzling 2:be605799793f 252 for (int j = 0; j < len; j++) {
mzling 2:be605799793f 253 //printf("Sending char %x \r\n", msg[j]);
mzling 3:14050370593a 254 _rn42.putc(msg[j]);
mzling 10:235c0b81c4c7 255
mzling 10:235c0b81c4c7 256 }
mzling 10:235c0b81c4c7 257 if (dataOut != NULL) {
mzling 10:235c0b81c4c7 258 int j;
mzling 10:235c0b81c4c7 259 for (j = 0; j < len-1; j+=2) {
mzling 10:235c0b81c4c7 260 dataOut[j] = (msg[j]<<8)|msg[j+1];
mzling 10:235c0b81c4c7 261 }
mzling 10:235c0b81c4c7 262 if (j < len) {
mzling 10:235c0b81c4c7 263 dataOut[j] = msg[j] << 8;
mzling 10:235c0b81c4c7 264 }
mzling 10:235c0b81c4c7 265
mzling 2:be605799793f 266 }
mzling 3:14050370593a 267 memcpy(_lastCmd, msg, 50);
mzling 2:be605799793f 268 free(checksum);
mzling 2:be605799793f 269 return ;
mzling 2:be605799793f 270 }
mzling 2:be605799793f 271
mzling 3:14050370593a 272 /**
mzling 3:14050370593a 273 * Sends readOnly Parameters, with START/END and checksum
mzling 3:14050370593a 274 * @author Michael Ling
mzling 3:14050370593a 275 * @date 2/4/2015
mzling 3:14050370593a 276 */
mzling 3:14050370593a 277 void BluetoothComm::send_read_only_values()
mzling 2:be605799793f 278 {
mzling 6:721f5e8a794d 279 //printf("SENDING RO VALUES!\r\n");
mzling 3:14050370593a 280 int msgLen = 2*_numReadOnlyParams+_escapesNeeded+7;
mzling 6:721f5e8a794d 281 // printf("msglen is %d\r\n", msgLen);
mzling 2:be605799793f 282 char message[msgLen];
mzling 6:721f5e8a794d 283 // printf("message of len 39 created\r\n");
mzling 2:be605799793f 284 message[0] = START;
mzling 6:721f5e8a794d 285 //printf("%x\r\n", message[0]);
mzling 2:be605799793f 286 message[1] = 0;
mzling 6:721f5e8a794d 287 //printf("%x\r\n", message[1]);
mzling 2:be605799793f 288 message[2] = READONLY_IND;
mzling 6:721f5e8a794d 289 //printf("3 bytes of message set\r\n");
mzling 2:be605799793f 290 int msgInd = 3;
mzling 2:be605799793f 291 int escapes = 0;
mzling 3:14050370593a 292 //printf("%d readonly parameters", _numReadOnlyParams);
mzling 3:14050370593a 293 for (int i = 0; i < _numReadOnlyParams; i++) {
mzling 3:14050370593a 294 if (i == _escapeNeeded[escapes]) {
mzling 6:721f5e8a794d 295 printf("Escape char. needed at index %d \r\n", i);
mzling 3:14050370593a 296 //char conflict = (char)(_readOnlyParams[i] & 0xff);
mzling 3:14050370593a 297 char conflict = (char)(_paramMap[_indexMap[i+_numVars]] & 0xff);
mzling 3:14050370593a 298 //printf("%x possibly has a conflict in %x \r\n", _readOnlyParams[i], conflict);
mzling 2:be605799793f 299 escapes += 1;
mzling 3:14050370593a 300 //message[msgInd+1] = (char) (_readOnlyParams[i] >> 8);
mzling 3:14050370593a 301 message[msgInd+1] = (char) (_paramMap[_indexMap[i+_numVars]] >> 8);
mzling 6:721f5e8a794d 302 printf("Set msgInd+1 to %x \r\n", message[msgInd+1]);
mzling 2:be605799793f 303 if (conflict == (char) 0xfe) {
mzling 2:be605799793f 304 message[msgInd] = 1;
mzling 2:be605799793f 305 message[msgInd+2] = 0xfc;
mzling 2:be605799793f 306 } else if (conflict == (char) 0xff) {
mzling 2:be605799793f 307 message[msgInd] = 1;
mzling 2:be605799793f 308 message[msgInd+2] = 0xfd;
mzling 2:be605799793f 309 } else {
mzling 2:be605799793f 310 message[msgInd] = 0;
mzling 2:be605799793f 311 message[msgInd+2] = conflict;
mzling 2:be605799793f 312 }
mzling 2:be605799793f 313 msgInd += 3;
mzling 2:be605799793f 314 } else {
mzling 3:14050370593a 315 // message[msgInd] = (char) (_readOnlyParams[i] >> 8);
mzling 3:14050370593a 316 // message[msgInd+1] = (char) (_readOnlyParams[i] & 0xff);
mzling 3:14050370593a 317 message[msgInd] = (char) (_paramMap[_indexMap[i+_numVars]] >> 8);
mzling 3:14050370593a 318 message[msgInd+1] = (char) (_paramMap[_indexMap[i+_numVars]] & 0xff);
mzling 2:be605799793f 319 msgInd += 2;
mzling 2:be605799793f 320 }
mzling 2:be605799793f 321 }
mzling 2:be605799793f 322 message[msgLen-4] = END;
mzling 3:14050370593a 323 char* checksum = get_checksum(message, msgLen-3);
mzling 2:be605799793f 324 message[msgLen-3] = checksum[0];
mzling 2:be605799793f 325 message[msgLen-2] = checksum[1];
mzling 2:be605799793f 326 message[msgLen-1] = checksum[2];
mzling 2:be605799793f 327 //printf("Sending the following readONly values: \r\n");
mzling 2:be605799793f 328 for (int j=0; j < msgLen; j++) {
mzling 3:14050370593a 329 printf("%x \r\n", message[j]);
mzling 3:14050370593a 330 _rn42.putc(message[j]);
mzling 2:be605799793f 331 }
mzling 3:14050370593a 332 memcpy(_lastCmd, message, 50);
mzling 2:be605799793f 333 free(checksum);
mzling 2:be605799793f 334 //printf("Finished sending readOnly values \r\n");
mzling 2:be605799793f 335
mzling 2:be605799793f 336 }
mzling 2:be605799793f 337
mzling 3:14050370593a 338 /**
mzling 3:14050370593a 339 * 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 3:14050370593a 340 * @param message The received message to check
mzling 3:14050370593a 341 * @param len Length of the message
mzling 3:14050370593a 342 * @author Michael Ling
mzling 3:14050370593a 343 * @date 2/4/2015
mzling 3:14050370593a 344 */
mzling 3:14050370593a 345 bool BluetoothComm::msg_check(char* message, int len)
mzling 2:be605799793f 346 {
mzling 2:be605799793f 347 if (message[0] != START) {
mzling 2:be605799793f 348 //printf("Improper START or END \r\n");
mzling 3:14050370593a 349 send_error(START_ERR);
mzling 2:be605799793f 350 return false;
mzling 2:be605799793f 351 }
mzling 2:be605799793f 352 if (message[len-4] != END) {
mzling 3:14050370593a 353 send_error(END_ERR);
mzling 2:be605799793f 354 return false;
mzling 2:be605799793f 355 }
mzling 2:be605799793f 356 bool write = message[2] & 0x80;
mzling 2:be605799793f 357 for (int i=2; i < len-5; i+=2) {
mzling 2:be605799793f 358 if (i == 2 && message[i] == READONLY_IND) {
mzling 2:be605799793f 359 break;
mzling 2:be605799793f 360 }
mzling 2:be605799793f 361 if (i == (len-5) && message[i] == READONLY_IND) {
mzling 2:be605799793f 362 break;
mzling 2:be605799793f 363 }
mzling 2:be605799793f 364 if (((message[i] & 0x80) !=0) && !write) {
mzling 2:be605799793f 365 //printf("Does not match READ format \r\n");
mzling 3:14050370593a 366 send_error((char)(((message[i] & 0x3f) << 3) | RW_ERR));
mzling 2:be605799793f 367 return false;
mzling 2:be605799793f 368 }
mzling 2:be605799793f 369 if (((message[i] & 0x80) != 0x80) && write) {
mzling 2:be605799793f 370 //printf("char %x Does not match WRITE format \r\n", message[i]);
mzling 3:14050370593a 371 send_error((char)(((message[i] & 0x3f) << 3) | RW_ERR));
mzling 2:be605799793f 372 return false;
mzling 2:be605799793f 373 }
mzling 2:be605799793f 374 short s;
mzling 2:be605799793f 375 if (write) {
mzling 2:be605799793f 376 s = (short) ((message[i] << 8) | message[i+1]);
mzling 2:be605799793f 377 } else {
mzling 2:be605799793f 378 s = (short) (message[i] << 8);
mzling 2:be605799793f 379 }
mzling 2:be605799793f 380 bool parity1 = (s & 0x4000) != 0;
mzling 2:be605799793f 381
mzling 2:be605799793f 382 if (parity1 != parity(s & 0x4000)) {
mzling 2:be605799793f 383 //printf("Parity error in VAR char \r\n");
mzling 3:14050370593a 384 send_error((char) (((message[i] & 0xbf) << 3) | PARITY_ERR));
mzling 2:be605799793f 385 return false;
mzling 2:be605799793f 386 }
mzling 2:be605799793f 387
mzling 2:be605799793f 388 char c = message[i] & 0x3f;
mzling 2:be605799793f 389 char c2 = message[i+1];
mzling 3:14050370593a 390 if ((int) c < 0 || (int) c > _numVars) {
mzling 2:be605799793f 391 //printf("VAR char out of range \r\n");
mzling 3:14050370593a 392 send_error((char) (((message[i] & 0xbf) << 3) | VAR_ERR));
mzling 2:be605799793f 393 return false;
mzling 2:be605799793f 394 }
mzling 2:be605799793f 395 if ((int) c2 < 0 || (int) c2 > 100) {
mzling 2:be605799793f 396 //printf("DATA char out of range");
mzling 3:14050370593a 397 send_error((char) (((message[i] & 0xbf) << 3) | DATA_ERR));
mzling 2:be605799793f 398 return false;
mzling 2:be605799793f 399 }
mzling 2:be605799793f 400 }
mzling 3:14050370593a 401 char* checksum = get_checksum(message, len-3);
mzling 2:be605799793f 402 if (checksum[0] != message[len-3]) {
mzling 3:14050370593a 403 //printf("checksum error in char 0, expected %x but got %x \r\n", checksum[0], message[len-3]);
mzling 2:be605799793f 404 free(checksum);
mzling 3:14050370593a 405 send_error(CHECKSUM_ERR);
mzling 2:be605799793f 406 return false;
mzling 2:be605799793f 407 }
mzling 2:be605799793f 408 if (checksum[1] != message[len-2]) {
mzling 2:be605799793f 409 //printf("Checksum error in char 1, expected %x but got %x \r\n", checksum[1], message[len-2]);
mzling 2:be605799793f 410 free(checksum);
mzling 3:14050370593a 411 send_error(CHECKSUM_ERR);
mzling 2:be605799793f 412 return false;
mzling 2:be605799793f 413 }
mzling 2:be605799793f 414 if (checksum[2] != message[len-1]) {
mzling 2:be605799793f 415 //printf("Checksum error in char 2, expected %x but got %x \r\n", checksum[2], message[len-1]);
mzling 2:be605799793f 416 free(checksum);
mzling 3:14050370593a 417 send_error(CHECKSUM_ERR);
mzling 2:be605799793f 418 return false;
mzling 2:be605799793f 419 }
mzling 2:be605799793f 420 free(checksum);
mzling 2:be605799793f 421
mzling 2:be605799793f 422 return true;
mzling 2:be605799793f 423 }
mzling 2:be605799793f 424
mzling 3:14050370593a 425 /**
mzling 3:14050370593a 426 * Checks a received readOnly message
mzling 3:14050370593a 427 * @param message The readonly message received
mzling 3:14050370593a 428 * @param len Length of the message
mzling 3:14050370593a 429 * @author Michael Ling
mzling 3:14050370593a 430 * @date 2/4/2015
mzling 3:14050370593a 431 */
mzling 3:14050370593a 432 void BluetoothComm::process_read_only(char* message, int len)
mzling 2:be605799793f 433 {
mzling 2:be605799793f 434 //printf("Message is a ReadOnly \r\n");
mzling 3:14050370593a 435 if (!msg_check(message, len)) {
mzling 6:721f5e8a794d 436 printf("msg_check failed on readonly! \r\n");
mzling 2:be605799793f 437 return;
mzling 2:be605799793f 438 }
mzling 6:721f5e8a794d 439 //PASS TO CTRLBED
mzling 3:14050370593a 440 _failures = 0;
mzling 2:be605799793f 441
mzling 2:be605799793f 442 //printf("Sending readOnly values \r\n");
mzling 3:14050370593a 443 send_read_only_values();
mzling 2:be605799793f 444 }
mzling 2:be605799793f 445
mzling 3:14050370593a 446 /**
mzling 3:14050370593a 447 * Checks received READ message, and places requested data into an array
mzling 3:14050370593a 448 * @param message The received READ request as a char array
mzling 3:14050370593a 449 * @param len Length of the READ message
mzling 3:14050370593a 450 * @author Michael Ling
mzling 3:14050370593a 451 * @date 2/4/2015
mzling 3:14050370593a 452 */
mzling 3:14050370593a 453 void BluetoothComm::process_read(char* message, int len)
mzling 2:be605799793f 454 {
mzling 2:be605799793f 455 //If the received message is an error message, resend the last command
mzling 3:14050370593a 456 if (message[2] == END) {
mzling 3:14050370593a 457 _failures += 1;
mzling 3:14050370593a 458 //printf("_failures: %d\r\n", _failures);
mzling 3:14050370593a 459 if (_failures < 5) {
mzling 3:14050370593a 460 send(_lastCmd);
mzling 2:be605799793f 461 } else {
mzling 3:14050370593a 462 _failures = 0;
mzling 2:be605799793f 463 }
mzling 2:be605799793f 464 return;
mzling 2:be605799793f 465 }
mzling 3:14050370593a 466 if (!msg_check(message, len)) {
mzling 3:14050370593a 467 //printf("msg_check failed on read! \r\n");
mzling 2:be605799793f 468 return;
mzling 2:be605799793f 469 }
mzling 6:721f5e8a794d 470 //PASS MSG TO CTRLBED
mzling 3:14050370593a 471 _failures = 0;
mzling 2:be605799793f 472 //printf("Message is a read \r\n");
mzling 3:14050370593a 473 char paramList[_numVars];
mzling 3:14050370593a 474 memset(paramList, 0xff, _numVars);
mzling 2:be605799793f 475
mzling 2:be605799793f 476 for (int i=2; i < len-5; i++) {
mzling 2:be605799793f 477 char msg = message[i] & 0xbf;
mzling 2:be605799793f 478 if ((msg & 0x80) != 0) {
mzling 6:721f5e8a794d 479 // printf("Got a non-read char %x...exiting \r\n", msg);
mzling 2:be605799793f 480 return;
mzling 2:be605799793f 481 }
mzling 2:be605799793f 482
mzling 2:be605799793f 483 int index = msg & 0xff;
mzling 2:be605799793f 484 //printf("Value at index %d requested \r\n", index);
mzling 3:14050370593a 485 paramList[index] = _paramMap[_indexMap[index]];
mzling 2:be605799793f 486
mzling 2:be605799793f 487 }
mzling 2:be605799793f 488 if (message[len-5] == READONLY_IND) {
mzling 6:721f5e8a794d 489 printf("Need to send RO vals\r\n");
mzling 3:14050370593a 490 send_read_only_values();
mzling 2:be605799793f 491 }
mzling 6:721f5e8a794d 492 printf("About to send PARAMLIST\r\n");
mzling 10:235c0b81c4c7 493 send_values(paramList, NULL);
mzling 2:be605799793f 494 }
mzling 2:be605799793f 495
mzling 3:14050370593a 496 /**
mzling 3:14050370593a 497 * Checks received WRITE message and writes to paramMap/SDCard
mzling 3:14050370593a 498 * @param message The received WRITE message
mzling 3:14050370593a 499 * @param len Length of the WRITE message
mzling 3:14050370593a 500 * @author Michael Ling
mzling 3:14050370593a 501 * @date 2/4/2015
mzling 3:14050370593a 502 */
mzling 10:235c0b81c4c7 503 void BluetoothComm::process_write(char* message, int len, short* dataOut)
mzling 2:be605799793f 504 {
mzling 3:14050370593a 505 if (message[2] == END) {
mzling 3:14050370593a 506 _failures += 1;
mzling 3:14050370593a 507 if (_failures < 5) {
mzling 3:14050370593a 508 send(_lastCmd);
mzling 2:be605799793f 509 } else {
mzling 3:14050370593a 510 _failures = 0;
mzling 2:be605799793f 511 }
mzling 2:be605799793f 512 return;
mzling 2:be605799793f 513 }
mzling 10:235c0b81c4c7 514 //dataOut = message;
mzling 3:14050370593a 515 if (!msg_check(message, len)) {
mzling 6:721f5e8a794d 516 printf("msg_check failed on write! \r\n");
mzling 2:be605799793f 517 return;
mzling 2:be605799793f 518 }
mzling 6:721f5e8a794d 519 //PASS msg. to ControlBed
mzling 3:14050370593a 520 char paramList[_numVars];
mzling 3:14050370593a 521 memset(paramList, 0xff, _numVars);
mzling 2:be605799793f 522 //printf("Message is a write \r\n");
mzling 2:be605799793f 523 for (int i=2; i < len-5; i+=2) {
mzling 2:be605799793f 524 char msg = message[i] & 0xbf;
mzling 2:be605799793f 525 if ((msg & 0x80) != 0x80) {
mzling 2:be605799793f 526 return;
mzling 2:be605799793f 527 }
mzling 2:be605799793f 528 int index = msg & 0x7f;
mzling 3:14050370593a 529 _paramMap[_indexMap[index]] = message[i+1];
mzling 2:be605799793f 530 paramList[index] = message[i+1];
mzling 2:be605799793f 531 //printf("Wrote %x to index %d of localValues \r\n", localValues[index], index);
mzling 2:be605799793f 532
mzling 2:be605799793f 533 }
mzling 10:235c0b81c4c7 534 send_values(paramList, dataOut);
mzling 3:14050370593a 535 write_params_to_sd_card();
mzling 2:be605799793f 536 //SD_Card.write(localValues)
mzling 2:be605799793f 537 //sendToControlBed(localValues+1, len-2)
mzling 2:be605799793f 538 }
mzling 2:be605799793f 539
mzling 3:14050370593a 540 /**
mzling 3:14050370593a 541 * Checks if received message is a read, write or readonly
mzling 3:14050370593a 542 * @param message The received message
mzling 3:14050370593a 543 * @param len Length of the message
mzling 3:14050370593a 544 * @author Michael Ling
mzling 3:14050370593a 545 * @date 2/4/2015
mzling 3:14050370593a 546 */
mzling 10:235c0b81c4c7 547 void BluetoothComm::process (char* message, int len, short* dataOut)
mzling 2:be605799793f 548 {
mzling 2:be605799793f 549 char c = message[2];
mzling 4:7e3bbf896e78 550 for (int i =0; i < len; i++) {
mzling 6:721f5e8a794d 551 //printf("Message character: %x \r\n", message[i]);
mzling 4:7e3bbf896e78 552 }
mzling 2:be605799793f 553 if (c == READONLY_IND) {
mzling 3:14050370593a 554 process_read_only(message, len);
mzling 2:be605799793f 555 return;
mzling 2:be605799793f 556 }
mzling 2:be605799793f 557 if ((c & 0x80) == 0) {
mzling 6:721f5e8a794d 558 //printf("Is a read\r\n");
mzling 3:14050370593a 559 process_read(message, len);
mzling 2:be605799793f 560 return;
mzling 2:be605799793f 561 } else {
mzling 10:235c0b81c4c7 562 process_write(message, len, dataOut);
mzling 2:be605799793f 563 return;
mzling 2:be605799793f 564 }
mzling 2:be605799793f 565 }
mzling 2:be605799793f 566
mzling 2:be605799793f 567 //Warning: do not put print statements in the function attachment(); it will interfere with receiving messages
mzling 3:14050370593a 568 /**
mzling 3:14050370593a 569 * Scans for data received through Bluetooth, and passes it on if it detects a message-like chunk. Should be run via an interuupt.
mzling 3:14050370593a 570 * @author Michael Ling
mzling 3:14050370593a 571 * @date 2/4/2015
mzling 3:14050370593a 572 */
mzling 10:235c0b81c4c7 573 void BluetoothComm::attachment(short* dataOut)
mzling 2:be605799793f 574 {
mzling 7:d4a32c830e11 575 boardLed1 = !boardLed1;
mzling 7:d4a32c830e11 576 // pc.printf("Entered attachment\r\n");
mzling 3:14050370593a 577 if (_rn42.readable()) {
mzling 3:14050370593a 578 _data=_rn42.getc();
mzling 2:be605799793f 579
mzling 3:14050370593a 580 // if (_data != NULL) {
mzling 3:14050370593a 581 char b = _data & 0xff;
mzling 3:14050370593a 582 //printf("Got char: %x \r\n", b);
mzling 3:14050370593a 583 if (b != NULL or _inMsg) {
mzling 3:14050370593a 584 // printf("Got char non null: %x \r\n", b);
mzling 3:14050370593a 585 }
mzling 3:14050370593a 586 //This marks the START of a message
mzling 3:14050370593a 587 if (_inMsg == false and b == START) {
mzling 3:14050370593a 588 // printf("Msg START received \r\n");
mzling 3:14050370593a 589 _inMsg = true;
mzling 3:14050370593a 590 _counter = 3;
mzling 3:14050370593a 591 _curMsg[_len] = b;
mzling 3:14050370593a 592 _len += 1;
mzling 3:14050370593a 593 } else if (_inMsg == true and b == START) {
mzling 3:14050370593a 594 // printf("Second start received, terminating\r\n");
mzling 3:14050370593a 595 _inMsg = false;
mzling 3:14050370593a 596 _counter = 0;
mzling 3:14050370593a 597 memset(_curMsg, 0, 50);
mzling 3:14050370593a 598 _rn42.rxBufferFlush();
mzling 10:235c0b81c4c7 599 process(_msg, _len, dataOut);
mzling 3:14050370593a 600 _len = 0;
mzling 3:14050370593a 601 } else if (_inMsg || _counter > 0 ) {
mzling 3:14050370593a 602 // printf("_inMsg or _counter > 0 \r\n");
mzling 3:14050370593a 603 _curMsg[_len] = b;
mzling 3:14050370593a 604 _len += 1;
mzling 3:14050370593a 605 if (!_inMsg) {
mzling 3:14050370593a 606 _counter -= 1;
mzling 2:be605799793f 607 }
mzling 3:14050370593a 608 //Marks end of message, and starts processing
mzling 3:14050370593a 609 if (_counter <= 0) {
mzling 3:14050370593a 610 // printf("End of message \r\n");
mzling 3:14050370593a 611 memset(_msg, 0, 50);
mzling 3:14050370593a 612 memcpy(_msg, _curMsg, 50);
mzling 3:14050370593a 613 memset(_curMsg, 0, 50);
mzling 3:14050370593a 614 _rn42.rxBufferFlush();
mzling 10:235c0b81c4c7 615 process(_msg, _len, dataOut);
mzling 3:14050370593a 616 _len = 0;
mzling 2:be605799793f 617 }
mzling 3:14050370593a 618 }
mzling 3:14050370593a 619 if (b == END) {
mzling 3:14050370593a 620 _inMsg = false;
mzling 2:be605799793f 621
mzling 3:14050370593a 622 // _rn42.putc(msg);
mzling 3:14050370593a 623 }
mzling 3:14050370593a 624 //_rn42.putc(_data);
mzling 2:be605799793f 625
mzling 3:14050370593a 626 }
mzling 2:be605799793f 627 }