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

PositionSmoothing: refactor _getMaxZSpeed()

Browse Source
This commit is contained in:
Matthias Grob 2024-11-28 17:34:35 +01:00
parent 092e5e8f9d
commit 7dcfeb2f77
1 changed files with 10 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -106,27 +106,22 @@ float PositionSmoothing::_getMaxXYSpeed(const Vector3f(&waypoints)[3]) const
float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const
{
const auto &start_position = waypoints[0];
const auto &target = waypoints[1];
const auto &next_target = waypoints[2];
const Vector3f &start_position = waypoints[0];
const Vector3f &target = waypoints[1];
const Vector3f &next_target = waypoints[2];
const Vector2f start_position_xy_z = {start_position.xy().norm(), start_position(2)};
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());
const float distance_start_target = fabs(target(2) - pos_traj(2));
float arrival_z_speed = 0.0f;
const bool target_next_different = fabsf(target(2) - next_target(2)) > 0.001f;
const float distance_target_next = fabsf(target(2) - next_target(2));
const bool target_next_different = distance_target_next > 0.001f;
Vector2f target_xy_z = {target.xy().norm(), target(2)};
Vector2f next_target_xy_z = {next_target.xy().norm(), next_target(2)};
Vector2f start_position_xy_z = {start_position.xy().norm(), start_position(2)};
if (target_next_different
) {
if (target_next_different) {
const float alpha = acosf(Vector2f((target_xy_z - start_position_xy_z)).unit_or_zero().dot(
Vector2f((target_xy_z - next_target_xy_z)).unit_or_zero()));
@ -137,6 +132,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));
float max_speed = math::min(_trajectory[2].getMaxVel(), math::trajectory::computeMaxSpeedFromDistance(
_trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(),
distance_start_target, arrival_z_speed));