Collision Prevention for multicopter acceleration based position flight mode #23507

Claudio-Chies · 2024-08-07T08:56:52Z

Solved Problem

Ports and extends the current collision prevention algorithm to FlightModeManualAcceleration

Associated PR's

Solution

The algorithm has two parts, velocity compensation, and constraining of the commanded acceleration.

Velocity compensation

For this, I have ported over the existing methods from the velocity based collision prevention method and transformed the velocity into an acceleration by subtracting the velocity set-point from the collision prevention velocity and applying a proportional gain ( MPC_XY_VEL_P_ACC ) to it.

Acceleration constraining

For this I'm calculating the minimal distance to the obstacle, and scaling it quadratic in the range of [0,1] distances proportional to the maximum velocity. (See image below)

For this we split the acceleration setpoint coming from the operator into a surface normal and tangential component, then using the normal and tangential obstacle distance we scale these components individual.
This way, if we are in front of a big even wall, we are free to move tangentially but are restricted in the surface normal direction.

whereby, the acceleration constraining is done in the distance between CP_DIST and MPC_VEL_MANUAL / MPC_XY_P

Drawbacks

Removed in 87a0ee6

Changelog Entry

For release notes:

Feature: Collision prevention for FlightModeManual Acceleration

Documentation: Need to update https://docs.px4.io/main/en/computer_vision/collision_prevention.html
WIP PX4/PX4-user_guide#3364

Drawbacks

If velocity is not close to set point, might cause crashes

Alternatives

dont use the max velocity and position error gain for the scaling factor and use a new normal and tangential scaling parameter.

Test coverage

Tested in SITL with 1D and 2D lidar
Tested on Hardware with 1D and 2D rotary Lidar Flightreview

Context

CP.mp4

Hardware Tests

1D Lidar

The oscillations which are present are due to flying close to the ground, and when pitching the distance sensor picks up the ground.

EDIT1:
Updated description to include tangential scaling

/claim #22464

src/lib/collision_prevention/CollisionPrevention.hpp

dirksavage88 · 2024-08-13T11:47:13Z

@Claudio-Chies is this something I can pull down to flight test? Or is it needing more time in the SITL?

Claudio-Chies · 2024-08-13T11:50:52Z

@dirksavage88 it should be ready to test, the only thing i haven't tested is how non-default gains in the position and velocity controller change the CP behavior

MaEtUgR

After an in person discussion I see a lot of value in simplifying the logic. The main pain points are:

Separate input data processing from guidance logic e.g. distance and direction to closest obstacle and distance to next obstacle in current velocity direction are the interface between obstacle map processing and guidance.
If possible avoid cases in guidance logic. E.g. three things to take care of:
- Stick should not accelerate over the configured collision prevention minimum distance
- Brake from current velocity before before minimum allowed distance
- If obstacle is closer/"comes closer" push away from it to minimum distance
  Probably these rules can run all the time, get superimposed and be readable, predictable (logged).

I added a commit for cosmetic and commenting things that I saw at a glance.

… get called

…cleanup

The default implementation for multicopter Position mode is FlightTaskManualAcceleration. The last missing piece was support for CollisionPrevention in this implementation.

…tests to reflect the change rewrite of obstacle_distance merging methods, and fix of various issues

The default implementation for multicopter Position mode is FlightTaskManualAcceleration. The last missing piece was support for CollisionPrevention in this implementation.

…ay size 72

…cle_dsitance message

src/lib/collision_prevention/CollisionPrevention.cpp

…f we are closer to the obstacle than the minimal distance

github-actions · 2024-11-21T10:23:08Z

FLASH Analysis

