SENS: SF45: Improved Datahandling and fixes

[Claudio-Chies]

General Improvement to the Sampling Rate and the Data handling of the Driver.

Basic Structure
We updated the internal logic as follows:

• If during a sweep of the sensor, a bin gets skipped, we assume the last measurement to be valid for that bin as well.
• If multiple measurements have been made in a bin, the lowest one is selected.
• To avoid issues with the message processing structure in CollisionPrevention, the sensor publishes all bins he has measured in the past 0.1s. This is because the sensor is publishing messages at a faster rate than CollisionPrevention is reading them, which might cause lost readings.

Other notable changes:

• greatly improved the Sampling Rate (tested up to 500hz)
• Fixed boot up issues
• Added Timeout functionality to measurements.
• removed the publishing to distance_sensor

Important note
During testing we came across some power issues, if you are having boot up issues, make sure to power the Sensor separately. And/or shorten the shipped cable. See docs for more info.

Changelog Entry

For release notes:

Bugfix SF45 rotary lidar driver for Collision Prevention
Documentation: docs.px4.io

Relevant PR's

• added sf45 as a seperate rangefinder PX4-user_guide#3448

Test coverage

• Tested on Hardware (Flightreview)

Note:
If there is a move towards a callback based internal obstacle map in collision prevention, then it would be best to publish a distance_sensor message instead of a obstacle_distance one.
here is a branch where this is already implemented:
https://github.com/PX4/PX4-Autopilot/tree/SF45-Rangefinder_approach

[Claudio-Chies]

@dirksavage88 Would be nice if you could also do a quick review.

[Claudio-Chies]

@dirksavage88
I added logic to remove stale values. Previously, all data was published with the current timestamp, so if a sector had an impossible reading, it would persist indefinitely (which has caused issues in the past). Now, there's an internal array that tracks timeouts for each sector and overwrites any data that's older than 0.5 seconds. While this threshold might seem high, it matches the timeout value used in Collision Prevention.

