Skip to content

Instantly share code, notes, and snippets.

@bangonkali
Created January 14, 2020 12:41
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save bangonkali/f4a836bf255e95f09709dc4db2e8fb9b to your computer and use it in GitHub Desktop.
Save bangonkali/f4a836bf255e95f09709dc4db2e8fb9b to your computer and use it in GitHub Desktop.
#include "SoftwareSerial.h"
SoftwareSerial swSer1;
void setup() {
// Debug serial
Serial.begin( 115200 );
// Serial connected to LIDAR sensor
// Serial.begin( 115200 );
swSer1.begin(115200, SWSERIAL_8N1, 33, 32, false, 256);
swSer1.enableIntTx(true);
delay( 1000 );
Serial.println( "Starting..." );
}
void loop() {
// If a byte arrives from debug Serial, perform a readings per seconds test
if ( swSer1.available() > 0 ) {
speedTest();
}
else {
// Perform one distance reading and show it on Serial
unsigned int distance = readLIDAR( 2000 );
if ( distance > 0 ) {
Serial.printf( "Distance (cm): %d\n", distance );
}
else {
Serial.println( "Timeout reading LIDAR" );
}
}
}
void speedTest() {
while ( swSer1.available() > 0 ) {
swSer1.read();
}
Serial.printf( "\n\nPerforming speed test...\n" );
long t0 = millis();
#define NUM_READINGS 1000
long accum = 0;
for ( int i = 0; i < NUM_READINGS; i++ ) {
accum += readLIDAR( 2000 );
}
long t1 = millis();
float readingsPerSecond = NUM_READINGS * 1000.0f / ( t1 - t0 );
float meanDistance = ((float)accum) / NUM_READINGS;
Serial.println( "\n\nSpeed test:" );
Serial.printf( "%f readings per second.\n", readingsPerSecond );
Serial.printf( "%f mean read distance.\n", meanDistance );
Serial.println( "\n\nHit another key to continue reading the sensor distance." );
while ( swSer1.available() == 0 ) {
delay( 10 );
}
while ( swSer1.available() > 0 ) {
swSer1.read();
}
}
/*
* This function reads the Serial until a valid packet is found or timeout passed.
* Param timeout: Timeout in milliseconds.
* Returns distance in cm or 0 if timeout happened.
*/
unsigned int readLIDAR( long timeout ) {
unsigned char readBuffer[ 9 ];
long t0 = millis();
while ( swSer1.available() < 9 ) {
if ( millis() - t0 > timeout ) {
// Timeout
return 0;
}
delay( 10 );
}
for ( int i = 0; i < 9; i++ ) {
readBuffer[ i ] = swSer1.read();
}
while ( ! detectFrame( readBuffer ) ) {
if ( millis() - t0 > timeout ) {
// Timeout
return 0;
}
while ( swSer1.available() == 0 ) {
delay( 10 );
}
for ( int i = 0; i < 8; i++ ) {
readBuffer[ i ] = readBuffer[ i + 1 ];
}
readBuffer[ 8 ] = swSer1.read();
}
// Distance is in bytes 2 and 3 of the 9 byte frame.
unsigned int distance = ( (unsigned int)( readBuffer[ 2 ] ) ) |
( ( (unsigned int)( readBuffer[ 3 ] ) ) << 8 );
return distance;
}
bool detectFrame( unsigned char *readBuffer ) {
return readBuffer[ 0 ] == 0x59 &&
readBuffer[ 1 ] == 0x59 &&
(unsigned char)(
0x59 +
0x59 +
readBuffer[ 2 ] +
readBuffer[ 3 ] +
readBuffer[ 4 ] +
readBuffer[ 5 ] +
readBuffer[ 6 ] +
readBuffer[ 7 ]
) == readBuffer[ 8 ];
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment