From 1454694bddd0a40a9da3c4170b0d28f5ab1efcf5 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Thu, 11 Apr 2019 11:46:52 +0200
Subject: [PATCH] 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 23af261629..ef58a7b995 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;
--
GitLab