Shih-Ho Hsieh / Mbed 2 deprecated pololu5mag_with_platform

5 sensors on a board with a moving platform.

Dependencies:   mbed

Fork of Motor_XYZ_UI_SPI_I2C_5mag by Shih-Ho Hsieh

parse_array/ParseArray_cpp.txt@23:eede1fe4494b, 2018-08-15 (annotated)

Committer:
hober
Date:
Wed Aug 15 02:42:43 2018 +0000
Branch:
envelope
Revision:
23:eede1fe4494b
Parent:
17:257a443c5454 
2018/08/15 remove altimu lib test.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hober 4:5cde69a36d74 1 #include "ParseArray.h"
hober 4:5cde69a36d74 2 #include <cstdlib>
hober 4:5cde69a36d74 3 // constructor
hober 4:5cde69a36d74 4 ParseArray::ParseArray()
hober 4:5cde69a36d74 5 {
hober 4:5cde69a36d74 6 hasCheckXOR = false;
hober 4:5cde69a36d74 7 hasCRC = false;
hober 4:5cde69a36d74 8 hasFooter = false;
hober 4:5cde69a36d74 9 hasHeader = false;
hober 4:5cde69a36d74 10
hober 4:5cde69a36d74 11 headerIndex=0;
hober 4:5cde69a36d74 12 headerLength = 0;
hober 4:5cde69a36d74 13 headerValueTmp="";
hober 4:5cde69a36d74 14
hober 4:5cde69a36d74 15 footerIndex = 0;
hober 4:5cde69a36d74 16 footerLength = 0;
hober 4:5cde69a36d74 17 footerValueTmp="";
hober 4:5cde69a36d74 18
hober 4:5cde69a36d74 19 checkCRCIndex = 0;
hober 4:5cde69a36d74 20 checkCRCLength = 0 ;
hober 4:5cde69a36d74 21 checkXORIndex = 0;
hober 4:5cde69a36d74 22 }
hober 4:5cde69a36d74 23 // string to byte function
hober 4:5cde69a36d74 24 byte* ParseArray::stringToByte(string hex, int length)
hober 4:5cde69a36d74 25 {
hober 4:5cde69a36d74 26 int NumberChars = hex.length();
hober 4:5cde69a36d74 27 byte* bytes = new byte[length];
hober 4:5cde69a36d74 28 long value;
hober 4:5cde69a36d74 29 string subS;
hober 4:5cde69a36d74 30 char arr[2];
hober 4:5cde69a36d74 31 //strtol( &hexstring[1], NULL, 16);
hober 4:5cde69a36d74 32 for (int i = 0; i < length; i += 1) {
hober 4:5cde69a36d74 33 subS = hex.substr(i * 3, 2);
hober 4:5cde69a36d74 34 value = strtol(subS.c_str() , NULL, 16);
hober 4:5cde69a36d74 35 bytes[i] = (byte)value;
hober 4:5cde69a36d74 36 }
hober 4:5cde69a36d74 37 return bytes;
hober 4:5cde69a36d74 38 }
hober 4:5cde69a36d74 39 // buffer setting
hober 4:5cde69a36d74 40 void ParseArray::setBufferValue(byte* buf, int lengthTmp, int checkSumStart)
hober 4:5cde69a36d74 41 {
hober 4:5cde69a36d74 42 if (hasCheckXOR) {
hober 4:5cde69a36d74 43 bufferLength = checkXORIndex + 1;
hober 4:5cde69a36d74 44 } else if (hasFooter) {
hober 4:5cde69a36d74 45 bufferLength = footerIndex + footerLength + 1;
hober 4:5cde69a36d74 46 } else if (hasHeader)
hober 4:5cde69a36d74 47 bufferLength = headerIndex + headerLength + 1;
hober 4:5cde69a36d74 48 else
hober 4:5cde69a36d74 49 bufferLength = lengthTmp;
hober 4:5cde69a36d74 50
hober 4:5cde69a36d74 51
hober 4:5cde69a36d74 52 bufferArray = new byte[bufferLength];
hober 4:5cde69a36d74 53
hober 4:5cde69a36d74 54 // header values
hober 4:5cde69a36d74 55 if (hasHeader) {
hober 4:5cde69a36d74 56 for (int i = headerIndex, j = 0; i < headerLength; i++, j++)
hober 4:5cde69a36d74 57 bufferArray[i] = headerValue[j];
hober 4:5cde69a36d74 58 for (int i = headerIndex + headerLength, j = 0; j < lengthTmp; i++, j++)
hober 4:5cde69a36d74 59 bufferArray[i] = buf[j];
hober 4:5cde69a36d74 60 } else {
hober 4:5cde69a36d74 61 for (int j = 0; j < lengthTmp; j++)
hober 4:5cde69a36d74 62 bufferArray[j] = buf[j];
hober 4:5cde69a36d74 63 }
hober 4:5cde69a36d74 64 // footer values
hober 4:5cde69a36d74 65 if (hasFooter) {
hober 4:5cde69a36d74 66 for (int i = footerIndex, j = 0; j < footerLength; i++, j++)
hober 4:5cde69a36d74 67 bufferArray[i] = footerValue[j];
hober 4:5cde69a36d74 68 }
hober 4:5cde69a36d74 69 // compute xorValue
hober 4:5cde69a36d74 70 if (hasCheckXOR) {
hober 4:5cde69a36d74 71 byte xorValue = 0;
hober 4:5cde69a36d74 72 for (int i = checkSumStart; i < bufferLength - 1; i++) {
hober 4:5cde69a36d74 73 xorValue ^= bufferArray[i];
hober 4:5cde69a36d74 74 }
hober 4:5cde69a36d74 75 bufferArray[bufferLength - 1] = xorValue;
hober 4:5cde69a36d74 76 }
hober 4:5cde69a36d74 77 }
hober 4:5cde69a36d74 78 void ParseArray::setBufferValueAdjustFooterIndex(byte* buf, int buflength, int checkSumStart)
hober 4:5cde69a36d74 79 {
hober 4:5cde69a36d74 80 bufferLength = (headerIndex + headerLength + buflength + footerLength + 1);
hober 4:5cde69a36d74 81 bufferArray = new byte[bufferLength];
hober 4:5cde69a36d74 82 int i = 0,j=0;
hober 4:5cde69a36d74 83 // header values
hober 4:5cde69a36d74 84 for ( i = headerIndex, j = 0; i < headerLength; i++, j++)
hober 4:5cde69a36d74 85 bufferArray[i] = headerValue[j];
hober 4:5cde69a36d74 86 for ( i = headerIndex + headerLength, j = 0; j < buflength; i++, j++)
hober 4:5cde69a36d74 87 bufferArray[i] = buf[j];
hober 4:5cde69a36d74 88
hober 4:5cde69a36d74 89 // footer values
hober 4:5cde69a36d74 90 for ( i = headerIndex + headerLength + buflength, j = 0; j < footerLength; i++, j++)
hober 4:5cde69a36d74 91 bufferArray[i] = footerValue[j];
hober 4:5cde69a36d74 92 // compute xorValue
hober 4:5cde69a36d74 93 byte xorValue = 0;
hober 4:5cde69a36d74 94 for ( i = checkSumStart; i < bufferLength - 1; i++) {
hober 4:5cde69a36d74 95 xorValue ^= bufferArray[i];
hober 4:5cde69a36d74 96 }
hober 4:5cde69a36d74 97 bufferArray[bufferLength - 1] = xorValue;
hober 4:5cde69a36d74 98 }
hober 4:5cde69a36d74 99
hober 4:5cde69a36d74 100 void ParseArray::setBufferValue(byte* buf,int lengthTmp)
hober 4:5cde69a36d74 101 {
hober 4:5cde69a36d74 102 if (hasCheckXOR) {
hober 4:5cde69a36d74 103 bufferLength = checkXORIndex + 1;
hober 4:5cde69a36d74 104 } else if (hasFooter) {
hober 4:5cde69a36d74 105 bufferLength = footerIndex + footerLength + 1;
hober 4:5cde69a36d74 106 } else if (hasHeader)
hober 4:5cde69a36d74 107 bufferLength = headerIndex + headerLength + 1;
hober 4:5cde69a36d74 108 else
hober 4:5cde69a36d74 109 bufferLength = lengthTmp;
hober 4:5cde69a36d74 110
hober 4:5cde69a36d74 111
hober 4:5cde69a36d74 112 bufferArray = new byte[bufferLength];
hober 4:5cde69a36d74 113
hober 4:5cde69a36d74 114 // header values
hober 4:5cde69a36d74 115 if (hasHeader) {
hober 4:5cde69a36d74 116 for (int i = headerIndex, j = 0; i < headerLength; i++, j++)
hober 4:5cde69a36d74 117 bufferArray[i] = headerValue[j];
hober 4:5cde69a36d74 118 for (int i = headerIndex + headerLength, j=0; j < lengthTmp; i++, j++)
hober 4:5cde69a36d74 119 bufferArray[i] = buf[j];
hober 4:5cde69a36d74 120 } else {
hober 4:5cde69a36d74 121 for (int j=0; j<lengthTmp; j++)
hober 4:5cde69a36d74 122 bufferArray[j] = buf[j];
hober 4:5cde69a36d74 123 }
hober 4:5cde69a36d74 124 // footer values
hober 4:5cde69a36d74 125 if (hasFooter) {
hober 4:5cde69a36d74 126 for (int i = footerIndex, j = 0; j < footerLength; i++, j++)
hober 4:5cde69a36d74 127 bufferArray[i] = footerValue[j];
hober 4:5cde69a36d74 128 }
hober 4:5cde69a36d74 129 // compute xorValue
hober 4:5cde69a36d74 130 if (hasCheckXOR) {
hober 4:5cde69a36d74 131 byte xorValue=0;
hober 4:5cde69a36d74 132 for (int i = 0; i < bufferLength - 1; i++) {
hober 4:5cde69a36d74 133 xorValue ^= bufferArray[i];
hober 4:5cde69a36d74 134 }
hober 4:5cde69a36d74 135 bufferArray[bufferLength - 1] = xorValue;
hober 4:5cde69a36d74 136 }
hober 4:5cde69a36d74 137 }
hober 4:5cde69a36d74 138
hober 4:5cde69a36d74 139
hober 4:5cde69a36d74 140 // header
hober 4:5cde69a36d74 141 void ParseArray::enableHeader(int headerindex, int headerlength, string headervalue)
hober 4:5cde69a36d74 142 {
hober 4:5cde69a36d74 143 hasHeader = true;
hober 4:5cde69a36d74 144 headerIndex = headerindex;
hober 4:5cde69a36d74 145 headerLength = headerlength;
hober 4:5cde69a36d74 146 headerValueTmp = headervalue;
hober 4:5cde69a36d74 147 headerValue = stringToByte(headervalue, headerlength);
hober 4:5cde69a36d74 148 }
hober 4:5cde69a36d74 149 // footer
hober 4:5cde69a36d74 150 void ParseArray::enableFooter(int footerindex, int footerlength, string footervalue)
hober 4:5cde69a36d74 151 {
hober 4:5cde69a36d74 152 hasFooter = true;
hober 4:5cde69a36d74 153 footerIndex = footerindex;
hober 4:5cde69a36d74 154 footerLength = footerlength;
hober 4:5cde69a36d74 155 footerValueTmp = footervalue;
hober 4:5cde69a36d74 156 footerValue = stringToByte(footervalue, footerlength);
hober 4:5cde69a36d74 157 }
hober 4:5cde69a36d74 158 // crc
hober 4:5cde69a36d74 159 void ParseArray::enableCRC(int crcindex, int crclength)
hober 4:5cde69a36d74 160 {
hober 4:5cde69a36d74 161 hasCRC = true;
hober 4:5cde69a36d74 162 checkCRCIndex = crcindex;
hober 4:5cde69a36d74 163 checkCRCLength = crclength;
hober 4:5cde69a36d74 164 }
hober 4:5cde69a36d74 165 // xorValue
hober 4:5cde69a36d74 166 void ParseArray::enableCheckXOR(int xorIndex)
hober 4:5cde69a36d74 167 {
hober 4:5cde69a36d74 168 hasCheckXOR = true;
hober 4:5cde69a36d74 169 checkXORIndex = xorIndex;
hober 4:5cde69a36d74 170 }
hober 4:5cde69a36d74 171
hober 4:5cde69a36d74 172
hober 4:5cde69a36d74 173
hober 4:5cde69a36d74 174 // check Header
hober 4:5cde69a36d74 175 bool ParseArray::testHeader(int hindex)
hober 4:5cde69a36d74 176 {
hober 4:5cde69a36d74 177 int i;
hober 4:5cde69a36d74 178 for (i = 0; i < headerLength; i++) {
hober 4:5cde69a36d74 179 if (bufferArray[(hindex + i) % bufferLength] != headerValue[i])
hober 4:5cde69a36d74 180 return false;
hober 4:5cde69a36d74 181 }
hober 4:5cde69a36d74 182 return true;
hober 4:5cde69a36d74 183 }
hober 4:5cde69a36d74 184 // check Footer
hober 4:5cde69a36d74 185 bool ParseArray::testFooter(int findex)
hober 4:5cde69a36d74 186 {
hober 4:5cde69a36d74 187 int i;
hober 4:5cde69a36d74 188 for (i = 0; i < footerLength; i++) {
hober 4:5cde69a36d74 189 if (bufferArray[(findex + i) % bufferLength] != footerValue[i])
hober 4:5cde69a36d74 190 return false;
hober 4:5cde69a36d74 191 }
hober 4:5cde69a36d74 192 return true;
hober 4:5cde69a36d74 193 }
hober 4:5cde69a36d74 194 // test xorValue
hober 7:ee0569d49c52 195 bool ParseArray::testXOR(int hindex)// set xindex as headerIndex
hober 4:5cde69a36d74 196 {
hober 4:5cde69a36d74 197 int i = 0;
hober 4:5cde69a36d74 198 byte xorValue = 0 ;
hober 7:ee0569d49c52 199 for (i = hindex; i < hindex+length; i++) {
hober 4:5cde69a36d74 200 xorValue ^= bufferArray[i % bufferLength];
hober 4:5cde69a36d74 201 }
hober 4:5cde69a36d74 202 if (xorValue == 0)
hober 4:5cde69a36d74 203 return true;
hober 4:5cde69a36d74 204 else
hober 4:5cde69a36d74 205 return false;
hober 4:5cde69a36d74 206 }
hober 4:5cde69a36d74 207 // parsing
hober 4:5cde69a36d74 208 bool ParseArray::parse(int curIndex)
hober 4:5cde69a36d74 209 {
hober 4:5cde69a36d74 210 // shift curIndex to end
hober 4:5cde69a36d74 211 if (hasCheckXOR) {
hober 4:5cde69a36d74 212 curIndex -= checkXORIndex;
hober 4:5cde69a36d74 213 curIndex += bufferLength;
hober 4:5cde69a36d74 214 curIndex %= bufferLength;
hober 4:5cde69a36d74 215 } else if (hasFooter) {
hober 4:5cde69a36d74 216 curIndex -= footerIndex;
hober 4:5cde69a36d74 217 curIndex += bufferLength;
hober 4:5cde69a36d74 218 curIndex %= bufferLength;
hober 4:5cde69a36d74 219 }
hober 4:5cde69a36d74 220
hober 4:5cde69a36d74 221 curIndex %= bufferLength;
hober 4:5cde69a36d74 222
hober 4:5cde69a36d74 223 //
hober 4:5cde69a36d74 224 if (hasHeader) {
hober 4:5cde69a36d74 225 // test header
hober 4:5cde69a36d74 226 if (testHeader(curIndex)) {
hober 4:5cde69a36d74 227 // pass header check
hober 4:5cde69a36d74 228 } else // fail header
hober 4:5cde69a36d74 229 return false;
hober 4:5cde69a36d74 230 }
hober 4:5cde69a36d74 231 // test footer
hober 4:5cde69a36d74 232 if (hasFooter) {
hober 4:5cde69a36d74 233 if (testFooter((curIndex + (footerIndex - headerIndex)) % bufferLength)) {
hober 4:5cde69a36d74 234 // pass footer check
hober 4:5cde69a36d74 235 } else // fail footer
hober 4:5cde69a36d74 236 return false;
hober 4:5cde69a36d74 237 }
hober 4:5cde69a36d74 238 // test xorValue
hober 4:5cde69a36d74 239 if (hasCheckXOR) {
hober 7:ee0569d49c52 240 if (!testXOR(curIndex)) { // fail check
hober 7:ee0569d49c52 241 printf("XOR fail");
hober 4:5cde69a36d74 242 return false;
hober 7:ee0569d49c52 243 }
hober 4:5cde69a36d74 244 }
hober 4:5cde69a36d74 245 // pass all test
hober 4:5cde69a36d74 246 currentHeaderIndex = curIndex;
hober 4:5cde69a36d74 247 currentFooterIndex = (curIndex + (footerIndex - headerIndex));
hober 4:5cde69a36d74 248 return true;
hober 4:5cde69a36d74 249 }
hober 4:5cde69a36d74 250 // compute xor
hober 4:5cde69a36d74 251 byte ParseArray::computeXOR(byte t1[], int len)
hober 4:5cde69a36d74 252 {
hober 4:5cde69a36d74 253 byte XOR =0;
hober 4:5cde69a36d74 254 for(int i=0; i<len; i++) {
hober 4:5cde69a36d74 255 XOR=XOR^t1[i]; // xor
hober 4:5cde69a36d74 256 }
hober 4:5cde69a36d74 257 return XOR;
hober 4:5cde69a36d74 258 }
hober 4:5cde69a36d74 259
hober 4:5cde69a36d74 260