Taking RPLIDAR Out for a Spin (Part 1)

Command Interface

RPLIDAR-A1 is 360-degree laser range scanner that uses a laser triangulation ranging principle to calculate distance. In this article, Jeff examines this device and its development package. He steps you through his project using RPLIDAR, detailing his creation of a simple command interface for the device.

The RPLIDAR-A1’s development package contains the laser head, a TTL-to-USB dongle and connecting cables. You can plug it into your PC and—as long as your USB port isn’t current limited—it will power up without a separate supply. Start-up current is around 600mA, but it scans at around 400mA, so most powered USB ports can handle it. Physically, the unit is about the size of your fist.

SAFETY AND SCOPE
The RPLIDAR A1 system use a low-power (less than 5mW) modulated-pulse infrared laser as its light source. The laser’s short emission time frame assures Class I laser safety standards, making it safe for both humans and pets. Although you won’t need to wear special glasses to use it, you should treat it as you would any potentially dangerous piece of equipment.

Figure 1 gives you a good look at how this laser system is set up. Note that the motor is driving the laser head with a drive belt—similar to many old turntables or disk drives. RPLIDAR emits a modulated infrared laser signal, which is then reflected by the object to be detected. The returning signal is sampled by a Vision Acquisition System (VSC). The VSC comprises an embedded DSP to process the sample data and output distance and angle values. The ranging scanner system is mounted on a spinning rotor with a built-in angular encoding system.

Multiple distance scans are performed during each rotation. The motor defaults to 5.5rps, but can operate from 1-10rps. Scans begin when requested with a single point sample based on angle of return, processing and communications timing. A new sample begins only after the previous one is completed (or times out) up to a maximum of approximately 8,000Hz. It has accuracies of less than 1% of the distance and greater than 1 degree of angle. RPLIDAR-A1 (Rev5) can measure distances up to 12m (40′) and down to 150mm (6″).

