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@19:f3eece6c024f, 2015-04-27 (annotated)

Committer:
mzling
Date:
Mon Apr 27 21:58:27 2015 +0000
Revision:
19:f3eece6c024f
Parent:
18:34ccf02fdbe7
Child:
20:be8dcc2344bc
trivial prints

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 16:b6b2c7b0f1c9 7
mzling 18:34ccf02fdbe7 8 BluetoothComm::BluetoothComm(PinName tx, PinName rx): _rn42(tx, rx), _len(0), _counter(0), _inMsg(false), _numVars(32), _numReadOnlyParams(12), _escapesNeeded(8)
mzling 2:be605799793f 9 {
mzling 5:92659a4c2f89 10
mzling 6:721f5e8a794d 11 /* Make sure baud rate is correct--rn42 cannot be read if code and MBED have different baud rates! */
mzling 6:721f5e8a794d 12 //_rn42.baud(9600);
mzling 16:b6b2c7b0f1c9 13
mzling 6:721f5e8a794d 14 _rn42.baud(115200);
mzling 2:be605799793f 15 int temp1[] = {0,1,2,3,4,8,9,10, -1};
mzling 2:be605799793f 16
mzling 3:14050370593a 17 for (int i = 0; i < _escapesNeeded+1; i++) {
mzling 3:14050370593a 18 _escapeNeeded[i] = temp1[i];
mzling 2:be605799793f 19 }
mzling 2:be605799793f 20
mzling 2:be605799793f 21
mzling 2:be605799793f 22 std::string temp2[] = {"KPStance", "KPSwing", "KPStanding", "KPSitting", "KPStandUp", "KPSitdown", "KDStance", "KDSwing",
mzling 2:be605799793f 23 "KDStanding", "KDSitting", "KDStandUp", "KDSitDown", "StandingAngle", "SittingAngle", "BentForwardAngle", "ForwardAngle", "RearAngle",
mzling 18:34ccf02fdbe7 24 "IMUAngle", "KneeFullRetract", "KneeFullExtend", "LockTime", "Rate", "StandupAsst", "StandupTime", "SitdownAsst", "SitdownTime", "WalkAngle",
mzling 19:f3eece6c024f 25 "StepLength", "StepTime", "MaxAmplitude", "StanceStart", "StanceEnd",
mzling 9:8024861fccac 26 "TorsoAng", "LKneeAng", "RKneeAng", "LHipAng", "RHipAng", "LHipTorque", "RHipTorque", "ExoAndKneeStates", "TorsoRefAngle", "LHipRefAngle",
mzling 9:8024861fccac 27 "RHipRefAngle", "Charge"};
mzling 2:be605799793f 28 //Populate the map of indices to param names
mzling 3:14050370593a 29 for (int j = 0; j < (_numVars + _numReadOnlyParams); j += 1) {
mzling 3:14050370593a 30 _indexMap[j] = temp2[j];
mzling 2:be605799793f 31 }
mzling 2:be605799793f 32
mzling 3:14050370593a 33
mzling 18:34ccf02fdbe7 34 write_params_to_sd_card();
mzling 16:b6b2c7b0f1c9 35
mzling 4:7e3bbf896e78 36 int temp4[] = {0x01fe, 0x02ac, 0x02ff, 0x0180, 0x0012, 0x0010, 0x0020, 0x00bf, 0x023f, 0x0123, 0x03a2, 0x10};
mzling 16:b6b2c7b0f1c9 37 //readData.write(_numReadOnlyParams, temp4);
mzling 4:7e3bbf896e78 38 for (int k = 0; k < _numReadOnlyParams; k += 1) {
mzling 4:7e3bbf896e78 39 _paramMap[_indexMap[_numVars + k]] = temp4[k];
mzling 4:7e3bbf896e78 40 }
mzling 18:34ccf02fdbe7 41 //pc.printf("Starting sdcard stuff\r\n");
mzling 18:34ccf02fdbe7 42 write_data_to_sd_card();
mzling 18:34ccf02fdbe7 43 //read_data_from_sd();
mzling 18:34ccf02fdbe7 44 // set_data(0, 0xdead);
mzling 18:34ccf02fdbe7 45 //set_data(1, 0xdead);
mzling 18:34ccf02fdbe7 46 //set_data(2, 0xdead);
mzling 18:34ccf02fdbe7 47 //set_data(3, 0xdead);
mzling 18:34ccf02fdbe7 48 // pc.printf("Data is written\r\n");
mzling 3:14050370593a 49 //Fill the parameter map with data from SD card
mzling 18:34ccf02fdbe7 50 read_data_from_sd();
mzling 18:34ccf02fdbe7 51 // pc.printf("Data is read\r\n");
mzling 18:34ccf02fdbe7 52 read_params_from_sd();
mzling 12:50f6f9ed5cc6 53 pc.printf("Finished BTComm init\r\n");
mzling 2:be605799793f 54
mzling 2:be605799793f 55 }
mzling 2:be605799793f 56
mzling 3:14050370593a 57 /**
mzling 3:14050370593a 58 * Reads read-only data from SD card, and fills in the parameter map.
mzling 3:14050370593a 59 * @author Michael Ling
mzling 3:14050370593a 60 * @date 2/4/2015
mzling 3:14050370593a 61 */
mzling 18:34ccf02fdbe7 62
mzling 18:34ccf02fdbe7 63
mzling 3:14050370593a 64 void BluetoothComm::read_data_from_sd()
mzling 3:14050370593a 65 {
mzling 18:34ccf02fdbe7 66 pc.printf("Beginning to read\r\n");
mzling 18:34ccf02fdbe7 67 int arr[_numReadOnlyParams];
mzling 18:34ccf02fdbe7 68 readData.read(_numReadOnlyParams, arr);
mzling 18:34ccf02fdbe7 69 pc.printf("Finished read\r\n");
mzling 3:14050370593a 70 for (int i = 0; i < _numReadOnlyParams; i += 1) {
mzling 18:34ccf02fdbe7 71 //pc.printf("arr[%d] is %x\r\n", i, arr[i]);
mzling 3:14050370593a 72 _paramMap[_indexMap[i + _numVars]] = arr[i];
mzling 16:b6b2c7b0f1c9 73
mzling 2:be605799793f 74 }
mzling 18:34ccf02fdbe7 75
mzling 16:b6b2c7b0f1c9 76
mzling 18:34ccf02fdbe7 77 }
mzling 2:be605799793f 78
mzling 3:14050370593a 79 /**
mzling 3:14050370593a 80 * Reads editable parameters from SD card, and fills in the parameter map
mzling 3:14050370593a 81 * @author Michael Ling
mzling 3:14050370593a 82 * @date 2/4/2015
mzling 3:14050370593a 83 */
mzling 18:34ccf02fdbe7 84
mzling 3:14050370593a 85 void BluetoothComm::read_params_from_sd()
mzling 3:14050370593a 86 {
mzling 3:14050370593a 87 int *arr = param.read(_numVars, arr);
mzling 3:14050370593a 88 for (int i = 0; i < _numVars; i += 1) {
mzling 3:14050370593a 89 _paramMap[_indexMap[i]] = arr[i];
mzling 2:be605799793f 90 }
mzling 16:b6b2c7b0f1c9 91
mzling 18:34ccf02fdbe7 92 }
mzling 2:be605799793f 93
mzling 3:14050370593a 94 /**
mzling 3:14050370593a 95 * Calculates parity--0 if c even, 1 if odd
mzling 3:14050370593a 96 * @param c The short that we want to calculate parity of
mzling 3:14050370593a 97 * @author Michael Ling
mzling 3:14050370593a 98 * @date 2/4/2015
mzling 3:14050370593a 99 */
mzling 2:be605799793f 100 bool BluetoothComm::parity(short c)
mzling 2:be605799793f 101 {
mzling 2:be605799793f 102 bool p = false;
mzling 2:be605799793f 103 while (c != 0) {
mzling 2:be605799793f 104 p = !p;
mzling 2:be605799793f 105 c = (short) (c & (c-1));
mzling 2:be605799793f 106 }
mzling 2:be605799793f 107 return p;
mzling 2:be605799793f 108 }
mzling 2:be605799793f 109
mzling 3:14050370593a 110 /**
mzling 3:14050370593a 111 * Calculates checksum of char array, sum of all chars in array
mzling 3:14050370593a 112 * @param b The char array to be summed up
mzling 3:14050370593a 113 * @param len Length of the array b
mzling 3:14050370593a 114 * @author Michael Ling
mzling 3:14050370593a 115 * @date 2/4/2015
mzling 3:14050370593a 116 */
mzling 3:14050370593a 117 char* BluetoothComm::get_checksum(char* b, int len)
mzling 2:be605799793f 118 {
mzling 2:be605799793f 119 char* checksum = (char*)malloc(3*sizeof(char));
mzling 2:be605799793f 120 short sum = 0;
mzling 2:be605799793f 121 for (int i = 0; i < len; i += 1) {
mzling 2:be605799793f 122 sum += ((short) b[i] & 0xff);
mzling 2:be605799793f 123 }
mzling 2:be605799793f 124 checksum[1] = (char) ((sum >> 8) & 0xff);
mzling 2:be605799793f 125 checksum[2] = (char) (sum & 0xff);
mzling 2:be605799793f 126 char check = (char) (sum & 0xff);
mzling 2:be605799793f 127 //Sets escape character if the checksum contains START or END
mzling 2:be605799793f 128 if (check == START || check == END) {
mzling 2:be605799793f 129 checksum[0] = 1;
mzling 2:be605799793f 130 checksum[2] = (char) (checksum[2] & 0x1);
mzling 2:be605799793f 131 } else {
mzling 2:be605799793f 132 checksum[0] = 0;
mzling 2:be605799793f 133 }
mzling 2:be605799793f 134 return checksum;
mzling 2:be605799793f 135 }
mzling 2:be605799793f 136
mzling 3:14050370593a 137 /**
mzling 3:14050370593a 138 * Sends error message.
mzling 3:14050370593a 139 * @param errCode character representation of the failure-causing error
mzling 3:14050370593a 140 * @author Michael Ling
mzling 3:14050370593a 141 * @data 2/4/2015
mzling 3:14050370593a 142 */
mzling 3:14050370593a 143 void BluetoothComm::send_error(char errCode)
mzling 2:be605799793f 144 {
mzling 3:14050370593a 145 _rn42.putc(START);
mzling 3:14050370593a 146 _rn42.putc(errCode);
mzling 3:14050370593a 147 _rn42.putc(END);
mzling 3:14050370593a 148 _rn42.putc(0);
mzling 3:14050370593a 149 _rn42.putc(0);
mzling 3:14050370593a 150 _rn42.putc(0);
mzling 2:be605799793f 151 }
mzling 16:b6b2c7b0f1c9 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 18:34ccf02fdbe7 181 void BluetoothComm::set_data(int index, short value) {
mzling 18:34ccf02fdbe7 182 //pc.printf("New value for %d\r\n", index);
mzling 18:34ccf02fdbe7 183 readData.write_to_index(index, (int)value);
mzling 18:34ccf02fdbe7 184
mzling 18:34ccf02fdbe7 185 }
mzling 18:34ccf02fdbe7 186
mzling 3:14050370593a 187 /**
mzling 3:14050370593a 188 * Writes the editable params. stored in _paramMap to the SD card
mzling 3:14050370593a 189 * @author Michael Ling
mzling 3:14050370593a 190 * @date 2/4/2015
mzling 3:14050370593a 191 */
mzling 18:34ccf02fdbe7 192
mzling 3:14050370593a 193 void BluetoothComm::write_params_to_sd_card()
mzling 3:14050370593a 194 {
mzling 2:be605799793f 195
mzling 3:14050370593a 196 int paramValues[_numVars];
mzling 3:14050370593a 197 for (int i = 0; i < _numVars; i += 1) {
mzling 3:14050370593a 198 paramValues[i] = (int) _paramMap[_indexMap[i]];
mzling 2:be605799793f 199 }
mzling 3:14050370593a 200 param.write(_numVars, paramValues);
mzling 2:be605799793f 201
mzling 18:34ccf02fdbe7 202 }
mzling 2:be605799793f 203
mzling 3:14050370593a 204 /**
mzling 3:14050370593a 205 * Write the read-only values stored in _paramMap to the SD card
mzling 3:14050370593a 206 * @author Michael Ling
mzling 3:14050370593a 207 * @date 2/4/2015
mzling 3:14050370593a 208 */
mzling 18:34ccf02fdbe7 209
mzling 3:14050370593a 210 void BluetoothComm::write_data_to_sd_card()
mzling 3:14050370593a 211 {
mzling 3:14050370593a 212 int dataValues[_numReadOnlyParams];
mzling 3:14050370593a 213 for (int i = 0; i < _numReadOnlyParams; i += 1) {
mzling 3:14050370593a 214 dataValues[i] = (int) _paramMap[_indexMap[i + _numVars]];
mzling 18:34ccf02fdbe7 215 //pc.printf("Index %d of dataValues set to %x\r\n", i, dataValues[i]);
mzling 2:be605799793f 216 }
mzling 3:14050370593a 217 readData.write(_numReadOnlyParams, dataValues);
mzling 18:34ccf02fdbe7 218 }
mzling 2:be605799793f 219
mzling 3:14050370593a 220 /**
mzling 3:14050370593a 221 * Sends the paramList with START/END, parity bits, and a checksum
mzling 3:14050370593a 222 * @param paramList List of parameters to be sent over Bluetooth, represented as a char array
mzling 16:b6b2c7b0f1c9 223 * @param message The message we want to send to Ctrlbed
mzling 16:b6b2c7b0f1c9 224 * @param dataOut Circular buffer that allow communication with Ctrlbed
mzling 3:14050370593a 225 * @author Michael Ling
mzling 3:14050370593a 226 * @date 2/4/2015
mzling 3:14050370593a 227 */
mzling 14:ac9949a0aff5 228 void BluetoothComm::send_values(char* paramList, char *message, short *dataOut)
mzling 2:be605799793f 229 {
mzling 3:14050370593a 230 char msg[2*_numVars+6];
mzling 2:be605799793f 231 int len=2;
mzling 2:be605799793f 232 //printf("Sending values \r\n");
mzling 2:be605799793f 233 msg[0] = START;
mzling 2:be605799793f 234 msg[1] = 0;
mzling 3:14050370593a 235 for (int i=0; i < _numVars; i++) {
mzling 15:a492356f5485 236
mzling 2:be605799793f 237 if (paramList[i] != 0xff) {
mzling 2:be605799793f 238 short s = (short)((i << 8) | paramList[i]);
mzling 3:14050370593a 239 //printf("In send_values, calculating parity of %x\r\n", s);
mzling 2:be605799793f 240 if (parity(s)) {
mzling 2:be605799793f 241 //printf("%x requires TRUE parity bit\r\n", s);
mzling 2:be605799793f 242 msg[len] = (char)(i | 0x40);
mzling 2:be605799793f 243 len += 1;
mzling 2:be605799793f 244 } else {
mzling 2:be605799793f 245 //printf("%x requires FALSE parity bit\r\n", s);
mzling 2:be605799793f 246 msg[len] = (char)i;
mzling 2:be605799793f 247 len += 1;
mzling 2:be605799793f 248 }
mzling 2:be605799793f 249 msg[len] = paramList[i];
mzling 2:be605799793f 250 len += 1;
mzling 2:be605799793f 251 }
mzling 2:be605799793f 252 }
mzling 2:be605799793f 253 msg[len] = END;
mzling 2:be605799793f 254 len += 1;
mzling 3:14050370593a 255 char* checksum = get_checksum(msg, len);
mzling 2:be605799793f 256 msg[len] = checksum[0];
mzling 2:be605799793f 257 msg[len+1] = checksum[1];
mzling 2:be605799793f 258 msg[len+2] = checksum[2];
mzling 2:be605799793f 259 len += 3;
mzling 16:b6b2c7b0f1c9 260 //This loop sends a reply to the phone
mzling 2:be605799793f 261 for (int j = 0; j < len; j++) {
mzling 15:a492356f5485 262 printf("Sending char %x \r\n", msg[j]);
mzling 3:14050370593a 263 _rn42.putc(msg[j]);
mzling 10:235c0b81c4c7 264
mzling 10:235c0b81c4c7 265 }
mzling 16:b6b2c7b0f1c9 266 //This loop passes the original write message to the ctrlbed
mzling 16:b6b2c7b0f1c9 267 //Each pair of characters is converted to a short, with orders reversed.
mzling 16:b6b2c7b0f1c9 268 //This is so that casting the short* back to char* restores the original message.
mzling 10:235c0b81c4c7 269 if (dataOut != NULL) {
mzling 17:80affee96096 270 int ind = 2;
mzling 10:235c0b81c4c7 271 int j;
mzling 10:235c0b81c4c7 272 for (j = 0; j < len-1; j+=2) {
mzling 14:ac9949a0aff5 273 dataOut[ind] = (message[j+1]<<8)|message[j];
mzling 16:b6b2c7b0f1c9 274
mzling 13:67492109a66d 275 ind += 1;
mzling 10:235c0b81c4c7 276 }
mzling 16:b6b2c7b0f1c9 277 //If the message has an odd number of characters
mzling 10:235c0b81c4c7 278 if (j < len) {
mzling 14:ac9949a0aff5 279 dataOut[ind] = message[j];
mzling 16:b6b2c7b0f1c9 280
mzling 10:235c0b81c4c7 281 }
mzling 15:a492356f5485 282 for (int k=0; k<len;k++) {
mzling 15:a492356f5485 283 printf("Index %d: %x\r\n", k, dataOut[k]);
mzling 15:a492356f5485 284 }
mzling 10:235c0b81c4c7 285
mzling 2:be605799793f 286 }
mzling 3:14050370593a 287 memcpy(_lastCmd, msg, 50);
mzling 2:be605799793f 288 free(checksum);
mzling 2:be605799793f 289 return ;
mzling 2:be605799793f 290 }
mzling 2:be605799793f 291
mzling 3:14050370593a 292 /**
mzling 3:14050370593a 293 * Sends readOnly Parameters, with START/END and checksum
mzling 3:14050370593a 294 * @author Michael Ling
mzling 3:14050370593a 295 * @date 2/4/2015
mzling 3:14050370593a 296 */
mzling 3:14050370593a 297 void BluetoothComm::send_read_only_values()
mzling 2:be605799793f 298 {
mzling 18:34ccf02fdbe7 299 pc.printf("Send read only\r\n");
mzling 18:34ccf02fdbe7 300 read_data_from_sd();
mzling 18:34ccf02fdbe7 301 pc.printf("Updated readonly map\r\n");
mzling 3:14050370593a 302 int msgLen = 2*_numReadOnlyParams+_escapesNeeded+7;
mzling 2:be605799793f 303 char message[msgLen];
mzling 2:be605799793f 304 message[0] = START;
mzling 2:be605799793f 305 message[1] = 0;
mzling 2:be605799793f 306 message[2] = READONLY_IND;
mzling 2:be605799793f 307 int msgInd = 3;
mzling 2:be605799793f 308 int escapes = 0;
mzling 3:14050370593a 309 for (int i = 0; i < _numReadOnlyParams; i++) {
mzling 3:14050370593a 310 if (i == _escapeNeeded[escapes]) {
mzling 3:14050370593a 311 char conflict = (char)(_paramMap[_indexMap[i+_numVars]] & 0xff);
mzling 2:be605799793f 312 escapes += 1;
mzling 18:34ccf02fdbe7 313 //message[msgInd+1] = (char) (_paramMap[_indexMap[i+_numVars]] >> 8);
mzling 18:34ccf02fdbe7 314 char conflict2 = (char)(_paramMap[_indexMap[i+_numVars]] >> 8);
mzling 18:34ccf02fdbe7 315 if (conflict2 == (char) 0xfe) {
mzling 18:34ccf02fdbe7 316 message[msgInd] = 2;
mzling 18:34ccf02fdbe7 317 message[msgInd+1] = 0xfc;
mzling 18:34ccf02fdbe7 318 } else if (conflict2 == (char) 0xff) {
mzling 18:34ccf02fdbe7 319 message[msgInd] = 2;
mzling 18:34ccf02fdbe7 320 message[msgInd+1] = 0xfd;
mzling 18:34ccf02fdbe7 321 } else {
mzling 18:34ccf02fdbe7 322 message[msgInd] = 0;
mzling 18:34ccf02fdbe7 323 message[msgInd+1] = conflict2;
mzling 18:34ccf02fdbe7 324 }
mzling 2:be605799793f 325 if (conflict == (char) 0xfe) {
mzling 18:34ccf02fdbe7 326 message[msgInd] |= 1;
mzling 2:be605799793f 327 message[msgInd+2] = 0xfc;
mzling 2:be605799793f 328 } else if (conflict == (char) 0xff) {
mzling 18:34ccf02fdbe7 329 message[msgInd] |= 1;
mzling 2:be605799793f 330 message[msgInd+2] = 0xfd;
mzling 2:be605799793f 331 } else {
mzling 18:34ccf02fdbe7 332 //message[msgInd] = 0;
mzling 2:be605799793f 333 message[msgInd+2] = conflict;
mzling 2:be605799793f 334 }
mzling 2:be605799793f 335 msgInd += 3;
mzling 2:be605799793f 336 } else {
mzling 3:14050370593a 337 message[msgInd] = (char) (_paramMap[_indexMap[i+_numVars]] >> 8);
mzling 3:14050370593a 338 message[msgInd+1] = (char) (_paramMap[_indexMap[i+_numVars]] & 0xff);
mzling 2:be605799793f 339 msgInd += 2;
mzling 2:be605799793f 340 }
mzling 2:be605799793f 341 }
mzling 2:be605799793f 342 message[msgLen-4] = END;
mzling 3:14050370593a 343 char* checksum = get_checksum(message, msgLen-3);
mzling 2:be605799793f 344 message[msgLen-3] = checksum[0];
mzling 2:be605799793f 345 message[msgLen-2] = checksum[1];
mzling 2:be605799793f 346 message[msgLen-1] = checksum[2];
mzling 2:be605799793f 347 for (int j=0; j < msgLen; j++) {
mzling 3:14050370593a 348 _rn42.putc(message[j]);
mzling 2:be605799793f 349 }
mzling 3:14050370593a 350 memcpy(_lastCmd, message, 50);
mzling 2:be605799793f 351 free(checksum);
mzling 2:be605799793f 352
mzling 2:be605799793f 353 }
mzling 2:be605799793f 354
mzling 3:14050370593a 355 /**
mzling 3:14050370593a 356 * 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 357 * @param message The received message to check
mzling 3:14050370593a 358 * @param len Length of the message
mzling 3:14050370593a 359 * @author Michael Ling
mzling 3:14050370593a 360 * @date 2/4/2015
mzling 3:14050370593a 361 */
mzling 3:14050370593a 362 bool BluetoothComm::msg_check(char* message, int len)
mzling 2:be605799793f 363 {
mzling 2:be605799793f 364 if (message[0] != START) {
mzling 3:14050370593a 365 send_error(START_ERR);
mzling 2:be605799793f 366 return false;
mzling 2:be605799793f 367 }
mzling 2:be605799793f 368 if (message[len-4] != END) {
mzling 3:14050370593a 369 send_error(END_ERR);
mzling 2:be605799793f 370 return false;
mzling 2:be605799793f 371 }
mzling 2:be605799793f 372 bool write = message[2] & 0x80;
mzling 2:be605799793f 373 for (int i=2; i < len-5; i+=2) {
mzling 2:be605799793f 374 if (i == 2 && message[i] == READONLY_IND) {
mzling 2:be605799793f 375 break;
mzling 2:be605799793f 376 }
mzling 2:be605799793f 377 if (i == (len-5) && message[i] == READONLY_IND) {
mzling 2:be605799793f 378 break;
mzling 2:be605799793f 379 }
mzling 2:be605799793f 380 if (((message[i] & 0x80) !=0) && !write) {
mzling 3:14050370593a 381 send_error((char)(((message[i] & 0x3f) << 3) | RW_ERR));
mzling 2:be605799793f 382 return false;
mzling 2:be605799793f 383 }
mzling 2:be605799793f 384 if (((message[i] & 0x80) != 0x80) && write) {
mzling 3:14050370593a 385 send_error((char)(((message[i] & 0x3f) << 3) | RW_ERR));
mzling 2:be605799793f 386 return false;
mzling 2:be605799793f 387 }
mzling 2:be605799793f 388 short s;
mzling 2:be605799793f 389 if (write) {
mzling 2:be605799793f 390 s = (short) ((message[i] << 8) | message[i+1]);
mzling 2:be605799793f 391 } else {
mzling 2:be605799793f 392 s = (short) (message[i] << 8);
mzling 2:be605799793f 393 }
mzling 2:be605799793f 394 bool parity1 = (s & 0x4000) != 0;
mzling 2:be605799793f 395
mzling 2:be605799793f 396 if (parity1 != parity(s & 0x4000)) {
mzling 3:14050370593a 397 send_error((char) (((message[i] & 0xbf) << 3) | PARITY_ERR));
mzling 2:be605799793f 398 return false;
mzling 2:be605799793f 399 }
mzling 2:be605799793f 400
mzling 2:be605799793f 401 char c = message[i] & 0x3f;
mzling 2:be605799793f 402 char c2 = message[i+1];
mzling 3:14050370593a 403 if ((int) c < 0 || (int) c > _numVars) {
mzling 3:14050370593a 404 send_error((char) (((message[i] & 0xbf) << 3) | VAR_ERR));
mzling 2:be605799793f 405 return false;
mzling 2:be605799793f 406 }
mzling 2:be605799793f 407 if ((int) c2 < 0 || (int) c2 > 100) {
mzling 3:14050370593a 408 send_error((char) (((message[i] & 0xbf) << 3) | DATA_ERR));
mzling 2:be605799793f 409 return false;
mzling 2:be605799793f 410 }
mzling 2:be605799793f 411 }
mzling 3:14050370593a 412 char* checksum = get_checksum(message, len-3);
mzling 2:be605799793f 413 if (checksum[0] != message[len-3]) {
mzling 2:be605799793f 414 free(checksum);
mzling 3:14050370593a 415 send_error(CHECKSUM_ERR);
mzling 2:be605799793f 416 return false;
mzling 2:be605799793f 417 }
mzling 2:be605799793f 418 if (checksum[1] != message[len-2]) {
mzling 2:be605799793f 419 free(checksum);
mzling 3:14050370593a 420 send_error(CHECKSUM_ERR);
mzling 2:be605799793f 421 return false;
mzling 2:be605799793f 422 }
mzling 2:be605799793f 423 if (checksum[2] != message[len-1]) {
mzling 2:be605799793f 424 free(checksum);
mzling 3:14050370593a 425 send_error(CHECKSUM_ERR);
mzling 2:be605799793f 426 return false;
mzling 2:be605799793f 427 }
mzling 2:be605799793f 428 free(checksum);
mzling 2:be605799793f 429
mzling 2:be605799793f 430 return true;
mzling 2:be605799793f 431 }
mzling 2:be605799793f 432
mzling 3:14050370593a 433 /**
mzling 3:14050370593a 434 * Checks a received readOnly message
mzling 3:14050370593a 435 * @param message The readonly message received
mzling 3:14050370593a 436 * @param len Length of the message
mzling 3:14050370593a 437 * @author Michael Ling
mzling 3:14050370593a 438 * @date 2/4/2015
mzling 3:14050370593a 439 */
mzling 3:14050370593a 440 void BluetoothComm::process_read_only(char* message, int len)
mzling 2:be605799793f 441 {
mzling 16:b6b2c7b0f1c9 442
mzling 3:14050370593a 443 if (!msg_check(message, len)) {
mzling 6:721f5e8a794d 444 printf("msg_check failed on readonly! \r\n");
mzling 2:be605799793f 445 return;
mzling 2:be605799793f 446 }
mzling 3:14050370593a 447 _failures = 0;
mzling 2:be605799793f 448
mzling 3:14050370593a 449 send_read_only_values();
mzling 2:be605799793f 450 }
mzling 2:be605799793f 451
mzling 3:14050370593a 452 /**
mzling 3:14050370593a 453 * Checks received READ message, and places requested data into an array
mzling 3:14050370593a 454 * @param message The received READ request as a char array
mzling 3:14050370593a 455 * @param len Length of the READ message
mzling 3:14050370593a 456 * @author Michael Ling
mzling 3:14050370593a 457 * @date 2/4/2015
mzling 3:14050370593a 458 */
mzling 3:14050370593a 459 void BluetoothComm::process_read(char* message, int len)
mzling 2:be605799793f 460 {
mzling 2:be605799793f 461 //If the received message is an error message, resend the last command
mzling 3:14050370593a 462 if (message[2] == END) {
mzling 3:14050370593a 463 _failures += 1;
mzling 3:14050370593a 464 if (_failures < 5) {
mzling 3:14050370593a 465 send(_lastCmd);
mzling 2:be605799793f 466 } else {
mzling 3:14050370593a 467 _failures = 0;
mzling 2:be605799793f 468 }
mzling 2:be605799793f 469 return;
mzling 2:be605799793f 470 }
mzling 3:14050370593a 471 if (!msg_check(message, len)) {
mzling 2:be605799793f 472 return;
mzling 2:be605799793f 473 }
mzling 6:721f5e8a794d 474 //PASS MSG TO CTRLBED
mzling 3:14050370593a 475 _failures = 0;
mzling 3:14050370593a 476 char paramList[_numVars];
mzling 3:14050370593a 477 memset(paramList, 0xff, _numVars);
mzling 2:be605799793f 478
mzling 2:be605799793f 479 for (int i=2; i < len-5; i++) {
mzling 2:be605799793f 480 char msg = message[i] & 0xbf;
mzling 2:be605799793f 481 if ((msg & 0x80) != 0) {
mzling 2:be605799793f 482 return;
mzling 2:be605799793f 483 }
mzling 2:be605799793f 484
mzling 2:be605799793f 485 int index = msg & 0xff;
mzling 3:14050370593a 486 paramList[index] = _paramMap[_indexMap[index]];
mzling 2:be605799793f 487
mzling 2:be605799793f 488 }
mzling 2:be605799793f 489 if (message[len-5] == READONLY_IND) {
mzling 3:14050370593a 490 send_read_only_values();
mzling 2:be605799793f 491 }
mzling 14:ac9949a0aff5 492 send_values(paramList, NULL, NULL);
mzling 2:be605799793f 493 }
mzling 2:be605799793f 494
mzling 3:14050370593a 495 /**
mzling 3:14050370593a 496 * Checks received WRITE message and writes to paramMap/SDCard
mzling 3:14050370593a 497 * @param message The received WRITE message
mzling 3:14050370593a 498 * @param len Length of the WRITE message
mzling 16:b6b2c7b0f1c9 499 * @param dataOut Buffer that contains message for Ctrlbed
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 16:b6b2c7b0f1c9 519
mzling 3:14050370593a 520 char paramList[_numVars];
mzling 3:14050370593a 521 memset(paramList, 0xff, _numVars);
mzling 16:b6b2c7b0f1c9 522 //Strips received message of parity and write bits, sets paramMap to the new values, then passes the phone/ctrlbed messages to sendValues
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 14:ac9949a0aff5 534 send_values(paramList, message, dataOut);
mzling 14:ac9949a0aff5 535
mzling 18:34ccf02fdbe7 536 write_params_to_sd_card();
mzling 16:b6b2c7b0f1c9 537
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 16:b6b2c7b0f1c9 544 * @param dataOut Buffer for sending to ctrlbed
mzling 3:14050370593a 545 * @author Michael Ling
mzling 3:14050370593a 546 * @date 2/4/2015
mzling 3:14050370593a 547 */
mzling 10:235c0b81c4c7 548 void BluetoothComm::process (char* message, int len, short* dataOut)
mzling 2:be605799793f 549 {
mzling 2:be605799793f 550 char c = message[2];
mzling 4:7e3bbf896e78 551 for (int i =0; i < len; i++) {
mzling 6:721f5e8a794d 552 //printf("Message character: %x \r\n", message[i]);
mzling 4:7e3bbf896e78 553 }
mzling 2:be605799793f 554 if (c == READONLY_IND) {
mzling 3:14050370593a 555 process_read_only(message, len);
mzling 2:be605799793f 556 return;
mzling 2:be605799793f 557 }
mzling 2:be605799793f 558 if ((c & 0x80) == 0) {
mzling 6:721f5e8a794d 559 //printf("Is a read\r\n");
mzling 3:14050370593a 560 process_read(message, len);
mzling 2:be605799793f 561 return;
mzling 2:be605799793f 562 } else {
mzling 10:235c0b81c4c7 563 process_write(message, len, dataOut);
mzling 2:be605799793f 564 return;
mzling 2:be605799793f 565 }
mzling 2:be605799793f 566 }
mzling 2:be605799793f 567
mzling 2:be605799793f 568 //Warning: do not put print statements in the function attachment(); it will interfere with receiving messages
mzling 3:14050370593a 569 /**
mzling 19:f3eece6c024f 570 * Scans for data received through Bluetooth, and passes it on if it detects a message-like chunk. Should be run via an interuppt.
mzling 16:b6b2c7b0f1c9 571 * @param dataOut Buffer for sending to Ctrlbed
mzling 3:14050370593a 572 * @author Michael Ling
mzling 3:14050370593a 573 * @date 2/4/2015
mzling 3:14050370593a 574 */
mzling 10:235c0b81c4c7 575 void BluetoothComm::attachment(short* dataOut)
mzling 2:be605799793f 576 {
mzling 13:67492109a66d 577
mzling 7:d4a32c830e11 578 // pc.printf("Entered attachment\r\n");
mzling 3:14050370593a 579 if (_rn42.readable()) {
mzling 3:14050370593a 580 _data=_rn42.getc();
mzling 2:be605799793f 581
mzling 3:14050370593a 582 // if (_data != NULL) {
mzling 3:14050370593a 583 char b = _data & 0xff;
mzling 3:14050370593a 584 //printf("Got char: %x \r\n", b);
mzling 3:14050370593a 585 if (b != NULL or _inMsg) {
mzling 3:14050370593a 586 // printf("Got char non null: %x \r\n", b);
mzling 3:14050370593a 587 }
mzling 3:14050370593a 588 //This marks the START of a message
mzling 3:14050370593a 589 if (_inMsg == false and b == START) {
mzling 16:b6b2c7b0f1c9 590 //boardLed4 = 1;
mzling 3:14050370593a 591 _inMsg = true;
mzling 3:14050370593a 592 _counter = 3;
mzling 3:14050370593a 593 _curMsg[_len] = b;
mzling 3:14050370593a 594 _len += 1;
mzling 3:14050370593a 595 } else if (_inMsg == true and b == START) {
mzling 3:14050370593a 596 _inMsg = false;
mzling 3:14050370593a 597 _counter = 0;
mzling 3:14050370593a 598 memset(_curMsg, 0, 50);
mzling 3:14050370593a 599 _rn42.rxBufferFlush();
mzling 10:235c0b81c4c7 600 process(_msg, _len, dataOut);
mzling 3:14050370593a 601 _len = 0;
mzling 3:14050370593a 602 } else if (_inMsg || _counter > 0 ) {
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 memset(_msg, 0, 50);
mzling 3:14050370593a 611 memcpy(_msg, _curMsg, 50);
mzling 3:14050370593a 612 memset(_curMsg, 0, 50);
mzling 3:14050370593a 613 _rn42.rxBufferFlush();
mzling 10:235c0b81c4c7 614 process(_msg, _len, dataOut);
mzling 3:14050370593a 615 _len = 0;
mzling 2:be605799793f 616 }
mzling 3:14050370593a 617 }
mzling 3:14050370593a 618 if (b == END) {
mzling 3:14050370593a 619 _inMsg = false;
mzling 2:be605799793f 620
mzling 16:b6b2c7b0f1c9 621
mzling 3:14050370593a 622 }
mzling 16:b6b2c7b0f1c9 623
mzling 2:be605799793f 624
mzling 3:14050370593a 625 }
mzling 2:be605799793f 626 }