Speed warnings fix (#200)

* Initial implementation of check platform speeds only when a SimplePhysicsPlatform is created. * Removed old code. Co-authored-by: Miguel Yermo <[email protected]> Co-authored-by: Lukas Winiwarter <[email protected]>
3dgeo-heidelberg · Mar 11, 2022 · d58185c · d58185c
1 parent 79b047a
commit d58185c
Show file tree

Hide file tree

Showing 6 changed files with 64 additions and 48 deletions.
diff --git 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);
 }
diff --git 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
@@ -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
@@ -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
@@ -17,6 +17,34 @@ void SimplePhysicsPlatform::_clone(std::shared_ptr<Platform> 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
@@ -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();
+
 };