diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp index 71ad256c2c..a4617d0e1e 100644 --- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp +++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp @@ -197,7 +197,11 @@ void ManualSmoothingXY::_velocitySlewRate(matrix::Vector2f &vel_sp, const float dt) { // Adjust velocity setpoint if demand exceeds acceleration. / - matrix::Vector2f acc = (vel_sp - _vel_sp_prev) / dt; + matrix::Vector2f acc{}; + + if (dt > FLT_EPSILON) { + acc = (vel_sp - _vel_sp_prev) / dt; + } if (acc.length() > _acc_state_dependent) { vel_sp = acc.normalized() * _acc_state_dependent * dt + _vel_sp_prev; diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp index 6c30f6794d..9b11fc3132 100644 --- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp +++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp @@ -146,7 +146,12 @@ void ManualSmoothingZ::velocitySlewRate(float &vel_sp, const float dt) { // limit vertical acceleration - float acc = (vel_sp - _vel_sp_prev) / dt; + float acc = 0.f; + + if (dt > FLT_EPSILON) { + acc = (vel_sp - _vel_sp_prev) / dt; + } + float max_acc = (acc < 0.0f) ? -_acc_state_dependent : _acc_state_dependent; if (fabsf(acc) > fabsf(max_acc)) {