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