Hi, i'm thinking of constructing a project using the tfmini lidar that like so many sonar projects ive seen use a radar screen to plot distance and angle to objects. Im using the tf mini over the ultrasonic sensor due to its larger range. One issue i cant solve is with tracking, i would like the system to denote if a target is moving closer or further away.
My thinking was that in the first sweep the lidar will log the distance and position of the objects in its area. Then on the following sweep of the area it will compare the change and thus determine a moving object from a stationary one and see if its moving closer or further away.
Any thought?
Thanks
That's exactly what I do.
The TFMini will work in single ping mode. I found that signaling the TFMini to ping, collecting the data, and moving to the next position is far better then allowing the TFMini to free run. I ping the TFMini 62 times per sweep. I store each ping response in an array that represents a ping position.
The TFMini will give 65K errors if you do not have solid connections from the mini to the uController.
A plastic geared servo will last about 250 hours of continuous use. So far my metal geared servo has lasted 10 months of continuous use.
That exactly what i want.
Can i ask how wide is your sweep? I was intending to use a 360 degree rotation
How do i go about saving the data in an array? Sorry quite new to arduino.
Thanks idahowalker
Much appreciated
Here is the h file that I am using:
/*
Arduino driver for Benewake TFMini time-of-flight distance sensor.
by Peter Jansen (December 11/2017)
This code is open source software in the public domain.
THIS SOFTWARE IS PROVIDED ''AS IS'' AND ANY
EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE AUTHOR(S) BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
The names of the contributors may not be used to endorse or promote products
derived from this software without specific prior written permission.
*/
#if (ARDUINO >= 100)
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
// Defines
#define TFMINI_BAUDRATE 115200
#define TFMINI_DEBUGMODE 0
// The frame size is nominally 9 characters, but we don't include the first two 0x59's marking the start of the frame
#define TFMINI_FRAME_SIZE 7
// Timeouts
#define TFMINI_MAXBYTESBEFOREHEADER 30
#define TFMINI_MAX_MEASUREMENT_ATTEMPTS 10
// States
#define READY 0
#define ERROR_SERIAL_NOHEADER 1
#define ERROR_SERIAL_BADCHECKSUM 2
#define ERROR_SERIAL_TOOMANYTRIES 3
#define MEASUREMENT_OK 10
//
// Driver Class Definition
//
class TFMini {
public:
TFMini(void);
// Configuration
boolean begin(Stream* _streamPtr);
void setSingleScanMode();
// Data collection
uint16_t getDistance();
uint16_t getRecentSignalStrength();
void externalTrigger();
private:
Stream* streamPtr;
int state;
uint16_t distance;
uint16_t strength;
// Low-level communication
void setStandardOutputMode();
void setConfigMode();
int takeMeasurement();
};
Here is the cpp file:
/*
Arduino driver for Benewake TFMini time-of-flight distance sensor.
by Peter Jansen (December 11/2017)
This code is open source software in the public domain.
THIS SOFTWARE IS PROVIDED ''AS IS'' AND ANY
EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE AUTHOR(S) BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
The names of the contributors may not be used to endorse or promote products
derived from this software without specific prior written permission.
*/
#include "TFMini.h"
// Constructor
TFMini::TFMini() {
// Empty constructor
}
boolean TFMini::begin(Stream* _streamPtr) {
// Store reference to stream/serial object
streamPtr = _streamPtr;
// Clear state
distance = -1;
strength = -1;
state = READY;
// Set standard output mode
setStandardOutputMode();
return true;
}
// Public: The main function to measure distance.
uint16_t TFMini::getDistance() {
int numMeasurementAttempts = 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");
// Serial.flush();
state = ERROR_SERIAL_TOOMANYTRIES;
distance = -1;
strength = -1;
return -1;
}
}
if (state == MEASUREMENT_OK) {
return distance;
} else {
return -1;
}
}
// Public: Return the most recent signal strength measuremenet from the TF Mini
uint16_t TFMini::getRecentSignalStrength() {
return strength;
}
// Private: Set the TF Mini into the correct measurement mode
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);
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);
streamPtr->write((uint8_t)0x06);
}
// Set configuration mode
void TFMini::setConfigMode() {
// advanced parameter configuration 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)0x01);
streamPtr->write((uint8_t)0x02);
}
// Set single scan mode (external trigger)
void TFMini::setSingleScanMode() {
setConfigMode();
// setting trigger source to external
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)0x40);
}
// Send external trigger
void TFMini::externalTrigger() {
setConfigMode();
// send trigger
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)0x41);
}
// Private: Handles the low-level bits of communicating with the TFMini, and detecting some communication errors.
int TFMini::takeMeasurement() {
int numCharsRead = 0;
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) {
if (streamPtr->available()) {
uint8_t curChar = streamPtr->read();
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.
lastChar = curChar;
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) {
state = ERROR_SERIAL_NOHEADER;
distance = -1;
strength = -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++) {
// Read one character
while (!streamPtr->available()) {
// wait for a character to become available
}
frame[i] = streamPtr->read();
// Store running checksum
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) {
state = ERROR_SERIAL_BADCHECKSUM;
distance = -1;
strength = -1;
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;
state = MEASUREMENT_OK;
// Return success
return 0;
}
Here is my servo sweep task:
void fLIDAR_ServoAspectChange( void * parameter )
{
// struct stu_XYZ_INFO pxXYZ;
// screen size 64, doing 62 scans for the range
int ServoMax = 1537;
int ServoMin = 1413;
int ServoIncrement = 2;
int ServoCurrent = 0;
while (1)
{
xEventGroupWaitBits (eg, evtLIDAR_ServoAspectChange, pdTRUE, pdTRUE, portMAX_DELAY) ;
if ( ServoCurrent == 0 )
{
xSemaphoreTake ( sema_LIDAR, xSemaphoreTicksToWait );
ledcWrite( Channel_LIDAR, usToTicks ( ServoMin ) );
xSemaphoreGive ( sema_LIDAR );
ServoCurrent = ServoMin;
x_LIDAR_INFO.CurrentCell = 1;
x_LIDAR_INFO.ServoSweepUp = true;
vTaskDelay( pdMS_TO_TICKS( ServoDelay40mS ) );
}
else
{
if ( x_LIDAR_INFO.ServoSweepUp )
{
ServoCurrent += ServoIncrement; // set next servo position to torque to
x_LIDAR_INFO.CurrentCell += 1;
xSemaphoreTake ( sema_LIDAR, xSemaphoreTicksToWait );
ledcWrite( Channel_LIDAR, usToTicks ( ServoCurrent ) );
xSemaphoreGive ( sema_LIDAR );
if ( ServoCurrent >= ServoMax )
{
x_LIDAR_INFO.ServoSweepUp = false; // toggle servo direction
//
xSemaphoreTake( sema_LIDAR_INFO, xSemaphoreTicksToWait );
xQueueOverwrite( xQ_LIDAR_INFO, (void *) &x_LIDAR_INFO );
xSemaphoreGive ( sema_LIDAR_INFO );
//
xSemaphoreTake( sema_LIDAR_FOR_ALARM, xSemaphoreTicksToWait );
xQueueOverwrite ( xQ_LIDAR_FOR_ALARM, (void *) &x_LIDAR_INFO );
xSemaphoreGive( sema_LIDAR_FOR_ALARM );
//
xEventGroupSetBits( eg, evtSendSerialToBrain );
}
} // if ( ServoSweepUp )
else // servo sweep down
{
ServoCurrent -= ServoIncrement;
x_LIDAR_INFO.CurrentCell -= 1; // set next cell position
xSemaphoreTake ( sema_LIDAR, xSemaphoreTicksToWait );
ledcWrite( Channel_LIDAR, usToTicks ( ServoCurrent ) );
xSemaphoreGive ( sema_LIDAR );
// has sweep completed
if ( ServoCurrent <= ServoMin )
{
x_LIDAR_INFO.ServoSweepUp = true; // toggle servo direction
//
xSemaphoreTake( sema_LIDAR_INFO, xSemaphoreTicksToWait );
xQueueOverwrite( xQ_LIDAR_INFO, (void *) &x_LIDAR_INFO );
xSemaphoreGive ( sema_LIDAR_INFO );
xSemaphoreTake( sema_LIDAR_FOR_ALARM, xSemaphoreTicksToWait );
xQueueOverwrite ( xQ_LIDAR_FOR_ALARM, (void *) &x_LIDAR_INFO );
xSemaphoreGive( sema_LIDAR_FOR_ALARM );
//
xEventGroupSetBits( eg, evtSendSerialToBrain );
} // if ( ServoCurrent <= ServoMin )
} // else // servo sweep down
} //
vTaskDelay( ServoDelay12mS );
xEventGroupSetBits( eg, evtDoLIDAR );
}
vTaskDelete( NULL );
}
My sweep range is between 1537uS and 1413uSc and the servo sweep increment is 2uS.
Here is the LIDAR ping task:
void fDoLIDAR( void * pvParameters )
{
int iLIDAR_Distance = 0;
while (1)
{
xEventGroupWaitBits (eg, evtDoLIDAR, pdTRUE, pdTRUE, portMAX_DELAY) ;
tfmini.externalTrigger();
iLIDAR_Distance = tfmini.getDistance();
if ( iLIDAR_Distance > LIDAR_Max_Distance )
{
Serial.print ( " TFMini distance issue " );
Serial.println ( iLIDAR_Distance );
}
xSemaphoreTake( sema_LIDAR_INFO, xSemaphoreTicksToWait );
x_LIDAR_INFO.Range[x_LIDAR_INFO.CurrentCell] = iLIDAR_Distance;
xSemaphoreGive( sema_LIDAR_INFO );
xEventGroupSetBits( eg, evtLIDAR_ServoAspectChange );
}
vTaskDelete( NULL );
}
You might notice that the servo aspect change task does its thing then fires off the lidar ping task, which does its thing and then fires off the servo sweep task. I found that a servo settle delay of then 12uS works good.
Here is where I place a single ping into the array: x_LIDAR_INFO.Range[x_LIDAR_INFO.CurrentCell] = iLIDAR_Distance;
The array is bundles up at the end of each sweep. forwards and backwards, and sent to another uController for image processing.
My TFMini sits on an X/Y platform that remains level.
Hello,
Have you saved the main file, servo sweep and lidar ping as separate cpp files under the same header one?
I noticed that your servo is stepped with time delays. With a range of 1537uS and 1413uS as you said, roughly what angle of rotation is that?
Many Thanks
I appreciate the quick response and useful code
The servo sweep distance is 180 degrees at 2500uS. The servo pulse width range is from 500uS for 0 degrees to 2500uS for 180 degrees; 1500uS is 90 degrees. If you put 2000 (2500-500) into your calculator and then press the / and then enter 180, and then select the = sign, you will get a number that represents uS per degree.
A servo needs time to move from one position to the next. Even though my increments are 2uS, I found that giving the metal geared servo less time, than 12uS, causes issues.
The h file is saved as an h file and the h file is saved as a cpp file.
Okay thanks, Ill give it a go with my setup
Cheers