naoki tanabe / fep2

kinect用

FEP.cpp@0:68470e808ae2, 2018-10-12 (annotated)

Committer:
tanabe2000
Date:
Fri Oct 12 04:39:28 2018 +0000
Revision:
0:68470e808ae2
kinect?

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tanabe2000 0:68470e808ae2 1 #include "FEP.h"
tanabe2000 0:68470e808ae2 2
tanabe2000 0:68470e808ae2 3 FEP::FEP(PinName tx, PinName rx, uint8_t addr, int baud) :
tanabe2000 0:68470e808ae2 4 RawSerial(tx, rx, baud)
tanabe2000 0:68470e808ae2 5 {
tanabe2000 0:68470e808ae2 6 _address = addr;
tanabe2000 0:68470e808ae2 7 }
tanabe2000 0:68470e808ae2 8
tanabe2000 0:68470e808ae2 9 void FEP::startReceive(uint16_t dataSize)
tanabe2000 0:68470e808ae2 10 {
tanabe2000 0:68470e808ae2 11 bufferSize = dataSize + 15;
tanabe2000 0:68470e808ae2 12 bufferPoint = 0;
tanabe2000 0:68470e808ae2 13 receivedBytes = 0;
tanabe2000 0:68470e808ae2 14 buffer = new uint8_t[bufferSize];
tanabe2000 0:68470e808ae2 15 data = new uint8_t[dataSize];
tanabe2000 0:68470e808ae2 16 attach(callback(this, &FEP::receiveByte));
tanabe2000 0:68470e808ae2 17 }
tanabe2000 0:68470e808ae2 18
tanabe2000 0:68470e808ae2 19 void FEP::getData(uint8_t data_[])
tanabe2000 0:68470e808ae2 20 {
tanabe2000 0:68470e808ae2 21 for(int i = 0; i < bufferSize - 15; i++) {
tanabe2000 0:68470e808ae2 22 data_[i] = data[i];
tanabe2000 0:68470e808ae2 23 }
tanabe2000 0:68470e808ae2 24 }
tanabe2000 0:68470e808ae2 25
tanabe2000 0:68470e808ae2 26 void FEP::sendData(uint8_t data_[], uint8_t length)
tanabe2000 0:68470e808ae2 27 {
tanabe2000 0:68470e808ae2 28 uint8_t checksum = 0x00;
tanabe2000 0:68470e808ae2 29
tanabe2000 0:68470e808ae2 30 printf("@TBN%03d%03d", _address, length + 1);
tanabe2000 0:68470e808ae2 31 for (int j = 0; j < length; j++) {
tanabe2000 0:68470e808ae2 32 checksum += data_[j];
tanabe2000 0:68470e808ae2 33 putc(data_[j]);
tanabe2000 0:68470e808ae2 34 }
tanabe2000 0:68470e808ae2 35 putc(checksum);
tanabe2000 0:68470e808ae2 36 putc('\r');
tanabe2000 0:68470e808ae2 37 putc('\n');
tanabe2000 0:68470e808ae2 38 }
tanabe2000 0:68470e808ae2 39
tanabe2000 0:68470e808ae2 40 void FEP::receiveByte()
tanabe2000 0:68470e808ae2 41 {
tanabe2000 0:68470e808ae2 42 buffer[bufferPoint % bufferSize] = getc();
tanabe2000 0:68470e808ae2 43
tanabe2000 0:68470e808ae2 44 if(bufferPoint != 0xff) {
tanabe2000 0:68470e808ae2 45 ++bufferPoint;
tanabe2000 0:68470e808ae2 46 } else {
tanabe2000 0:68470e808ae2 47 bufferPoint = (255%bufferSize)+1;
tanabe2000 0:68470e808ae2 48 }
tanabe2000 0:68470e808ae2 49
tanabe2000 0:68470e808ae2 50 ++receivedBytes;
tanabe2000 0:68470e808ae2 51
tanabe2000 0:68470e808ae2 52 if(receivedBytes >= bufferSize) checkData();
tanabe2000 0:68470e808ae2 53 }
tanabe2000 0:68470e808ae2 54
tanabe2000 0:68470e808ae2 55 void FEP::checkData()
tanabe2000 0:68470e808ae2 56 {
tanabe2000 0:68470e808ae2 57 for(int i = 0; i < bufferSize; i++) {
tanabe2000 0:68470e808ae2 58 if(buffer[i % bufferSize] == FEP_HEADER0 && buffer[(i + 1) % bufferSize] == FEP_HEADER1 && buffer[(i + 2) % bufferSize] == FEP_HEADER2) {
tanabe2000 0:68470e808ae2 59 uint8_t checksum = 0x00;
tanabe2000 0:68470e808ae2 60 for(int j = 9; j < bufferSize - 6; j++) {
tanabe2000 0:68470e808ae2 61 checksum += buffer[(i + j)% bufferSize];
tanabe2000 0:68470e808ae2 62 }
tanabe2000 0:68470e808ae2 63 if(checksum == buffer[(i + bufferSize - 6)% bufferSize]) {
tanabe2000 0:68470e808ae2 64 for(int j = 9; j < bufferSize - 6; j++) {
tanabe2000 0:68470e808ae2 65 data[j - 9] = buffer[(i + j) % bufferSize];
tanabe2000 0:68470e808ae2 66 }
tanabe2000 0:68470e808ae2 67 receivedBytes = 0;
tanabe2000 0:68470e808ae2 68 return;
tanabe2000 0:68470e808ae2 69 }
tanabe2000 0:68470e808ae2 70 }
tanabe2000 0:68470e808ae2 71 }
tanabe2000 0:68470e808ae2 72 }