diff --git a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index b52c5dfef1..ccde6fd37e 100644 --- a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -36,8 +36,8 @@ */ #include "FlightTaskAutoLineSmoothVel.hpp" -#include -#include + +#include "TrajectoryConstraints.hpp" using namespace matrix; @@ -178,53 +178,55 @@ float FlightTaskAutoLineSmoothVel::_constrainAbs(float val, float max) return math::sign(val) * math::min(fabsf(val), fabsf(max)); } -float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget(float next_target_speed) const +float FlightTaskAutoLineSmoothVel::_getMaxXYSpeed() const { - // Compute the maximum allowed speed at the waypoint assuming that we want to - // connect the two lines (prev-current and current-next) - // with a tangent circle with constant speed and desired centripetal acceleration: a_centripetal = speed^2 / radius - // The circle should in theory start and end at the intersection of the lines and the waypoint's acceptance radius. - // This is not exactly true in reality since Navigator switches the waypoint so we have to take in account that - // the real acceptance radius is smaller. - // It can be that the next waypoint is the last one or that the drone will have to stop for some other reason - // so we have to make sure that the speed at the current waypoint allows to stop at the next waypoint. - float speed_at_target = 0.0f; - - const float distance_current_next = (_target - _next_wp).xy().norm(); - const bool waypoint_overlap = (_target - _prev_wp).xy().norm() < _target_acceptance_radius; - const bool yaw_align_check_pass = (_param_mpc_yaw_mode.get() != 4) || _yaw_sp_aligned; - - - if (distance_current_next > 0.001f && - !waypoint_overlap && - yaw_align_check_pass) { - Vector3f pos_traj; - pos_traj(0) = _trajectory[0].getCurrentPosition(); - pos_traj(1) = _trajectory[1].getCurrentPosition(); - pos_traj(2) = _trajectory[2].getCurrentPosition(); - // Max speed between current and next - const float max_speed_current_next = _getMaxSpeedFromDistance(distance_current_next, next_target_speed); - const float alpha = acosf(Vector2f((_target - pos_traj).xy()).unit_or_zero().dot( - Vector2f((_target - _next_wp).xy()).unit_or_zero())); - // We choose a maximum centripetal acceleration of MPC_ACC_HOR * MPC_XY_TRAJ_P to take in account - // that there is a jerk limit (a direct transition from line to circle is not possible) - // MPC_XY_TRAJ_P should be between 0 and 1. - float accel_tmp = _param_mpc_xy_traj_p.get() * _param_mpc_acc_hor.get(); - float max_speed_in_turn = math::trajectory::computeMaxSpeedInWaypoint(alpha, - accel_tmp, - _target_acceptance_radius); - speed_at_target = math::min(math::min(max_speed_in_turn, max_speed_current_next), _mc_cruise_speed); + Vector3f pos_traj(_trajectory[0].getCurrentPosition(), + _trajectory[1].getCurrentPosition(), + _trajectory[2].getCurrentPosition()); + + math::trajectory::VehicleDynamicLimits config; + config.z_accept_rad = _param_nav_mc_alt_rad.get(); + config.xy_accept_rad = _target_acceptance_radius; + config.max_acc_xy = _trajectory[0].getMaxAccel(); + config.max_jerk = _trajectory[0].getMaxJerk(); + config.max_speed_xy = _mc_cruise_speed; + config.max_acc_xy_radius_scale = _param_mpc_xy_traj_p.get(); + + Vector3f waypoints[3]; + waypoints[0] = pos_traj; + + // constrain velocity to go to the position setpoint first if our tracking is really bad + if ((pos_traj - _position_setpoint).longerThan(_target_acceptance_radius)) { + waypoints[1] = _position_setpoint; + waypoints[2] = _target; + + } else { + waypoints[1] = _target; + waypoints[2] = _next_wp; } - return speed_at_target; + float max_speed = math::trajectory::computeXYSpeedFromWaypoints<3>(waypoints, config); + + return max_speed; } -float FlightTaskAutoLineSmoothVel::_getMaxSpeedFromDistance(float braking_distance, float final_speed) const +float FlightTaskAutoLineSmoothVel::_getMaxZSpeed() const { - float max_speed = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_auto.get(), - _param_mpc_acc_hor.get(), - braking_distance, - final_speed); + Vector3f pos_traj(_trajectory[0].getCurrentPosition(), + _trajectory[1].getCurrentPosition(), + _trajectory[2].getCurrentPosition()); + float z_setpoint = _target(2); + + if ((pos_traj - _position_setpoint).longerThan(_target_acceptance_radius)) { + z_setpoint = _position_setpoint(2); + } + + const float distance_start_target = fabs(z_setpoint - pos_traj(2)); + const float arrival_z_speed = 0.f; + + float max_speed = math::min(_trajectory[2].getMaxVel(), math::trajectory::computeMaxSpeedFromDistance( + _trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(), + distance_start_target, arrival_z_speed)); return max_speed; } @@ -242,33 +244,43 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints() _velocity_setpoint.setAll(0.f); } else { - if (PX4_ISFINITE(_position_setpoint(0)) && - PX4_ISFINITE(_position_setpoint(1))) { - // Use position setpoints to generate velocity setpoints + const bool xy_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1)); + const bool z_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(2)); - // Get various path specific vectors - Vector3f pos_traj; - pos_traj(0) = _trajectory[0].getCurrentPosition(); - pos_traj(1) = _trajectory[1].getCurrentPosition(); - pos_traj(2) = _trajectory[2].getCurrentPosition(); - Vector2f pos_traj_to_dest_xy = (_position_setpoint - pos_traj).xy(); - Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero()); + if (xy_pos_setpoint_valid && z_pos_setpoint_valid) { + // Use 3D position setpoint to generate a 3D velocity setpoint + Vector3f pos_traj(_trajectory[0].getCurrentPosition(), + _trajectory[1].getCurrentPosition(), + _trajectory[2].getCurrentPosition()); + Vector3f u_pos_traj_to_dest = (_position_setpoint - pos_traj).unit_or_zero(); - const bool has_reached_altitude = fabsf(_position_setpoint(2) - pos_traj(2)) < _param_nav_mc_alt_rad.get(); + const float xy_speed = _getMaxXYSpeed(); + const float z_speed = _getMaxZSpeed(); - // If the drone has to change altitude, stop at the waypoint, otherwise fly through - const float arrival_speed = has_reached_altitude ? _getSpeedAtTarget(0.f) : 0.f; - const Vector2f max_arrival_vel = u_pos_traj_to_dest_xy * arrival_speed; + Vector3f vel_sp_constrained = u_pos_traj_to_dest * sqrtf(xy_speed * xy_speed + z_speed * z_speed); + math::trajectory::clampToXYNorm(vel_sp_constrained, xy_speed); + math::trajectory::clampToZNorm(vel_sp_constrained, z_speed); - Vector2f vel_abs_max_xy(_getMaxSpeedFromDistance(fabsf(pos_traj_to_dest_xy(0)), max_arrival_vel(0)), - _getMaxSpeedFromDistance(fabsf(pos_traj_to_dest_xy(1)), max_arrival_vel(1))); + for (int i = 0; i < 3; i++) { + // If available, constrain the velocity using _velocity_setpoint(.) + if (PX4_ISFINITE(_velocity_setpoint(i))) { + _velocity_setpoint(i) = _constrainOneSide(vel_sp_constrained(i), _velocity_setpoint(i)); + } else { + _velocity_setpoint(i) = vel_sp_constrained(i); + } + } + } - const Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * _mc_cruise_speed; + else if (xy_pos_setpoint_valid) { + // Use 2D position setpoint to generate a 2D velocity setpoint - // Constrain the norm of each component using min and max values - Vector2f vel_sp_constrained_xy(_constrainAbs(vel_sp_xy(0), vel_abs_max_xy(0)), - _constrainAbs(vel_sp_xy(1), vel_abs_max_xy(1))); + // Get various path specific vectors + Vector2f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition()); + Vector2f pos_traj_to_dest_xy = Vector2f(_position_setpoint.xy()) - pos_traj; + Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero()); + + Vector2f vel_sp_constrained_xy = u_pos_traj_to_dest_xy * _getMaxXYSpeed(); for (int i = 0; i < 2; i++) { // If available, constrain the velocity using _velocity_setpoint(.) @@ -281,9 +293,11 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints() } } - if (PX4_ISFINITE(_position_setpoint(2))) { - const float vel_sp_z = (_position_setpoint(2) - _trajectory[2].getCurrentPosition()) * - _param_mpc_z_traj_p.get(); // Generate a velocity target for the trajectory using a simple P loop + else if (z_pos_setpoint_valid) { + // Use Z position setpoint to generate a Z velocity setpoint + + const float z_dir = math::sign(_position_setpoint(2) - _trajectory[2].getCurrentPosition()); + const float vel_sp_z = z_dir * _getMaxZSpeed(); // If available, constrain the velocity using _velocity_setpoint(.) if (PX4_ISFINITE(_velocity_setpoint(2))) { @@ -292,9 +306,9 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints() } else { _velocity_setpoint(2) = vel_sp_z; } - - _want_takeoff = _velocity_setpoint(2) < -0.3f; } + + _want_takeoff = _velocity_setpoint(2) < -0.3f; } } @@ -361,7 +375,7 @@ void FlightTaskAutoLineSmoothVel::_generateTrajectory() _trajectory[i].updateDurations(_velocity_setpoint(i)); } - VelocitySmoothing::timeSynchronization(_trajectory, 2); // Synchronize x and y only + VelocitySmoothing::timeSynchronization(_trajectory, 3); _jerk_setpoint = jerk_sp_smooth; _acceleration_setpoint = accel_sp_smooth; diff --git a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp index 1ec21c6d2d..65ea01c6e6 100644 --- a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp +++ b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp @@ -71,9 +71,8 @@ protected: static float _constrainAbs(float val, float max); /** Constrain the value -max <= val <= max */ - /** Give 0 if next is the last target **/ - float _getSpeedAtTarget(float next_target_speed) const; - float _getMaxSpeedFromDistance(float braking_distance, float final_speed) const; + float _getMaxXYSpeed() const; + float _getMaxZSpeed() const; void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */ void _updateTrajConstraints(); @@ -91,7 +90,6 @@ protected: (ParamFloat) _param_mpc_acc_up_max, (ParamFloat) _param_mpc_acc_down_max, (ParamFloat) _param_mpc_jerk_auto, - (ParamFloat) _param_mpc_xy_traj_p, - (ParamFloat) _param_mpc_z_traj_p + (ParamFloat) _param_mpc_xy_traj_p ); }; diff --git a/src/lib/mathlib/math/TrajMath.hpp b/src/lib/mathlib/math/TrajMath.hpp index ad042583ce..4d662ff8cc 100644 --- a/src/lib/mathlib/math/TrajMath.hpp +++ b/src/lib/mathlib/math/TrajMath.hpp @@ -66,7 +66,8 @@ inline float computeMaxSpeedFromDistance(const float jerk, const float accel, co float c = - 2.0f * accel * braking_distance - sqr(final_speed); float max_speed = 0.5f * (-b + sqrtf(sqr(b) - 4.0f * c)); - return max_speed; + // don't slow down more than the end speed, even if the conservative accel ramp time requests it + return max(max_speed, final_speed); } /* Compute the maximum tangential speed in a circle defined by two line segments of length "d" 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 25fa314289..80e1ccccea 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -259,16 +259,6 @@ PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.0f); */ PARAM_DEFINE_FLOAT(MPC_XY_TRAJ_P, 0.5f); -/** - * Proportional gain for vertical trajectory position error - * - * @min 0.1 - * @max 1.0 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_Z_TRAJ_P, 0.3f); - /** * Cruise speed when angle prev-current/current-next setpoint * is 90 degrees. It should be lower than MPC_XY_CRUISE.