Merge remote-tracking branch 'issue/servo-monitor'

LSTS · Sep 24, 2024 · 67773fd · 67773fd
2 parents 240160c + 0289ea5
commit 67773fd
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 0 deletions.
diff --git a/src/DUNE/Monitors/ServoPositionMonitor.hpp b/src/DUNE/Monitors/ServoPositionMonitor.hpp
@@ -128,6 +128,13 @@ namespace DUNE
         return false;
       }
 
+      //! Reset the values
+      void
+      reset()
+      {
+        m_valid_last = false;
+      }
+
     private:
       //! DelayedTrigger object for position fault detection
       DelayedTrigger<T>* m_pos_trigger;

diff --git a/src/Monitors/Servos/Task.cpp b/src/Monitors/Servos/Task.cpp
@@ -49,6 +49,8 @@ namespace Monitors
     static const unsigned c_servo_count = 4;
     //! Error timeout
     static const float c_error_timeout = 5.0;
+    //! Valid SetServoPosition timeout
+    static const float c_valid_set_servo_position_timeout = 0.01;
 
     enum FaultBits
     {
@@ -114,6 +116,8 @@ namespace Monitors
       uint8_t m_on_fault[c_servo_count];
       //! Timer for error timeout
       Time::Counter<float> m_timer;
+      //! Timer for servo position fault detection timeout
+      Time::Counter<float> m_timer_set_servo;
       //! Queue for error reports.
       std::queue<double> m_queue[c_servo_count];
       //! Major failure in one of the servos.
@@ -287,6 +291,7 @@ namespace Monitors
           m_set_servo[i] = 0.0;
           m_on_fault[i] = FT_NONE;
           m_timer.setTop(c_error_timeout);
+          m_timer_set_servo.setTop(c_valid_set_servo_position_timeout);
         }
 
         if (m_args.pos_fault_detect || m_args.curr_fault_detect)
@@ -301,6 +306,16 @@ namespace Monitors
 
         if (m_args.pos_fault_detect)
         {
+          if (m_timer_set_servo.overflow())
+          {
+            int id = msg->id;
+            if (m_pos_monitor[id] != NULL)
+            {
+              m_pos_monitor[id]->reset();
+            }
+            return;
+          }
+
           testPositionFaults(msg);
           m_timer.reset();
         }
@@ -313,6 +328,7 @@ namespace Monitors
           return;
 
         m_set_servo[msg->id] = msg->value;
+        m_timer_set_servo.reset();
       }
 
       void