From 56ea4fa6faf014e871f20ba6f9fb55fa74a06e89 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Fri, 10 Aug 2018 15:59:34 +0200 Subject: [PATCH] FlightTask StraighLine: check values before dividing --- src/lib/FlightTasks/tasks/Utility/StraightLine.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp b/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp index da3a32404f..681fab26b6 100644 --- a/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp +++ b/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp @@ -83,7 +83,11 @@ void StraightLine::generateSetpoints(matrix::Vector3f &position_setpoint, matrix float speed_sp = dist_to_target > acc_dec_distance ? _desired_speed : _desired_speed_at_target; float max_acc_dec = speed_sp > speed_sp_prev ? _desired_acceleration : -_desired_deceleration; - float acc_track = (speed_sp - speed_sp_prev) / _deltatime; + float acc_track = 0.0f; + + if (_deltatime > FLT_EPSILON) { + acc_track = (speed_sp - speed_sp_prev) / _deltatime; + } if (fabs(acc_track) > fabs(max_acc_dec)) { // accelerate/decelerate with desired acceleration/deceleration towards target @@ -192,7 +196,7 @@ void StraightLine::setSpeed(const float &speed) { float vel_max = getMaxVel(); - if (speed > 0 && speed < vel_max) { + if (speed > FLT_EPSILON && speed < vel_max) { _desired_speed = speed; } else if (speed > vel_max) { @@ -204,7 +208,7 @@ void StraightLine::setSpeedAtTarget(const float &speed_at_target) { float vel_max = getMaxVel(); - if (speed_at_target > 0 && speed_at_target < vel_max) { + if (speed_at_target > FLT_EPSILON && speed_at_target < vel_max) { _desired_speed_at_target = speed_at_target; } else if (speed_at_target > vel_max) { @@ -216,7 +220,7 @@ void StraightLine::setAcceleration(const float &acc) { float acc_max = getMaxVel(); - if (acc > 0 && acc < acc_max) { + if (acc > FLT_EPSILON && acc < acc_max) { _desired_acceleration = acc; } else if (acc > acc_max) { @@ -226,7 +230,7 @@ void StraightLine::setAcceleration(const float &acc) void StraightLine::setDeceleration(const float &dec) { - if (dec > 0 && dec < DECELERATION_MAX) { + if (dec > FLT_EPSILON && dec < DECELERATION_MAX) { _desired_deceleration = dec; } else if (dec > DECELERATION_MAX) {