diff --git a/src/lib/mathlib/math/TrajMath.hpp b/src/lib/mathlib/math/TrajMath.hpp index 163ec7ef0a..959b454692 100644 --- a/src/lib/mathlib/math/TrajMath.hpp +++ b/src/lib/mathlib/math/TrajMath.hpp @@ -110,5 +110,44 @@ inline float computeBrakingDistanceFromVelocity(const float velocity, const floa return velocity * (velocity / (2.0f * accel) + accel_delay_max / jerk); } +/* Compute the maximum distance between a point and a circle given a direction vector pointing from the point + * towards the circle. The point can be inside or outside the circle. + * _ + * ,=' '=, __ + * P-->------/-------A Distance = PA + * Dir | x | + * \ / + * "=,_,=" + * Equation to solve: ||(point - circle_pos) + direction_unit * distance_to_circle|| = radius + * + * @param pos position of the point + * @param circle_pos position of the center of the circle + * @param radius radius of the circle + * @param direction vector pointing from the point towards the circle + * + * @return longest distance between the point to the circle in the direction indicated by the vector or NAN if the + * vector does not point towards the circle + */ +inline float getMaxDistanceToCircle(const matrix::Vector2f &pos, const matrix::Vector2f &circle_pos, float radius, + const matrix::Vector2f &direction) +{ + matrix::Vector2f center_to_pos = pos - circle_pos; + const float b = 2.f * center_to_pos.dot(direction.unit_or_zero()); + const float c = center_to_pos.norm_squared() - radius * radius; + const float delta = b * b - 4.f * c; + + float distance_to_circle; + + if (delta >= 0.f && direction.longerThan(0.f)) { + distance_to_circle = fmaxf((-b + sqrtf(delta)) / 2.f, 0.f); + + } else { + // Never intersecting the circle + distance_to_circle = NAN; + } + + return distance_to_circle; +} + } /* namespace traj */ } /* namespace math */ diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index a5d0c00aab..23743cf3d0 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -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); diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index 5fb0b83ec0..8fead25cbb 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -166,6 +166,7 @@ protected: (ParamFloat) _param_mpc_land_speed, (ParamFloat) _param_mpc_land_crawl_speed, (ParamInt) _param_mpc_land_rc_help, + (ParamFloat) _param_mpc_land_radius, (ParamFloat) _param_mpc_land_alt1, // altitude at which we start ramping down speed (ParamFloat) @@ -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. */ diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp index d1b7743625..7363685cfc 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp @@ -75,7 +75,8 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, { // maximum commanded acceleration and velocity Vector2f acceleration_scale(_param_mpc_acc_hor.get(), _param_mpc_acc_hor.get()); - Vector2f velocity_scale(_param_mpc_vel_manual.get(), _param_mpc_vel_manual.get()); + const float velocity_sc = fminf(_param_mpc_vel_manual.get(), _velocity_constraint); + Vector2f velocity_scale(velocity_sc, velocity_sc); acceleration_scale *= 2.f; // because of drag the average acceleration is half diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp index 0389838563..8bc2bb7531 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp @@ -60,6 +60,9 @@ public: void generateSetpoints(matrix::Vector2f stick_xy, const float yaw, const float yaw_sp, const matrix::Vector3f &pos, const matrix::Vector2f &vel_sp_feedback, const float dt); void getSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, matrix::Vector3f &acc_sp); + float getMaxAcceleration() { return _param_mpc_acc_hor.get(); }; + float getMaxJerk() { return _param_mpc_jerk_max.get(); }; + void setVelocityConstraint(float vel) { _velocity_constraint = fmaxf(vel, FLT_EPSILON); }; private: void applyJerkLimit(const float dt); @@ -79,6 +82,8 @@ private: matrix::Vector2f _acceleration_setpoint; matrix::Vector2f _acceleration_setpoint_prev; + float _velocity_constraint{INFINITY}; + DEFINE_PARAMETERS( (ParamFloat) _param_mpc_vel_manual, (ParamFloat) _param_mpc_vel_man_side, diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 50b541a6ed..181ae47e5a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -478,6 +478,20 @@ PARAM_DEFINE_FLOAT(MPC_LAND_CRWL, 0.3f); */ PARAM_DEFINE_INT32(MPC_LAND_RC_HELP, 0); +/** + * User assisted landing radius + * + * When user assisted descent is enabled (see MPC_LAND_RC_HELP), + * this parameter controls the maximum position adjustment + * allowed from the original landing point. + * + * @unit m + * @min 0 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_LAND_RADIUS, 1000.f); + /** * Takeoff climb rate *