ObstacleMath: move repeated/static code in SF45 & CollisionPrevention to ObstacleMath library

[mahimayoga]

No functional change, only refactoring.

Solved Problem

Some repeated code across CollisionPrevention.cpp and lightware_sf45_serial.cpp.

Solution

Move some repeated/static code to ObstacleMath library.

Part of this was already done in the following pr's:

1. SENS: SF45: Scale the measured distance with pitch and roll #24142
2. SF45: Adjust _handle_missed_bins() logic #24248

Test coverage

• Pure refactoring, no functional change.

Context

• In the long run the SF45 driver should no longer publish obstacle maps. rather just a distance measurement and an associated orientation. The map logic should be separated. The functions in the ObstacleMath library were created with this in mind.

[github-actions]

🔎 FLASH Analysis

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

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +80  +0.0%     +80    .text
  +121%    +364  +121%    +364    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -5.2%    -284  -5.2%    -284    ../../src/lib/collision_prevention/CollisionPrevention.cpp
+0.0%     +54  [ = ]       0    .debug_abbrev
  -1.9%     -71  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  +3.6%     +69  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
-0.0%      -8  [ = ]       0    .debug_aranges
  -8.5%     -32  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
   +40%     +32  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0%     +80  [ = ]       0    .debug_frame
+0.0% +3.84Ki  [ = ]       0    .debug_info
  -1.0%    -911  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
   +19% +4.73Ki  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
+0.0%    +584  [ = ]       0    .debug_line
  -4.9%    -609  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
   +55% +1.19Ki  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  -0.4%      -5  [ = ]       0    task/task_cancelpt.c
+0.1% +2.79Ki  [ = ]       0    .debug_loc
  -5.3% -1.33Ki  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  +133% +4.57Ki  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -0.2%     -15  [ = ]       0    ../../src/modules/flight_mode_manager/FlightModeManager.cpp
  +0.0%     +13  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
  -0.1%    -455  [ = ]       0    [section .debug_loc]
+0.0%    +329  [ = ]       0    .debug_ranges
  -5.1%    -280  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  +154%    +664  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  -0.1%     -48  [ = ]       0    [section .debug_ranges]
  +1.6%      +1  [ = ]       0    task/task_cancelpt.c
-0.0%     -87  [ = ]       0    .debug_str
  +2.9%    +126  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
 -52.2%    -213  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
+0.0%     +68  [ = ]       0    .strtab
 -10.2%    -154  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  +258%    +222  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
  +0.0%     +32  [ = ]       0    [section .strtab]
+0.0%     +80  [ = ]       0    .symtab
 -19.7%    -224  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  +400%    +256  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  +4.2%     +16  [ = ]       0    ../../src/lib/parameters/autosave.cpp
  -7.0%     -64  [ = ]       0    ../../src/lib/version/version.c
  +0.3%     +16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  +0.1%     +80  [ = ]       0    [section .symtab]
-0.7%     -80  [ = ]       0    [Unmapped]
+0.0% +7.70Ki  +0.0%     +80    TOTAL

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

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +96  +0.0%     +96    .text
  +121%    +364  +121%    +364    ../../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
  -5.2%    -284  -5.2%    -284    ../../src/lib/collision_prevention/CollisionPrevention.cpp
+0.0%     +54  [ = ]       0    .debug_abbrev
  -1.9%     -71  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  +3.6%     +69  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
-0.0%      -8  [ = ]       0    .debug_aranges
  -8.5%     -32  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
   +40%     +32  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0%     +80  [ = ]       0    .debug_frame
+0.0% +3.84Ki  [ = ]       0    .debug_info
  -1.0%    -911  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
   +19% +4.73Ki  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
+0.0%    +584  [ = ]       0    .debug_line
  -4.9%    -609  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
   +55% +1.19Ki  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  -0.4%      -5  [ = ]       0    task/task_cancelpt.c
+0.1% +2.81Ki  [ = ]       0    .debug_loc
  -5.3% -1.31Ki  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  +133% +4.57Ki  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -0.1%    -457  [ = ]       0    [section .debug_loc]
+0.0%    +330  [ = ]       0    .debug_ranges
  -5.1%    -280  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  +154%    +664  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  -0.1%     -48  [ = ]       0    [section .debug_ranges]
  +3.1%      +2  [ = ]       0    task/task_cancelpt.c
-0.0%     -87  [ = ]       0    .debug_str
  +2.9%    +126  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
 -52.2%    -213  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
+0.0%     +68  [ = ]       0    .strtab
 -10.2%    -154  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  +258%    +222  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
  +0.0%     +32  [ = ]       0    [section .strtab]
+0.0%     +80  [ = ]       0    .symtab
 -19.7%    -224  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  +400%    +256  [ = ]       0    ../../src/lib/collision_prevention/ObstacleMath.cpp
  +4.2%     +16  [ = ]       0    ../../src/lib/parameters/autosave.cpp
  -7.0%     -64  [ = ]       0    ../../src/lib/version/version.c
  +0.3%     +16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  +0.1%     +80  [ = ]       0    [section .symtab]
-0.1%     -96  [ = ]       0    [Unmapped]
+0.0% +7.72Ki  +0.0%     +96    TOTAL

Updated: 2025-02-07T10:10:28

[sfuhrer]

Only some details, looks clean!