NagaokaRoboticsClub_mbedTeam / Mbed OS kinect_fep_rx_sample

a_team_fep_rx_from_kinect_sample

FEP2/FEP.cpp@0:338b7241e445, 2018-10-04 (annotated)

Committer:
skouki
Date:
Thu Oct 04 10:54:05 2018 +0000
Revision:
0:338b7241e445
v1;

Who changed what in which revision?

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