MPC: Survey: Added a turning radius to altitude changes in FlightModeAuto

[Claudio-Chies]

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:

Without Fix:

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:

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

• Tested in SIH, Flightreview
• Tested on Hardware FlightReview
  

[github-actions]

FLASH Analysis

px4_fmu-v5x

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0% +3.52Ki  [ = ]       0    .debug_info
  +7.3% +3.52Ki  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
+0.0% +2.08Ki  [ = ]       0    .debug_loc
  +8.0% +1.86Ki  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  +0.0%    +220  [ = ]       0    [section .debug_loc]
+0.0%    +660  [ = ]       0    .debug_line
  +7.0%    +660  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
+0.0%    +456  [ = ]       0    .debug_ranges
   +10%    +456  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
+0.0%    +264  +0.0%    +264    .text
  +8.6%    +250  +8.6%    +250    ../../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    .symtab
  +5.9%     +32  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
+0.0%     +24  [ = ]       0    .debug_frame
+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.4%      +5  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  -0.0%      -9  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
-1.1%    -261  [ = ]       0    [Unmapped]
+0.0% +6.82Ki  +0.0%    +264    TOTAL

px4_fmu-v6x

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0% +3.52Ki  [ = ]       0    .debug_info
  +7.3% +3.52Ki  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
+0.0% +2.08Ki  [ = ]       0    .debug_loc
  +8.0% +1.86Ki  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  +0.0%    +220  [ = ]       0    [section .debug_loc]
+0.0%    +660  [ = ]       0    .debug_line
  +7.0%    +660  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
+0.0%    +456  [ = ]       0    .debug_ranges
   +10%    +456  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
+0.0%    +264  +0.0%    +264    .text
  +8.6%    +250  +8.6%    +250    ../../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    .symtab
  +5.9%     +32  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
+0.0%     +24  [ = ]       0    .debug_frame
+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.4%      +5  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  -0.0%      -9  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
-0.4%    -265  [ = ]       0    [Unmapped]
+0.0% +6.82Ki  +0.0%    +264    TOTAL

Updated: 2024-11-29T10:57:58

[MaEtUgR]

Thanks for looking into it! We also tested on the real vehicle and it solves the issue.
I allowed myself to commit some refactoring which should not change the result.
We should really opt for a 3D solution of this whole calculation because it seems to add duplication make it hard to follow all the steps. But I agree it's a bigger change we need to tackle. I see the vertical calculation is now analogous to

PX4-Autopilot/src/lib/motion_planning/TrajectoryConstraints.hpp

Lines 87 to 92 in ab32001

	const float alpha = acosf(Vector2f((target - start_position).xy()).unit_or_zero().dot(
	Vector2f((target - next_target).xy()).unit_or_zero()));
	const float safe_alpha = constrain(alpha, 0.f, M_PI_F - FLT_EPSILON);
	float accel_tmp = config.max_acc_xy_radius_scale * config.max_acc_xy;
	float max_speed_in_turn = computeMaxSpeedInWaypoint(safe_alpha, accel_tmp, config.xy_accept_rad);
	speed_at_target = min(max_speed_in_turn, exit_speed, config.max_speed_xy);

but honestly can't follow and verify every step of both the versions exactly.

[MaEtUgR]

Thanks for checking. Your latest commit is exactly what I was thinking about but I wanted to confirm with you 👍