px4_fmu-v5x

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +1.4%    +344  [ = ]       0    [Unmapped]
  -0.0%    -344  -0.0%    -344    .text
     +28% +1.23Ki   +28% +1.23Ki    ../../src/lib/collision_prevention/CollisionPrevention.cpp
    +8.4%    +158  +8.4%    +158    ../../src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
    +4.1%    +146  +4.1%    +146    ../../src/modules/flight_mode_manager/FlightModeManager.cpp
    +7.7%    +114  +7.7%    +114    ../../src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp
    +0.8%     +58  +0.8%     +58    ../../src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
    +1.1%     +50  +1.1%     +50    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
    +6.2%     +50  +6.2%     +50    ../../src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp
    +1.1%     +40  +1.1%     +40    ../../src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
    -0.3%     -32  -0.3%     -32    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
    -0.0%     -62  -0.0%     -62    [section .text]
    -0.1%     -94  -0.1%     -94    ROMFS/nsh_romfsimg.c
    -7.4%    -224  -7.4%    -224    src/modules/flight_mode_manager/FlightTasks_generated.cpp
    [DEL]    -400  [DEL]    -400    ../../src/lib/motion_planning/ManualVelocitySmoothingXY.cpp
    [DEL] -1.38Ki  [DEL] -1.38Ki    ../../src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
  -0.2%    -360  [ = ]       0    .debug_aranges
  -0.1%    -704  [ = ]       0    .symtab
     +39%    +320  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
     +53%    +128  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp
     +15%     +96  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp
    +9.3%     +64  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
    +3.0%     +32  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
    -0.0%     -48  [ = ]       0    [section .symtab]
    [DEL]    -192  [ = ]       0    ../../src/lib/motion_planning/ManualVelocitySmoothingXY.cpp
   -23.2%    -208  [ = ]       0    src/modules/flight_mode_manager/FlightTasks_generated.cpp
    [DEL]    -896  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
  -0.2% -1.03Ki  [ = ]       0    .debug_frame
  -0.2% -1.36Ki  [ = ]       0    .strtab
     +39%    +437  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
     +22%    +158  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
     +47%    +152  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp
     +12%     +87  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp
    +2.7%     +48  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
    -0.1%    -108  [ = ]       0    [section .strtab]
   -26.7%    -312  [ = ]       0    src/modules/flight_mode_manager/FlightTasks_generated.cpp
    [DEL]    -329  [ = ]       0    ../../src/lib/motion_planning/ManualVelocitySmoothingXY.cpp
    [DEL] -1.49Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
  -0.1% -2.05Ki  [ = ]       0    .debug_str
     +34% +1.03Ki  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
    +1.1%    +750  [ = ]       0    ../../src/modules/flight_mode_manager/FlightModeManager.cpp
     +13%     +87  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp
    +1.7%     +48  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
    +8.8%     +37  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp
     +29%     +15  [ = ]       0    ../../src/lib/motion_planning/ManualVelocitySmoothingZ.cpp
    +1.0%     +10  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
    -0.6%      -1  [ = ]       0    ../../src/lib/motion_planning/HeadingSmoothing.cpp
    -4.4%     -14  [ = ]       0    ../../src/lib/rtl/rtl_time_estimator.cpp
    -3.7%     -18  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
    -0.0%     -30  [ = ]       0    [section .debug_str]
    [DEL]     -73  [ = ]       0    ../../src/lib/motion_planning/ManualVelocitySmoothingXY.cpp
    -2.7%    -841  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
    -6.7% -1.41Ki  [ = ]       0    src/modules/flight_mode_manager/FlightTasks_generated.cpp
    [DEL] -1.64Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
  -0.2% -4.06Ki  [ = ]       0    .debug_abbrev
    -0.2% -4.06Ki  [ = ]       0    armv7-m/arm_vectors.c
  -0.3% -4.18Ki  [ = ]       0    .debug_ranges
     +16%    +832  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
    +4.2%    +160  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
    +2.2%     +48  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
    +2.0%     +48  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp
    +0.8%     +40  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
    +2.7%     +24  [ = ]       0    ../../src/modules/flight_mode_manager/FlightModeManager.cpp
    -1.0%     -40  [ = ]       0    src/modules/flight_mode_manager/FlightTasks_generated.cpp
    -0.6%     -80  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
   -10.5%    -328  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp
    -0.1%    -352  [ = ]       0    [section .debug_ranges]
    [DEL]    -584  [ = ]       0    ../../src/lib/motion_planning/ManualVelocitySmoothingXY.cpp
    [DEL] -3.95Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
  -0.2% -8.43Ki  [ = ]       0    .debug_line
     +15% +1.68Ki  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
    +7.5%    +556  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
    +6.6%    +407  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
    +5.8%    +407  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp
    +1.8%    +380  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
    +1.0%    +150  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
    +0.4%     +46  [ = ]       0    ../../src/modules/flight_mode_manager/FlightModeManager.cpp
    -0.4%    -111  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
    -4.0%    -312  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp
    -3.8%    -398  [ = ]       0    src/modules/flight_mode_manager/FlightTasks_generated.cpp
    [DEL] -2.25Ki  [ = ]       0    ../../src/lib/motion_planning/ManualVelocitySmoothingXY.cpp
    [DEL] -8.96Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
  -0.3% -14.7Ki  [ = ]       0    .debug_loc
     +26% +4.97Ki  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
    +3.6%    +539  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
    +5.4%    +457  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
    +5.2%    +457  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp
    +1.5%    +350  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
    +0.0%     +14  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
    -0.7%    -340  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
    -4.0%    -676  [ = ]       0    src/modules/flight_mode_manager/FlightTasks_generated.cpp
    -8.1%    -992  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp
    -0.2% -2.46Ki  [ = ]       0    [section .debug_loc]
    [DEL] -2.60Ki  [ = ]       0    ../../src/lib/motion_planning/ManualVelocitySmoothingXY.cpp
    [DEL] -14.4Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
  -0.3% -83.8Ki  [ = ]       0    .debug_info
     +12% +9.18Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
    +8.6% +7.10Ki  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
    +7.7% +6.30Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp
    +7.8% +6.30Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
    +4.1% +5.64Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
    +2.1% +2.69Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
    +0.3%    +327  [ = ]       0    ../../src/modules/flight_mode_manager/FlightModeManager.cpp
    -0.5%    -820  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
    -1.6% -1.49Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp
    -1.7% -2.17Ki  [ = ]       0    src/modules/flight_mode_manager/FlightTasks_generated.cpp
    [DEL] -18.9Ki  [ = ]       0    ../../src/lib/motion_planning/ManualVelocitySmoothingXY.cpp
    [DEL] -98.0Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
  -0.2%  -120Ki  -0.0%    -344    TOTAL

