From 9e8c3ff0dcf2bb2a2614a16d6b8a84116bc96b97 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Mon, 23 Jul 2018 16:34:54 +0200 Subject: [PATCH] FlightTask StraightLine: check if target and origin are the same --- src/lib/FlightTasks/tasks/Utility/StraightLine.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp b/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp index 79a52f41c5..2147470c02 100644 --- a/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp +++ b/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp @@ -100,6 +100,11 @@ void StraightLine::generateSetpoints(matrix::Vector3f &position_setpoint, matrix float StraightLine::getMaxAcc() { + // check if origin and target are different points + if ((_target - _origin).length() < FLT_EPSILON) { + return MPC_ACC_HOR_MAX.get(); + } + // unit vector in the direction of the straight line Vector3f u_orig_to_target = (_target - _origin).unit_or_zero(); @@ -130,6 +135,11 @@ float StraightLine::getMaxAcc() float StraightLine::getMaxVel() { + // check if origin and target are different points + if ((_target - _origin).length() < FLT_EPSILON) { + return MPC_XY_VEL_MAX.get(); + } + // unit vector in the direction of the straight line Vector3f u_orig_to_target = (_target - _origin).unit_or_zero();