COMMUNICATIONS
I mentioned earlier that the development kit included a TTL-to-USB dongle. I’ll want to communicate via TTL, so I won’t be using that. The designers used a 7-pin Molex connector (#0022035075) for the TTL output of the interface board. This is a 2.54mm (0.1″) pitch Mini-SPOX system. The interface board just translates the separate electronics and motor connectors into a single connector. If you want to connect directly to those, you’ll need to use a Molex (#538-53048-0450) & (#538- 53048-0310), which is a 1.25mm (0.05″) pitch PicoBlade system.

You can use the included dongle if you wish to connect with your PC and talk to it—using the API via a Windows application—or simply a serial terminal program. The software development kit that comes with it has a few examples that will give you a head start on using RPLIDAR, but I’m not going to use those. I will use the API document “Interface Protocol and Application Notes” for this discussion.

Note that in Figure 2 the protective cap was removed from the rotating turret. You can see the laser on the right, receiver on the top center and the vertical circuit board that does all of the control and computations. The turret spins via the belted DC motor on the left. Getting power and communication to and from a spinning turret is an engineering feat. You can see evidence of the two coils that wirelessly pass power from the stationary base to the rotating turret in Figure 3. With the turret circuit board removed, note the SMT (surface mount technology) devices in the center of the hollow shaft. Communications are sent via LED transmitter/receiver pair, eliminating the need for any brushes.

After I gained some confidence in the communications, I started by running the serial terminal application RealTerm on my PC to invoke some of the API commands. I decided to talk TTL directly to my Arduino Mega, and set aside the dongle. The RPLIDAR uses 115200 baud, 8N1 serial protocol, so it was easy to set up “Serial1” on the Mega for this purpose. This still leaves “Serial” console for programming and debugging. Let’s look at the API offered by RPLIDAR.

Requests take the form of a packet of data, as shown in Table 1. The simplest is a 2-byte packet, which contains the Start byte (used in all request packets) and a Command value. Without a payload count and payload data, the checksum is not required. Let’s look at a simple command like STOP. To send a STOP packet, just send the 2 bytes 0xA5 and 0x25. If not already idle, the laser diode and the measurement system will be disabled, and the idle state will be entered.

There is no response by the RPLIDAR to this request, so it doesn’t give us any feedback. So, let’s look at the RESET command. While this command also states that it has no response, it actually does. It’s a sign-on power-up message (which was acknowledged by a communication with Slamtec). This command is the 2-byte sequence 0xA5 and 0x40. When sent, RPLIDAR will respond with some ASCII text similar to this:

RP LIDAR System.
Firmware Ver 1.27 - rc8, HW Ver 5
Model: 18

This readable text is not the norm and does not follow the response format of any of the commands that support a response. Responses have their own packet format of data, shown in Table 2.

THE SCAN COMMAND
I found the 4-byte Data Response Length and Send Mode really confusing. Let’s take a look at the SCAN command to dissect this. The complete response packet is as follows: 0xA5, 0x5A, 0x05, 0x00, 0x00, 0x40, 0x81. The 4-byte Data Response Length and Send Mode for this command is 0x05, 0x00, 0x00, 0x40. First the 4-byte value is sent little-endian. That means the value is actually 0x40000005 or in binary 01000000 00000000 00000000 00000101. The Data Response Length is the last 30 bits of this—000000 00000000 0000000 00000101—or just 101 = 5. The most significant (MS) 2 bits of the 32-bit value is 01 or 1. According to the datasheet, presently there are only two possible Send Modes, 0 and 1, where 0=a single packet, and 1=multiple packets. In this case, the response will not stop at this response packet.

The last byte is a Data Type and can be used to identify the format you will be looking for in the next packet (remember 1=multiple packets). We know that, in this case, the following packet will have 5 bytes. While every command uses the response packet format, additional packets will have a unique format (DataType) used only for this command. For the SCAN command, a data packet will be sent after every completed scan. This will continue until it receives a STOP command. The format for this packet is shown in Table 3.

The quality byte is a combination of the MS 6-bit value, based on the strength of the reflected sample and the lower 2 bits, which indicate if the sample is the first after the 0-degree rotor position. A binary value of “01” indicates this is the start of a new rotation, and “10” identifies all other samples. The angle is bits 15:1 value, with bit 0 always set to “1.” The angle value is a fixed-point combination of bits 15:8 (integer) and bits 7:1 (fraction) in degrees (0-359.x). The distance is a 16-bit fixed-point combination of bits 15:2 (integer) and bits 1:0 (fraction) in millimeters. Note that a distance of “0” indicates an invalid measurement—probably less than the minimum distance of around 150mm.

I’ll be tossing out any fractional values reported and dealing with the just integers. It can be noted that samples are not taken at exactly the same position on each rotation. Presently, there is no command to take a sample at some arbitrary angle. The system merely continues to sample and report its present position and distance calculated. A STOP command is the only way to halt SCAN activity.

All packets are sent at 115200 baud without any communication handshaking. This means your system must be fast enough to accept all the data being sent, or there will be a loss of sync. There seems to be no way to resync the data, since scan data has no sync byte, as with a request and response packet.

API COMMANDS
Table 4 is a complete list of the functions available. You’ll mainly be interested in the first three commands. The last four commands will give you some feedback on the operation of the system. The express scan is a way to send data more quickly by compressing it and communicating using larger chunks. Each of these commands uses the same request and response formats. The format for the data packet that follows the response (if any) will be specific to each command.

An Arduino library for the RPLIDAR [2] contains several example programs, and I will not be using that here. Instead, I want the communication to be more visible to you, so that you can customize it to your individual requirements. The 7-wire connection to my Arduino Mega is shown in Figure 4. It is a straightforward 5V TTL interface with Serial1 of the Mega. The motor control line is connected to a digital pin to turn the motor ON or OFF.

My primary interest is to give the user a way of exploring all the API commands. This is accomplished via a serial terminal program connected to the Arduino via the standard USB Serial port. The first order of business was to decide on using a two-letter command format. It’s always a good idea to give the user help on what commands are supported. This is done through a menu that will be displayed by sending a “?.” Most commands begin with the first two letters of the command, but due to limitations of using only two letters, the menu shows all the commands, and capitalizes those used for the command.

The SCAN command is therefore displayed as “SCan” meaning for a user to type in “S” and “C” followed by a <CR> (carriage return). I’ll use that convention from here forward in the article when I mention commands. The last command in Table 4 is “GET_LIDAR_CONF” and it has configuration values that can be requested. One is the maximum distance of the scanning mode. The menu item displayed for that command is “maX Distance” which indicates the two-letter command is “XD.” Should a command require a value, its menu item is displayed with an appended “=?” which suggests the command format will be two letters followed by the equal sign, a decimal value and <CR>.

SERIAL EVENT
A special function, SerialEvent can be used with receiving serial data. It is not actually an interrupt, but will be called once every time through the loop function, so it is dependent on the loop timing not being restricted in any way. I’ll be using this function for both the console (Serial) and the RPLIDAR (Serial1). Note that an UNO could be used, but since communication with the RPLIDAR is the highest priority, it should be connected to Serial and a software serial routine could be implemented for console I/O.

Initial testing was accomplished by hand assembling the API-required data packet for each of the API’s commands I sent using Real Term. Constructing a more friendly user interface seemed appropriate, because I wanted to off load as much of the work as possible. Because the menu is displayed as a list, I wanted to give priority to those commands that are more useful by splitting this into a main menu (Listing 1) and a sub menu (Listing 2).

You’ll notice that most of the informational commands are on the sub menu, which is available with the “SM” command. Meanwhile, commands that may be used frequently are on the main menu and are available with the “?” command. You also may have noticed some other commands that don’t seem to be part of the RPLIDAR’s API. We’ll get to them later. But first let’s see what happens when a command is entered.

User input commands are expected to end with a <CR>, so serialEvent() gathers user input into the userInput[] array. The first character is saved as userCommand and the second character is saved as userSubCommand. These variables are initially set to “0” so the main loop() has nothing to do unless a command has been received, which alters those variables. Should a third character in the userInput[] array be the equal character “=”, an integer value is expected before the <CR>. I wanted a way to set some parameters, even though the RPLIDAR APIs don’t require data input (for the most part). Anyway, this integer value is saved as the commandValue.

So, let us assume the “SC” command was entered, therefore userCommand = S and userSubCommand = C. In the main loop(), there are only two things to do—execute some code when the userCommand <> 0 (user has input a command), and responseCommand <> 0 (RPLIDAR has responded to a request). We have not yet discussed gathering RPLIDAR’s responses via serial1Event(), so let’s continue with the SC command.

Using the Arduino’s switch/case function, we can zero in on the SC command (Listing 3). Here switch is based on userCommand, and so execution will look for the case “S.” Execution branches to the code at case S and this has a secondary switch/case function based on the userSubCommand. Again, execution branches to the code at case C. We now execute any code for the scan command requested by the user.

The rplidarScan() routine will begin a scan. This action can only be interrupted by a Stop command as it continuously repeats its scan routine. The scan routine sends out laser pulses and looks for a return of the reflected pulses up to its maximum flight time. If the pulses are returned, then a distance is calculated and the present rotation angle and distance are sent out as a 5-byte packet. This repeats ad nauseam and is the subject of a gotcha.

There is no flow control and the 5-byte packet has no absolute unique byte that can be used to sync the data. That means that if you can’t take the data as fast as they are sent, you will lose data and become un-synced. You might continue to interpret data incorrectly, because there is no indication that this has happened. This means that you must be absolutely sure to handle everything that is sent. Using an Arduino has its limitations. Once started, I want to be sure I spend all my time handling this function, so as not to cause any delay in this process. The rplidarScan() function has two components: start the scan and turn on the motor.

Serial1.write(0xA5);
Serial1.write(0x20);
digitalWrite(RPLIDAR_MOTOR, true);

The userCommand is then set to ‘0’ using the flushCommand() routine. And we can begin to look for an RPLIDAR response.

SERIAL1 EVENT
Enter the serial1Event() function (Listing 4). Once we have received any characters from the Serial1 port (RPLIDAR), we want to stay in this routine until we have completed the task. The switch/case function is used here based on the switch variable mode. As we begin the mode=0, we look for start byte 1 (0xA5). Receiving this byte sets the mode=1. While mode=1, we look for start byte 2 (0x5A). Receiving this byte sets the mode=2. This is where things begin to get tricky. When in mode=2 we are expecting an additional 5 bytes.

If the last byte = 0x20, we have a response from the special request 0x84, GET_LIDAR_CONF, which will have one of six responses. We need to read an additional packet to determine the specifics for this request. From that packet, we can set responseCommand = the first byte, which will be 0x71, 0x72, 0x74, 0x75, 0x7C or 0x7F. Set the mode=0. And we’re done here.

Otherwise, we can use the first byte to determine what should be done next. Because these responses all have additional packets that are custom to the request, I assign each a unique mode# and handle those packets in separate routines. Each of these collect additional packet data and assign a unique responseCommand value, before setting the mode=0 and exiting.

All this is pretty ordinary, ending up with the responseDescriptor[] array holding RPLIDAR’s data packet info, each with a unique responseCommand value used back in loop(), except for responseCommand=5. This is the Scan command’s response. It has been reassigned a mode=3.

Since RPLIDAR will be sending 5 bytes for every scan, I wait for the buffer to have more than 4 bytes before the next 5 bytes are read into the responseDescriptor[] array. The first byte holds the quality value and the start bit (bit0). If bit0=1, scanCount is incremented. Remember, this bit indicates the first scan completed after the rotation has reached 0 degrees. I use this to keep track of complete rotations. I want to stop the scan when I have fulfilled my requested number of complete rotations. This is presently set to 1.

The second and third bytes contain the angle in degrees and fraction (which I throw away). The angle is used to index into the distance[] array. Each entry in this array will hold the latest distance from bytes four and five. Once scanCount=2, I end the scan by sending the Stop command. Distances reported as “0” are illegal. These couldn’t be calculated, because they were either less than the minimum distance (less than 150mm) or further than the maximum distance (greater than 12m). I do a quick check to determine how many entries in the distance[] array have legal entries, and this is saved in the variable dataCount. The mode is set to 0, and no response code is necessary, except the array is flagged using newData = true, and we’re done here.

LET’S SEE WHAT WE GOT
Ordinarily, I wouldn’t want to get all the data back, unless I were trying to do some mapping. The DAta command will dump 360 degrees of data, saved in the distance[] array. This command can be used to verify that the data returned from RPLIDAR are correct (or at least look good). You may have noticed that I am not doing anything with the quality value. You might want to use this value for qualifying each computation. For example, only accept the angle and distance if it has a quality value greater than some predetermined value. For this version, I’ll be taking all angles and distances.

What’s dumped is the list of distances computed for angles 0-359. The table is initialized to all “0“s, so any angle that wasn’t measured will be “0.“ You can choose to view these as a minimum distance, a maximum distance, or no plot point. I wanted to see these points plotted, so I wrote a simple program in Liberty Basic to do this on my PC. This program is only about 150 lines of code. The application consists of a graphic window centered on the screen.

A pop-up box asks which comm port the Arduino/RPLIDAR is connected to. Next, it sends out a SCan command and a DAta command, which returns a 360-item list of distances. Once the list is received, the plot routine displays the data as plotted points, as seen from the RPLIDAR. This request and plot action continue repeatedly. Liberty Basic has some easy-to-use plotting commands that make this simple to execute, with minimum math required. In fact, the only math is for the zoom function I added to allow you to plot from 12m down to 1m, full scale (Listing 5).

If you think like a XY plotter, you can see how these commands are used to rotate the direction of travel, move out some distance, plot a mark and then move back. This is done once for each of the 360 array entries. The TUrn command internally calculates the X and Y steps necessary to move in a vectored direction, so no math is required!

WHY AN INTERMEDIATE INTERFACE?
Certainly, I could have coded my Liberty Basic application to request data using just the API commands directly interfacing with RPLIDAR. Although the API commands may be simple, you would need to interpret the 5 bytes of data being returned for every scan point. While you are doing this and storing the data, you need to keep track of where you are, and STop the scan when you have sufficient data. It’s not really a big deal at this point, but I believe the best way to interface with this is to be totally insulated from the data collection process. This allows a user to retrieve only pertinent information, and is absolutely critical for equipment with limited processing power.

Next month, we’ll look at the plot made using the Liberty Basic application, and determine exactly what data we need. We want to be sure that any processor interfacing with this project doesn’t get bogged down with an abundance of data. As always, it seems there’s too much to do and too little time. 

Read Part 2 Here

RESOURCES

See article code archive for code for this article.

References:

Molex | www.molex.com
Slamtec | www.slamtec.com

PUBLISHED IN CIRCUIT CELLAR MAGAZINE • MARCH 2020 #356 – Get a PDF of the issue

Jeff Bachiochi

Website | + posts

Jeff Bachiochi (pronounced BAH-key-AH-key) has been writing for Circuit Cellar since 1988. His background includes product design and manufacturing. You can reach him at: jeff.bachiochi@imaginethatnow.com or at: www.imaginethatnow.com.

• Jeff Bachiochi

  https://circuitcellar.com/author/jeff-bachiochi/

  Don’t Touch That!
• Jeff Bachiochi

  https://circuitcellar.com/author/jeff-bachiochi/

  It’s All HD Now
• Jeff Bachiochi

  https://circuitcellar.com/author/jeff-bachiochi/

  Build Quiz Game Based on Key Fob
• Jeff Bachiochi

  https://circuitcellar.com/author/jeff-bachiochi/

  Heart Monitoring from Home