SENS: SF45: Improved Datahandling and fixes

[Claudio-Chies]

Changes:

1. Moved to the MAV_SENSOR_ORIENTATION ENUM to be consistent with the orientations used in DistanceSensor.msg

2. Removed the publishing to distance_sensor.

3. The data in each bin is now the smallest measured value, and not the last measured value.

4. Added obstacle_distance_fused and distance_sensor to the logged vision and avoidance topics

Changelog Entry

For release notes:

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

Alternatives

for the distance sensor, we could also calculate a quaternion for each measurement, and then set the orientation quaternion, via that. Then we could remove the obstacle_distance publishing.
This would add some complexity in the driver here, but offload all data handling to Collision prevention.
As the obstacle_distance publishing was already in place, i opted to stay with this approach.

Relevant PR's

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

Test coverage

• Tested on Hardware (Flightreview) please note here an offset was hardcoded which is not necessary, it just aligns the array differently for easier debugging

[Claudio-Chies]

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

[dirksavage88]

I never actually had the chance to test these on the bench. The non-standard Yaw cfg as well.

[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

    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%     +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%     +48  +0.0%     +48    .text
  +0.9%     +48  +0.9%     +48    ../../src/modules/logger/logged_topics.cpp
+0.0%      +4  [ = ]       0    .debug_frame
-0.2%     -54  [ = ]       0    [Unmapped]
+0.0%    +456  +0.0%     +48    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%     +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%     +48  +0.0%     +48    .text
  +0.9%     +48  +0.9%     +48    ../../src/modules/logger/logged_topics.cpp
+0.0%      +4  [ = ]       0    .debug_frame
-0.1%     -58  [ = ]       0    [Unmapped]
+0.0%    +452  +0.0%     +48    TOTAL

Updated: 2024-11-29T12:44:31

[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

[dirksavage88]

Looks like you covered all the edge cases here. Eventually setting the cycle delay can be done in PX4, but getting it working with default settings is probably best. Lightware studio should really be used once to set the scan angle and enable scanning at first, and then not touched again unless debugging.

[Claudio-Chies]

Yes, but usually you wouldn't change the cycle delay after the initial setup, no?
For the UAS use case, the lowest cycle delay is probably the most suited? Or I don't know a need for a slower one from the top of my head.