diff --git a/AirLib/include/common/VectorMath.hpp b/AirLib/include/common/VectorMath.hpp index 114a673416..732994eb8e 100644 --- a/AirLib/include/common/VectorMath.hpp +++ b/AirLib/include/common/VectorMath.hpp @@ -324,6 +324,9 @@ class VectorMathT { static Vector3T toAngularVelocity(const QuaternionT& start, const QuaternionT& end, RealT dt) { + if (dt == 0) + return Vector3T(0, 0, 0); + RealT p_s, r_s, y_s; toEulerianAngle(start, p_s, r_s, y_s); diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PidController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PidController.hpp index 69128e6a6d..fdc53880a9 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PidController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PidController.hpp @@ -99,7 +99,7 @@ class PidController : public IUpdatable { if (dt > min_dt_) { integrator->update(dt, error, last_time_); - float error_der = (error - last_error_) / dt; + float error_der = dt > 0 ? (error - last_error_) / dt : 0; dterm = error_der * config_.kd; last_error_ = error; } diff --git a/MavLinkCom/MavLinkTest/Commands.h b/MavLinkCom/MavLinkTest/Commands.h index 3113f60edb..c05f51a6ea 100644 --- a/MavLinkCom/MavLinkTest/Commands.h +++ b/MavLinkCom/MavLinkTest/Commands.h @@ -691,7 +691,7 @@ class PidController proportionalGain = error * kProportional_; } if (kDerivative_ != 0) { - float derivative = (error - previous_error_) / dt; + float derivative = dt > 0 ? (error - previous_error_) / dt : 0; derivativeGain = derivative * kDerivative_; } if (kIntegral_ != 0) { diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp index 0f3ab3946b..ee89d0c3bb 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp @@ -514,8 +514,8 @@ void PawnSimApi::updateKinematics(float dt) next_kinematics.twist.angular = msr::airlib::VectorMath::toAngularVelocity( kinematics_->getPose().orientation, next_kinematics.pose.orientation, dt); - next_kinematics.accelerations.linear = (next_kinematics.twist.linear - kinematics_->getTwist().linear) / dt; - next_kinematics.accelerations.angular = (next_kinematics.twist.angular - kinematics_->getTwist().angular) / dt; + next_kinematics.accelerations.linear = dt > 0 ? (next_kinematics.twist.linear - kinematics_->getTwist().linear) / dt : next_kinematics.accelerations.linear; + next_kinematics.accelerations.angular = dt > 0 ? (next_kinematics.twist.angular - kinematics_->getTwist().angular) / dt : next_kinematics.accelerations.angular; kinematics_->setState(next_kinematics); kinematics_->update();