px4_fmu-v6x

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.5%    +335  [ = ]       0    [Unmapped]
  -0.0%    -336  -0.0%    -336    .text
     +28% +1.23Ki   +28% +1.23Ki    ../../src/lib/collision_prevention/CollisionPrevention.cpp
    +8.4%    +158  +8.4%    +158    ../../src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
    +4.1%    +146  +4.1%    +146    ../../src/modules/flight_mode_manager/FlightModeManager.cpp
    +7.7%    +114  +7.7%    +114    ../../src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp
    +0.8%     +58  +0.8%     +58    ../../src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
    +1.1%     +50  +1.1%     +50    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
    +6.2%     +50  +6.2%     +50    ../../src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp
    +1.1%     +40  +1.1%     +40    ../../src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
    -0.3%     -32  -0.3%     -32    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
    -0.0%     -54  -0.0%     -54    [section .text]
    -0.1%     -94  -0.1%     -94    ROMFS/nsh_romfsimg.c
    -7.4%    -224  -7.4%    -224    src/modules/flight_mode_manager/FlightTasks_generated.cpp
    [DEL]    -400  [DEL]    -400    ../../src/lib/motion_planning/ManualVelocitySmoothingXY.cpp
    [DEL] -1.38Ki  [DEL] -1.38Ki    ../../src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
  -0.2%    -360  [ = ]       0    .debug_aranges
  -0.1%    -704  [ = ]       0    .symtab
     +39%    +320  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
     +53%    +128  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp
     +15%     +96  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp
    +9.3%     +64  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
    +3.0%     +32  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
    -0.0%     -48  [ = ]       0    [section .symtab]
    [DEL]    -192  [ = ]       0    ../../src/lib/motion_planning/ManualVelocitySmoothingXY.cpp
   -23.2%    -208  [ = ]       0    src/modules/flight_mode_manager/FlightTasks_generated.cpp
    [DEL]    -896  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
  -0.2% -1.03Ki  [ = ]       0    .debug_frame
  -0.2% -1.36Ki  [ = ]       0    .strtab
     +39%    +437  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
     +22%    +158  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
     +47%    +152  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp
     +12%     +87  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp
    +2.7%     +48  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
    -0.1%    -108  [ = ]       0    [section .strtab]
   -26.7%    -312  [ = ]       0    src/modules/flight_mode_manager/FlightTasks_generated.cpp
    [DEL]    -329  [ = ]       0    ../../src/lib/motion_planning/ManualVelocitySmoothingXY.cpp
    [DEL] -1.49Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
  -0.1% -2.05Ki  [ = ]       0    .debug_str
     +34% +1.03Ki  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
    +1.1%    +750  [ = ]       0    ../../src/modules/flight_mode_manager/FlightModeManager.cpp
     +13%     +87  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp
    +2.0%     +58  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
    +8.8%     +37  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp
     +29%     +15  [ = ]       0    ../../src/lib/motion_planning/ManualVelocitySmoothingZ.cpp
    +1.0%     +10  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
    -0.6%      -1  [ = ]       0    ../../src/lib/motion_planning/HeadingSmoothing.cpp
    -4.4%     -14  [ = ]       0    ../../src/lib/rtl/rtl_time_estimator.cpp
    -3.7%     -18  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
    -0.0%     -30  [ = ]       0    [section .debug_str]
    [DEL]     -73  [ = ]       0    ../../src/lib/motion_planning/ManualVelocitySmoothingXY.cpp
    -2.7%    -841  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
    -6.7% -1.41Ki  [ = ]       0    src/modules/flight_mode_manager/FlightTasks_generated.cpp
    [DEL] -1.65Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
  -0.2% -4.06Ki  [ = ]       0    .debug_abbrev
    -0.2% -4.06Ki  [ = ]       0    armv7-m/arm_vectors.c
  -0.3% -4.18Ki  [ = ]       0    .debug_ranges
     +16%    +832  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
    +4.2%    +160  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
    +2.2%     +48  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
    +2.0%     +48  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp
    +0.8%     +40  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
    +2.7%     +24  [ = ]       0    ../../src/modules/flight_mode_manager/FlightModeManager.cpp
    -1.0%     -40  [ = ]       0    src/modules/flight_mode_manager/FlightTasks_generated.cpp
    -0.6%     -80  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
   -10.5%    -328  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp
    -0.1%    -352  [ = ]       0    [section .debug_ranges]
    [DEL]    -584  [ = ]       0    ../../src/lib/motion_planning/ManualVelocitySmoothingXY.cpp
    [DEL] -3.95Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
  -0.2% -8.43Ki  [ = ]       0    .debug_line
     +15% +1.68Ki  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
    +7.5%    +556  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
    +6.6%    +407  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
    +5.8%    +407  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp
    +1.8%    +380  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
    +1.0%    +150  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
    +0.4%     +46  [ = ]       0    ../../src/modules/flight_mode_manager/FlightModeManager.cpp
    -0.4%    -111  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
    -4.0%    -312  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp
    -3.8%    -398  [ = ]       0    src/modules/flight_mode_manager/FlightTasks_generated.cpp
    [DEL] -2.25Ki  [ = ]       0    ../../src/lib/motion_planning/ManualVelocitySmoothingXY.cpp
    [DEL] -8.96Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
  -0.3% -14.7Ki  [ = ]       0    .debug_loc
     +26% +4.97Ki  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
    +3.6%    +539  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
    +5.4%    +457  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
    +5.2%    +457  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp
    +1.5%    +337  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
    -0.7%    -340  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
    -4.1%    -689  [ = ]       0    src/modules/flight_mode_manager/FlightTasks_generated.cpp
    -8.1%    -992  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp
    -0.2% -2.47Ki  [ = ]       0    [section .debug_loc]
    [DEL] -2.60Ki  [ = ]       0    ../../src/lib/motion_planning/ManualVelocitySmoothingXY.cpp
    [DEL] -14.4Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
  -0.3% -83.4Ki  [ = ]       0    .debug_info
     +12% +9.18Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
    +8.7% +7.10Ki  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
    +7.7% +6.30Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp
    +7.9% +6.30Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
    +4.1% +5.64Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
    +2.1% +2.69Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
    +0.3%    +327  [ = ]       0    ../../src/modules/flight_mode_manager/FlightModeManager.cpp
    -0.5%    -820  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
    -1.6% -1.49Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp
    -1.7% -2.17Ki  [ = ]       0    src/modules/flight_mode_manager/FlightTasks_generated.cpp
    [DEL] -18.9Ki  [ = ]       0    ../../src/lib/motion_planning/ManualVelocitySmoothingXY.cpp
    [DEL] -97.5Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
  -0.3%  -120Ki  -0.0%    -336    TOTAL

