From aedf280b01116cdee91eacec7eb03e4463f284c9 Mon Sep 17 00:00:00 2001 From: Balduin Date: Wed, 23 Jul 2025 10:30:57 +0200 Subject: [PATCH] capture position once to rule out changes while function runs --- .../flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp | 7 ++++--- 1 file changed, 4 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 048bc34c6d..f7b9d103aa 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -267,12 +267,13 @@ void FlightTaskAuto::_prepareLandSetpoints() Sticks::rotateIntoHeadingFrameXY(sticks_ne, _yaw, _land_heading); const bool land_radius_enabled = _param_mpc_land_radius.get() > 0.0f; - const bool position_valid = Vector2f(_position).isAllFinite(); + const Vector3f pos = _position; // To avoid it updating in between + const bool position_valid = Vector2f(pos).isAllFinite(); float max_speed = INFINITY; if (position_valid && land_radius_enabled) { - const float distance_to_circle = math::trajectory::getMaxDistanceToCircle(_position.xy(), _initial_land_position.xy(), + const float distance_to_circle = math::trajectory::getMaxDistanceToCircle(pos.xy(), _initial_land_position.xy(), _param_mpc_land_radius.get(), sticks_ne); if (PX4_ISFINITE(distance_to_circle)) { @@ -293,7 +294,7 @@ void FlightTaskAuto::_prepareLandSetpoints() PX4_INFO(" max speed: %.2f", (double) max_speed); _stick_acceleration_xy.setVelocityConstraint(max_speed); - _stick_acceleration_xy.generateSetpoints(sticks_xy, _yaw, _land_heading, _position, + _stick_acceleration_xy.generateSetpoints(sticks_xy, _yaw, _land_heading, pos, _velocity_setpoint_feedback.xy(), _deltatime); _stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint);