fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 17:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

PositionSmoothing: refactor _getMaxZSpeed()

Browse Source
This commit is contained in:
Matthias Grob
2024-12-02 14:52:31 +01:00
parent 092e5e8f9d
commit 7dcfeb2f77
1 changed files with 10 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/motion_planning/PositionSmoothing.cpp
+10 -14
View File
@@ -106,27 +106,22 @@ float PositionSmoothing::_getMaxXYSpeed(const Vector3f(&waypoints)[3]) const
float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const
{ {
const auto &start_position = waypoints[0]; const Vector3f &start_position = waypoints[0];
const auto &target = waypoints[1]; const Vector3f &target = waypoints[1];
const auto &next_target = waypoints[2]; 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(), Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
_trajectory[1].getCurrentPosition(), _trajectory[1].getCurrentPosition(),
_trajectory[2].getCurrentPosition()); _trajectory[2].getCurrentPosition());
const float distance_start_target = fabs(target(2) - pos_traj(2));
float arrival_z_speed = 0.0f; 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)); if (target_next_different) {
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
) {
const float alpha = acosf(Vector2f((target_xy_z - start_position_xy_z)).unit_or_zero().dot( 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())); 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()); 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( float max_speed = math::min(_trajectory[2].getMaxVel(), math::trajectory::computeMaxSpeedFromDistance(
_trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(), _trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(),
distance_start_target, arrival_z_speed)); distance_start_target, arrival_z_speed));