Hayley Cashdollar / dataComm

fs and ftg working. bent forward not working

Fork of dataComm by Bradley Perry

dataComm.cpp@1:ad39c297a768, 2015-03-20 (annotated)

Committer:
mzling
Date:
Fri Mar 20 23:08:01 2015 +0000
Revision:
1:ad39c297a768
Parent:
0:f0dc2775ec68
Child:
2:53547eb587fb
Added range arguments for new variables

Who changed what in which revision?

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