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