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