Revert "DUNE/Monitors/Servo: Reduced monitor constraints."

This reverts commit b7a1d5a.

etc/auv/monitors.ini

@@ -203,8 +203,13 @@ Deactivation Time                       = 0
 Debug Level                             = None
 Execution Priority                      = 10
 Position Fault Detection                = true
+Position Rate Factor                    = 0.1
+Position Error Threshold                = 12.0
+Position Error Delay                    = 5.0
+Position Error Samples                  = 5
 Failure Error Timeout                   = 120.0
 Failure Error Count                     = 5
+Maximum Rotation Rate                   = 333.3
 Current Lower Threshold                 = 0.5
 Current Upper Threshold                 = 0.9
 Maximum Lower Faults                    = 20

etc/testing/replays/servomonitor-replay.ini

@@ -48,6 +48,11 @@ Deactivation Time                       = 0
 Debug Level                             = None
 Execution Priority                      = 10
 Position Fault Detection                = true
+Position Rate Factor                    = 0.1
+Position Error Threshold                = 12.0
+Position Error Delay                    = 5.0
+Position Error Samples                  = 5
+Maximum Rotation Rate                   = 333.3
 Current Lower Threshold                 = 0.5
 Current Upper Threshold                 = 0.9
 Maximum Lower Faults                    = 20

src/DUNE/Monitors/ServoCurrentMonitor.hpp

@@ -59,6 +59,10 @@ namespace DUNE
         m_last_fault = Time::Clock::get() - c_fault_cooldown;
       }
 
+      //! Destructor
+      ~ServoCurrentMonitor(void)
+      { }
+
       //! Update function
       //! @param value current measurement to be used in the detection
       //! @return value of the exceeded current (if <= 0 then no fault was detected)

src/DUNE/Monitors/ServoPositionMonitor.hpp

@@ -1,8 +1,6 @@
 //***************************************************************************
 // Copyright 2007-2024 Universidade do Porto - Faculdade de Engenharia      *
 // Laboratório de Sistemas e Tecnologia Subaquática (LSTS)                  *
-//                                                                          *
-// Copyright 2017 OceanScan - Marine Systems & Technology, Lda.             *
 //***************************************************************************
 // This file is part of DUNE: Unified Navigation Environment.               *
 //                                                                          *
@@ -34,28 +32,43 @@
 
 // DUNE headers.
 #include <DUNE/Math/Angles.hpp>
-#include <DUNE/Time/Counter.hpp>
-#include <DUNE/Time/Delta.hpp>
 #include <DUNE/Utils/String.hpp>
+#include <DUNE/Monitors/DelayedTrigger.hpp>
 
