changed to be compatible with struct
Fork of dataComm by
dataComm.cpp@3:2091104c9b61, 2015-04-24 (annotated)
- Committer:
- mzling
- Date:
- Fri Apr 24 19:34:49 2015 +0000
- Revision:
- 3:2091104c9b61
- Parent:
- 2:53547eb587fb
- Child:
- 4:86d9e33652b6
Added gait params
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 | 2:53547eb587fb | 8 | |
mzling | 3:2091104c9b61 | 9 | dataComm::dataComm(): _len(0), _counter(0), _inMsg(false), _numVars(32), _numReadOnlyParams(12), _escapesNeeded(8) |
mzling | 0:f0dc2775ec68 | 10 | { |
mzling | 0:f0dc2775ec68 | 11 | |
mzling | 2:53547eb587fb | 12 | |
mzling | 0:f0dc2775ec68 | 13 | int temp1[] = {0,1,2,3,4,8,9,10, -1}; |
mzling | 0:f0dc2775ec68 | 14 | |
mzling | 0:f0dc2775ec68 | 15 | for (int i = 0; i < _escapesNeeded+1; i++) { |
mzling | 0:f0dc2775ec68 | 16 | _escapeNeeded[i] = temp1[i]; |
mzling | 0:f0dc2775ec68 | 17 | } |
mzling | 0:f0dc2775ec68 | 18 | |
mzling | 0:f0dc2775ec68 | 19 | |
mzling | 0:f0dc2775ec68 | 20 | std::string temp2[] = {"KPStance", "KPSwing", "KPStanding", "KPSitting", "KPStandUp", "KPSitdown", "KDStance", "KDSwing", |
mzling | 0:f0dc2775ec68 | 21 | "KDStanding", "KDSitting", "KDStandUp", "KDSitDown", "StandingAngle", "SittingAngle", "BentForwardAngle", "ForwardAngle", "RearAngle", |
mzling | 3:2091104c9b61 | 22 | "IMUAngle", "KneeFullRetract", "KneeFullExtend", "LockTime", "Rate", "StandupAsst", "StandupTime", "SitdownAsst", "SitdownTime", "WalkAngle", |
mzling | 3:2091104c9b61 | 23 | "StepLength", "StepTime", "PeakFlexion", "PhaseShift", "WalkSpeed", |
mzling | 0:f0dc2775ec68 | 24 | "TorsoAng", "LKneeAng", "RKneeAng", "LHipAng", "RHipAng", "LHipTorque", "RHipTorque", "ExoAndKneeStates", "TorsoRefAngle", "LHipRefAngle", |
mzling | 0:f0dc2775ec68 | 25 | "RHipRefAngle", "Charge"}; |
mzling | 0:f0dc2775ec68 | 26 | //Populate the map of indices to param names |
mzling | 0:f0dc2775ec68 | 27 | for (int j = 0; j < (_numVars + _numReadOnlyParams); j += 1) { |
mzling | 0:f0dc2775ec68 | 28 | _indexMap[j] = temp2[j]; |
mzling | 0:f0dc2775ec68 | 29 | } |
mzling | 0:f0dc2775ec68 | 30 | |
mzling | 0:f0dc2775ec68 | 31 | for (int j = 0; j < _numVars; j += 1) { |
mzling | 0:f0dc2775ec68 | 32 | _paramMap[_indexMap[j]] = generic_get(_indexMap[j]); |
mzling | 0:f0dc2775ec68 | 33 | } |
mzling | 0:f0dc2775ec68 | 34 | |
mzling | 0:f0dc2775ec68 | 35 | // pc.printf("Initialized PARAM \r\n"); |
mzling | 0:f0dc2775ec68 | 36 | int temp4[] = {0x01fe, 0x02ac, 0x02ff, 0x0180, 0x0012, 0x0010, 0x0020, 0x00bf, 0x023f, 0x0123, 0x03a2, 0x10}; |
mzling | 0:f0dc2775ec68 | 37 | //readData.write(_numReadOnlyParams, temp4); |
mzling | 0:f0dc2775ec68 | 38 | for (int k = 0; k < _numReadOnlyParams; k += 1) { |
mzling | 0:f0dc2775ec68 | 39 | _paramMap[_indexMap[_numVars + k]] = temp4[k]; |
mzling | 0:f0dc2775ec68 | 40 | } |
mzling | 0:f0dc2775ec68 | 41 | //pc.printf("Test: %x\r\n", _paramMap["TorsoAng"]); |
mzling | 0:f0dc2775ec68 | 42 | |
mzling | 0:f0dc2775ec68 | 43 | |
mzling | 0:f0dc2775ec68 | 44 | |
mzling | 0:f0dc2775ec68 | 45 | } |
mzling | 0:f0dc2775ec68 | 46 | |
mzling | 2:53547eb587fb | 47 | /** |
mzling | 2:53547eb587fb | 48 | * Returns the stored value of the specified variable |
mzling | 2:53547eb587fb | 49 | * @param var The variable to be gotten (as a string). |
mzling | 2:53547eb587fb | 50 | * @author Michael Ling |
mzling | 2:53547eb587fb | 51 | * @date 3/31/2015 |
mzling | 2:53547eb587fb | 52 | */ |
mzling | 0:f0dc2775ec68 | 53 | short dataComm::generic_get(std::string var) |
mzling | 0:f0dc2775ec68 | 54 | { |
mzling | 2:53547eb587fb | 55 | if (var.compare("SittingAngle") == 0){ |
mzling | 2:53547eb587fb | 56 | return (short)((sittingAngle-MIN_SIT)/(MAX_SIT-MIN_SIT)*100); |
mzling | 2:53547eb587fb | 57 | }else if(var.compare( "BentForwardAngle")==0){ |
mzling | 2:53547eb587fb | 58 | return (short)((bentAngle-MIN_BENT)/(MAX_BENT-MIN_BENT)*100); |
mzling | 2:53547eb587fb | 59 | } else if (var.compare("StandupAsst")==0){ |
mzling | 2:53547eb587fb | 60 | return (short)((fsm.get_standup_asst()-MIN_SUASST)/(MAX_SUASST-MIN_SUASST)*100); |
mzling | 2:53547eb587fb | 61 | } else if (var.compare("SitdownAsst")==0) { |
mzling | 2:53547eb587fb | 62 | return (short)((fsm.get_sitdown_asst()-MIN_SDASST)/(MAX_SDASST-MIN_SDASST)*100); |
mzling | 2:53547eb587fb | 63 | } else if (var.compare("SitdownTime")==0){ |
mzling | 2:53547eb587fb | 64 | return (short)((tSittingDown-MIN_SDTIME)/(MAX_SDTIME-MIN_SDTIME)*100); |
mzling | 2:53547eb587fb | 65 | } else if (var.compare( "StandingAngle")==0){ |
mzling | 2:53547eb587fb | 66 | return (short)((stand_adjust-MIN_STAND)/(MAX_STAND-MIN_STAND)*100); |
mzling | 2:53547eb587fb | 67 | } else if (var.compare("WalkAngle")==0){ |
mzling | 2:53547eb587fb | 68 | return (short)((fsm.get_backbias()-MIN_WALK)/(MAX_WALK-MIN_WALK)*100); |
mzling | 2:53547eb587fb | 69 | } |
mzling | 2:53547eb587fb | 70 | return 0; |
mzling | 2:53547eb587fb | 71 | |
mzling | 0:f0dc2775ec68 | 72 | } |
mzling | 0:f0dc2775ec68 | 73 | |
mzling | 2:53547eb587fb | 74 | /** |
mzling | 2:53547eb587fb | 75 | * Sets a new value for the given variable |
mzling | 2:53547eb587fb | 76 | * @param var The variable to be set, as a string |
mzling | 2:53547eb587fb | 77 | * @param newval New value for the variable |
mzling | 2:53547eb587fb | 78 | * @author Michael Ling |
mzling | 2:53547eb587fb | 79 | * @date 3/31/2015 |
mzling | 2:53547eb587fb | 80 | */ |
mzling | 0:f0dc2775ec68 | 81 | void dataComm::generic_set(std::string var, short newval) |
mzling | 0:f0dc2775ec68 | 82 | { |
mzling | 0:f0dc2775ec68 | 83 | mbedLED3 = 1; |
mzling | 3:2091104c9b61 | 84 | //pc.printf("%s\r\n", var); |
mzling | 2:53547eb587fb | 85 | //newval is a short from 0-100, and needs to be converted a more |
mzling | 0:f0dc2775ec68 | 86 | if (var.compare( "SittingAngle")==0){ |
mzling | 1:ad39c297a768 | 87 | sittingAngle = MIN_SIT + (float)newval/100*(MAX_SIT-MIN_SIT); |
mzling | 1:ad39c297a768 | 88 | // pc.printf("%d\r\n", (short)((sittingAngle-70)/40*100)); |
mzling | 0:f0dc2775ec68 | 89 | } else if (var.compare( "BentForwardAngle")==0){ |
mzling | 2:53547eb587fb | 90 | |
mzling | 1:ad39c297a768 | 91 | bentAngle = MIN_BENT+(float)newval/100*(MAX_BENT-MIN_BENT); |
mzling | 1:ad39c297a768 | 92 | // pc.printf("%d\r\n", (short)((bentAngle-90)/50*100)); |
mzling | 0:f0dc2775ec68 | 93 | } else if (var.compare("StandupAsst")==0) { |
mzling | 1:ad39c297a768 | 94 | fsm.set_standup_asst(MIN_SUASST+(float)newval/100*(MAX_SUASST-MIN_SUASST)); |
mzling | 1:ad39c297a768 | 95 | // pc.printf("%d\r\n", (short)fsm.get_standup_asst()); |
mzling | 0:f0dc2775ec68 | 96 | } else if (var.compare("SitdownAsst")==0) { |
mzling | 1:ad39c297a768 | 97 | fsm.set_sitdown_asst(MIN_SDASST+(float)newval/100*(MAX_SDASST-MIN_SDASST)); |
mzling | 1:ad39c297a768 | 98 | // pc.printf("%d\r\n", (short)fsm.get_sitdown_asst()); |
mzling | 0:f0dc2775ec68 | 99 | } else if (var.compare("SitdownTime")==0) { |
mzling | 1:ad39c297a768 | 100 | tSittingDown = MIN_SDTIME + (float)newval/100*(MAX_SDTIME-MIN_SDTIME); |
mzling | 1:ad39c297a768 | 101 | // pc.printf("%d\r\n", (short)tSittingDown); |
mzling | 0:f0dc2775ec68 | 102 | } else if (var.compare("StandingAngle")==0) { |
mzling | 2:53547eb587fb | 103 | pc.printf("LED\r\n"); |
mzling | 2:53547eb587fb | 104 | motorLED = 1; |
mzling | 1:ad39c297a768 | 105 | stand_adjust = MIN_STAND + float(newval)/100*(MAX_STAND-MIN_STAND); |
mzling | 2:53547eb587fb | 106 | encoder_L.init(zero_ang_L+stand_adjust); |
mzling | 2:53547eb587fb | 107 | encoder_R.init(zero_ang_R+stand_adjust); |
mzling | 1:ad39c297a768 | 108 | //pc.printf("%d\r\n", (short)((stand_adjust+15)/30*100)); |
mzling | 0:f0dc2775ec68 | 109 | } else if (var.compare("WalkAngle")==0) { |
mzling | 1:ad39c297a768 | 110 | fsm.set_backbias(MIN_WALK+(float)newval/100*(MAX_WALK-MIN_WALK)); |
mzling | 1:ad39c297a768 | 111 | // pc.printf("%d\r\n", (short)fsm.get_backbias()); |
mzling | 0:f0dc2775ec68 | 112 | } |
mzling | 0:f0dc2775ec68 | 113 | |
mzling | 0:f0dc2775ec68 | 114 | } |
mzling | 0:f0dc2775ec68 | 115 | /** |
mzling | 0:f0dc2775ec68 | 116 | * Calculates parity--0 if c even, 1 if odd |
mzling | 0:f0dc2775ec68 | 117 | * @param c The short that we want to calculate parity of |
mzling | 0:f0dc2775ec68 | 118 | * @author Michael Ling |
mzling | 0:f0dc2775ec68 | 119 | * @date 2/4/2015 |
mzling | 0:f0dc2775ec68 | 120 | */ |
mzling | 0:f0dc2775ec68 | 121 | bool dataComm::parity(short c) |
mzling | 0:f0dc2775ec68 | 122 | { |
mzling | 0:f0dc2775ec68 | 123 | bool p = false; |
mzling | 0:f0dc2775ec68 | 124 | while (c != 0) { |
mzling | 0:f0dc2775ec68 | 125 | p = !p; |
mzling | 0:f0dc2775ec68 | 126 | c = (short) (c & (c-1)); |
mzling | 0:f0dc2775ec68 | 127 | } |
mzling | 0:f0dc2775ec68 | 128 | return p; |
mzling | 0:f0dc2775ec68 | 129 | } |
mzling | 0:f0dc2775ec68 | 130 | |
mzling | 0:f0dc2775ec68 | 131 | /** |
mzling | 0:f0dc2775ec68 | 132 | * Calculates checksum of char array, sum of all chars in array |
mzling | 0:f0dc2775ec68 | 133 | * @param b The char array to be summed up |
mzling | 0:f0dc2775ec68 | 134 | * @param len Length of the array b |
mzling | 0:f0dc2775ec68 | 135 | * @author Michael Ling |
mzling | 0:f0dc2775ec68 | 136 | * @date 2/4/2015 |
mzling | 0:f0dc2775ec68 | 137 | */ |
mzling | 0:f0dc2775ec68 | 138 | char* dataComm::get_checksum(char* b, int len) |
mzling | 0:f0dc2775ec68 | 139 | { |
mzling | 0:f0dc2775ec68 | 140 | char* checksum = (char*)malloc(3*sizeof(char)); |
mzling | 0:f0dc2775ec68 | 141 | short sum = 0; |
mzling | 0:f0dc2775ec68 | 142 | for (int i = 0; i < len; i += 1) { |
mzling | 0:f0dc2775ec68 | 143 | sum += ((short) b[i] & 0xff); |
mzling | 0:f0dc2775ec68 | 144 | } |
mzling | 0:f0dc2775ec68 | 145 | checksum[1] = (char) ((sum >> 8) & 0xff); |
mzling | 0:f0dc2775ec68 | 146 | checksum[2] = (char) (sum & 0xff); |
mzling | 0:f0dc2775ec68 | 147 | char check = (char) (sum & 0xff); |
mzling | 0:f0dc2775ec68 | 148 | //Sets escape character if the checksum contains START or END |
mzling | 0:f0dc2775ec68 | 149 | if (check == START || check == END) { |
mzling | 0:f0dc2775ec68 | 150 | checksum[0] = 1; |
mzling | 0:f0dc2775ec68 | 151 | checksum[2] = (char) (checksum[2] & 0x1); |
mzling | 0:f0dc2775ec68 | 152 | } else { |
mzling | 0:f0dc2775ec68 | 153 | checksum[0] = 0; |
mzling | 0:f0dc2775ec68 | 154 | } |
mzling | 0:f0dc2775ec68 | 155 | return checksum; |
mzling | 0:f0dc2775ec68 | 156 | } |
mzling | 0:f0dc2775ec68 | 157 | |
mzling | 0:f0dc2775ec68 | 158 | /** |
mzling | 0:f0dc2775ec68 | 159 | * Sends error message. |
mzling | 0:f0dc2775ec68 | 160 | * @param errCode character representation of the failure-causing error |
mzling | 0:f0dc2775ec68 | 161 | * @author Michael Ling |
mzling | 0:f0dc2775ec68 | 162 | * @data 2/4/2015 |
mzling | 0:f0dc2775ec68 | 163 | |
mzling | 0:f0dc2775ec68 | 164 | void dataComm::send_error(char errCode) |
mzling | 0:f0dc2775ec68 | 165 | { |
mzling | 0:f0dc2775ec68 | 166 | _rn42.putc(START); |
mzling | 0:f0dc2775ec68 | 167 | _rn42.putc(errCode); |
mzling | 0:f0dc2775ec68 | 168 | _rn42.putc(END); |
mzling | 0:f0dc2775ec68 | 169 | _rn42.putc(0); |
mzling | 0:f0dc2775ec68 | 170 | _rn42.putc(0); |
mzling | 0:f0dc2775ec68 | 171 | _rn42.putc(0); |
mzling | 0:f0dc2775ec68 | 172 | } |
mzling | 0:f0dc2775ec68 | 173 | */ |
mzling | 0:f0dc2775ec68 | 174 | |
mzling | 0:f0dc2775ec68 | 175 | |
mzling | 0:f0dc2775ec68 | 176 | |
mzling | 0:f0dc2775ec68 | 177 | |
mzling | 0:f0dc2775ec68 | 178 | |
mzling | 0:f0dc2775ec68 | 179 | |
mzling | 0:f0dc2775ec68 | 180 | |
mzling | 0:f0dc2775ec68 | 181 | char* dataComm::convert_to_char_array(short int* message, int len) { |
mzling | 0:f0dc2775ec68 | 182 | char *retval = new char[len*2]; |
mzling | 0:f0dc2775ec68 | 183 | for (int i = 2; i < len; i+=2) { |
mzling | 0:f0dc2775ec68 | 184 | short int val = message[i]; |
mzling | 0:f0dc2775ec68 | 185 | retval[(i-1)*2] = (val >> 8) & 0xff; |
mzling | 0:f0dc2775ec68 | 186 | retval[(i-1)*2+1] = val & 0xff; |
mzling | 0:f0dc2775ec68 | 187 | } |
mzling | 0:f0dc2775ec68 | 188 | return retval; |
mzling | 0:f0dc2775ec68 | 189 | } |
mzling | 0:f0dc2775ec68 | 190 | |
mzling | 0:f0dc2775ec68 | 191 | /** |
mzling | 0:f0dc2775ec68 | 192 | * 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 | 193 | * @param message The received message to check |
mzling | 0:f0dc2775ec68 | 194 | * @param len Length of the message |
mzling | 0:f0dc2775ec68 | 195 | * @author Michael Ling |
mzling | 0:f0dc2775ec68 | 196 | * @date 2/4/2015 |
mzling | 0:f0dc2775ec68 | 197 | */ |
mzling | 0:f0dc2775ec68 | 198 | bool dataComm::msg_check(char* message, int len) |
mzling | 0:f0dc2775ec68 | 199 | { |
mzling | 2:53547eb587fb | 200 | //Checks the message starts with 0xff |
mzling | 0:f0dc2775ec68 | 201 | if (message[0] != START) { |
mzling | 0:f0dc2775ec68 | 202 | //printf("Improper START or END \r\n"); |
mzling | 0:f0dc2775ec68 | 203 | //send_error(START_ERR); |
mzling | 0:f0dc2775ec68 | 204 | return false; |
mzling | 0:f0dc2775ec68 | 205 | } |
mzling | 0:f0dc2775ec68 | 206 | //printf("got a start\r\n"); |
mzling | 2:53547eb587fb | 207 | //Unlike on databed, the message is not guaranteed to be exactly LEN long, hence we're allowed to backtrack looking for END |
mzling | 0:f0dc2775ec68 | 208 | while (message[len-4] != END && len >= 6) { |
mzling | 0:f0dc2775ec68 | 209 | len -= 1; |
mzling | 0:f0dc2775ec68 | 210 | } |
mzling | 0:f0dc2775ec68 | 211 | if (message[len-4] != END) { |
mzling | 0:f0dc2775ec68 | 212 | //send_error(END_ERR); |
mzling | 0:f0dc2775ec68 | 213 | return false; |
mzling | 0:f0dc2775ec68 | 214 | } |
mzling | 0:f0dc2775ec68 | 215 | bool write = message[2] & 0x80; |
mzling | 0:f0dc2775ec68 | 216 | for (int i=2; i < len-5; i+=2) { |
mzling | 0:f0dc2775ec68 | 217 | if (i == 2 && message[i] == READONLY_IND) { |
mzling | 0:f0dc2775ec68 | 218 | break; |
mzling | 0:f0dc2775ec68 | 219 | } |
mzling | 0:f0dc2775ec68 | 220 | if (i == (len-5) && message[i] == READONLY_IND) { |
mzling | 0:f0dc2775ec68 | 221 | break; |
mzling | 0:f0dc2775ec68 | 222 | } |
mzling | 0:f0dc2775ec68 | 223 | if (((message[i] & 0x80) !=0) && !write) { |
mzling | 0:f0dc2775ec68 | 224 | //printf("Does not match READ format \r\n"); |
mzling | 0:f0dc2775ec68 | 225 | // send_error((char)(((message[i] & 0x3f) << 3) | RW_ERR)); |
mzling | 0:f0dc2775ec68 | 226 | return false; |
mzling | 0:f0dc2775ec68 | 227 | } |
mzling | 0:f0dc2775ec68 | 228 | if (((message[i] & 0x80) != 0x80) && write) { |
mzling | 0:f0dc2775ec68 | 229 | //printf("char %x Does not match WRITE format \r\n", message[i]); |
mzling | 0:f0dc2775ec68 | 230 | //send_error((char)(((message[i] & 0x3f) << 3) | RW_ERR)); |
mzling | 0:f0dc2775ec68 | 231 | return false; |
mzling | 0:f0dc2775ec68 | 232 | } |
mzling | 0:f0dc2775ec68 | 233 | short s; |
mzling | 0:f0dc2775ec68 | 234 | if (write) { |
mzling | 0:f0dc2775ec68 | 235 | s = (short) ((message[i] << 8) | message[i+1]); |
mzling | 0:f0dc2775ec68 | 236 | } else { |
mzling | 0:f0dc2775ec68 | 237 | s = (short) (message[i] << 8); |
mzling | 0:f0dc2775ec68 | 238 | } |
mzling | 2:53547eb587fb | 239 | //Checks the parity bit of the var/data pair. |
mzling | 0:f0dc2775ec68 | 240 | bool parity1 = (s & 0x4000) != 0; |
mzling | 0:f0dc2775ec68 | 241 | |
mzling | 0:f0dc2775ec68 | 242 | if (parity1 != parity(s & 0x4000)) { |
mzling | 0:f0dc2775ec68 | 243 | //printf("Parity error in VAR char \r\n"); |
mzling | 0:f0dc2775ec68 | 244 | //send_error((char) (((message[i] & 0xbf) << 3) | PARITY_ERR)); |
mzling | 0:f0dc2775ec68 | 245 | return false; |
mzling | 0:f0dc2775ec68 | 246 | } |
mzling | 0:f0dc2775ec68 | 247 | |
mzling | 0:f0dc2775ec68 | 248 | char c = message[i] & 0x3f; |
mzling | 0:f0dc2775ec68 | 249 | char c2 = message[i+1]; |
mzling | 2:53547eb587fb | 250 | //Makes sure the var is a valid one |
mzling | 0:f0dc2775ec68 | 251 | if ((int) c < 0 || (int) c > _numVars) { |
mzling | 0:f0dc2775ec68 | 252 | //printf("VAR char out of range \r\n"); |
mzling | 0:f0dc2775ec68 | 253 | //send_error((char) (((message[i] & 0xbf) << 3) | VAR_ERR)); |
mzling | 0:f0dc2775ec68 | 254 | return false; |
mzling | 0:f0dc2775ec68 | 255 | } |
mzling | 2:53547eb587fb | 256 | //Makes sure the new value is in the range 0-100 |
mzling | 0:f0dc2775ec68 | 257 | if ((int) c2 < 0 || (int) c2 > 100) { |
mzling | 0:f0dc2775ec68 | 258 | //printf("DATA char out of range"); |
mzling | 0:f0dc2775ec68 | 259 | //send_error((char) (((message[i] & 0xbf) << 3) | DATA_ERR)); |
mzling | 0:f0dc2775ec68 | 260 | return false; |
mzling | 0:f0dc2775ec68 | 261 | } |
mzling | 0:f0dc2775ec68 | 262 | } |
mzling | 0:f0dc2775ec68 | 263 | char* checksum = get_checksum(message, len-3); |
mzling | 0:f0dc2775ec68 | 264 | if (checksum[0] != message[len-3]) { |
mzling | 0:f0dc2775ec68 | 265 | //printf("checksum error in char 0, expected %x but got %x \r\n", checksum[0], message[len-3]); |
mzling | 0:f0dc2775ec68 | 266 | free(checksum); |
mzling | 0:f0dc2775ec68 | 267 | // send_error(CHECKSUM_ERR); |
mzling | 0:f0dc2775ec68 | 268 | return false; |
mzling | 0:f0dc2775ec68 | 269 | } |
mzling | 0:f0dc2775ec68 | 270 | if (checksum[1] != message[len-2]) { |
mzling | 0:f0dc2775ec68 | 271 | //printf("Checksum error in char 1, expected %x but got %x \r\n", checksum[1], message[len-2]); |
mzling | 0:f0dc2775ec68 | 272 | free(checksum); |
mzling | 0:f0dc2775ec68 | 273 | // send_error(CHECKSUM_ERR); |
mzling | 0:f0dc2775ec68 | 274 | return false; |
mzling | 0:f0dc2775ec68 | 275 | } |
mzling | 0:f0dc2775ec68 | 276 | if (checksum[2] != message[len-1]) { |
mzling | 0:f0dc2775ec68 | 277 | //printf("Checksum error in char 2, expected %x but got %x \r\n", checksum[2], message[len-1]); |
mzling | 0:f0dc2775ec68 | 278 | free(checksum); |
mzling | 0:f0dc2775ec68 | 279 | // send_error(CHECKSUM_ERR); |
mzling | 0:f0dc2775ec68 | 280 | return false; |
mzling | 0:f0dc2775ec68 | 281 | } |
mzling | 0:f0dc2775ec68 | 282 | free(checksum); |
mzling | 0:f0dc2775ec68 | 283 | |
mzling | 0:f0dc2775ec68 | 284 | return true; |
mzling | 0:f0dc2775ec68 | 285 | } |
mzling | 0:f0dc2775ec68 | 286 | |
mzling | 0:f0dc2775ec68 | 287 | |
mzling | 0:f0dc2775ec68 | 288 | |
mzling | 0:f0dc2775ec68 | 289 | /** |
mzling | 0:f0dc2775ec68 | 290 | * Checks received WRITE message and writes to paramMap/SDCard |
mzling | 0:f0dc2775ec68 | 291 | * @param message The received WRITE message |
mzling | 0:f0dc2775ec68 | 292 | * @param len Length of the WRITE message |
mzling | 0:f0dc2775ec68 | 293 | * @author Michael Ling |
mzling | 0:f0dc2775ec68 | 294 | * @date 2/4/2015 |
mzling | 0:f0dc2775ec68 | 295 | */ |
mzling | 0:f0dc2775ec68 | 296 | void dataComm::process_write(short int* msg, int len) |
mzling | 0:f0dc2775ec68 | 297 | //void dataComm::process_write(char *msg, int len) |
mzling | 0:f0dc2775ec68 | 298 | { |
mzling | 2:53547eb587fb | 299 | |
mzling | 0:f0dc2775ec68 | 300 | if (msg[0] == 0) { |
mzling | 0:f0dc2775ec68 | 301 | return; |
mzling | 0:f0dc2775ec68 | 302 | } |
mzling | 2:53547eb587fb | 303 | //dataIn is formatted so that a direct cast to char* will reproduce the original message from the phone. |
mzling | 0:f0dc2775ec68 | 304 | char *message = (char*) msg; |
mzling | 0:f0dc2775ec68 | 305 | /* |
mzling | 0:f0dc2775ec68 | 306 | for (int i = 0; i < len*2; i += 2) { |
mzling | 0:f0dc2775ec68 | 307 | message[i] = msg[i/2] & 0xff; |
mzling | 0:f0dc2775ec68 | 308 | message[i+1] = (msg[i/2] >> 8) & 0xff; |
mzling | 0:f0dc2775ec68 | 309 | } |
mzling | 0:f0dc2775ec68 | 310 | for (int i = 0; i < len*2; i+=1) { |
mzling | 0:f0dc2775ec68 | 311 | //if (message[i] != 0) { |
mzling | 0:f0dc2775ec68 | 312 | pc.printf("%x, ",message[i]); |
mzling | 0:f0dc2775ec68 | 313 | //} |
mzling | 0:f0dc2775ec68 | 314 | } |
mzling | 0:f0dc2775ec68 | 315 | pc.printf("\r\n");*/ |
mzling | 0:f0dc2775ec68 | 316 | |
mzling | 0:f0dc2775ec68 | 317 | /* |
mzling | 0:f0dc2775ec68 | 318 | char *message = convert_to_char_array(msg, len); |
mzling | 0:f0dc2775ec68 | 319 | if (message[0] == 0xff) { |
mzling | 0:f0dc2775ec68 | 320 | |
mzling | 0:f0dc2775ec68 | 321 | char message[len/2]; |
mzling | 0:f0dc2775ec68 | 322 | for (int i = 4; i < len; i += 4) { |
mzling | 0:f0dc2775ec68 | 323 | message[(i-4)/2] = msg[i]; |
mzling | 0:f0dc2775ec68 | 324 | message[(i-4)/2+1] = msg[i+1]; |
mzling | 0:f0dc2775ec68 | 325 | }*/ |
mzling | 0:f0dc2775ec68 | 326 | /* |
mzling | 0:f0dc2775ec68 | 327 | for (int j=0;j<len;j+=1){ |
mzling | 0:f0dc2775ec68 | 328 | //if (message[j] != 0) { |
mzling | 0:f0dc2775ec68 | 329 | printf("%d: %x\r\n", j, message[j]); |
mzling | 0:f0dc2775ec68 | 330 | //} |
mzling | 0:f0dc2775ec68 | 331 | } |
mzling | 0:f0dc2775ec68 | 332 | //} |
mzling | 0:f0dc2775ec68 | 333 | mbedLED1 = 1; |
mzling | 0:f0dc2775ec68 | 334 | /* |
mzling | 0:f0dc2775ec68 | 335 | if (message[2] == END) { |
mzling | 0:f0dc2775ec68 | 336 | _failures += 1; |
mzling | 0:f0dc2775ec68 | 337 | if (_failures < 5) { |
mzling | 0:f0dc2775ec68 | 338 | send(_lastCmd); |
mzling | 0:f0dc2775ec68 | 339 | } else { |
mzling | 0:f0dc2775ec68 | 340 | _failures = 0; |
mzling | 0:f0dc2775ec68 | 341 | } |
mzling | 0:f0dc2775ec68 | 342 | printf("Exited\r\n"); |
mzling | 0:f0dc2775ec68 | 343 | |
mzling | 0:f0dc2775ec68 | 344 | return; |
mzling | 0:f0dc2775ec68 | 345 | } |
mzling | 0:f0dc2775ec68 | 346 | |
mzling | 0:f0dc2775ec68 | 347 | */ |
mzling | 0:f0dc2775ec68 | 348 | |
mzling | 0:f0dc2775ec68 | 349 | if (!msg_check(message, len*2)) { |
mzling | 0:f0dc2775ec68 | 350 | return; |
mzling | 0:f0dc2775ec68 | 351 | } |
mzling | 0:f0dc2775ec68 | 352 | mbedLED2 = 1; |
mzling | 2:53547eb587fb | 353 | //If the msgCheck passes, we change the local paramMap and set variables to their new values |
mzling | 0:f0dc2775ec68 | 354 | for (int i=2; i < len*2-5; i+=2) { |
mzling | 0:f0dc2775ec68 | 355 | //printf("Loop %d\r\n", i); |
mzling | 0:f0dc2775ec68 | 356 | if (message[i] == END) { |
mzling | 0:f0dc2775ec68 | 357 | return; |
mzling | 0:f0dc2775ec68 | 358 | } |
mzling | 0:f0dc2775ec68 | 359 | char msgc = message[i] & 0xbf; |
mzling | 0:f0dc2775ec68 | 360 | if ((msgc & 0x80) != 0x80) { |
mzling | 0:f0dc2775ec68 | 361 | return; |
mzling | 0:f0dc2775ec68 | 362 | } |
mzling | 0:f0dc2775ec68 | 363 | int index = msgc & 0x7f; |
mzling | 0:f0dc2775ec68 | 364 | |
mzling | 0:f0dc2775ec68 | 365 | _paramMap[_indexMap[index]] = message[i+1]; |
mzling | 0:f0dc2775ec68 | 366 | generic_set(_indexMap[index], message[i+1]); |
mzling | 2:53547eb587fb | 367 | |
mzling | 0:f0dc2775ec68 | 368 | } |
mzling | 0:f0dc2775ec68 | 369 | } |