TFmini(Micro LiDAR Module) library tested on mbedos5.x
Dependents: 2018NHK_gakugaku_robo 2018NHK_gaku_ver2 2019NHK_A_sensor
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 }
Generated on Sun Jul 31 2022 07:00:52 by
1.7.2