From 837df6c4a3bd1d44bd80d70aaf2524f58fdf7777 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 11 Apr 2019 11:46:52 +0200 Subject: [PATCH 1/2] FlightTaskAuto: separate default speed and limit It wasn't possible to fly faster than cruise speed even if planned in the mission. Limiting the planned cruise speed is necessary because the smoothed trajectory mission plans to the _mc_cruise_speed and if that's higher than the maximum it gets capped for safety by the position controller and the result is a jerky flight. --- src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index 23af261629ab..ef58a7b99572 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp @@ -128,11 +128,14 @@ bool FlightTaskAuto::_evaluateTriplets() // Always update cruise speed since that can change without waypoint changes. _mc_cruise_speed = _sub_triplet_setpoint->get().current.cruising_speed; - if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f) || (_mc_cruise_speed > _constraints.speed_xy)) { - // Use default limit. + if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f)) { + // If no speed is planned use the default cruise speed as limit _mc_cruise_speed = _constraints.speed_xy; } + // Ensure planned cruise speed is below the maximum such that the smooth trajectory doesn't get capped + _mc_cruise_speed = math::min(_mc_cruise_speed, _param_mpc_xy_vel_max.get()); + // Temporary target variable where we save the local reprojection of the latest navigator current triplet. Vector3f tmp_target; From 9abc7f384869f08bf43f3105b1f7d951016008fe Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 3 Apr 2019 11:02:02 +0200 Subject: [PATCH 2/2] FlightTasks: fix mission DO_CHANGE_SPEED This fixes the issue where the DO_CHANGE_SPEED command was ignored and the drone always travelled at the MPC_XY_CRUISE velocity. --- .../tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index 226f0231bfd7..57b4df131dfa 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -167,7 +167,7 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints() Vector2f u_pos_traj_to_dest_xy(Vector2f(pos_traj_to_dest).unit_or_zero()); float speed_sp_track = Vector2f(pos_traj_to_dest).length() * _param_mpc_xy_traj_p.get(); - speed_sp_track = math::constrain(speed_sp_track, 0.0f, _param_mpc_xy_cruise.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;