From b4bcbb22fb661aaf839fd11f6fda212df0484591 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 27 Aug 2026 19:07:00 +0200 Subject: [PATCH] fix(FlightTaskAuto): passed waypoint check in 3D This should not have real downsides but in priciple the driftaway could also happen vertically. --- src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp | 4 ++-- src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 9bd8b488068..1af8065e355 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -768,8 +768,8 @@ bool FlightTaskAuto::isTargetModified() const bool FlightTaskAuto::_hasPassedCurrentWaypoint() const { - const Vector2f u_previous_to_current = Vector2f(_triplet_current - _triplet_previous).unit_or_zero(); - return u_previous_to_current * Vector2f(_triplet_current - _position) < 0.f; + const Vector3f u_previous_to_current = (_triplet_current - _triplet_previous).unit_or_zero(); + return u_previous_to_current * (_triplet_current - _position) < 0.f; } void FlightTaskAuto::_updateTrajConstraints() diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index 46a54c467fb..db9a0637a53 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -195,6 +195,6 @@ private: bool _evaluatePositionSetpointTriplet(); bool _isFinite(const position_setpoint_s &sp); /**< Checks if all waypoint triplets are finite. */ bool _evaluateGlobalReference(); /**< Check is global reference is available. */ - bool _hasPassedCurrentWaypoint() const; /**< True if the vehicle is horizontally past the current waypoint */ + bool _hasPassedCurrentWaypoint() const; /**< True if the vehicle is past the current waypoint */ void _set_heading_from_mode(); /**< @see MPC_YAW_MODE */ };