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/envelope.cpp@23:eede1fe4494b, 2018-08-15 (annotated)

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hober 7:ee0569d49c52 1 #include "envelope.h"
hober 8:33d34a775873 2 //#include "mbed.h"
hober 7:ee0569d49c52 3
hober 7:ee0569d49c52 4 Envelope::Envelope()
hober 7:ee0569d49c52 5 {
hober 7:ee0569d49c52 6 hasCheckXOR = false;
hober 7:ee0569d49c52 7 hasCRC = false;
hober 7:ee0569d49c52 8 hasFooter = false;
hober 7:ee0569d49c52 9 hasHeader = false;
hober 7:ee0569d49c52 10 payload = NULL;
hober 7:ee0569d49c52 11 //header
hober 7:ee0569d49c52 12 hasHeader = false;
hober 7:ee0569d49c52 13 headerIndex = 0;
hober 7:ee0569d49c52 14 headerValue = NULL;
hober 7:ee0569d49c52 15 headerLength = 0;
hober 7:ee0569d49c52 16
hober 7:ee0569d49c52 17 // Footer
hober 8:33d34a775873 18 hasFooter = false;
hober 7:ee0569d49c52 19 footerIndex = 0;
hober 7:ee0569d49c52 20 footerValue = NULL;
hober 7:ee0569d49c52 21 footerLength = 0;
hober 7:ee0569d49c52 22
hober 7:ee0569d49c52 23 // check
hober 7:ee0569d49c52 24 // CRC
hober 7:ee0569d49c52 25 hasCRC = false;
hober 7:ee0569d49c52 26 checkCRCIndex = 0;
hober 7:ee0569d49c52 27 checkCRCLength = 0;
hober 7:ee0569d49c52 28 // check xor
hober 7:ee0569d49c52 29 hasCheckXOR = false;
hober 7:ee0569d49c52 30 checkXORIndex = 0;
hober 7:ee0569d49c52 31
hober 8:33d34a775873 32 payload = NULL;
hober 8:33d34a775873 33 payloadLength = 0;
hober 7:ee0569d49c52 34 checkSumStart=0;
hober 8:33d34a775873 35 envelopeArray = NULL;
hober 7:ee0569d49c52 36 }
hober 7:ee0569d49c52 37
hober 7:ee0569d49c52 38 Envelope::Envelope(const Envelope &en)
hober 7:ee0569d49c52 39 {
hober 7:ee0569d49c52 40 *this = en;
hober 7:ee0569d49c52 41 }
hober 7:ee0569d49c52 42
hober 8:33d34a775873 43 Envelope& Envelope::setEnvelopeData(const char *buf, int bufLength, int checkSumStart)
hober 7:ee0569d49c52 44 {
hober 8:33d34a775873 45 if(payload == NULL) payload = new char [payloadLength];
hober 8:33d34a775873 46 for(int i = 0; i < payloadLength; i++) {
hober 7:ee0569d49c52 47 if(i < bufLength) payload[i] = buf[i];
hober 7:ee0569d49c52 48 else payload[i] = 0;
hober 7:ee0569d49c52 49 }
hober 7:ee0569d49c52 50 this->checkSumStart=checkSumStart;
hober 7:ee0569d49c52 51 return *this;
hober 7:ee0569d49c52 52 }
hober 7:ee0569d49c52 53
hober 8:33d34a775873 54 Envelope& Envelope::setEnvelopeDataAdjustFooterIndex(const char *buf, int bufLength, int checkSumStart)
hober 7:ee0569d49c52 55 {
hober 8:33d34a775873 56 if(payload) delete payload;
hober 8:33d34a775873 57 payload = new char [bufLength];
hober 7:ee0569d49c52 58 for(int i = 0; i < bufLength; i++) payload[i] = buf[i];
hober 7:ee0569d49c52 59 this->checkSumStart=checkSumStart;
hober 7:ee0569d49c52 60 return *this;
hober 7:ee0569d49c52 61 }
hober 7:ee0569d49c52 62
hober 7:ee0569d49c52 63 void Envelope::enableHeader(std::string headervalue, int headerindex)
hober 7:ee0569d49c52 64 {
hober 7:ee0569d49c52 65 hasHeader = true;
hober 7:ee0569d49c52 66 headerIndex = headerindex;
hober 7:ee0569d49c52 67 headerLength = headervalue.length();
hober 8:33d34a775873 68 if(headerValue) delete headerValue;
hober 7:ee0569d49c52 69 headerValue = new char [headerLength];
hober 7:ee0569d49c52 70 for(int i = 0; i < headerLength; i++) headerValue[i] = headervalue[i];
hober 7:ee0569d49c52 71 }
hober 7:ee0569d49c52 72
hober 7:ee0569d49c52 73 void Envelope::enableFooter(std::string footervalue, int footerindex)
hober 7:ee0569d49c52 74 {
hober 7:ee0569d49c52 75 hasFooter = true;
hober 7:ee0569d49c52 76 footerIndex = footerindex;
hober 7:ee0569d49c52 77 footerLength = footervalue.length();
hober 8:33d34a775873 78 if(footerValue) delete footerValue;
hober 7:ee0569d49c52 79 footerValue = new char [footerLength];
hober 7:ee0569d49c52 80 for(int i = 0; i < footerLength; i++) footerValue[i] = footervalue[i];
hober 7:ee0569d49c52 81 payloadLength = footerindex - headerLength;
hober 8:33d34a775873 82 envelopeArray = new char[length()];
hober 7:ee0569d49c52 83 }
hober 7:ee0569d49c52 84
hober 7:ee0569d49c52 85 void Envelope::enableCheckXOR(int xorIndex)
hober 7:ee0569d49c52 86 {
hober 7:ee0569d49c52 87 hasCheckXOR = true;
hober 7:ee0569d49c52 88 checkXORIndex = xorIndex;
hober 7:ee0569d49c52 89 }
hober 7:ee0569d49c52 90
hober 7:ee0569d49c52 91 char * Envelope::getEnvelopeArray()
hober 7:ee0569d49c52 92 {
hober 7:ee0569d49c52 93 int envelopeLength = length();
hober 8:33d34a775873 94 int i = 0;
hober 8:33d34a775873 95 if(hasHeader) i = appendData(envelopeArray, i, headerValue, headerLength);
hober 8:33d34a775873 96 i = appendData(envelopeArray, i, payload, payloadLength);
hober 8:33d34a775873 97 if(hasFooter) i = appendData(envelopeArray, i, footerValue, footerLength);
hober 8:33d34a775873 98 if(hasCheckXOR) envelopeArray[i] = generateXor(envelopeArray, envelopeLength-1);
hober 8:33d34a775873 99 return envelopeArray;
hober 7:ee0569d49c52 100 }
hober 7:ee0569d49c52 101
hober 7:ee0569d49c52 102 char Envelope::generateXor(char *input, int length)
hober 7:ee0569d49c52 103 {
hober 7:ee0569d49c52 104 Xor = 0;
hober 7:ee0569d49c52 105 for(int i = checkSumStart; i < length; i++) Xor ^= input[i];
hober 7:ee0569d49c52 106 return Xor;
hober 7:ee0569d49c52 107 }
hober 7:ee0569d49c52 108
hober 7:ee0569d49c52 109 int Envelope::appendData(char *toAppend, int startIndex, char *data, int dataLength)
hober 7:ee0569d49c52 110 {
hober 7:ee0569d49c52 111 if(toAppend == NULL || data == NULL) return -1; // error
hober 7:ee0569d49c52 112 for(int i = 0; i < dataLength; i++) toAppend[ i + startIndex ] = data[i];
hober 7:ee0569d49c52 113 return startIndex + dataLength;
hober 7:ee0569d49c52 114 }
hober 7:ee0569d49c52 115
hober 7:ee0569d49c52 116 bool Envelope::getHasFooter() const
hober 7:ee0569d49c52 117 {
hober 7:ee0569d49c52 118 return hasFooter;
hober 7:ee0569d49c52 119 }
hober 7:ee0569d49c52 120
hober 7:ee0569d49c52 121 Envelope::~Envelope()
hober 7:ee0569d49c52 122 {
hober 8:33d34a775873 123 if(headerValue) delete headerValue;
hober 8:33d34a775873 124 if(footerValue) delete footerValue;
hober 8:33d34a775873 125 if(payload) delete payload;
hober 8:33d34a775873 126 if(envelopeArray) delete envelopeArray;
hober 7:ee0569d49c52 127 }
hober 7:ee0569d49c52 128
hober 7:ee0569d49c52 129 bool Envelope::getHasHeader() const
hober 7:ee0569d49c52 130 {
hober 7:ee0569d49c52 131 return hasHeader;
hober 7:ee0569d49c52 132 }
hober 7:ee0569d49c52 133
hober 7:ee0569d49c52 134 int Envelope::getFooterLength() const
hober 7:ee0569d49c52 135 {
hober 7:ee0569d49c52 136 return footerLength;
hober 7:ee0569d49c52 137 }
hober 7:ee0569d49c52 138
hober 7:ee0569d49c52 139 Envelope &Envelope::operator=(const Envelope & en)
hober 7:ee0569d49c52 140 {
hober 7:ee0569d49c52 141 hasHeader = en.hasHeader;
hober 7:ee0569d49c52 142 headerLength = en.headerLength;
hober 7:ee0569d49c52 143 headerIndex = en.headerIndex;
hober 8:33d34a775873 144 if(headerValue) delete headerValue;
hober 7:ee0569d49c52 145 headerValue = new char[headerLength];
hober 7:ee0569d49c52 146 appendData(headerValue, 0, en.headerValue, headerLength);
hober 8:33d34a775873 147 // printf("header replication pass\n");
hober 7:ee0569d49c52 148
hober 7:ee0569d49c52 149 hasFooter = en.hasFooter;
hober 7:ee0569d49c52 150 footerLength = en.footerLength;
hober 7:ee0569d49c52 151 footerIndex = en.footerIndex;
hober 8:33d34a775873 152 if(footerValue) delete footerValue;
hober 7:ee0569d49c52 153 footerValue = new char[footerLength];
hober 7:ee0569d49c52 154 appendData(footerValue, 0, en.footerValue, footerLength);
hober 8:33d34a775873 155 // printf("Footer replication pass\n");
hober 7:ee0569d49c52 156
hober 7:ee0569d49c52 157 hasCheckXOR = en.hasCheckXOR;
hober 7:ee0569d49c52 158 checkCRCIndex = en.checkXORIndex;
hober 7:ee0569d49c52 159 Xor = en.Xor;
hober 7:ee0569d49c52 160
hober 7:ee0569d49c52 161 payloadLength = en.payloadLength;
hober 8:33d34a775873 162 if(payload) delete payload;
hober 7:ee0569d49c52 163 payload = new char[payloadLength];
hober 7:ee0569d49c52 164 appendData(payload, 0, en.payload, payloadLength);
hober 8:33d34a775873 165 // printf("payload replication pass\n");
hober 7:ee0569d49c52 166
hober 7:ee0569d49c52 167 checkSumStart = en.checkSumStart;
hober 8:33d34a775873 168 return *this;
hober 7:ee0569d49c52 169 }
hober 7:ee0569d49c52 170
hober 7:ee0569d49c52 171 int Envelope::getHeaderLength() const
hober 7:ee0569d49c52 172 {
hober 7:ee0569d49c52 173 return headerLength;
hober 7:ee0569d49c52 174 }
hober 7:ee0569d49c52 175
hober 7:ee0569d49c52 176 int Envelope::getPayloadLength() const
hober 7:ee0569d49c52 177 {
hober 7:ee0569d49c52 178 return payloadLength;
hober 7:ee0569d49c52 179 }
hober 7:ee0569d49c52 180
hober 7:ee0569d49c52 181 char * Envelope::getPayload() const
hober 7:ee0569d49c52 182 {
hober 7:ee0569d49c52 183 return payload;
hober 7:ee0569d49c52 184 }
hober 7:ee0569d49c52 185
hober 7:ee0569d49c52 186 int Envelope::length() const
hober 7:ee0569d49c52 187 {
hober 7:ee0569d49c52 188 int envelopeLength = payloadLength;
hober 7:ee0569d49c52 189 if(hasHeader) envelopeLength += headerLength;
hober 7:ee0569d49c52 190 if(hasFooter) envelopeLength += footerLength;
hober 7:ee0569d49c52 191 if(hasCheckXOR) envelopeLength++;
hober 7:ee0569d49c52 192 return envelopeLength;
hober 7:ee0569d49c52 193 }
hober 7:ee0569d49c52 194
hober 7:ee0569d49c52 195 bool Envelope::checkXOR() const
hober 7:ee0569d49c52 196 {
hober 7:ee0569d49c52 197 return hasCheckXOR;
hober 7:ee0569d49c52 198 }
hober 7:ee0569d49c52 199
hober 7:ee0569d49c52 200 int Envelope::getFooterIndex() const
hober 7:ee0569d49c52 201 {
hober 7:ee0569d49c52 202 return footerIndex;
hober 7:ee0569d49c52 203 }
hober 7:ee0569d49c52 204
hober 7:ee0569d49c52 205 int Envelope::getHeaderIndex() const
hober 7:ee0569d49c52 206 {
hober 7:ee0569d49c52 207 return headerIndex;
hober 7:ee0569d49c52 208 }
hober 7:ee0569d49c52 209
hober 7:ee0569d49c52 210 char * Envelope::getFooter() const
hober 7:ee0569d49c52 211 {
hober 7:ee0569d49c52 212 return footerValue;
hober 7:ee0569d49c52 213 }
hober 7:ee0569d49c52 214
hober 7:ee0569d49c52 215 char * Envelope::getHeader() const
hober 7:ee0569d49c52 216 {
hober 7:ee0569d49c52 217 return headerValue;
hober 7:ee0569d49c52 218 }
hober 7:ee0569d49c52 219