Shih-Ho Hsieh / Mbed 2 deprecated Motor_XYZ_UI_SPI_8mag_encoder_F746ZG

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

parse_array/envelopetracker.cpp@0:9e6e3dc903c0, 2018-12-22 (annotated)

Committer:
hober
Date:
Sat Dec 22 01:07:34 2018 +0000
Revision:
0:9e6e3dc903c0
20181222 backup

Who changed what in which revision?

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