Created
January 14, 2020 12:41
-
-
Save bangonkali/f4a836bf255e95f09709dc4db2e8fb9b to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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