fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 07:23:34 +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-03-22 10:03:48 +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);
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
+3
View File
@@ -166,6 +166,7 @@ protected:
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
(ParamFloat<px4::params::MPC_LAND_CRWL>) _param_mpc_land_crawl_speed,
(ParamInt<px4::params::MPC_LAND_RC_HELP>) _param_mpc_land_rc_help,
(ParamFloat<px4::params::MPC_LAND_RADIUS>) _param_mpc_land_radius,
(ParamFloat<px4::params::MPC_LAND_ALT1>)
_param_mpc_land_alt1, // altitude at which we start ramping down speed
(ParamFloat<px4::params::MPC_LAND_ALT2>)
@@ -201,6 +202,8 @@ private:
WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
matrix::Vector3f _initial_land_position;
void _limitYawRate(); /**< Limits the rate of change of the yaw setpoint. */
bool _evaluateTriplets(); /**< Checks and sets triplets. */
bool _isFinite(const position_setpoint_s &sp); /**< Checks if all waypoint triplets are finite. */