MaEtUgR

Nice! Thanks for putting the effort in to not only make this work with FlightTaskManualAcceleration (current default Position mode) but also adding some improvements and bearing through all my partly unrelated review. I think it's at a good big iteration state now and we can follow up with individual improvements iteratively.

algora-pbc · 2024-11-25T07:57:42Z

💵 To receive payouts, sign up on Algora, link your Github account and connect with Stripe.

Claudio-Chies force-pushed the pr-Collision_Prevention branch 4 times, most recently from a3d8d8b to d130612 Compare August 9, 2024 13:01

Claudio-Chies changed the title ~~Collision Prevention for MPC Flightmode Acceleration~~ Collision Prevention for manual flight mode Aug 12, 2024

Claudio-Chies force-pushed the pr-Collision_Prevention branch 2 times, most recently from dcfa190 to 9cf4838 Compare August 12, 2024 09:58

Claudio-Chies marked this pull request as ready for review August 12, 2024 10:00

bresch reviewed Aug 12, 2024

View reviewed changes

src/lib/collision_prevention/CollisionPrevention.hpp Outdated Show resolved Hide resolved

Claudio-Chies force-pushed the pr-Collision_Prevention branch 6 times, most recently from 7fa1fcc to 077226b Compare August 20, 2024 08:51

