Multiple Slamtec LIDAR Connection Issues with MATLAB - windows

Question

I'm running into some initial LIDAR connection issue with simultaneously connecting 4 Slamtec RPLIDAR A3 using MATALB
with the provided interface library found here: https://github.com/ENSTABretagneRobotics/Hardware-MATLAB
The issue is that I am having to retry the connection on at least one of the LIDARS before it connects.
And it can also vary with LIDAR that is. That is, all but one LIDAR connects the first time.
One time, it could be LIDAR on one COM port, another time it could be a LIDAR on another COM port.
This is the way it is set up right now.
Basically MATALB loads the provided interface library, hardwarex.dll. That exposes some library methods to be used by MATLAB.
The method to connect the LIDAR does the following:
Opens the RS232 port
Sets port options
Gets some info and health statuses form lidar
Sets the motor PWM to zero (stop lidar motor)
Uses express scan mode option
Here somewhere the communication will error out.
Using a serial sniffer I was able to see that the LIDAR errors out after the following message to the LIDAR:
a5 f0 02 ff 03 ab a5 25 a5 82 05 00 00 00 00 00 22
Which I tracked to the following library methods, in that order
SetMotorPWMRequestRPLIDAR()
CheckMotorControlSupportRequestRPLIDAR()
StopRequestRPLIDAR()
StartExpressScanRequestRPLIDAR() <-- Error here
To which the LIDAR responds with:
a5 5a 54 00 00 40 82
Where as a successfully connection response from the LIDAR much longer in content.
Things I've tried
Drain (force all write data) the write buffer with the interface libraries DrainComputerRS232Port() method before and/or after any write to lidar.
Setting the TX/Write OS FIFO buffer to FILE_FLAG_NO_BUFFERING (ie. WriteFile()).
Changing the Hardware FIFO buffer form max (16) to min (1).
Using MATLAB's serial() command to flush any input or output buffers prior to loading the library or trying the connections.
This is the system and settings I am working with
Lidar (x4):
Slamtec RPLIDAR A3
Firmware 1.26
Connected via USB (no USB hub used)
No other COM port devices connected
Computer
OS: Windows 10 Pro - Build 1903
CPU: Intel Xeon 3.00Ghz
RAM: 64 GB
HD: SSD - 512GB NVMe
Serial Port Settings
Boud Rate: 256000
Timeout: 1000
Software
MATLAB R2018b (9.5.0)
I've been banging my head on the wall with this. Any help is much much appreciated!

Answer 1

