From d58185c18644ba05ff5b47052a80ca8cad4510c3 Mon Sep 17 00:00:00 2001 From: miguelyermo-heidelberg <83340465+miguelyermo-heidelberg@users.noreply.github.com> Date: Fri, 11 Mar 2022 17:15:30 +0100 Subject: [PATCH] Speed warnings fix (#200) * Initial implementation of check platform speeds only when a SimplePhysicsPlatform is created. * Removed old code. Co-authored-by: Miguel Yermo Co-authored-by: Lukas Winiwarter --- src/platform/GroundVehiclePlatform.cpp | 2 +- src/platform/HelicopterPlatform.cpp | 11 ++++++---- src/platform/MovingPlatform.cpp | 27 ++++--------------------- src/platform/MovingPlatform.h | 24 ++++------------------ src/platform/SimplePhysicsPlatform.cpp | 28 ++++++++++++++++++++++++++ src/platform/SimplePhysicsPlatform.h | 20 ++++++++++++++++++ 6 files changed, 64 insertions(+), 48 deletions(-) diff --git a/src/platform/GroundVehiclePlatform.cpp b/src/platform/GroundVehiclePlatform.cpp index 0dfe17d0c..2db2b8d86 100644 --- a/src/platform/GroundVehiclePlatform.cpp +++ b/src/platform/GroundVehiclePlatform.cpp @@ -117,5 +117,5 @@ void GroundVehiclePlatform::setDestination(glm::dvec3 dest) { } void GroundVehiclePlatform::prepareSimulation(int simFrequency_hz) { - MovingPlatform::prepareSimulation(simFrequency_hz); + SimplePhysicsPlatform::prepareSimulation(simFrequency_hz); } \ No newline at end of file diff --git a/src/platform/HelicopterPlatform.cpp b/src/platform/HelicopterPlatform.cpp index 114cab4d2..ace23527f 100644 --- a/src/platform/HelicopterPlatform.cpp +++ b/src/platform/HelicopterPlatform.cpp @@ -59,7 +59,7 @@ void HelicopterPlatform::prepareSimulation(int simFrequency_hz){ cfg_slowdownFactor = 1.0 - cfg_slowdown_magnitude / simFrequency_hz; cfg_speedupFactor = 1.0 + cfg_speedup_magnitude / simFrequency_hz; Platform::prepareSimulation(simFrequency_hz); - MovingPlatform::prepareSimulation(simFrequency_hz); + SimplePhysicsPlatform::prepareSimulation(simFrequency_hz); } void HelicopterPlatform::initLegManual(){ // Set directional attitude @@ -90,22 +90,25 @@ void HelicopterPlatform::initLeg(){ } bool HelicopterPlatform::waypointReached(){ + bool wayPointReached{}; if(smoothTurn && cache_turning){ cache_turnIterations--; if(cache_turnIterations <= 0){ cache_turning = false; cache_speedUpFinished = false; logging::INFO("Waypoint passed (smooth turn)!"); - return true; + wayPointReached = true; } } if(MovingPlatform::waypointReached()){ cache_turning = false; cache_speedUpFinished = false; - return true; + wayPointReached = true; } - return false; + + if (wayPointReached) SimplePhysicsPlatform::checkSpeedLimit(); + return wayPointReached; } void HelicopterPlatform::updateStaticCache(){ diff --git a/src/platform/MovingPlatform.cpp b/src/platform/MovingPlatform.cpp index ec02c6218..cef953f1f 100644 --- a/src/platform/MovingPlatform.cpp +++ b/src/platform/MovingPlatform.cpp @@ -46,10 +46,10 @@ void MovingPlatform::doSimStep(int simFrequency_hz) { } } -void MovingPlatform::prepareSimulation(int simFrequency_hz) { - movePerSec_m_stepMagnitude = - cfg_settings_movePerSec_m / (double)simFrequency_hz; -} +//void MovingPlatform::prepareSimulation(int simFrequency_hz) { +// movePerSec_m_stepMagnitude = +// cfg_settings_movePerSec_m / (double)simFrequency_hz; +//} void MovingPlatform::initLegManual() { // Set Platform Orientation towards destination @@ -112,25 +112,6 @@ bool MovingPlatform::waypointReached() { // m / (m/cycle) => cycles left to reach waypoint bool result = (glm::l2Norm(cached_vectorToTarget) / glm::l2Norm(velocity)) < 1.0; if (result) { - if (!engineLimitReached) { - if (userSpeedLimitReached) { - logging::INFO("User speed (movePerSec_m) reached."); - } else { - logging::INFO("Leg is too short to achieve " - "the desired (movePerSec_m) speed."); - } - } else { - if (userSpeedLimitReached) { - logging::INFO("User speed (movePerSec_m) reached."); - } else { - logging::INFO("User speed (movePerSec_m) not reached " - "due to engine limitations. " - "Consider increasing the variable " - "engine_max_force in your " - "platform settings."); } - } - engineLimitReached = false; - userSpeedLimitReached = false; logging::INFO("Waypoint reached!"); } return result; diff --git a/src/platform/MovingPlatform.h b/src/platform/MovingPlatform.h index a5cc9e65a..74f2ffef1 100644 --- a/src/platform/MovingPlatform.h +++ b/src/platform/MovingPlatform.h @@ -15,22 +15,6 @@ class MovingPlatform : public Platform { */ glm::dvec3 velocity = glm::dvec3(0, 0, 0); -protected: - /** - * @brief How many meter does the platform move in each simulation step - */ - double movePerSec_m_stepMagnitude = 0.0; - /** - * @brief Flag to store if the engine max thrust was reached in a given leg. - The value is reset en each leg. - */ - bool engineLimitReached = false; - /** - * @brief Flag to store if the user-provided movePerSec_m speed was achieved - for a given leg. - */ - bool userSpeedLimitReached = false; - public: // *** CONSTRUCTION / DESTRUCTION *** // // ************************************ // @@ -68,10 +52,10 @@ class MovingPlatform : public Platform { * @see Platform::waypointReached */ bool waypointReached() override; - /** - * @see Platform::prepareSimulation - */ - void prepareSimulation(int simFrequency_hz) override; +// /** +// * @see Platform::prepareSimulation +// */ +// void prepareSimulation(int simFrequency_hz) override; /** * @see Platform::getVelocity */ diff --git a/src/platform/SimplePhysicsPlatform.cpp b/src/platform/SimplePhysicsPlatform.cpp index a6fb11e18..29d8983d1 100644 --- a/src/platform/SimplePhysicsPlatform.cpp +++ b/src/platform/SimplePhysicsPlatform.cpp @@ -17,6 +17,34 @@ void SimplePhysicsPlatform::_clone(std::shared_ptr p){ // *** M E T H O D S *** // // *********************** // +void SimplePhysicsPlatform::prepareSimulation(int simFrequency_hz) { + movePerSec_m_stepMagnitude = + cfg_settings_movePerSec_m / (double)simFrequency_hz; +} + +void SimplePhysicsPlatform::checkSpeedLimit() +{ + if (!engineLimitReached) { + if (userSpeedLimitReached) { + logging::INFO("User speed (movePerSec_m) reached."); + } else { + logging::INFO("Leg is too short to achieve " + "the desired (movePerSec_m) speed."); + } + } else { + if (userSpeedLimitReached) { + logging::INFO("User speed (movePerSec_m) reached."); + } else { + logging::INFO("User speed (movePerSec_m) not reached " + "due to engine limitations. " + "Consider increasing the variable " + "engine_max_force in your " + "platform settings."); } + } + engineLimitReached = false; + userSpeedLimitReached = false; +} + void SimplePhysicsPlatform::doPhysicsStep(int simFrequency_hz) { // ############## BEGIN Update vehicle position ################# glm::dvec3 drag_accel = getVelocity() * (mCfg_drag * simFrequency_hz); diff --git a/src/platform/SimplePhysicsPlatform.h b/src/platform/SimplePhysicsPlatform.h index 14b80bfa0..997e1a572 100644 --- a/src/platform/SimplePhysicsPlatform.h +++ b/src/platform/SimplePhysicsPlatform.h @@ -17,6 +17,20 @@ class SimplePhysicsPlatform : public MovingPlatform { * @brief Gravity acceleration vector */ glm::dvec3 mCfg_g_accel = glm::dvec3(0, 0, -9.81); + /** + * @brief How many meter does the platform move in each simulation step + */ + double movePerSec_m_stepMagnitude = 0.0; + /** + * @brief Flag to store if the engine max thrust was reached in a given leg. + The value is reset en each leg. + */ + bool engineLimitReached = false; + /** + * @brief Flag to store if the user-provided movePerSec_m speed was achieved + for a given leg. + */ + bool userSpeedLimitReached = false; public: /** * @brief Drag magnitude @@ -34,6 +48,10 @@ class SimplePhysicsPlatform : public MovingPlatform { // *** M E T H O D S *** // // *********************** // + /** + * @see Platform::prepareSimulation + */ + void prepareSimulation(int simFrequency_hz) override; /** * @brief Phyisics step for simple phyisics platform simulation * @param simFrequency_hz Simulation frequency @@ -49,4 +67,6 @@ class SimplePhysicsPlatform : public MovingPlatform { */ virtual void doControlStep(int simFrequency_hz); + void checkSpeedLimit(); + }; \ No newline at end of file