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

MC auto: add maximum RC assist distance during landing

Browse Source
This commit is contained in:
bresch
2023-02-05 13:44:45 +01:00
committed by Mathieu Bresciani
parent d6fa42fefd
commit c7bddda1db
6 changed files with 88 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
+25 -2
View File
@@ -229,9 +229,10 @@ void FlightTaskAuto::_prepareLandSetpoints()
}
if (_type_previous != WaypointType::land) {
// initialize yaw
// initialize yaw and xy-position
_land_heading = _yaw_setpoint;
_stick_acceleration_xy.resetPosition(Vector2f(_target(0), _target(1)));
_initial_land_position = Vector3f(_target(0), _target(1), NAN);
}
// Update xy-position in case of landing position changes (etc. precision landing)
@@ -248,7 +249,29 @@ void FlightTaskAuto::_prepareLandSetpoints()
_deltatime);
}
_stick_acceleration_xy.generateSetpoints(_sticks.getPitchRollExpo(), _yaw, _land_heading, _position,
Vector2f sticks_xy = _sticks.getPitchRollExpo();
Vector2f sticks_ne = sticks_xy;
Sticks::rotateIntoHeadingFrameXY(sticks_ne, _yaw, _land_heading);
const float distance_to_circle = math::trajectory::getMaxDistanceToCircle(_position.xy(), _initial_land_position.xy(),
_param_mpc_land_radius.get(), sticks_ne);
float max_speed;
if (PX4_ISFINITE(distance_to_circle)) {
max_speed = math::trajectory::computeMaxSpeedFromDistance(_stick_acceleration_xy.getMaxJerk(),
_stick_acceleration_xy.getMaxAcceleration(), distance_to_circle, 0.f);
if (max_speed < 0.5f) {
sticks_xy.setZero();
}
} else {
max_speed = 0.f;
sticks_xy.setZero();
}
_stick_acceleration_xy.setVelocityConstraint(max_speed);
_stick_acceleration_xy.generateSetpoints(sticks_xy, _yaw, _land_heading, _position,
_velocity_setpoint_feedback.xy(), _deltatime);
_stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint);