Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

MPC: Survey: Added a turning radius to altitude changes in surveys #23971

Open
wants to merge 2 commits into
base: main
Choose a base branch
from
Open

MPC: Survey: Added a turning radius to altitude changes in surveys #23971

wants to merge 2 commits into from

Conversation

Claudio-Chies
Copy link
Contributor

Solved Problem

When conducting a survey, the drone tended to slow down and accelerate again when covering large altitude changes, with terrain following enabled.

Example Survey:
image

Without Fix:
image
This is best seen when looking at the velocity change in x and z direction in the plot above.

Solution

To solve this issue i added a turning radius for the vertical component, whereby I simplify the problem so that we consider the 2D case where the vertical component of the incoming vector is the norm of the xy component, and the z component.
I also removed the requirement for the XY turning radius that the altitude between the current and next waypoint have to be the same.
With Fix:
image
As we can see in the Plot above, the velocity changes close to the waypoints are gone.

Changelog Entry

For release notes:

Bugfix: removed slowdown during ground following surveys with large altitude changes.

Alternatives

We could also directly generalize it for the 3D case, but that would require alot more changes.

Test coverage

@Claudio-Chies
Included turning radius calculations for vertical changes and removed…
b53f849 
… the requirement for the same altitude in the 2D turning radius logic.
@Claudio-Chies Claudio-Chies self-assigned this Nov 19, 2024
@github-actions GitHub Actions
Copy link

FLASH Analysis

px4_fmu-v5x 
    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0% +3.62Ki  [ = ]       0    .debug_info
    +7.5% +3.62Ki  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  +0.0% +2.41Ki  [ = ]       0    .debug_loc
    +9.3% +2.16Ki  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
    +0.0%    +256  [ = ]       0    [section .debug_loc]
  +0.0%    +673  [ = ]       0    .debug_line
    +7.1%    +673  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  +0.0%    +416  [ = ]       0    .debug_ranges
    +9.4%    +416  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  +0.0%    +248  +0.0%    +248    .text
    +8.0%    +234  +8.0%    +234    ../../src/lib/motion_planning/PositionSmoothing.cpp
    +0.0%     +14  +0.0%     +14    [section .text]
  +0.0%     +68  [ = ]       0    .strtab
    +7.0%     +68  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  +0.0%     +32  [ = ]       0    .debug_frame
  +0.0%     +32  [ = ]       0    .symtab
    +5.9%     +32  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  +0.0%      +8  [ = ]       0    .debug_aranges
  +0.0%      +3  [ = ]       0    .debug_abbrev
    +0.0%      +3  [ = ]       0    armv7-m/arm_vectors.c
  -0.0%      -4  [ = ]       0    .debug_str
    +0.3%      +4  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
    -0.0%      -8  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  -1.0%    -252  [ = ]       0    [Unmapped]
  +0.0% +7.22Ki  +0.0%    +248    TOTAL
px4_fmu-v6x 
    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0% +3.62Ki  [ = ]       0    .debug_info
    +7.5% +3.62Ki  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  +0.0% +2.41Ki  [ = ]       0    .debug_loc
    +9.3% +2.16Ki  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
    +0.0%    +256  [ = ]       0    [section .debug_loc]
  +0.0%    +673  [ = ]       0    .debug_line
    +7.1%    +673  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  +0.0%    +416  [ = ]       0    .debug_ranges
    +9.4%    +416  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  +0.0%    +248  +0.0%    +248    .text
    +8.0%    +234  +8.0%    +234    ../../src/lib/motion_planning/PositionSmoothing.cpp
    +0.0%     +14  +0.0%     +14    [section .text]
  +0.0%     +68  [ = ]       0    .strtab
    +7.0%     +68  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  +0.0%     +32  [ = ]       0    .debug_frame
  +0.0%     +32  [ = ]       0    .symtab
    +5.9%     +32  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  +0.0%      +8  [ = ]       0    .debug_aranges
  +0.0%      +3  [ = ]       0    .debug_abbrev
    +0.0%      +3  [ = ]       0    armv7-m/arm_vectors.c
  -0.0%      -4  [ = ]       0    .debug_str
    +0.3%      +4  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
    -0.0%      -8  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  -0.3%    -252  [ = ]       0    [Unmapped]
  +0.0% +7.22Ki  +0.0%    +248    TOTAL

@Claudio-Chies
MPC: PositonSmoothing, change test to reflect that we have to come in…
2bfb198 
…withing the acceptance radius, and not exact position.
@github-actions GitHub Actions
Copy link

FLASH Analysis

px4_fmu-v5x 
    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0% +3.62Ki  [ = ]       0    .debug_info
    +7.5% +3.62Ki  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  +0.0% +2.41Ki  [ = ]       0    .debug_loc
    +9.3% +2.16Ki  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
    +0.0%    +256  [ = ]       0    [section .debug_loc]
  +0.0%    +673  [ = ]       0    .debug_line
    +7.1%    +673  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  +0.0%    +416  [ = ]       0    .debug_ranges
    +9.4%    +416  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  +0.0%    +248  +0.0%    +248    .text
    +8.0%    +234  +8.0%    +234    ../../src/lib/motion_planning/PositionSmoothing.cpp
    +0.0%     +14  +0.0%     +14    [section .text]
  +0.0%     +68  [ = ]       0    .strtab
    +7.0%     +68  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  +0.0%     +32  [ = ]       0    .debug_frame
  +0.0%     +32  [ = ]       0    .symtab
    +5.9%     +32  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  +0.0%      +8  [ = ]       0    .debug_aranges
  +0.0%      +3  [ = ]       0    .debug_abbrev
    +0.0%      +3  [ = ]       0    armv7-m/arm_vectors.c
  -0.0%      -4  [ = ]       0    .debug_str
    +0.3%      +4  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
    -0.0%      -8  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  -1.0%    -252  [ = ]       0    [Unmapped]
  +0.0% +7.22Ki  +0.0%    +248    TOTAL
px4_fmu-v6x 
    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0% +3.62Ki  [ = ]       0    .debug_info
    +7.5% +3.62Ki  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  +0.0% +2.41Ki  [ = ]       0    .debug_loc
    +9.3% +2.16Ki  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
    +0.0%    +256  [ = ]       0    [section .debug_loc]
  +0.0%    +673  [ = ]       0    .debug_line
    +7.1%    +673  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  +0.0%    +416  [ = ]       0    .debug_ranges
    +9.4%    +416  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  +0.0%    +248  +0.0%    +248    .text
    +8.0%    +234  +8.0%    +234    ../../src/lib/motion_planning/PositionSmoothing.cpp
    +0.0%     +14  +0.0%     +14    [section .text]
  +0.0%     +68  [ = ]       0    .strtab
    +7.0%     +68  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  +0.0%     +32  [ = ]       0    .debug_frame
  +0.0%     +32  [ = ]       0    .symtab
    +5.9%     +32  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  +0.0%      +8  [ = ]       0    .debug_aranges
  +0.0%      +3  [ = ]       0    .debug_abbrev
    +0.0%      +3  [ = ]       0    armv7-m/arm_vectors.c
  -0.0%      -4  [ = ]       0    .debug_str
    +0.3%      +4  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
    -0.0%      -8  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  -0.3%    -252  [ = ]       0    [Unmapped]
  +0.0% +7.22Ki  +0.0%    +248    TOTAL

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@MaEtUgR MaEtUgR Awaiting requested review from MaEtUgR

At least 1 approving review is required to merge this pull request.

Assignees

@Claudio-Chies Claudio-Chies

Labels
Multirotor 🛸
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
1 participant
@Claudio-Chies