fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added possibility to modify the start position from external sources, as its done in the _getMaxXYSpeed

Browse Source
This commit is contained in:
Claudio Chies 2024-11-29 10:51:13 +01:00 committed by Matthias Grob
parent 7dcfeb2f77
commit 1a165a4956
1 changed files with 5 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/lib/motion_planning/PositionSmoothing.cpp
View File

@ -106,7 +106,10 @@ float PositionSmoothing::_getMaxXYSpeed(const Vector3f(&waypoints)[3]) const
float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const
{
const Vector3f &start_position = waypoints[0];
const Vector3f &start_position = {_trajectory[0].getCurrentPosition(),
_trajectory[1].getCurrentPosition(),
_trajectory[2].getCurrentPosition()
};
const Vector3f &target = waypoints[1];
const Vector3f &next_target = waypoints[2];
@ -114,10 +117,6 @@ float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const
const Vector2f target_xy_z = {target.xy().norm(), target(2)};
const Vector2f next_target_xy_z = {next_target.xy().norm(), next_target(2)};
Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
_trajectory[1].getCurrentPosition(),
_trajectory[2].getCurrentPosition());
float arrival_z_speed = 0.0f;
const bool target_next_different = fabsf(target(2) - next_target(2)) > 0.001f;
@ -132,7 +131,7 @@ float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const
arrival_z_speed = math::min(max_speed_in_turn, _trajectory[2].getMaxVel());
}
const float distance_start_target = fabs(target(2) - pos_traj(2));
const float distance_start_target = fabs(target(2) - start_position(2));
float max_speed = math::min(_trajectory[2].getMaxVel(), math::trajectory::computeMaxSpeedFromDistance(
_trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(),
distance_start_target, arrival_z_speed));