changed to be compatible with struct
Fork of dataComm by
dataComm.cpp@7:26d706d285c1, 2015-05-13 (annotated)
- Committer:
- perr1940
- Date:
- Wed May 13 19:21:55 2015 +0000
- Revision:
- 7:26d706d285c1
- Parent:
- 6:502959ea39e4
- Child:
- 8:78435e587de1
- Child:
- 9:7486fb079085
publish for comparing;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mzling | 0:f0dc2775ec68 | 1 | #include "mbed.h" |
mzling | 4:86d9e33652b6 | 2 | |
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 | 6:502959ea39e4 | 9 | //Much code is shared with BluetoothComm class, on the control bed |
mzling | 6:502959ea39e4 | 10 | dataComm::dataComm(): _len(0), _counter(0), _inMsg(false), _numVars(34), _numReadOnlyParams(12), _escapesNeeded(8) |
mzling | 0:f0dc2775ec68 | 11 | { |
mzling | 5:c95a0006ba68 | 12 | mbedLED1 = 1; |
mzling | 4:86d9e33652b6 | 13 | int temp1[] = {0,1,2,3,4,8,9,10,-1}; |
mzling | 4:86d9e33652b6 | 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 | 4:86d9e33652b6 | 18 | |
mzling | 4:86d9e33652b6 | 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 | 6:502959ea39e4 | 23 | "StepLength", "StepTime", "HipFlex", "PhaseShift", "MaxAmplitude", "StanceStart", "StanceEnd", |
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 | 4:86d9e33652b6 | 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 | 4:86d9e33652b6 | 34 | |
mzling | 5:c95a0006ba68 | 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 | 4:86d9e33652b6 | 37 | |
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 | 2:53547eb587fb | 45 | /** |
mzling | 2:53547eb587fb | 46 | * Returns the stored value of the specified variable |
mzling | 2:53547eb587fb | 47 | * @param var The variable to be gotten (as a string). |
mzling | 2:53547eb587fb | 48 | * @author Michael Ling |
mzling | 2:53547eb587fb | 49 | * @date 3/31/2015 |
mzling | 2:53547eb587fb | 50 | */ |
mzling | 0:f0dc2775ec68 | 51 | short dataComm::generic_get(std::string var) |
mzling | 0:f0dc2775ec68 | 52 | { |
mzling | 6:502959ea39e4 | 53 | //Translate the raw value of the parameter to a short between 0-100 |
mzling | 4:86d9e33652b6 | 54 | if (var.compare("SittingAngle") == 0) { |
mzling | 2:53547eb587fb | 55 | return (short)((sittingAngle-MIN_SIT)/(MAX_SIT-MIN_SIT)*100); |
mzling | 4:86d9e33652b6 | 56 | } else if(var.compare( "BentForwardAngle")==0) { |
mzling | 2:53547eb587fb | 57 | return (short)((bentAngle-MIN_BENT)/(MAX_BENT-MIN_BENT)*100); |
mzling | 4:86d9e33652b6 | 58 | } else if (var.compare("StandupAsst")==0) { |
mzling | 2:53547eb587fb | 59 | return (short)((fsm.get_standup_asst()-MIN_SUASST)/(MAX_SUASST-MIN_SUASST)*100); |
mzling | 2:53547eb587fb | 60 | } else if (var.compare("SitdownAsst")==0) { |
mzling | 2:53547eb587fb | 61 | return (short)((fsm.get_sitdown_asst()-MIN_SDASST)/(MAX_SDASST-MIN_SDASST)*100); |
mzling | 4:86d9e33652b6 | 62 | } else if (var.compare("SitdownTime")==0) { |
mzling | 2:53547eb587fb | 63 | return (short)((tSittingDown-MIN_SDTIME)/(MAX_SDTIME-MIN_SDTIME)*100); |
mzling | 4:86d9e33652b6 | 64 | } else if (var.compare( "StandingAngle")==0) { |
mzling | 2:53547eb587fb | 65 | return (short)((stand_adjust-MIN_STAND)/(MAX_STAND-MIN_STAND)*100); |
mzling | 4:86d9e33652b6 | 66 | } else if (var.compare("WalkAngle")==0) { |
mzling | 2:53547eb587fb | 67 | return (short)((fsm.get_backbias()-MIN_WALK)/(MAX_WALK-MIN_WALK)*100); |
mzling | 4:86d9e33652b6 | 68 | } else if (var.compare("StepTime")==0) { |
perr1940 | 7:26d706d285c1 | 69 | return (short) ((mm_gait_params.time_steps-MIN_STEPTIME)/(MAX_STEPTIME-MIN_STEPTIME)*100); |
mzling | 6:502959ea39e4 | 70 | } else if (var.compare("PhaseShift") == 0) { |
perr1940 | 7:26d706d285c1 | 71 | return (short) ((mm_gait_params.peak_time-MIN_PHASESHIFT)/(MAX_PHASESHIFT-MIN_PHASESHIFT)*100); |
mzling | 6:502959ea39e4 | 72 | } else if (var.compare("WalkAngle") == 0) { |
perr1940 | 7:26d706d285c1 | 73 | return (short) ((mm_gait_params.walking_angle-MIN_WALK)/(MAX_WALK-MIN_WALK)*100); |
mzling | 6:502959ea39e4 | 74 | } else if (var.compare("StepLength") == 0) { |
perr1940 | 7:26d706d285c1 | 75 | return (short) ((mm_gait_params.end_angle-MIN_STEPLEN)/(MAX_STEPLEN-MIN_STEPLEN)*100); |
mzling | 6:502959ea39e4 | 76 | } else if (var.compare("HipFlex") == 0) { |
perr1940 | 7:26d706d285c1 | 77 | return (short) ((mm_gait_params.max_angle-MIN_HIPFLEX)/(MAX_HIPFLEX-MIN_HIPFLEX)*100); |
mzling | 2:53547eb587fb | 78 | } |
mzling | 2:53547eb587fb | 79 | return 0; |
mzling | 4:86d9e33652b6 | 80 | |
mzling | 0:f0dc2775ec68 | 81 | } |
mzling | 0:f0dc2775ec68 | 82 | |
mzling | 2:53547eb587fb | 83 | /** |
mzling | 2:53547eb587fb | 84 | * Sets a new value for the given variable |
mzling | 2:53547eb587fb | 85 | * @param var The variable to be set, as a string |
mzling | 2:53547eb587fb | 86 | * @param newval New value for the variable |
mzling | 2:53547eb587fb | 87 | * @author Michael Ling |
mzling | 2:53547eb587fb | 88 | * @date 3/31/2015 |
mzling | 2:53547eb587fb | 89 | */ |
mzling | 0:f0dc2775ec68 | 90 | void dataComm::generic_set(std::string var, short newval) |
mzling | 0:f0dc2775ec68 | 91 | { |
mzling | 4:86d9e33652b6 | 92 | mbedLED3 = 1; |
mzling | 6:502959ea39e4 | 93 | //newval is a short from 0-100, and needs to be converted to a raw value. |
mzling | 6:502959ea39e4 | 94 | //We do this by making a 100-point scale between MIN and MAX param values |
mzling | 4:86d9e33652b6 | 95 | if (var.compare( "SittingAngle")==0) { |
mzling | 4:86d9e33652b6 | 96 | sittingAngle = MIN_SIT + (float)newval/100*(MAX_SIT-MIN_SIT); |
mzling | 4:86d9e33652b6 | 97 | // pc.printf("%d\r\n", (short)((sittingAngle-70)/40*100)); |
mzling | 4:86d9e33652b6 | 98 | } else if (var.compare( "BentForwardAngle")==0) { |
mzling | 4:86d9e33652b6 | 99 | bentAngle = MIN_BENT+(float)newval/100*(MAX_BENT-MIN_BENT); |
mzling | 4:86d9e33652b6 | 100 | // pc.printf("%d\r\n", (short)((bentAngle-90)/50*100)); |
mzling | 4:86d9e33652b6 | 101 | } else if (var.compare("StandupAsst")==0) { |
mzling | 4:86d9e33652b6 | 102 | fsm.set_standup_asst(MIN_SUASST+(float)newval/100*(MAX_SUASST-MIN_SUASST)); |
mzling | 4:86d9e33652b6 | 103 | // pc.printf("%d\r\n", (short)fsm.get_standup_asst()); |
mzling | 4:86d9e33652b6 | 104 | } else if (var.compare("SitdownAsst")==0) { |
mzling | 4:86d9e33652b6 | 105 | fsm.set_sitdown_asst(MIN_SDASST+(float)newval/100*(MAX_SDASST-MIN_SDASST)); |
mzling | 4:86d9e33652b6 | 106 | // pc.printf("%d\r\n", (short)fsm.get_sitdown_asst()); |
mzling | 4:86d9e33652b6 | 107 | } else if (var.compare("SitdownTime")==0) { |
mzling | 4:86d9e33652b6 | 108 | tSittingDown = MIN_SDTIME + (float)newval/100*(MAX_SDTIME-MIN_SDTIME); |
mzling | 4:86d9e33652b6 | 109 | // pc.printf("%d\r\n", (short)tSittingDown); |
mzling | 4:86d9e33652b6 | 110 | } else if (var.compare("StandingAngle")==0) { |
perr1940 | 7:26d706d285c1 | 111 | //pc.printf("LED\r\n"); |
mzling | 4:86d9e33652b6 | 112 | motorLED = 1; |
mzling | 4:86d9e33652b6 | 113 | stand_adjust = MIN_STAND + float(newval)/100*(MAX_STAND-MIN_STAND); |
mzling | 4:86d9e33652b6 | 114 | encoder_L.init(zero_ang_L+stand_adjust); |
mzling | 4:86d9e33652b6 | 115 | encoder_R.init(zero_ang_R+stand_adjust); |
mzling | 4:86d9e33652b6 | 116 | //pc.printf("%d\r\n", (short)((stand_adjust+15)/30*100)); |
mzling | 4:86d9e33652b6 | 117 | } else if (var.compare("WalkAngle")==0) { |
mzling | 4:86d9e33652b6 | 118 | fsm.set_backbias(MIN_WALK+(float)newval/100*(MAX_WALK-MIN_WALK)); |
mzling | 4:86d9e33652b6 | 119 | // pc.printf("%d\r\n", (short)fsm.get_backbias()); |
mzling | 4:86d9e33652b6 | 120 | } else if (var.compare("StepTime")==0) { |
perr1940 | 7:26d706d285c1 | 121 | mm_gait_params.time_steps = (float)newval/100*(MAX_STEPTIME-MIN_STEPTIME); |
mzling | 6:502959ea39e4 | 122 | } else if (var.compare("PhaseShift") == 0) { |
perr1940 | 7:26d706d285c1 | 123 | mm_gait_params.peak_time = (float)newval/100*(MAX_PHASESHIFT-MIN_PHASESHIFT); |
mzling | 6:502959ea39e4 | 124 | } else if (var.compare("WalkAngle") == 0) { |
perr1940 | 7:26d706d285c1 | 125 | mm_gait_params.walking_angle = (float)newval/100*(MAX_WALK-MIN_WALK); |
mzling | 6:502959ea39e4 | 126 | } else if (var.compare("StepLength") == 0) { |
perr1940 | 7:26d706d285c1 | 127 | mm_gait_params.end_angle = (float)newval/100*(MAX_STEPLEN-MIN_STEPLEN); |
mzling | 6:502959ea39e4 | 128 | } else if (var.compare("HipFlex") == 0) { |
perr1940 | 7:26d706d285c1 | 129 | mm_gait_params.max_angle = (float)newval/100*(MAX_HIPFLEX-MIN_HIPFLEX); |
mzling | 4:86d9e33652b6 | 130 | } |
mzling | 4:86d9e33652b6 | 131 | //MORE else blocks for gait params |
mzling | 4:86d9e33652b6 | 132 | |
mzling | 0:f0dc2775ec68 | 133 | } |
mzling | 0:f0dc2775ec68 | 134 | /** |
mzling | 0:f0dc2775ec68 | 135 | * Calculates parity--0 if c even, 1 if odd |
mzling | 0:f0dc2775ec68 | 136 | * @param c The short that we want to calculate parity of |
mzling | 0:f0dc2775ec68 | 137 | * @author Michael Ling |
mzling | 0:f0dc2775ec68 | 138 | * @date 2/4/2015 |
mzling | 0:f0dc2775ec68 | 139 | */ |
mzling | 0:f0dc2775ec68 | 140 | bool dataComm::parity(short c) |
mzling | 0:f0dc2775ec68 | 141 | { |
mzling | 0:f0dc2775ec68 | 142 | bool p = false; |
mzling | 0:f0dc2775ec68 | 143 | while (c != 0) { |
mzling | 0:f0dc2775ec68 | 144 | p = !p; |
mzling | 0:f0dc2775ec68 | 145 | c = (short) (c & (c-1)); |
mzling | 0:f0dc2775ec68 | 146 | } |
mzling | 0:f0dc2775ec68 | 147 | return p; |
mzling | 0:f0dc2775ec68 | 148 | } |
mzling | 0:f0dc2775ec68 | 149 | |
mzling | 0:f0dc2775ec68 | 150 | /** |
mzling | 0:f0dc2775ec68 | 151 | * Calculates checksum of char array, sum of all chars in array |
mzling | 0:f0dc2775ec68 | 152 | * @param b The char array to be summed up |
mzling | 0:f0dc2775ec68 | 153 | * @param len Length of the array b |
mzling | 0:f0dc2775ec68 | 154 | * @author Michael Ling |
mzling | 0:f0dc2775ec68 | 155 | * @date 2/4/2015 |
mzling | 0:f0dc2775ec68 | 156 | */ |
mzling | 0:f0dc2775ec68 | 157 | char* dataComm::get_checksum(char* b, int len) |
mzling | 0:f0dc2775ec68 | 158 | { |
mzling | 0:f0dc2775ec68 | 159 | char* checksum = (char*)malloc(3*sizeof(char)); |
mzling | 0:f0dc2775ec68 | 160 | short sum = 0; |
mzling | 0:f0dc2775ec68 | 161 | for (int i = 0; i < len; i += 1) { |
mzling | 0:f0dc2775ec68 | 162 | sum += ((short) b[i] & 0xff); |
mzling | 0:f0dc2775ec68 | 163 | } |
mzling | 0:f0dc2775ec68 | 164 | checksum[1] = (char) ((sum >> 8) & 0xff); |
mzling | 0:f0dc2775ec68 | 165 | checksum[2] = (char) (sum & 0xff); |
mzling | 0:f0dc2775ec68 | 166 | char check = (char) (sum & 0xff); |
mzling | 0:f0dc2775ec68 | 167 | //Sets escape character if the checksum contains START or END |
mzling | 0:f0dc2775ec68 | 168 | if (check == START || check == END) { |
mzling | 0:f0dc2775ec68 | 169 | checksum[0] = 1; |
mzling | 0:f0dc2775ec68 | 170 | checksum[2] = (char) (checksum[2] & 0x1); |
mzling | 0:f0dc2775ec68 | 171 | } else { |
mzling | 0:f0dc2775ec68 | 172 | checksum[0] = 0; |
mzling | 0:f0dc2775ec68 | 173 | } |
mzling | 0:f0dc2775ec68 | 174 | return checksum; |
mzling | 0:f0dc2775ec68 | 175 | } |
mzling | 0:f0dc2775ec68 | 176 | |
mzling | 0:f0dc2775ec68 | 177 | |
mzling | 0:f0dc2775ec68 | 178 | |
mzling | 0:f0dc2775ec68 | 179 | /** |
mzling | 0:f0dc2775ec68 | 180 | * 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 | 181 | * @param message The received message to check |
mzling | 0:f0dc2775ec68 | 182 | * @param len Length of the message |
mzling | 0:f0dc2775ec68 | 183 | * @author Michael Ling |
mzling | 0:f0dc2775ec68 | 184 | * @date 2/4/2015 |
mzling | 0:f0dc2775ec68 | 185 | */ |
mzling | 0:f0dc2775ec68 | 186 | bool dataComm::msg_check(char* message, int len) |
mzling | 0:f0dc2775ec68 | 187 | { |
mzling | 2:53547eb587fb | 188 | //Checks the message starts with 0xff |
mzling | 0:f0dc2775ec68 | 189 | if (message[0] != START) { |
mzling | 0:f0dc2775ec68 | 190 | //printf("Improper START or END \r\n"); |
mzling | 4:86d9e33652b6 | 191 | |
mzling | 0:f0dc2775ec68 | 192 | return false; |
mzling | 0:f0dc2775ec68 | 193 | } |
mzling | 0:f0dc2775ec68 | 194 | //printf("got a start\r\n"); |
mzling | 2:53547eb587fb | 195 | //Unlike on databed, the message is not guaranteed to be exactly LEN long, hence we're allowed to backtrack looking for END |
mzling | 6:502959ea39e4 | 196 | //Otherwise, the msg_check procedure is the same |
mzling | 0:f0dc2775ec68 | 197 | while (message[len-4] != END && len >= 6) { |
mzling | 0:f0dc2775ec68 | 198 | len -= 1; |
mzling | 0:f0dc2775ec68 | 199 | } |
mzling | 0:f0dc2775ec68 | 200 | if (message[len-4] != END) { |
mzling | 0:f0dc2775ec68 | 201 | //send_error(END_ERR); |
mzling | 0:f0dc2775ec68 | 202 | return false; |
mzling | 0:f0dc2775ec68 | 203 | } |
mzling | 0:f0dc2775ec68 | 204 | bool write = message[2] & 0x80; |
mzling | 0:f0dc2775ec68 | 205 | for (int i=2; i < len-5; i+=2) { |
mzling | 0:f0dc2775ec68 | 206 | if (i == 2 && message[i] == READONLY_IND) { |
mzling | 0:f0dc2775ec68 | 207 | break; |
mzling | 0:f0dc2775ec68 | 208 | } |
mzling | 0:f0dc2775ec68 | 209 | if (i == (len-5) && message[i] == READONLY_IND) { |
mzling | 0:f0dc2775ec68 | 210 | break; |
mzling | 0:f0dc2775ec68 | 211 | } |
mzling | 0:f0dc2775ec68 | 212 | if (((message[i] & 0x80) !=0) && !write) { |
mzling | 0:f0dc2775ec68 | 213 | //printf("Does not match READ format \r\n"); |
mzling | 0:f0dc2775ec68 | 214 | return false; |
mzling | 0:f0dc2775ec68 | 215 | } |
mzling | 0:f0dc2775ec68 | 216 | if (((message[i] & 0x80) != 0x80) && write) { |
mzling | 0:f0dc2775ec68 | 217 | //printf("char %x Does not match WRITE format \r\n", message[i]); |
mzling | 0:f0dc2775ec68 | 218 | return false; |
mzling | 0:f0dc2775ec68 | 219 | } |
mzling | 0:f0dc2775ec68 | 220 | short s; |
mzling | 0:f0dc2775ec68 | 221 | if (write) { |
mzling | 0:f0dc2775ec68 | 222 | s = (short) ((message[i] << 8) | message[i+1]); |
mzling | 0:f0dc2775ec68 | 223 | } else { |
mzling | 0:f0dc2775ec68 | 224 | s = (short) (message[i] << 8); |
mzling | 0:f0dc2775ec68 | 225 | } |
mzling | 2:53547eb587fb | 226 | //Checks the parity bit of the var/data pair. |
mzling | 0:f0dc2775ec68 | 227 | bool parity1 = (s & 0x4000) != 0; |
mzling | 0:f0dc2775ec68 | 228 | |
mzling | 0:f0dc2775ec68 | 229 | if (parity1 != parity(s & 0x4000)) { |
mzling | 0:f0dc2775ec68 | 230 | //printf("Parity error in VAR char \r\n"); |
mzling | 0:f0dc2775ec68 | 231 | return false; |
mzling | 0:f0dc2775ec68 | 232 | } |
mzling | 0:f0dc2775ec68 | 233 | |
mzling | 0:f0dc2775ec68 | 234 | char c = message[i] & 0x3f; |
mzling | 0:f0dc2775ec68 | 235 | char c2 = message[i+1]; |
mzling | 2:53547eb587fb | 236 | //Makes sure the var is a valid one |
mzling | 0:f0dc2775ec68 | 237 | if ((int) c < 0 || (int) c > _numVars) { |
mzling | 0:f0dc2775ec68 | 238 | //printf("VAR char out of range \r\n"); |
mzling | 0:f0dc2775ec68 | 239 | return false; |
mzling | 0:f0dc2775ec68 | 240 | } |
mzling | 2:53547eb587fb | 241 | //Makes sure the new value is in the range 0-100 |
mzling | 0:f0dc2775ec68 | 242 | if ((int) c2 < 0 || (int) c2 > 100) { |
mzling | 0:f0dc2775ec68 | 243 | //printf("DATA char out of range"); |
mzling | 0:f0dc2775ec68 | 244 | return false; |
mzling | 0:f0dc2775ec68 | 245 | } |
mzling | 0:f0dc2775ec68 | 246 | } |
mzling | 0:f0dc2775ec68 | 247 | char* checksum = get_checksum(message, len-3); |
mzling | 0:f0dc2775ec68 | 248 | if (checksum[0] != message[len-3]) { |
mzling | 0:f0dc2775ec68 | 249 | //printf("checksum error in char 0, expected %x but got %x \r\n", checksum[0], message[len-3]); |
mzling | 0:f0dc2775ec68 | 250 | free(checksum); |
mzling | 0:f0dc2775ec68 | 251 | return false; |
mzling | 0:f0dc2775ec68 | 252 | } |
mzling | 0:f0dc2775ec68 | 253 | if (checksum[1] != message[len-2]) { |
mzling | 0:f0dc2775ec68 | 254 | //printf("Checksum error in char 1, expected %x but got %x \r\n", checksum[1], message[len-2]); |
mzling | 0:f0dc2775ec68 | 255 | free(checksum); |
mzling | 0:f0dc2775ec68 | 256 | return false; |
mzling | 0:f0dc2775ec68 | 257 | } |
mzling | 0:f0dc2775ec68 | 258 | if (checksum[2] != message[len-1]) { |
mzling | 0:f0dc2775ec68 | 259 | //printf("Checksum error in char 2, expected %x but got %x \r\n", checksum[2], message[len-1]); |
mzling | 0:f0dc2775ec68 | 260 | free(checksum); |
mzling | 0:f0dc2775ec68 | 261 | return false; |
mzling | 0:f0dc2775ec68 | 262 | } |
mzling | 0:f0dc2775ec68 | 263 | free(checksum); |
mzling | 0:f0dc2775ec68 | 264 | |
mzling | 0:f0dc2775ec68 | 265 | return true; |
mzling | 0:f0dc2775ec68 | 266 | } |
mzling | 0:f0dc2775ec68 | 267 | |
mzling | 0:f0dc2775ec68 | 268 | |
mzling | 0:f0dc2775ec68 | 269 | |
mzling | 0:f0dc2775ec68 | 270 | /** |
mzling | 0:f0dc2775ec68 | 271 | * Checks received WRITE message and writes to paramMap/SDCard |
mzling | 0:f0dc2775ec68 | 272 | * @param message The received WRITE message |
mzling | 0:f0dc2775ec68 | 273 | * @param len Length of the WRITE message |
mzling | 0:f0dc2775ec68 | 274 | * @author Michael Ling |
mzling | 0:f0dc2775ec68 | 275 | * @date 2/4/2015 |
mzling | 0:f0dc2775ec68 | 276 | */ |
mzling | 0:f0dc2775ec68 | 277 | void dataComm::process_write(short int* msg, int len) |
mzling | 0:f0dc2775ec68 | 278 | { |
mzling | 2:53547eb587fb | 279 | |
mzling | 0:f0dc2775ec68 | 280 | if (msg[0] == 0) { |
mzling | 0:f0dc2775ec68 | 281 | return; |
mzling | 0:f0dc2775ec68 | 282 | } |
mzling | 4:86d9e33652b6 | 283 | //dataIn is formatted so that a direct cast to char* will reproduce the original message from the phone. |
mzling | 4:86d9e33652b6 | 284 | char *message = (char*) msg; |
mzling | 0:f0dc2775ec68 | 285 | |
mzling | 0:f0dc2775ec68 | 286 | |
mzling | 0:f0dc2775ec68 | 287 | if (!msg_check(message, len*2)) { |
mzling | 0:f0dc2775ec68 | 288 | return; |
mzling | 0:f0dc2775ec68 | 289 | } |
mzling | 0:f0dc2775ec68 | 290 | mbedLED2 = 1; |
mzling | 2:53547eb587fb | 291 | //If the msgCheck passes, we change the local paramMap and set variables to their new values |
mzling | 0:f0dc2775ec68 | 292 | for (int i=2; i < len*2-5; i+=2) { |
mzling | 0:f0dc2775ec68 | 293 | //printf("Loop %d\r\n", i); |
mzling | 0:f0dc2775ec68 | 294 | if (message[i] == END) { |
mzling | 0:f0dc2775ec68 | 295 | return; |
mzling | 0:f0dc2775ec68 | 296 | } |
mzling | 0:f0dc2775ec68 | 297 | char msgc = message[i] & 0xbf; |
mzling | 0:f0dc2775ec68 | 298 | if ((msgc & 0x80) != 0x80) { |
mzling | 0:f0dc2775ec68 | 299 | return; |
mzling | 0:f0dc2775ec68 | 300 | } |
mzling | 0:f0dc2775ec68 | 301 | int index = msgc & 0x7f; |
mzling | 4:86d9e33652b6 | 302 | |
mzling | 0:f0dc2775ec68 | 303 | _paramMap[_indexMap[index]] = message[i+1]; |
mzling | 0:f0dc2775ec68 | 304 | generic_set(_indexMap[index], message[i+1]); |
mzling | 4:86d9e33652b6 | 305 | |
mzling | 0:f0dc2775ec68 | 306 | } |
mzling | 0:f0dc2775ec68 | 307 | } |