SENS: SF45: Scale the measured distance with pitch and roll

[Claudio-Chies]

To prevent oscillations caused by the coupling of pitch and roll, which increase the sensor's measured distance, we account for these effects by introducing scaling based on pitch and roll into the measurements. This adjustment ensures accurate collision prevention.
Issue was solved inside of collision prevention by @mahimayoga in #24107, whereby this implementation is aequivalent.

Pitch

Roll

Solution

Pitch

Roll

Test coverage

• Rough test on Hardware, should be flight tested before Merging.

[bresch]

What about extracting this function to a library so we don't have 2 times the exact same implementation?

[Claudio-Chies]

This would be an option, in the long run, i would move away from publishing the obstacle_distance from the sensor, and just publish each measurement with its orientation set as the quaternion, but for this we need a callback based collision prevention map logic, which it is currently not.

If its worth it for the time inbetween i cant say, there you would probably know better.

[mahimayoga]

If its worth it for the time inbetween i cant say, there you would probably know better.

@bresch, what do you think?

[MaEtUgR]

i would move away from publishing the obstacle_distance from the sensor

Now I see where all the hassle is coming from and I agree with you the driver should only publish data and there needs to be a central instance like an obstacle distance manager in CollisionPrevention taking care of maintaining fusion of different sensors, maintaining timeouts and similar. Let's go in steps.

[github-actions]

🔎 FLASH Analysis

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

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +96  +0.0%     +96    .text
  [NEW]    +300  [NEW]    +300    ../../src/lib/collision_prevention/ObstacleMath.cpp
  +0.0%     +13  +0.0%     +13    [section .text]
  +0.2%      +3  +0.2%      +3    ../../src/systemcmds/ver/ver.cpp
  -0.1%      -4  -0.1%      -4    ../../src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
  -3.8%    -216  -3.8%    -216    ../../src/lib/collision_prevention/CollisionPrevention.cpp
+0.1% +1.81Ki  [ = ]       0    .debug_abbrev
  -0.5%     -18  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  [NEW] +1.77Ki  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
+0.0%     +24  [ = ]       0    .debug_aranges
  -4.1%     -16  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  [NEW]     +48  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0%     +64  [ = ]       0    .debug_frame
+0.1% +19.4Ki  [ = ]       0    .debug_info
  -5.5% -4.92Ki  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  [NEW] +24.3Ki  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
+0.0% +1.02Ki  [ = ]       0    .debug_line
  -6.7%    -882  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  [NEW] +1.90Ki  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  +0.1%      +1  [ = ]       0    task/task_cancelpt.c
+0.0%    +486  [ = ]       0    .debug_loc
 -10.7% -2.97Ki  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  [NEW] +3.45Ki  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
+0.0%     +10  [ = ]       0    .debug_ranges
  -6.6%    -384  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  [NEW]    +400  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  +3.1%      +2  [ = ]       0    task/task_cancelpt.c
+0.0%    +208  [ = ]       0    .debug_str
  +0.1%     +52  [ = ]       0    
  -1.9%     -82  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  [NEW]    +246  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -0.8%      -8  [ = ]       0    ../../src/lib/tecs/TECS.cpp
+0.5%      +1  [ = ]       0    .shstrtab
+0.0%    +103  [ = ]       0    .strtab
  [NEW]     +86  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
  +0.1%     +49  [ = ]       0    [section .strtab]
+0.0%     +64  [ = ]       0    .symtab
  [NEW]     +64  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -7.0%     -64  [ = ]       0    ../../src/lib/version/version.c
  +0.3%     +16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  +0.1%     +48  [ = ]       0    [section .symtab]
-0.8%     -96  [ = ]       0    [Unmapped]
+0.0% +23.2Ki  +0.0%     +96    TOTAL

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

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +80  +0.0%     +80    .text
  [NEW]    +300  [NEW]    +300    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -0.1%      -4  -0.1%      -4    ../../src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
  -3.8%    -216  -3.8%    -216    ../../src/lib/collision_prevention/CollisionPrevention.cpp
+0.1% +1.81Ki  [ = ]       0    .debug_abbrev
  -0.5%     -18  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  [NEW] +1.77Ki  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
+0.0%     +24  [ = ]       0    .debug_aranges
  -4.1%     -16  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  [NEW]     +48  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0%     +64  [ = ]       0    .debug_frame
+0.1% +19.4Ki  [ = ]       0    .debug_info
  -5.5% -4.92Ki  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  [NEW] +24.3Ki  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
+0.0% +1.02Ki  [ = ]       0    .debug_line
  -6.7%    -882  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  [NEW] +1.90Ki  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  +0.1%      +1  [ = ]       0    task/task_cancelpt.c
+0.0%    +456  [ = ]       0    .debug_loc
 -10.8% -3.00Ki  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  [NEW] +3.45Ki  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
+0.0%      +8  [ = ]       0    .debug_ranges
  -6.6%    -384  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  [NEW]    +400  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0%    +208  [ = ]       0    .debug_str
  +0.1%     +52  [ = ]       0    
  -1.9%     -82  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  [NEW]    +246  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -0.8%      -8  [ = ]       0    ../../src/lib/tecs/TECS.cpp
+0.5%      +1  [ = ]       0    .shstrtab
+0.0%    +103  [ = ]       0    .strtab
  [NEW]     +86  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
  +0.1%     +49  [ = ]       0    [section .strtab]
+0.0%     +64  [ = ]       0    .symtab
  [NEW]     +64  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -7.0%     -64  [ = ]       0    ../../src/lib/version/version.c
  +0.3%     +16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  +0.1%     +48  [ = ]       0    [section .symtab]
-0.1%     -80  [ = ]       0    [Unmapped]
+0.0% +23.1Ki  +0.0%     +80    TOTAL

Updated: 2025-01-20T09:45:32

[mahimayoga]

Flight tested today with SF45: https://review.px4.io/plot_app?log=8b0933c5-c941-4344-a32f-fea664dca00f No weird behaviour, everything working as expected.

Also did a quick bench test with the lightware laser lidar (non rotating) to test that project_distance_on_horizontal_plane is working fine when called in CollisionPrevention.cpp as well: