Skip to content

Add functions to set mode, range limit, and signal threshold #12

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

Add functions to set mode, range limit, and signal threshold #12

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
e5dd4bc
implement reading and writing mode
mgrau Jul 1, 2019
0acfbde
format using clang-format
mgrau Jul 1, 2019
d787b92
add functions to set RangeLimit and SignalThreshold
mgrau Sep 22, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
247 changes: 189 additions & 58 deletions src/TFMini.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
/*
Arduino driver for Benewake TFMini time-of-flight distance sensor.
by Peter Jansen (December 11/2017)
modified for EAWAG volaser (2019)
This code is open source software in the public domain.

THIS SOFTWARE IS PROVIDED ''AS IS'' AND ANY
Expand All @@ -21,61 +22,78 @@ derived from this software without specific prior written permission.
#include "TFMini.h"

// Constructor
TFMini::TFMini() {
TFMini::TFMini()
{
// Empty constructor
}


boolean TFMini::begin(Stream* _streamPtr) {
boolean TFMini::begin(Stream *_streamPtr)
{
// Store reference to stream/serial object
streamPtr = _streamPtr;

// Clear state
distance = -1;
strength = -1;
mode = -1;
state = READY;

// Set standard output mode
setStandardOutputMode();

return true;
}


// Public: The main function to measure distance.
uint16_t TFMini::getDistance() {
// Public: The main function to measure distance.
uint16_t TFMini::getDistance()
{
int numMeasurementAttempts = 0;
while (takeMeasurement() != 0) {
while (takeMeasurement() != 0)
{
numMeasurementAttempts += 1;
if (numMeasurementAttempts > TFMINI_MAX_MEASUREMENT_ATTEMPTS) {
Serial.println ("TF Mini error: too many measurement attempts");
Serial.println ("Last error:");
if (state == ERROR_SERIAL_NOHEADER) Serial.println("ERROR_SERIAL_NOHEADER");
if (state == ERROR_SERIAL_BADCHECKSUM) Serial.println("ERROR_SERIAL_BADCHECKSUM");
if (state == ERROR_SERIAL_TOOMANYTRIES) Serial.println("ERROR_SERIAL_TOOMANYTRIES");

if (numMeasurementAttempts > TFMINI_MAX_MEASUREMENT_ATTEMPTS)
{
Serial.println("TF Mini error: too many measurement attempts");
Serial.println("Last error:");
if (state == ERROR_SERIAL_NOHEADER)
Serial.println("ERROR_SERIAL_NOHEADER");
if (state == ERROR_SERIAL_BADCHECKSUM)
Serial.println("ERROR_SERIAL_BADCHECKSUM");
if (state == ERROR_SERIAL_TOOMANYTRIES)
Serial.println("ERROR_SERIAL_TOOMANYTRIES");

state = ERROR_SERIAL_TOOMANYTRIES;
distance = -1;
strength = -1;
return -1;
strength = -1;
return -1;
}
}

if (state == MEASUREMENT_OK) {
if (state == MEASUREMENT_OK)
{
return distance;
} else {
}
else
{
return -1;
}
}

// Public: Return the most recent signal strength measuremenet from the TF Mini
uint16_t TFMini::getRecentSignalStrength() {
uint16_t TFMini::getRecentSignalStrength()
{
return strength;
}

// Public: Return the measurement mode
uint8_t TFMini::getMode()
{
return mode;
}

// Private: Set the TF Mini into the correct measurement mode
void TFMini::setStandardOutputMode() {
void TFMini::setStandardOutputMode()
{
// Set to "standard" output mode (this is found in the debug documents)
streamPtr->write((uint8_t)0x42);
streamPtr->write((uint8_t)0x57);
Expand All @@ -88,7 +106,8 @@ void TFMini::setStandardOutputMode() {
}

// Set configuration mode
void TFMini::setConfigMode() {
void TFMini::setConfigMode()
{
// advanced parameter configuration mode
streamPtr->write((uint8_t)0x42);
streamPtr->write((uint8_t)0x57);
Expand All @@ -97,11 +116,27 @@ void TFMini::setConfigMode() {
streamPtr->write((uint8_t)0x00);
streamPtr->write((uint8_t)0x00);
streamPtr->write((uint8_t)0x01);
streamPtr->write((uint8_t)0x02);
streamPtr->write((uint8_t)0x02);
delay(100);
}

// Exit configuration mode
void TFMini::unsetConfigMode()
{
streamPtr->write((uint8_t)0x42);
streamPtr->write((uint8_t)0x57);
streamPtr->write((uint8_t)0x02);
streamPtr->write((uint8_t)0x00);
streamPtr->write((uint8_t)0x00);
streamPtr->write((uint8_t)0x00);
streamPtr->write((uint8_t)0x00);
streamPtr->write((uint8_t)0x02);
delay(100);
}

// Set single scan mode (external trigger)
void TFMini::setSingleScanMode() {
void TFMini::setSingleScanMode()
{
setConfigMode();
// setting trigger source to external
streamPtr->write((uint8_t)0x42);
Expand All @@ -112,11 +147,95 @@ void TFMini::setSingleScanMode() {
streamPtr->write((uint8_t)0x00);
streamPtr->write((uint8_t)0x00);
streamPtr->write((uint8_t)0x40);
delay(100);
unsetConfigMode();
}

// Set measurement mode
void TFMini::setMeasurementMode(MODE _mode)
{
setConfigMode();

// fix detection pattern
streamPtr->write((uint8_t)0x42);
streamPtr->write((uint8_t)0x57);
streamPtr->write((uint8_t)0x02);
streamPtr->write((uint8_t)0x00);
streamPtr->write((uint8_t)0x00);
streamPtr->write((uint8_t)0x00);
streamPtr->write((uint8_t)0x01); //0x00=unlocked, 0x01=locked
streamPtr->write((uint8_t)0x14);

delay(100);

// setting measurement mode
streamPtr->write((uint8_t)0x42);
streamPtr->write((uint8_t)0x57);
streamPtr->write((uint8_t)0x02);
streamPtr->write((uint8_t)0x00);
streamPtr->write((uint8_t)0x00);
streamPtr->write((uint8_t)0x00);
streamPtr->write((uint8_t)_mode);
streamPtr->write((uint8_t)0x11);

delay(100);

unsetConfigMode();
}

// Set maximum distance
void TFMini::setRangeLimit(uint16_t range) {
setConfigMode();

streamPtr->write((uint8_t)0x42);
streamPtr->write((uint8_t)0x57);
streamPtr->write((uint8_t)0x02);
streamPtr->write((uint8_t)0x00);
streamPtr->write((uint8_t)(range & 0xff));
streamPtr->write((uint8_t)((range >> 8) & 0xff));
streamPtr->write((uint8_t)(range > 0 ? 0x01 : 0x00)); // 0x00=range limit disabled, 0x01=enabled
streamPtr->write((uint8_t)0x19);

delay(100);

unsetConfigMode();
}

// Set maximum distance
void TFMini::setSignalThreshold(uint8_t min, uint16_t max) {
setConfigMode();

// Set minimum signal strength
streamPtr->write((uint8_t)0x42);
streamPtr->write((uint8_t)0x57);
streamPtr->write((uint8_t)0x02);
streamPtr->write((uint8_t)0x00);
streamPtr->write((uint8_t)min);
streamPtr->write((uint8_t)0x00);
streamPtr->write((uint8_t)0x00);
streamPtr->write((uint8_t)0x20);

delay(100);

// Set maximum signal strength
streamPtr->write((uint8_t)0x42);
streamPtr->write((uint8_t)0x57);
streamPtr->write((uint8_t)0x02);
streamPtr->write((uint8_t)0x00);
streamPtr->write((uint8_t)(max & 0xff));
streamPtr->write((uint8_t)((max >> 8) & 0xff));
streamPtr->write((uint8_t)0x1d); // distance to be returned if max signal is exceeded
streamPtr->write((uint8_t)0x21);

delay(100);

unsetConfigMode();
}

// Send external trigger
void TFMini::externalTrigger() {
setConfigMode();
void TFMini::externalTrigger()
{
setConfigMode();
// send trigger
streamPtr->write((uint8_t)0x42);
streamPtr->write((uint8_t)0x57);
Expand All @@ -126,84 +245,96 @@ void TFMini::externalTrigger() {
streamPtr->write((uint8_t)0x00);
streamPtr->write((uint8_t)0x00);
streamPtr->write((uint8_t)0x41);
delay(100);
unsetConfigMode();
}

// Private: Handles the low-level bits of communicating with the TFMini, and detecting some communication errors.
int TFMini::takeMeasurement() {
int TFMini::takeMeasurement()
{
int numCharsRead = 0;
uint8_t lastChar = 0x00;
uint8_t lastChar = 0x00;

// Step 1: Read the serial stream until we see the beginning of the TF Mini header, or we timeout reading too many characters.
while (1) {
while (1)
{

if (streamPtr->available()) {
if (streamPtr->available())
{
uint8_t curChar = streamPtr->read();

if ((lastChar == 0x59) && (curChar == 0x59)) {
if ((lastChar == 0x59) && (curChar == 0x59))
{
// Break to begin frame
break;

} else {
// We have not seen two 0x59's in a row -- store the current character and continue reading.
}
else
{
// We have not seen two 0x59's in a row -- store the current character and continue reading.
lastChar = curChar;
numCharsRead += 1;
}
numCharsRead += 1;
}
}

// Error detection: If we read more than X characters without finding a frame header, then it's likely there is an issue with
// the Serial connection, and we should timeout and throw an error.
if (numCharsRead > TFMINI_MAXBYTESBEFOREHEADER) {
// Error detection: If we read more than X characters without finding a frame header, then it's likely there is an issue with
// the Serial connection, and we should timeout and throw an error.
if (numCharsRead > TFMINI_MAXBYTESBEFOREHEADER)
{
state = ERROR_SERIAL_NOHEADER;
distance = -1;
strength = -1;
if (TFMINI_DEBUGMODE == 1) Serial.println("ERROR: no header");
return -1;
strength = -1;
mode = -1;
if (TFMINI_DEBUGMODE == 1)
Serial.println("ERROR: no header");
return -1;
}

}

// Step 2: Read one frame from the TFMini
uint8_t frame[TFMINI_FRAME_SIZE];

uint8_t checksum = 0x59 + 0x59;
for (int i=0; i<TFMINI_FRAME_SIZE; i++) {
for (int i = 0; i < TFMINI_FRAME_SIZE; i++)
{
// Read one character
while (!streamPtr->available()) {
while (!streamPtr->available())
{
// wait for a character to become available
}
}
frame[i] = streamPtr->read();

// Store running checksum
if (i < TFMINI_FRAME_SIZE-2) {
if (i < TFMINI_FRAME_SIZE - 2)
{
checksum += frame[i];
}
}

// Step 2A: Compare checksum
// Last byte in the frame is an 8-bit checksum
uint8_t checksumByte = frame[TFMINI_FRAME_SIZE-1];
if (checksum != checksumByte) {
// Last byte in the frame is an 8-bit checksum
uint8_t checksumByte = frame[TFMINI_FRAME_SIZE - 1];
if (checksum != checksumByte)
{
state = ERROR_SERIAL_BADCHECKSUM;
distance = -1;
strength = -1;
if (TFMINI_DEBUGMODE == 1) Serial.println("ERROR: bad checksum");
if (TFMINI_DEBUGMODE == 1)
Serial.println("ERROR: bad checksum");
return -1;
}


// Step 3: Interpret frame
uint16_t dist = (frame[1] << 8) + frame[0];
uint16_t st = (frame[3] << 8) + frame[2];
uint8_t reserved = frame[4];
uint8_t originalSignalQuality = frame[5];


// Step 4: Store values
distance = dist;
strength = st;
mode = reserved;
state = MEASUREMENT_OK;

// Return success
return 0;
return 0;
}

Loading