diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index 47ccead8ee84..fa50ce4ce3f2 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 4a4231097852..aaa63da236ad 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp @@ -108,6 +108,7 @@ class FlightTaskAuto : public FlightTask 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 8dab82f238f8..b62dc0086d07 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 e73225e6d87c..9b0c2d623bf2 100644 --- a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp +++ b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp @@ -66,4 +66,5 @@ class FlightTaskAutoLine : public FlightTaskAutoMapper 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 2e49d4187618..a4682c5d3a32 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 - - // 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()); - - // 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); - - Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * speed_sp_track; - - 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); + if (_param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned) { + // Wait for the yaw setpoint to be aligned + _velocity_setpoint.setAll(0.f); + + } else { + if (PX4_ISFINITE(_position_setpoint(0)) && + PX4_ISFINITE(_position_setpoint(1))) { + // Use position setpoints to generate velocity setpoints + + // 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()); + + // 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); + + Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * speed_sp_track; + + 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 b3d3f3ff4572..8fd2d49a6d4f 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);