Claudio-Chies requested review from bresch and MaEtUgR August 20, 2024 09:31

Claudio-Chies mentioned this pull request Aug 27, 2024

Collision Prevention Update PX4/PX4-user_guide#3364

Merged

1 task

Claudio-Chies force-pushed the pr-Collision_Prevention branch 2 times, most recently from 1e40431 to bd72ecf Compare October 1, 2024 07:23

MaEtUgR reviewed Oct 1, 2024

View reviewed changes

Claudio-Chies marked this pull request as draft October 2, 2024 08:46

Claudio-Chies force-pushed the pr-Collision_Prevention branch 2 times, most recently from e0b6707 to d346da5 Compare October 4, 2024 11:46

Claudio-Chies force-pushed the pr-Collision_Prevention branch 2 times, most recently from 6d72e11 to 1075492 Compare October 29, 2024 09:51

Claudio-Chies marked this pull request as ready for review October 31, 2024 10:36

Claudio-Chies force-pushed the pr-Collision_Prevention branch from c5834b2 to de8fc84 Compare November 5, 2024 14:16

MaEtUgR and others added 10 commits November 20, 2024 14:49

CollisionPrevention: move main functions to the top in the order they…

3378c9b

… get called

CollisionPrevention: only save quaternion and yaw on attitude update