-//! Maximum timestep for evaluation.
-static const float c_max_delta = 1.0f;
-//! Maximum timestep for delay evaluation.
-static const float c_delay = 1.0f;
-//! Allowed actuation difference.
-static const float c_error = (float)DUNE::Math::Angles::radians(5.0);
-//! Minimum position for range checks.
-static const float c_min_position = (float)DUNE::Math::Angles::radians(10.0);
+//! Minimum difference in degrees to account for accumulated change
+static const float c_min_diff = (float)DUNE::Math::Angles::radians(3.0);
 
 namespace DUNE
 {
   namespace Monitors
   {
-    template<typename T> class ServoPositionMonitor
+    template <typename T>
+    class ServoPositionMonitor
     {
     public:
       //! Constructor
-      ServoPositionMonitor() : m_timer_zero(c_delay), m_timer_range(c_delay), m_tracking(false) {}
+      //! @param[in] error_threshold value above which a position fault detection will trigger
+      //! @param[in] rate_factor factor to compare between accumulated difference in position and command sent to servo
+      //! @param[in] detection_delay delay in seconds before a detection is triggered
+      //! @param[in] average_samples number of samples to use in the delayed trigger
+      //! @param[in] max_rotation maximum rotation rate of the servo in radians per second
+      ServoPositionMonitor(const T error_threshold, const T rate_factor,
+                           const double detection_delay, const unsigned average_samples,
+                           const T max_rotation):
+        c_error_threshold(error_threshold),
+        c_rate_factor(rate_factor),
+        c_rate_interval(detection_delay),
+        c_max_rotation(max_rotation),
+        m_valid_last(false)
+      {
+        m_pos_trigger = new DelayedTrigger<T>(error_threshold, detection_delay, average_samples);
+      }
+
+      //! Destructor
+      ~ServoPositionMonitor(void)
+      {
+        Memory::clear(m_pos_trigger);
+      }
 
       //! Update function with position and command values
       //! Describe fault detected
@@ -66,27 +79,48 @@ namespace DUNE
       bool
       updateAndTest(const T position, const T command, std::string* description)
       {
-        double tstep = m_delta.getDelta();
+        float now = Time::Clock::get();
 
-        if (tstep < 0)
-          return false;
-
-        // too much time has passed for a proper evaluation.
-        if (tstep > c_max_delta)
+        if (m_valid_last)
         {
-          reset();
-          return false;
+          T max_rot = c_max_rotation * (now - m_rotation_timer);
+          m_rotation_timer = now;
+
+          m_accum_pos += std::fabs(position - m_last_position);
+          m_accum_com += Math::trimValue(std::fabs(command - m_last_command),
+                                         0.0, max_rot);
+
+          if (now - m_rate_time > c_rate_interval)
+          {
+            if (m_accum_pos < c_rate_factor * m_accum_com &&
+                std::fabs(position - command) > c_min_diff)
+            {
+              *description = Utils::String::str("position change is %.2fº, "
+                                                "command change is %.2fº",
+                                                Math::Angles::degrees(m_accum_pos),
+                                                Math::Angles::degrees(m_accum_com));
+              return true;
+            }
+
+            m_valid_last = false;
+          }
         }
-
-        if (zeroFeedback(position, command))
+        else
         {
-          *description = DTR("unable to center servo");
-          return true;
+          m_accum_pos = 0.0;
+          m_accum_com = 0.0;
+          m_last_position = position;
+          m_last_command = command;
+          m_rate_time = now;
+          m_rotation_timer = now;
+
+          m_valid_last = true;
         }
 
-        if (rangeFeedback(position, command))
+        if (m_pos_trigger->updateAndTest(std::fabs(position - command)))
         {
-          *description = DTR("unable to reach position");
+          *description = Utils::String::str("position error above %.2fº",
+                                            Math::Angles::degrees(c_error_threshold));
           return true;
         }
 
@@ -95,88 +129,30 @@ namespace DUNE
       }
 
     private:
-      //! Check if servo fails to reply when zeroed.
-      //! @param[in] position position measurement to be used in the detection
-      //! @param[in] command set position command to be used in the detection
-      //! @return true if servo fails to reply, false otherwise.
-      bool
-      zeroFeedback(const T position, const T command)
-      {
-        // command is not near zero.
-        if (std::fabs(command) > c_error / 2)
-          m_timer_zero.reset();
-
-        // position is near zero.
-        if (std::fabs(position - command) <= c_error)
-          m_timer_zero.reset();
-
-        if (m_timer_zero.overflow())
-          return true;
-
-        return false;
-      }
-
-      //! Check if servo fails to reply to position.
-      //! @param[in] position position measurement to be used in the detection
-      //! @param[in] command set position command to be used in the detection
-      //! @return true if servo fails to reply, false otherwise.
-      bool
-      rangeFeedback(const T position, const T command)
-      {
-        // command is below range.
-        if (std::fabs(command) < c_min_position)
-        {
-          resetRange();
-          return false;
-        }
-
-        // setup values.
-        if (!m_tracking)
-        {
-          m_tracking = true;
-          m_command = command;
-        }
-
-        // command is no longer near reference.
-        if (std::fabs(command - m_command) > c_error / 2)
-          resetRange();
-
-        // position is near reference.
-        if (std::fabs(position - m_command) < c_error)
-          resetRange();
-
-        // unable to follow position.
-        if (m_timer_range.overflow())
-          return true;
-
-        return false;
-      }
-
-      void
-      reset()
-      {
-        m_timer_zero.reset();
-        resetRange();
-      }
-
-      void
-      resetRange()
-      {
-        m_command = 0.0;
-        m_tracking = false;
-        m_timer_range.reset();
-      }
-
-      //! Update timestep.
-      Time::Delta m_delta;
-      //! Zero Response Counter.
-      Time::Counter<float> m_timer_zero;
-      //! Range Response Counter.
-      Time::Counter<float> m_timer_range;
-      //! Range command reference.
-      T m_command;
-      //! Now tracking range.
-      bool m_tracking;
+      //! DelayedTrigger object for position fault detection
+      DelayedTrigger<T>* m_pos_trigger;
+      //! Error threshold
+      const T c_error_threshold;
+      //! Rate factor
+      const T c_rate_factor;
+      //! Rate time interval for accumulating difference
+      const double c_rate_interval;
+      //! Maximum rotation rate
+      const T c_max_rotation;
+      //! Rate time reference
+      double m_rate_time;
+      //! Rotation rate counter
+      double m_rotation_timer;
+      //! "last" variables have significant values
+      bool m_valid_last;
+      //! Last position read
+      T m_last_position;
+      //! Last command sent
+      T m_last_command;
+      //! Accumulated position
+      T m_accum_pos;
+      //! Accumulated command
+      T m_accum_com;
     };
   }
 }