MC auto: add maximum RC assist distance during landing

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

View File

@ -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 */

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);

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. */

View File

@ -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

View File

@ -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,

View File

@ -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
*