I'm going to answer my own question. And anyone is interested in a more detailed discussion please refer to the issue posted on the MATLAB RPLIDAR repo:
https://github.com/ENSTABretagneRobotics/Hardware-MATLAB/issues/2
As I mentioned, when debugging, the error seemed to happen ConnectRPLIDAR() --> StartExpressScanRequestRPLIDAR(), then specifically here:
// Receive the first data response (2 data responses needed for angles computation...).
memset(pRPLIDAR->esdata_prev, 0, sizeof(pRPLIDAR->esdata_prev));
if (ReadAllRS232Port(&pRPLIDAR->RS232Port, pRPLIDAR->esdata_prev, sizeof(pRPLIDAR->esdata_prev)) != EXIT_SUCCESS)
{
// Failure
printf("A RPLIDAR is not responding correctly. \n");
return EXIT_FAILURE;
}
What seemed to have happened before that is after the command being send out in WriteAllRS232Port(), sometimes it would not read a response in the ReadAllRS232Port(), esdata_prev would be nothing.
We tried implementing a mSleep(500) delay before that second ReadAllRS232Port(), and it seemed to help (my guess that the lidar was slow to respond), but the issue did not get resolved fully.
The following is what made it work every time with 4 lidars:
inline int StartExpressScanRequestRPLIDAR(RPLIDAR* pRPLIDAR)
{
unsigned char reqbuf[] = { START_FLAG1_RPLIDAR,EXPRESS_SCAN_REQUEST_RPLIDAR,0x05,0,0,0,0,0,0x22 };
unsigned char descbuf[7];
unsigned char sync = 0;
unsigned char ChkSum = 0;
// Send request to output/tx OS FIFO buffer for port
if (WriteAllRS232Port(&pRPLIDAR->RS232Port, reqbuf, sizeof(reqbuf)) != EXIT_SUCCESS)
{
printf("Error writing data to a RPLIDAR. \n");
return EXIT_FAILURE;
}
// Receive the response descriptor.
memset(descbuf, 0, sizeof(descbuf)); // Alocate memory
if (ReadAllRS232Port(&pRPLIDAR->RS232Port, descbuf, sizeof(descbuf)) != EXIT_SUCCESS)
{
printf("A RPLIDAR is not responding correctly. \n");
return EXIT_FAILURE;
}
// Quick check of the response descriptor.
if ((descbuf[2] != 0x54) || (descbuf[5] != 0x40) || (descbuf[6] != MEASUREMENT_CAPSULED_RESPONSE_RPLIDAR))
{
printf("A RPLIDAR is not responding correctly. \n");
return EXIT_FAILURE;
}
// Keep anticipating a port read buffer for 1 second
int timeout = 1500;
// Check it every 5 ms
// Note on Checking Period Value:
// Waiting on 82 bytes in lidar payload
// 10 bits per byte for the serial communication
// 820 bits / 256000 baud = 0.0032s = 3.2ms
int checkingperiod = 5;
RS232PORT* pRS232Port = &pRPLIDAR->RS232Port;
int i;
int count = 0;
// Wait for something to show up on the input buffer on port
if (!WaitForRS232Port(&pRPLIDAR->RS232Port, timeout, checkingperiod))
{
//Success - Something is there
// If anything is on the input buffer, wait until there is enough
count = 0;
for (i = 0; i < 50; i++)
{
// Check the input FIFO buffer on the port
GetFIFOComputerRS232Port(pRS232Port->hDev, &count);
// Check if there is enough to get a full payload read
if (count >= sizeof(pRPLIDAR->esdata_prev))
{
// Thre is enough, stop waiting
break;
}
else
{
// Not enough, wait a little
mSleep(checkingperiod);
}
}
}
else
{
//Failure - After waiting for an input buffer, it wasn't there
printf("[StartExpressScanRequestRPLIDAR] : Failed to detect response on the input FIFO buffer. \n");
return EXIT_FAILURE;
}
// Receive the first data response (2 data responses needed for angles computation...).
memset(pRPLIDAR->esdata_prev, 0, sizeof(pRPLIDAR->esdata_prev));
if (ReadAllRS232Port(&pRPLIDAR->RS232Port, pRPLIDAR->esdata_prev, sizeof(pRPLIDAR->esdata_prev)) != EXIT_SUCCESS)
{
// Failure
printf("A RPLIDAR is not responding correctly. \n");
return EXIT_FAILURE;
}
// Analyze the first data response.
sync = (pRPLIDAR->esdata_prev[0] & 0xF0)|(pRPLIDAR->esdata_prev[1]>>4);
if (sync != START_FLAG1_RPLIDAR)
{
printf("A RPLIDAR is not responding correctly : Bad sync1 or sync2. \n");
return EXIT_FAILURE;
}
ChkSum = (pRPLIDAR->esdata_prev[1]<<4)|(pRPLIDAR->esdata_prev[0] & 0x0F);
// Force ComputeChecksumRPLIDAR() to compute until the last byte...
if (ChkSum != ComputeChecksumRPLIDAR(pRPLIDAR->esdata_prev+2, sizeof(pRPLIDAR->esdata_prev)-1))
{
printf("A RPLIDAR is not responding correctly : Bad ChkSum. \n");
return EXIT_FAILURE;
}
return EXIT_SUCCESS;
}
So in the above code, we are waiting for the OS read FIFO buffer to show something within 1.5s, checking every 5ms (WaitForRS232Port()). If anything shows up, makes sure to wait to have enough, the size of the payload (GetFIFOComputerRS232Port()).
I'm not sure if it made a difference but we also removed the OS write FIFO buffer by changing it from 0 to FILE_FLAG_NO_BUFFERING:
File: OSComputerRS232Port.h
...
hDev = CreateFile(
tstr,
GENERIC_READ|GENERIC_WRITE,
0, // Must be opened with exclusive-access.
NULL, // No security attributes.
OPEN_EXISTING, // Must use OPEN_EXISTING.
FILE_FLAG_NO_BUFFERING, // Not overlapped I/O. Should use FILE_FLAG_WRITE_THROUGH and maybe also FILE_FLAG_NO_BUFFERING?
NULL // hTemplate must be NULL for comm devices.
);
...