TFmini(Micro LiDAR Module) library tested on mbedos5.x

Dependents:   2018NHK_gakugaku_robo 2018NHK_gaku_ver2 2019NHK_A_sensor

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers TFmini.cpp Source File

TFmini.cpp

00001 #include "TFmini.h"
00002 
00003 TFmini::TFmini(PinName serialTX, PinName serialRX) :
00004     RawSerial(serialTX, serialRX, TF_DEFAULT_BAUD)
00005 {
00006     attach(callback(this, &TFmini::receiveByte));
00007 }
00008 
00009 void TFmini::receiveByte()
00010 {
00011     buffer[bufferPoint % TF_SERIAL_BUFFER_SIZE] = getc();
00012  
00013     if(bufferPoint != 0xff) {
00014         ++bufferPoint;
00015     } else {
00016         bufferPoint = (255 % TF_SERIAL_BUFFER_SIZE)+1;
00017     }
00018  
00019     ++receivedBytes;
00020  
00021     if(receivedBytes >= TF_SERIAL_BUFFER_SIZE){
00022         checkData();
00023     }
00024 }
00025 void TFmini::checkData()
00026 {
00027     for(int i = 0; i < TF_SERIAL_BUFFER_SIZE; i++) {
00028         if(buffer[i % TF_SERIAL_BUFFER_SIZE] == TF_HEADER_FIRST_BYTE && buffer[(i + 1) % TF_SERIAL_BUFFER_SIZE] == TF_HEADER_SECOND_BYTE) {
00029             uint8_t checksum = 0x00;
00030             for(int j = 0; j < TF_SERIAL_BUFFER_SIZE - 1; j++) {
00031                 checksum += buffer[(i + j)% TF_SERIAL_BUFFER_SIZE];
00032             }
00033             if(checksum == buffer[(i + TF_SERIAL_BUFFER_SIZE - 1)% TF_SERIAL_BUFFER_SIZE]) {
00034                 for(int j = 0; j < TF_SERIAL_BUFFER_SIZE - 3; j++) {
00035                     data[j] = buffer[(i + 2 + j) % TF_SERIAL_BUFFER_SIZE];
00036                 }
00037                 receivedBytes = 0;
00038  
00039                 assemble();
00040                 return;
00041             }
00042         }
00043     }
00044 }
00045 
00046 void TFmini::assemble()
00047 {
00048     distance = (data[0] & 0xFF) | ((data[1] << 8) & 0xFF00);
00049     strength = (data[2] & 0xFF) | ((data[3] << 8) & 0xFF00);
00050     reserved = data[4];
00051     rawQual = data[5];
00052 }
00053 
00054 int TFmini::getDistance()
00055 {
00056     return distance;
00057 }
00058 
00059 int TFmini::getStrength()
00060 {
00061     return strength;
00062 }