From 15ec73629bad9087a3ae52dd95dae87966eb2701 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 27 Jun 2019 14:07:48 +0200 Subject: [PATCH] MPC auto - Add MPC_YAW_MODE: towards waypoint (yaw first) mode. This mode ensures that the vehicle yaws towards the next waypoint before accelerating. This is required for drones with front vision and aerodynamic multicopters such as standard vtol planes or highspeed multirotors. --- .../FlightTasks/tasks/Auto/FlightTaskAuto.cpp | 9 ++ .../FlightTasks/tasks/Auto/FlightTaskAuto.hpp | 1 + .../tasks/AutoLine/FlightTaskAutoLine.cpp | 15 +++- .../tasks/AutoLine/FlightTaskAutoLine.hpp | 1 + .../FlightTaskAutoLineSmoothVel.cpp | 90 ++++++++++--------- .../mc_pos_control/mc_pos_control_params.c | 3 +- 6 files changed, 74 insertions(+), 45 deletions(-) diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index 47ccead8ee..fa50ce4ce3 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp @@ -118,6 +118,8 @@ void FlightTaskAuto::_limitYawRate() { const float yawrate_max = math::radians(_param_mpc_yawrauto_max.get()); + _yaw_sp_aligned = true; + if (PX4_ISFINITE(_yaw_setpoint) && PX4_ISFINITE(_yaw_sp_prev)) { // Limit the rate of change of the yaw setpoint const float dyaw_desired = matrix::wrap_pi(_yaw_setpoint - _yaw_sp_prev); @@ -126,10 +128,16 @@ void FlightTaskAuto::_limitYawRate() _yaw_setpoint = _yaw_sp_prev + dyaw; _yaw_setpoint = matrix::wrap_pi(_yaw_setpoint); _yaw_sp_prev = _yaw_setpoint; + + // The yaw setpoint is aligned when its rate is not saturated + _yaw_sp_aligned = fabsf(dyaw_desired) < fabsf(dyaw_max); } if (PX4_ISFINITE(_yawspeed_setpoint)) { _yawspeed_setpoint = math::constrain(_yawspeed_setpoint, -yawrate_max, yawrate_max); + + // The yaw setpoint is aligned when its rate is not saturated + _yaw_sp_aligned = fabsf(_yawspeed_setpoint) < yawrate_max; } } @@ -300,6 +308,7 @@ void FlightTaskAuto::_set_heading_from_mode() switch (_param_mpc_yaw_mode.get()) { case 0: // Heading points towards the current waypoint. + case 4: // Same as 0 but yaw fisrt and then go v = Vector2f(_target) - Vector2f(_position); break; diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp index 4a42310978..aaa63da236 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp @@ -108,6 +108,7 @@ protected: int _mission_gear = landing_gear_s::GEAR_KEEP; float _yaw_sp_prev = NAN; + bool _yaw_sp_aligned{false}; ObstacleAvoidance _obstacle_avoidance; /**< class adjusting setpoints according to external avoidance module's input */ diff --git a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp index 8dab82f238..b62dc0086d 100644 --- a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp +++ b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp @@ -49,8 +49,19 @@ void FlightTaskAutoLine::_generateSetpoints() _generateHeadingAlongTrack(); } - _generateAltitudeSetpoints(); - _generateXYsetpoints(); + if (_param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned) { + // Wait for the yaw setpoint to be aligned + if (!_position_locked) { + _velocity_setpoint.setAll(0.f); + _position_setpoint = _position; + _position_locked = true; + } + + } else { + _position_locked = false; + _generateAltitudeSetpoints(); + _generateXYsetpoints(); + } } void FlightTaskAutoLine::_setSpeedAtTarget() diff --git a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp index e73225e6d8..9b0c2d623b 100644 --- a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp +++ b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp @@ -66,4 +66,5 @@ protected: private: void _setSpeedAtTarget(); /**< Sets desiered speed at target */ float _speed_at_target = 0.0f; + bool _position_locked{false}; }; diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index 2e49d41876..a4682c5d3a 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -145,61 +145,67 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints() _checkEkfResetCounters(); _want_takeoff = false; - if (PX4_ISFINITE(_position_setpoint(0)) && - PX4_ISFINITE(_position_setpoint(1))) { - // Use position setpoints to generate velocity setpoints + if (_param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned) { + // Wait for the yaw setpoint to be aligned + _velocity_setpoint.setAll(0.f); - // Get various path specific vectors. */ - Vector2f pos_traj; - pos_traj(0) = _trajectory[0].getCurrentPosition(); - pos_traj(1) = _trajectory[1].getCurrentPosition(); - Vector2f pos_sp_xy(_position_setpoint); - Vector2f pos_traj_to_dest(pos_sp_xy - pos_traj); - Vector2f u_prev_to_dest = Vector2f(pos_sp_xy - Vector2f(_prev_wp)).unit_or_zero(); - Vector2f prev_to_pos(pos_traj - Vector2f(_prev_wp)); - Vector2f closest_pt = Vector2f(_prev_wp) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest); - Vector2f u_pos_traj_to_dest_xy(Vector2f(pos_traj_to_dest).unit_or_zero()); + } else { + if (PX4_ISFINITE(_position_setpoint(0)) && + PX4_ISFINITE(_position_setpoint(1))) { + // Use position setpoints to generate velocity setpoints - // Compute the maximum possible velocity on the track given the remaining distance, the maximum acceleration and the maximum jerk. - // We assume a constant acceleration profile with a delay of 2*accel/jerk (time to reach the desired acceleration from opposite max acceleration) - // Equation to solve: 0 = vel^2 - 2*acc*(x - vel*2*acc/jerk) - // To avoid high gain at low distance due to the sqrt, we take the minimum of this velocity and a slope of "traj_p" m/s per meter - float b = 4.f * _param_mpc_acc_hor.get() * _param_mpc_acc_hor.get() / _param_mpc_jerk_auto.get(); - float c = - 2.f * _param_mpc_acc_hor.get() * pos_traj_to_dest.length(); - float max_speed = 0.5f * (-b + sqrtf(b * b - 4.f * c)); - float speed_sp_track = math::min(max_speed, pos_traj_to_dest.length() * _param_mpc_xy_traj_p.get()); - speed_sp_track = math::constrain(speed_sp_track, 0.0f, _mc_cruise_speed); + // Get various path specific vectors. */ + Vector2f pos_traj; + pos_traj(0) = _trajectory[0].getCurrentPosition(); + pos_traj(1) = _trajectory[1].getCurrentPosition(); + Vector2f pos_sp_xy(_position_setpoint); + Vector2f pos_traj_to_dest(pos_sp_xy - pos_traj); + Vector2f u_prev_to_dest = Vector2f(pos_sp_xy - Vector2f(_prev_wp)).unit_or_zero(); + Vector2f prev_to_pos(pos_traj - Vector2f(_prev_wp)); + Vector2f closest_pt = Vector2f(_prev_wp) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest); + Vector2f u_pos_traj_to_dest_xy(Vector2f(pos_traj_to_dest).unit_or_zero()); - Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * speed_sp_track; + // Compute the maximum possible velocity on the track given the remaining distance, the maximum acceleration and the maximum jerk. + // We assume a constant acceleration profile with a delay of 2*accel/jerk (time to reach the desired acceleration from opposite max acceleration) + // Equation to solve: 0 = vel^2 - 2*acc*(x - vel*2*acc/jerk) + // To avoid high gain at low distance due to the sqrt, we take the minimum of this velocity and a slope of "traj_p" m/s per meter + float b = 4.f * _param_mpc_acc_hor.get() * _param_mpc_acc_hor.get() / _param_mpc_jerk_auto.get(); + float c = - 2.f * _param_mpc_acc_hor.get() * pos_traj_to_dest.length(); + float max_speed = 0.5f * (-b + sqrtf(b * b - 4.f * c)); + float speed_sp_track = math::min(max_speed, pos_traj_to_dest.length() * _param_mpc_xy_traj_p.get()); + speed_sp_track = math::constrain(speed_sp_track, 0.0f, _mc_cruise_speed); - for (int i = 0; i < 2; i++) { - // If available, constrain the velocity using _velocity_setpoint(.) - if (PX4_ISFINITE(_velocity_setpoint(i))) { - _velocity_setpoint(i) = _constrainOneSide(vel_sp_xy(i), _velocity_setpoint(i)); + Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * speed_sp_track; - } else { - _velocity_setpoint(i) = vel_sp_xy(i); + for (int i = 0; i < 2; i++) { + // If available, constrain the velocity using _velocity_setpoint(.) + if (PX4_ISFINITE(_velocity_setpoint(i))) { + _velocity_setpoint(i) = _constrainOneSide(vel_sp_xy(i), _velocity_setpoint(i)); + + } else { + _velocity_setpoint(i) = vel_sp_xy(i); + } + + _velocity_setpoint(i) += (closest_pt(i) - _trajectory[i].getCurrentPosition()) * + _param_mpc_xy_traj_p.get(); // Along-track setpoint + cross-track P controller } - _velocity_setpoint(i) += (closest_pt(i) - _trajectory[i].getCurrentPosition()) * - _param_mpc_xy_traj_p.get(); // Along-track setpoint + cross-track P controller } - } + 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 - 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 + // If available, constrain the velocity using _velocity_setpoint(.) + if (PX4_ISFINITE(_velocity_setpoint(2))) { + _velocity_setpoint(2) = _constrainOneSide(vel_sp_z, _velocity_setpoint(2)); - // If available, constrain the velocity using _velocity_setpoint(.) - if (PX4_ISFINITE(_velocity_setpoint(2))) { - _velocity_setpoint(2) = _constrainOneSide(vel_sp_z, _velocity_setpoint(2)); + } else { + _velocity_setpoint(2) = vel_sp_z; + } - } else { - _velocity_setpoint(2) = vel_sp_z; + _want_takeoff = _velocity_setpoint(2) < -0.3f; } - - _want_takeoff = _velocity_setpoint(2) < -0.3f; } } 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 b3d3f3ff45..8fd2d49a6d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -762,11 +762,12 @@ PARAM_DEFINE_FLOAT(MPC_SPOOLUP_TIME, 1.0f); * Specifies the heading in Auto. * * @min 0 - * @max 3 + * @max 4 * @value 0 towards waypoint * @value 1 towards home * @value 2 away from home * @value 3 along trajectory + * @value 4 towards waypoint (yaw first) * @group Mission */ PARAM_DEFINE_INT32(MPC_YAW_MODE, 0);