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/envelopetracker.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 "envelopetracker.h"
hober 8:33d34a775873 2 #include "mbed.h"
hober 8:33d34a775873 3
hober 8:33d34a775873 4 extern Serial pc;
hober 8:33d34a775873 5 extern DigitalOut mag_test;
hober 7:ee0569d49c52 6 EnvelopeTracker::EnvelopeTracker()
hober 7:ee0569d49c52 7 {
hober 7:ee0569d49c52 8 currentIndex = 0;
hober 7:ee0569d49c52 9 envelopeLength = 0;
hober 7:ee0569d49c52 10 envelopeDataLength = 0;
hober 7:ee0569d49c52 11 currentDataIndex = 0;
hober 7:ee0569d49c52 12 bufferTailIndex = -1;
hober 8:33d34a775873 13 envelopeListIndex = 0;
hober 8:33d34a775873 14 envelopeListLength = 100;
hober 8:33d34a775873 15 envelopeListIndexFront = -1;
hober 8:33d34a775873 16 envelopeList = new Envelope*[envelopeListLength];
hober 7:ee0569d49c52 17 }
hober 7:ee0569d49c52 18
hober 7:ee0569d49c52 19 void EnvelopeTracker::parse(char * buf, int bufLength)
hober 7:ee0569d49c52 20 {
hober 7:ee0569d49c52 21 int byteAmountToAddInBuffer = 0;
hober 8:33d34a775873 22 int bufOffset = 0;
hober 8:33d34a775873 23 char * data;
hober 8:33d34a775873 24 Envelope* newEnvelope;/*
hober 7:ee0569d49c52 25 int bufIndex;
hober 7:ee0569d49c52 26 if(bufferTailIndex < 0){
hober 7:ee0569d49c52 27 bufIndex = 0;
hober 7:ee0569d49c52 28 }
hober 7:ee0569d49c52 29 else if(bufferTailIndex<currentIndex){
hober 7:ee0569d49c52 30 bufIndex = -(bufferTailIndex + bufferLength - currentIndex)-1;
hober 7:ee0569d49c52 31 }
hober 7:ee0569d49c52 32 else{
hober 7:ee0569d49c52 33 bufIndex = -(bufferTailIndex - currentIndex)-1;
hober 7:ee0569d49c52 34 }*/
hober 7:ee0569d49c52 35 while(1){
hober 7:ee0569d49c52 36 byteAmountToAddInBuffer = (bufLength<bufferLength/2)?bufLength:bufferLength/2;
hober 7:ee0569d49c52 37 for(int i = 0; i < byteAmountToAddInBuffer; i++)
hober 7:ee0569d49c52 38 bufferArray[(i+bufferTailIndex+1)%bufferLength] = buf[i+bufOffset];
hober 7:ee0569d49c52 39 bufferTailIndex += byteAmountToAddInBuffer;
hober 7:ee0569d49c52 40 bufferTailIndex %= bufferLength;
hober 7:ee0569d49c52 41
hober 7:ee0569d49c52 42 int numToParse=(currentIndex>bufferTailIndex)?(bufferTailIndex + bufferLength - currentIndex):(bufferTailIndex - currentIndex);
hober 7:ee0569d49c52 43 for(;numToParse>=envelopeLength-1;numToParse--)
hober 7:ee0569d49c52 44 {
hober 7:ee0569d49c52 45 if(testHeader()&&testFooter()&&testXOR()){
hober 7:ee0569d49c52 46
hober 7:ee0569d49c52 47 // pass all test
hober 7:ee0569d49c52 48 currentHeaderIndex = currentIndex;
hober 7:ee0569d49c52 49 currentDataIndex = currentIndex + headerLength;
hober 7:ee0569d49c52 50
hober 8:33d34a775873 51 data = new char[envelopeDataLength];
hober 7:ee0569d49c52 52 for(int i = 0; i < envelopeDataLength; i++){
hober 7:ee0569d49c52 53 data[i] = bufferArray[(i+currentDataIndex)%bufferLength];
hober 7:ee0569d49c52 54 }
hober 8:33d34a775873 55 newEnvelope = &(envelope.setEnvelopeData(data, envelopeDataLength));
hober 8:33d34a775873 56 if(envelopeListIndexFront == -1) envelopeListIndex = 0;
hober 8:33d34a775873 57 envelopeList[envelopeListIndex] = newEnvelope;
hober 8:33d34a775873 58 envelopeListIndex++;
hober 8:33d34a775873 59 envelopeListIndex %= envelopeListLength;
hober 8:33d34a775873 60 if(envelopeListIndexFront == -1) envelopeListIndexFront = 0;
hober 7:ee0569d49c52 61 }
hober 7:ee0569d49c52 62 ++currentIndex %= bufferLength;
hober 7:ee0569d49c52 63
hober 7:ee0569d49c52 64 }
hober 7:ee0569d49c52 65 if(byteAmountToAddInBuffer != bufLength){
hober 7:ee0569d49c52 66 bufLength -= bufferLength/2;
hober 7:ee0569d49c52 67 bufOffset += bufferLength/2;
hober 7:ee0569d49c52 68 }
hober 7:ee0569d49c52 69 else break;
hober 7:ee0569d49c52 70 }
hober 7:ee0569d49c52 71 }
hober 7:ee0569d49c52 72
hober 7:ee0569d49c52 73 int EnvelopeTracker::bufferDataLength()
hober 7:ee0569d49c52 74 {
hober 7:ee0569d49c52 75 if(bufferTailIndex < 0) return 0;
hober 7:ee0569d49c52 76 else if(bufferTailIndex<currentIndex) return bufferTailIndex+bufferLength-currentIndex;
hober 7:ee0569d49c52 77 else return bufferTailIndex-currentIndex;
hober 7:ee0569d49c52 78
hober 7:ee0569d49c52 79 }
hober 7:ee0569d49c52 80
hober 7:ee0569d49c52 81 void EnvelopeTracker::resetBuffer()
hober 7:ee0569d49c52 82 {
hober 7:ee0569d49c52 83 currentIndex = 0;
hober 7:ee0569d49c52 84 bufferTailIndex = -1;
hober 7:ee0569d49c52 85 }
hober 7:ee0569d49c52 86
hober 7:ee0569d49c52 87 void EnvelopeTracker::setEnvelope(const Envelope &value)
hober 7:ee0569d49c52 88 {
hober 7:ee0569d49c52 89 envelope = value;
hober 8:33d34a775873 90 // printf("envelope assigned\n");
hober 7:ee0569d49c52 91 envelopeLength = envelope.length();
hober 7:ee0569d49c52 92 envelopeDataLength = envelope.getPayloadLength();
hober 7:ee0569d49c52 93 headerLength = envelope.getHeaderLength();
hober 7:ee0569d49c52 94 if(bufferLength<envelopeLength) bufferLength = 3*envelopeLength;
hober 7:ee0569d49c52 95 }
hober 7:ee0569d49c52 96
hober 7:ee0569d49c52 97 bool EnvelopeTracker::testHeader()
hober 7:ee0569d49c52 98 {
hober 7:ee0569d49c52 99 if(!envelope.getHasHeader()) return true; // no header to test
hober 7:ee0569d49c52 100 char * header = envelope.getHeader();
hober 7:ee0569d49c52 101 for(int i = 0; i < envelope.getHeaderLength(); i++)
hober 7:ee0569d49c52 102 if(header[i] != bufferArray[(i+currentIndex)%bufferLength]) return false;
hober 7:ee0569d49c52 103 return true;
hober 7:ee0569d49c52 104 }
hober 7:ee0569d49c52 105
hober 7:ee0569d49c52 106 bool EnvelopeTracker::testFooter()
hober 7:ee0569d49c52 107 {
hober 7:ee0569d49c52 108 if(!envelope.getHasFooter()) return true; // no footer to test
hober 7:ee0569d49c52 109 char * footer = envelope.getFooter();
hober 7:ee0569d49c52 110 int footerIndex = envelope.getFooterIndex();
hober 7:ee0569d49c52 111 for(int i = 0; i < envelope.getFooterLength(); i++)
hober 7:ee0569d49c52 112 if(footer[i] != bufferArray[(i+currentIndex+footerIndex)%bufferLength]){
hober 7:ee0569d49c52 113 return false;
hober 7:ee0569d49c52 114 }
hober 7:ee0569d49c52 115 return true;
hober 7:ee0569d49c52 116 }
hober 7:ee0569d49c52 117
hober 7:ee0569d49c52 118 bool EnvelopeTracker::testXOR()
hober 7:ee0569d49c52 119 {
hober 7:ee0569d49c52 120 if(!envelope.checkXOR()) return true; //no Xor to check
hober 7:ee0569d49c52 121 char x_or = 0;
hober 7:ee0569d49c52 122 for(int i = 0; i < envelopeLength; i++)
hober 7:ee0569d49c52 123 {
hober 7:ee0569d49c52 124 x_or ^= bufferArray[(i + currentIndex) % bufferLength];
hober 7:ee0569d49c52 125 }
hober 7:ee0569d49c52 126 return x_or == 0;
hober 7:ee0569d49c52 127 }
hober 7:ee0569d49c52 128
hober 7:ee0569d49c52 129 void EnvelopeTracker::setBufferLength(int value)
hober 7:ee0569d49c52 130 {
hober 7:ee0569d49c52 131 int dataLength = bufferDataLength();
hober 7:ee0569d49c52 132 if(value < dataLength || value < 2 * envelopeLength) return;
hober 7:ee0569d49c52 133 if(bufferArray == NULL){
hober 7:ee0569d49c52 134 bufferLength = value;
hober 7:ee0569d49c52 135 bufferArray = new char [bufferLength];
hober 7:ee0569d49c52 136 return;
hober 7:ee0569d49c52 137 }
hober 7:ee0569d49c52 138 char * tmp = new char [value];
hober 7:ee0569d49c52 139 for(int i = 0, j = currentIndex; i < dataLength; i++, j++)
hober 7:ee0569d49c52 140 {
hober 7:ee0569d49c52 141 tmp[i] = bufferArray[j%bufferLength];
hober 7:ee0569d49c52 142 }
hober 7:ee0569d49c52 143 bufferLength = value;
hober 7:ee0569d49c52 144 delete bufferArray;
hober 7:ee0569d49c52 145 bufferArray = tmp;
hober 7:ee0569d49c52 146 currentIndex = 0;
hober 7:ee0569d49c52 147 bufferTailIndex = dataLength;
hober 7:ee0569d49c52 148 }
hober 7:ee0569d49c52 149
hober 7:ee0569d49c52 150 EnvelopeTracker::~EnvelopeTracker()
hober 7:ee0569d49c52 151 {
hober 8:33d34a775873 152 if(bufferArray) delete bufferArray;
hober 7:ee0569d49c52 153 }
hober 7:ee0569d49c52 154
hober 7:ee0569d49c52 155