fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-08-21 12:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

capture position once to rule out changes while function runs

Browse Source
This commit is contained in:
Balduin
2025-07-23 10:30:57 +02:00
parent 7105c6df3e
commit aedf280b01
1 changed files with 4 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
+4 -3
View File
@@ -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);