AP_Proximity: add RPLidar S2, remove memory copy for each measurement #26835

mirkix · 2024-04-17T06:58:47Z

This PR adds RPLidar S2 support. RPLidar S2 sends out data with 1MBit/s via serial, at the moment the driver copy / move data in the driver internal buffer with each measurement, which result in high MCU load (S2 send up to 32.000 measurements per second). With this PR the driver consume the data direct from serial driver which result in a less MCU load.

For RPLidar S2 set SERIALx_BAUD 1000

Tested with:

rmackay9 · 2024-04-17T07:05:19Z

I am so happy to see this!!

libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp

rmackay9 · 2024-04-17T11:42:10Z

libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp

                return;
            }

-            // descriptor packet has 7 byte in total
-            if (_byte_count < sizeof(_descriptor)) {
+            _uart->read(&_payload[0], sizeof(_descriptor));


I think maybe we should change this to read (or peek) just a single byte until we find the RPLIDAR_PREAMBLE byte. As it is, it always reads 7 bytes at a time so if a stray byte gets in there it will only recover if it gets lucky and the preamble byte of some later packet happens to be at a 7-byte interval in the buffer.

I think one easy way for a stray byte to be in the buffer is immediately after a RESET (see above). Just from inspection I would have thought that after, "_ujart->discard_input()" is called another message could arrive or we could catch half a message as it is on its way into the buffer.

@rmackay9 What I saw, after a reset the response will send by RPLidar, we will skip that after a second, until then no new message is send by RPLidar so we can use the read of 7 bytes, otherwise there is a protocol error. But I check to add the byte looking think for RPILIDAR_PREAMBLE.

rmackay9 · 2024-04-17T11:50:14Z

Good to see that @peterbarker is on the review list 'cuz he's the expert on our serial parsers.

BTW, I think this parser does need to be different from most because it has to process so much data that we don't want to use an intermediate buffer.

Txs again!

peterbarker

What hardware has this been tested on?

Has this been tested on Rover running at 50Hz?

Has this been tested with injection of a (small!) amount of serial corruption? Under Valgrind?

What benchmarking has this had on real hardware? I would suspect it is faster than the existing code, but I think we need to know. microseconds-spent-doing-this-stuff-per-10-seconds-of-runtime is the sort of number I'm thinking here.

I certainly don't hate this.

libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp

peterbarker · 2024-04-26T13:44:28Z

libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp

-            // looking for 0x52 at start of buffer; the 62 following
-            // bytes are "information"
-            if (!make_first_byte_in_payload('R')) { // that's 'R' as in RPiLidar
+            if (AP_HAL::millis() - _last_reset_ms < 1000) {


Why is this limit here?

It is a wait for 1000 ms after a Lidar reset. We can make sure the Lidar has rebooted and skip the startup message.

peterbarker · 2024-04-26T13:47:53Z

libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp

@@ -75,15 +74,6 @@ void AP_Proximity_RPLidarA2::update(void)
        return;
    }

-    // request device info 3sec after reset
-    // required for S1 support that sends only 9 bytes after a reset (A1,A2 send 63)


Has this PR been tested on S1?

@peterbarker Yes, tested with S1. Also with A1 and S2, S1 and S2 send the same data after reset. What is tested is also in the header of this PR.

peterbarker · 2024-04-26T13:57:07Z

libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp

+            if (_uart->available() < sizeof(_payload.device_info)) {
+                return;
+            }
+            if ((_read_result = _uart->read(&_payload[0], sizeof(_payload.device_info))) > 0) {


Any result here (and in similar places) which is not equal to (here) sizeof(_payload.device_info) is invalid. In the case that read is short for some reason we'd end up parsing essentially random data.

It only reads if we have enough data to read, otherwise we have to wait. Or I may not understand your concern.

Don't trust what available() says. There are paths through ArduPilot which can lead to available() returning a number of bytes then read() returning fewer bytes! These are exceptional cases, like the user asking for serial pass-through.

peterbarker · 2024-04-26T14:04:25Z

libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp

+            }
+            if ((_read_result = _uart->read(&_payload[0], sizeof(_payload.device_info))) > 0) {
+                _bytes_read += _read_result;
+            } else {
                return;


This return means that in the case you don't get the number you're after you'll continue to loop endlessly. Again, you've asked how many bytes are available, if you don't get the number you expect then something drastic is probably in order.

I should say that's not an impossibility, BTW. serial-port-passthrough and other hijinks can do strange things.

No, we will no endlessly loop here, because there is a timeout in the driver that triggers a reset. How should I go in an endless loop here?

mirkix · 2024-04-29T14:03:07Z

What hardware has this been tested on?

It is tested with the hardware in the description of the PR, A1, S1 and S2

Has this been tested on Rover running at 50Hz?

Has this been tested with injection of a (small!) amount of serial corruption? Under Valgrind?

No. It was tested by unplug and re plug it again while running and it recovers safely.

What benchmarking has this had on real hardware? I would suspect it is faster than the existing code, but I think we need to know. microseconds-spent-doing-this-stuff-per-10-seconds-of-runtime is the sort of number I'm thinking here.

Yes it is much faster then the code before, because we are not copy data in the buffer after each measurement. @peterbarker How can I get the microseconds-spent for this?

I certainly don't hate this.

peterbarker · 2024-05-07T04:35:16Z

It is tested with the hardware in the description of the PR, A1, S1 and S2

OK, I can test this on A2.

Has this been tested on Rover running at 50Hz?
Has this been tested with injection of a (small!) amount of serial corruption? Under Valgrind?

No. It was tested by unplug and re plug it again while running and it recovers safely.

We do need to test it under the conditions I mention.

What benchmarking has this had on real hardware? I would suspect it is faster than the existing code, but I think we need to know. microseconds-spent-doing-this-stuff-per-10-seconds-of-runtime is the sort of number I'm thinking here.

Yes it is much faster then the code before, because we are not copy data in the buffer after each measurement. @peterbarker How can I get the microseconds-spent for this?

Usually you would disable_interrupts_save, grab a uint32_t micros , do your work, get final timestamp, restore interrupts. Search for ENABLE_EKF_TIMING

rmackay9 · 2024-05-13T02:43:15Z

Hi @mirkix @peterbarker,

I wonder what the next steps on this PR are? More changes from @mirkix? more feedback required from @peterbarker?

rmackay9 · 2024-05-14T04:03:09Z

@mirkix,

We discussed this on the dev call this morning and PeterB pointed out two things that we need to do before merging:

run it through Valgrind ideally with some serial corruption to ensure there are no buffer overrun issues. It should be possible to do this within SITL. sim_vehicle has an argument for running Valgrind (maybe it's -V?). PeterB also says that there should be a SIM_xxx parameter to turn on some serial corruption
test with an A2 (PeterB said he could do this)

giacomo892 · 2024-11-18T11:22:40Z

Hello, we have succesfully tested this PR with RPLidar C1

mirkix requested review from rmackay9 and peterbarker April 17, 2024 06:58

mirkix added Library Driver NeedsTesting labels Apr 17, 2024

rmackay9 mentioned this pull request Apr 17, 2024

Copter/Rover-4.5.0 issues list #26103

Open

92 tasks

rmackay9 reviewed Apr 17, 2024

View reviewed changes

libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp Outdated Show resolved Hide resolved

rmackay9 reviewed Apr 17, 2024

View reviewed changes

mirkix force-pushed the pr-addRPLidarS2 branch from 77171d5 to 43aaf4e Compare April 18, 2024 07:50

peterbarker requested changes Apr 26, 2024

View reviewed changes

AP_Proximity: add RPLidar S2, remove memory copy for each measurement

f08db1f

mirkix force-pushed the pr-addRPLidarS2 branch from 43aaf4e to f08db1f Compare April 29, 2024 14:00

mirkix requested a review from peterbarker April 29, 2024 14:35

rmackay9 added the DevCallTopic label May 13, 2024

CraigElder removed the DevCallTopic label May 13, 2024

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

AP_Proximity: add RPLidar S2, remove memory copy for each measurement #26835

AP_Proximity: add RPLidar S2, remove memory copy for each measurement #26835

mirkix commented Apr 17, 2024 •

edited

Loading

rmackay9 commented Apr 17, 2024

rmackay9 Apr 17, 2024 •

edited

Loading

mirkix Apr 18, 2024 •

edited

Loading

rmackay9 commented Apr 17, 2024

peterbarker left a comment

peterbarker Apr 26, 2024

mirkix Apr 29, 2024

peterbarker Apr 26, 2024

mirkix Apr 29, 2024 •

edited

Loading

peterbarker Apr 26, 2024

mirkix Apr 29, 2024

peterbarker May 7, 2024

peterbarker Apr 26, 2024

mirkix Apr 29, 2024 •

edited

Loading

mirkix commented Apr 29, 2024 •

edited

Loading

peterbarker commented May 7, 2024

rmackay9 commented May 13, 2024

rmackay9 commented May 14, 2024

giacomo892 commented Nov 18, 2024

AP_Proximity: add RPLidar S2, remove memory copy for each measurement #26835

Are you sure you want to change the base?

AP_Proximity: add RPLidar S2, remove memory copy for each measurement #26835

Conversation

mirkix commented Apr 17, 2024 • edited Loading

rmackay9 commented Apr 17, 2024

rmackay9 Apr 17, 2024 • edited Loading

Choose a reason for hiding this comment

mirkix Apr 18, 2024 • edited Loading

Choose a reason for hiding this comment

rmackay9 commented Apr 17, 2024

peterbarker left a comment

Choose a reason for hiding this comment

peterbarker Apr 26, 2024

Choose a reason for hiding this comment

mirkix Apr 29, 2024

Choose a reason for hiding this comment

peterbarker Apr 26, 2024

Choose a reason for hiding this comment

mirkix Apr 29, 2024 • edited Loading

Choose a reason for hiding this comment

peterbarker Apr 26, 2024

Choose a reason for hiding this comment

mirkix Apr 29, 2024

Choose a reason for hiding this comment

peterbarker May 7, 2024

Choose a reason for hiding this comment

peterbarker Apr 26, 2024

Choose a reason for hiding this comment

mirkix Apr 29, 2024 • edited Loading

Choose a reason for hiding this comment

mirkix commented Apr 29, 2024 • edited Loading

peterbarker commented May 7, 2024

rmackay9 commented May 13, 2024

rmackay9 commented May 14, 2024

giacomo892 commented Nov 18, 2024

mirkix commented Apr 17, 2024 •

edited

Loading

rmackay9 Apr 17, 2024 •

edited

Loading

mirkix Apr 18, 2024 •

edited

Loading

mirkix Apr 29, 2024 •

edited

Loading

mirkix Apr 29, 2024 •

edited

Loading

mirkix commented Apr 29, 2024 •

edited

Loading