[github-actions]

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 64 byte (0 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +64  +0.0%     +64    .text
  +0.9%     +48  +0.9%     +48    ../../src/modules/logger/logged_topics.cpp
  +0.0%     +13  +0.0%     +13    [section .text]
  +0.2%      +3  +0.2%      +3    ../../src/systemcmds/ver/ver.cpp
+0.0%     +56  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
-0.0%      -8  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0%      +4  [ = ]       0    .debug_frame
+0.0%    +205  [ = ]       0    .debug_info
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
  +0.7%    +209  [ = ]       0    ../../src/modules/logger/logged_topics.cpp
+0.0%     +35  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  +0.9%     +54  [ = ]       0    ../../src/modules/logger/logged_topics.cpp
  +0.5%      +6  [ = ]       0    task/task_cancelpt.c
+0.0%    +139  [ = ]       0    .debug_loc
  +1.4%    +139  [ = ]       0    ../../src/modules/logger/logged_topics.cpp
+0.0%     +49  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  +4.0%     +56  [ = ]       0    ../../src/modules/logger/logged_topics.cpp
  +1.5%      +1  [ = ]       0    task/task_cancelpt.c
-0.4%     -64  [ = ]       0    [Unmapped]
+0.0%    +480  +0.0%     +64    TOTAL

px4_fmu-v6x [Total VM Diff: 56 byte (0 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +56  +0.0%     +56    .text
  +0.9%     +48  +0.9%     +48    ../../src/modules/logger/logged_topics.cpp
  +0.0%      +5  +0.0%      +5    [section .text]
  +0.2%      +3  +0.2%      +3    ../../src/systemcmds/ver/ver.cpp
+0.0%     +56  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
-0.0%      -8  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0%      +4  [ = ]       0    .debug_frame
+0.0%    +205  [ = ]       0    .debug_info
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
  +0.7%    +209  [ = ]       0    ../../src/modules/logger/logged_topics.cpp
+0.0%     +27  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  +0.9%     +54  [ = ]       0    ../../src/modules/logger/logged_topics.cpp
  -0.2%      -2  [ = ]       0    task/task_cancelpt.c
+0.0%    +139  [ = ]       0    .debug_loc
  +1.4%    +139  [ = ]       0    ../../src/modules/logger/logged_topics.cpp
+0.0%     +49  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  +4.0%     +56  [ = ]       0    ../../src/modules/logger/logged_topics.cpp
  +1.5%      +1  [ = ]       0    task/task_cancelpt.c
-0.1%     -56  [ = ]       0    [Unmapped]
+0.0%    +472  +0.0%     +56    TOTAL

Updated: 2024-12-13T12:28:50

[dirksavage88]

@Claudio-Chies these changes look good, but I have not had a chance to test on the sensor.

Also I am not an approving reviewer. Maybe @alexcekay would be willing to since he has looked through this code before.

[Claudio-Chies]

another issue i found, which i'll fix in the next commit.
There can be bins which have no measurements as the sampling rate is too slow (here at 100hz, which seems currently be the limit the driver still properly works at according to @alexcekay.
to not run into the issue that the drone can not move into that direction, i'm filling in all bins between the last bin and the current one.
The problem can be seen below where bin 50, 49 and others are missing, but bin 46 gets 3 measurements. (point of change of rotation)

measurement happend
scaled_yaw:     -68,    current_bin:    58,     distance:         0.6900
measurement happend
scaled_yaw:     -77,    current_bin:    57,     distance:         0.6500
measurement happend
scaled_yaw:     -94,    current_bin:    53,     distance:         1.1900
measurement happend
scaled_yaw:     -103,   current_bin:    51,     distance:         2.8700
measurement happend
scaled_yaw:     -118,   current_bin:    48,     distance:         1.2900
measurement happend
measurement happend
scaled_yaw:     -123,   current_bin:    47,     distance:         1.2000
measurement happend
measurement happend
measurement happend
scaled_yaw:     -129,   current_bin:    46,     distance:         0.9800
measurement happend
measurement happend
scaled_yaw:     -126,   current_bin:    47,     distance:         1.0200

[Claudio-Chies]

Screencast.from.11-22-2024.11.25.34.AM.webm

Attached we can see a replay of a log, with low and high angle set to -130, 130 respectively, and at 100hz.
The obstacle_distance message publishes the smallest measurement (if multiple exist) for all sectors between the last and current sector (if no measurements were made for certain sectors)

[github-actions]

FLASH Analysis

px4_fmu-v5x

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%    +209  [ = ]       0    .debug_info
    +0.7%    +209  [ = ]       0    ../../src/modules/logger/logged_topics.cpp
  +0.0%    +139  [ = ]       0    .debug_loc
    +1.4%    +125  [ = ]       0    ../../src/modules/logger/logged_topics.cpp
    +0.0%     +14  [ = ]       0    [section .debug_loc]
  +0.0%     +72  +0.0%     +72    .text
    +0.9%     +48  +0.9%     +48    ../../src/modules/logger/logged_topics.cpp
    +0.0%     +24  +0.0%     +24    [section .text]
  +0.0%     +56  [ = ]       0    .debug_ranges
    +4.0%     +56  [ = ]       0    ../../src/modules/logger/logged_topics.cpp
  +0.0%     +54  [ = ]       0    .debug_line
    +0.9%     +54  [ = ]       0    ../../src/modules/logger/logged_topics.cpp
  +0.0%      +4  [ = ]       0    .debug_frame
  -0.3%     -74  [ = ]       0    [Unmapped]
  +0.0%    +460  +0.0%     +72    TOTAL

px4_fmu-v6x

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%    +209  [ = ]       0    .debug_info
    +0.7%    +209  [ = ]       0    ../../src/modules/logger/logged_topics.cpp
  +0.0%    +139  [ = ]       0    .debug_loc
    +1.4%    +125  [ = ]       0    ../../src/modules/logger/logged_topics.cpp
    +0.0%     +14  [ = ]       0    [section .debug_loc]
  +0.0%     +72  +0.0%     +72    .text
    +0.9%     +48  +0.9%     +48    ../../src/modules/logger/logged_topics.cpp
    +0.0%     +24  +0.0%     +24    [section .text]
  +0.0%     +56  [ = ]       0    .debug_ranges
    +4.0%     +56  [ = ]       0    ../../src/modules/logger/logged_topics.cpp
  +0.0%     +54  [ = ]       0    .debug_line
    +0.9%     +54  [ = ]       0    ../../src/modules/logger/logged_topics.cpp
  +0.0%      +4  [ = ]       0    .debug_frame
  -0.1%     -70  [ = ]       0    [Unmapped]
  +0.0%    +464  +0.0%     +72    TOTAL

[dirksavage88]

That makes sense. Yeah at the rotating point where it shifts to the opposite direction I do notice there are more samples

[alexcekay]

Due to some reports of the driver not starting up in some cases and the update rate being too slow, I have made the following changes:

• Use non-blocking UART reads. Prior to this change, the driver used V_TIME / V_MIN to achieve this. This is not supported on all platforms. For example, on a v5x / v6x, V_TIME / V_MIN won't have any effect. This can cause the driver to lock up completely in some cases.
• Checking the response of the Product Name, Update Rate, Distance Output messages to avoid being out of sync with the real sensor state.
• Improved the parser to handle cases where the read does not return the exact number of bytes requested with the start byte at index 0. This was necessary to achieve a stable, higher parsing rate for sensor data.
• Increased the output rate to avoid gaps in the bins.
• Changed the timeout logic from counter based to time based. This is useful as the counter increases faster/slower depending on the number of reads. The timeout-time does not depend on the update rate.

I did test the changes on the bench

• The driver started correctly every time in a test of 300 v5x starts
• The bin data looks good

It would be great to test the changes in the field.

[Claudio-Chies]

Ok, so after having quite some issues with data not getting properly into the collision prevention obstacle map, i figured out the issue is that collision prevention has a sampling rate compared to this driver, causing messages to get lost, and by this some sectors would remain empty.
to fix this i went back with the timeout approach whereby i only discard measured sectors after a timeout which is currently hardcoded to 0.1s. this way we can avoid sectors getting lost.
tested on hardware, and looks good.

[dirksavage88]

Ok, so after having quite some issues with data not getting properly into the collision prevention obstacle map, i figured out the issue is that collision prevention has a sampling rate compared to this driver, causing messages to get lost, and by this some sectors would remain empty. to fix this i went back with the timeout approach whereby i only discard measured sectors after a timeout which is currently hardcoded to 0.1s. this way we can avoid sectors getting lost. tested on hardware, and looks good.

Was the SF45 update rate too fast for collision prevention ?

I'm also working on an offboard-based ST Lidar array (4 i2c sensors) that publishes over microdds to /fmu/in/obstacle_distance and was experimenting with having each sensor be it's own publisher to the obstacle distance array, and how each sensor publishes to a specific set of bins on the obstacle array (for only it's specific zonal coverage), but not sure if collision prevention can handle the varying bin-filled obstacle messages (depending on the timing and each sensor's FoV)

[Claudio-Chies]

e SF45 update rate too fast for collision prevention ?

Yes, seems like it. Collision prevention updates the obstacle map each time it gets called by StickAccelerationXY::generateSetpoints and uses the last published message.
it might be an idea to split out the update of the internal obstacle map into a separate functionality which gets called with each sensor update. If you have 4 distance sensors in an array, it might be best to either merge them into one obstacle map, where you can also add some timeout functionality, or publish them as distance sensors. There you'll have to add distance sensors to dds_topics.yaml, maybe more configuration is necessary, but there I'm not certain.

The image above is the SF45 publishing as a DistanceSensor at 500hz, and you can see the timing difference between it and obstacle_distance_fused

below you can see how the SF45 now published the data

Screencast.from.12-05-2024.09.41.40.AM.webm

[alexcekay]

it might be an idea to split out the update of the internal obstacle map into a separate functionality which gets called with each sensor update

I think this is generally a good solution. Because now there is a lot of logic in the sensor (like the timeout logic for the obstacle map) that will most likely be needed by any kind of rotating LIDAR that wants to publish an obstacle map. From my point of view, the whole obstacle map is not really directly usable with a rotating LIDAR.

There it might be better to publish the obstacle map directly with only one array entry updated (as before) and have some intermediate module that builds a usable obstacle map for collision detection (and other modules that might need it as input). Or use the DistanceSensor message and its quaternion q instead (as you also mentioned some time ago 👍).

[Claudio-Chies]

@alexcekay yes, I absolutely agree, some module which updates the internal map of collision prevention which gets called based on new measurements would be ideal, then sending the SF45 Data as Distance_Sensor messages would be the perfect solution.
for future reference: the branch with the DistanceSensor approach
https://github.com/PX4/PX4-Autopilot/tree/SF45-Rangefinder_approach

[sfuhrer]

Looks generally very clean and reasonable.
Would appreciate if you could update the PR description. I think it's quite outdated, especially with Alex's contributions. Including the release note section.