forked from Archive/PX4-Autopilot
MC auto: add maximum RC assist distance during landing
This commit is contained in:
parent
d6fa42fefd
commit
c7bddda1db
|
@ -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 */
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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. */
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
|
||||
(ParamFloat<px4::params::MPC_VEL_MAN_SIDE>) _param_mpc_vel_man_side,
|
||||
|
|
|
@ -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
|
||||
*
|
||||
|
|
Loading…
Reference in New Issue