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