kinect用
Diff: FEP.cpp
- Revision:
- 0:68470e808ae2
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/FEP.cpp Fri Oct 12 04:39:28 2018 +0000 @@ -0,0 +1,72 @@ +#include "FEP.h" + +FEP::FEP(PinName tx, PinName rx, uint8_t addr, int baud) : + RawSerial(tx, rx, baud) +{ + _address = addr; +} + +void FEP::startReceive(uint16_t dataSize) +{ + bufferSize = dataSize + 15; + bufferPoint = 0; + receivedBytes = 0; + buffer = new uint8_t[bufferSize]; + data = new uint8_t[dataSize]; + attach(callback(this, &FEP::receiveByte)); +} + +void FEP::getData(uint8_t data_[]) +{ + for(int i = 0; i < bufferSize - 15; i++) { + data_[i] = data[i]; + } +} + +void FEP::sendData(uint8_t data_[], uint8_t length) +{ + uint8_t checksum = 0x00; + + printf("@TBN%03d%03d", _address, length + 1); + for (int j = 0; j < length; j++) { + checksum += data_[j]; + putc(data_[j]); + } + putc(checksum); + putc('\r'); + putc('\n'); +} + +void FEP::receiveByte() +{ + buffer[bufferPoint % bufferSize] = getc(); + + if(bufferPoint != 0xff) { + ++bufferPoint; + } else { + bufferPoint = (255%bufferSize)+1; + } + + ++receivedBytes; + + if(receivedBytes >= bufferSize) checkData(); +} + +void FEP::checkData() +{ + for(int i = 0; i < bufferSize; i++) { + if(buffer[i % bufferSize] == FEP_HEADER0 && buffer[(i + 1) % bufferSize] == FEP_HEADER1 && buffer[(i + 2) % bufferSize] == FEP_HEADER2) { + uint8_t checksum = 0x00; + for(int j = 9; j < bufferSize - 6; j++) { + checksum += buffer[(i + j)% bufferSize]; + } + if(checksum == buffer[(i + bufferSize - 6)% bufferSize]) { + for(int j = 9; j < bufferSize - 6; j++) { + data[j - 9] = buffer[(i + j) % bufferSize]; + } + receivedBytes = 0; + return; + } + } + } +}