0cdff14

CollisionPrevention: restore rate limited warning for no data, minor …

0092ac3

…cleanup

Remove FlightTaskManualPositionSmoothVel

d0a5faf

The default implementation for multicopter Position mode is FlightTaskManualAcceleration. The last missing piece was support for CollisionPrevention in this implementation.

CollisionPrevention: changed to resolution of 5 degrees, and adapted …

2200159

…tests to reflect the change rewrite of obstacle_distance merging methods, and fix of various issues

Remove FlightTaskManualPositionSmoothVel

27818e2

The default implementation for multicopter Position mode is FlightTaskManualAcceleration. The last missing piece was support for CollisionPrevention in this implementation.

CollisionPrevention: fix early return and use unified bin count + arr…

6eeab5e

…ay size 72

CollisionPrevention: slightly simplify _transformSetpoint()

36dd39a

CollisionPrevention: follow parameter variable naming convention

08f2f39

CollisionPrevention: prevent illegal array index with malicious obsta…

ec1efcc

…cle_dsitance message

Claudio-Chies force-pushed the pr-Collision_Prevention branch from f654979 to ec1efcc Compare November 20, 2024 13:51

PX4 deleted a comment from github-actions bot Nov 20, 2024

Claudio-Chies commented Nov 21, 2024

View reviewed changes

src/lib/collision_prevention/CollisionPrevention.cpp Show resolved Hide resolved

PX4 deleted a comment from github-actions bot Nov 21, 2024

Claudio-Chies force-pushed the pr-Collision_Prevention branch 2 times, most recently from 59edc23 to 3b4efab Compare November 21, 2024 09:51

PX4 deleted a comment from github-actions bot Nov 21, 2024

CollisionPrevention: Added Case where velocity gets reduced to zero i…

f05f032

…f we are closer to the obstacle than the minimal distance

Claudio-Chies force-pushed the pr-Collision_Prevention branch from 3b4efab to f05f032 Compare November 21, 2024 10:16

PX4 deleted a comment from github-actions bot Nov 21, 2024

MaEtUgR approved these changes Nov 21, 2024

View reviewed changes

PX4 deleted a comment from github-actions bot Nov 21, 2024

Claudio-Chies merged commit db13b9c into main Nov 21, 2024
62 checks passed

Claudio-Chies deleted the pr-Collision_Prevention branch November 21, 2024 10:36

algora-pbc bot mentioned this pull request Nov 25, 2024

Fix and rewrite collision prevention #22464

Closed

algora-pbc bot added the 🙋 Bounty claim label Nov 25, 2024

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Collision Prevention for multicopter acceleration based position flight mode #23507

Collision Prevention for multicopter acceleration based position flight mode #23507

Claudio-Chies commented Aug 7, 2024 •

edited by Igor-Misic

Loading

dirksavage88 commented Aug 13, 2024

Claudio-Chies commented Aug 13, 2024

MaEtUgR left a comment

github-actions bot commented Nov 21, 2024

MaEtUgR left a comment

algora-pbc bot commented Nov 25, 2024

Collision Prevention for multicopter acceleration based position flight mode #23507

Collision Prevention for multicopter acceleration based position flight mode #23507

Conversation

Claudio-Chies commented Aug 7, 2024 • edited by Igor-Misic Loading

Solved Problem

Associated PR's

Solution

Velocity compensation

Acceleration constraining

Drawbacks

Changelog Entry

Drawbacks

Alternatives

Test coverage

Context

Hardware Tests

1D Lidar

dirksavage88 commented Aug 13, 2024

Claudio-Chies commented Aug 13, 2024

MaEtUgR left a comment

Choose a reason for hiding this comment

github-actions bot commented Nov 21, 2024

FLASH Analysis

MaEtUgR left a comment

Choose a reason for hiding this comment

algora-pbc bot commented Nov 25, 2024

Claudio-Chies commented Aug 7, 2024 •

edited by Igor-Misic

Loading