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

FixedwingPositionControl: handle degenerate tangent setpoint in navigatePathTangent()

Browse Source
This commit is contained in:
Thomas Stastny 2023-08-21 13:57:10 +02:00
parent 413f66549d
commit 3ffb57bcce
1 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/fw_pos_control/FixedwingPositionControl.cpp
View File

@ -2926,12 +2926,16 @@ void FixedwingPositionControl::navigateLoiter(const Vector2f &loiter_center, con
loiter_center + unit_vec_center_to_closest_pt * radius, path_curvature);
}
void FixedwingPositionControl::navigatePathTangent(const matrix::Vector2f &vehicle_pos,
const matrix::Vector2f &position_setpoint,
const matrix::Vector2f &tangent_setpoint,
const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature)
{
if (tangent_setpoint.norm() <= FLT_EPSILON) {
// degenerate case: no direction. maintain the last npfg command.
return;
}
const Vector2f unit_path_tangent{tangent_setpoint.normalized()};
_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
_closest_point_on_path = position_setpoint;