From 090db6a1158984878ef0bdd74889f7753a3528d1 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Mon, 23 Jul 2018 16:33:17 +0200 Subject: [PATCH] FlightTask StraightLine: set max vel/acc/dec if value is bigger --- .../tasks/Utility/StraightLine.cpp | 22 +++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp b/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp index 4a9f4012a9..0b76bbaca9 100644 --- a/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp +++ b/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp @@ -187,22 +187,34 @@ void StraightLine::setOrigin(const matrix::Vector3f &origin) void StraightLine::setSpeed(const float &speed) { - if (speed > 0 && speed < getMaxVel()) { + float vel_max = getMaxVel(); + + if (speed > 0 && speed < vel_max) { _desired_speed = speed; + } else if (speed > vel_max) { + _desired_speed = vel_max; } } void StraightLine::setSpeedAtTarget(const float &speed_at_target) { - if (speed_at_target > 0 && speed_at_target < getMaxVel()) { + float vel_max = getMaxVel(); + + if (speed_at_target > 0 && speed_at_target < vel_max) { _desired_speed_at_target = speed_at_target; + } else if (speed_at_target > vel_max) { + _desired_speed_at_target = vel_max; } } void StraightLine::setAcceleration(const float &acc) { - if (acc > 0 && acc < getMaxAcc()) { + float acc_max = getMaxVel(); + + if (acc > 0 && acc < acc_max) { _desired_acceleration = acc; + } else if (acc > acc_max) { + _desired_acceleration = acc_max; } } @@ -210,5 +222,7 @@ void StraightLine::setDeceleration(const float &dec) { if (dec > 0 && dec < DECELERATION_MAX) { _desired_deceleration = dec; + } else if (dec > DECELERATION_MAX) { + _desired_deceleration = DECELERATION_MAX; } -} \ No newline at end of file +}