From 0c6021c2699333679599679b9d3c8de8aea1ea07 Mon Sep 17 00:00:00 2001 From: Luis Venancio Date: Mon, 9 Sep 2024 15:57:12 +0100 Subject: [PATCH] Revert "DUNE/Monitors/Servo: Reduced monitor constraints." This reverts commit b7a1d5aaad038188d4bb353d8fa2f78370c5afa0. --- etc/auv/monitors.ini | 5 + etc/testing/replays/servomonitor-replay.ini | 5 + src/DUNE/Monitors/ServoCurrentMonitor.hpp | 4 + src/DUNE/Monitors/ServoPositionMonitor.hpp | 196 +++++++++----------- src/Monitors/Servos/Task.cpp | 67 ++++++- 5 files changed, 157 insertions(+), 120 deletions(-) diff --git a/etc/auv/monitors.ini b/etc/auv/monitors.ini index ad8022d06e..17f290c1ff 100644 --- a/etc/auv/monitors.ini +++ b/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 diff --git a/etc/testing/replays/servomonitor-replay.ini b/etc/testing/replays/servomonitor-replay.ini index c8019c59ea..e57e203950 100644 --- a/etc/testing/replays/servomonitor-replay.ini +++ b/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 diff --git a/src/DUNE/Monitors/ServoCurrentMonitor.hpp b/src/DUNE/Monitors/ServoCurrentMonitor.hpp index 89000cab49..7b7ea58c6a 100644 --- a/src/DUNE/Monitors/ServoCurrentMonitor.hpp +++ b/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) diff --git a/src/DUNE/Monitors/ServoPositionMonitor.hpp b/src/DUNE/Monitors/ServoPositionMonitor.hpp index 207d044902..6b7bfc95ce 100644 --- a/src/DUNE/Monitors/ServoPositionMonitor.hpp +++ b/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 -#include -#include #include +#include -//! 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 class ServoPositionMonitor + template + 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(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 m_timer_zero; - //! Range Response Counter. - Time::Counter m_timer_range; - //! Range command reference. - T m_command; - //! Now tracking range. - bool m_tracking; + //! DelayedTrigger object for position fault detection + DelayedTrigger* 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; }; } } diff --git a/src/Monitors/Servos/Task.cpp b/src/Monitors/Servos/Task.cpp index 523d2037ee..b89cac6f67 100644 --- a/src/Monitors/Servos/Task.cpp +++ b/src/Monitors/Servos/Task.cpp @@ -64,6 +64,16 @@ namespace Monitors { //! Monitor position faults bool pos_fault_detect; + //! Threshold for fault detection in the position error (command vs position) + float pos_error_threshold; + //! Factor in detection for comparing accumulated difference (command vs position) + float rate_factor; + //! Delay for the detection triggering using DelayedTrigger class + double pos_error_delay; + //! Number of samples for the moving average in the DelayedTrigger object + unsigned pos_error_samples; + //! Maximum rotation rate of the servos + double max_rotation_rate; //! Monitor electric current faults bool curr_fault_detect; //! Current lower threshold to consider abnormal behavior @@ -121,6 +131,33 @@ namespace Monitors .defaultValue("false") .description("Enable position fault detection"); + param("Position Error Threshold", m_args.pos_error_threshold) + .defaultValue("12.0") + .minimumValue("0.0") + .units(Units::Degree) + .description("Threshold for position error in fault detection"); + + param("Position Rate Factor", m_args.rate_factor) + .defaultValue("0.05") + .description("Factor in fault detection for comparison between accumulated " + "differences"); + + param("Position Error Delay", m_args.pos_error_delay) + .defaultValue("5.0") + .units(Units::Second) + .description("Delay for the detection triggering using DelayedTrigger class"); + + param("Position Error Samples", m_args.pos_error_samples) + .defaultValue("5") + .minimumValue("1") + .description("Number of samples for the moving average in the " + "DelayedTrigger object"); + + param("Maximum Rotation Rate", m_args.max_rotation_rate) + .defaultValue("333.3") + .units(Units::DegreePerSecond) + .description("Maximum rotation rate allowed by the servo"); + param("Current Fault Detection", m_args.curr_fault_detect) .defaultValue("false") .description("Enable servo electric current fault detection"); @@ -176,8 +213,8 @@ namespace Monitors for (unsigned i = 0; i < c_servo_count; ++i) { - m_pos_monitor[i] = nullptr; - m_curr_monitor[i] = nullptr; + m_pos_monitor[i] = NULL; + m_curr_monitor[i] = NULL; } // Register handler routines. @@ -187,14 +224,20 @@ namespace Monitors } void - onUpdateParameters() override + onUpdateParameters(void) { + if (paramChanged(m_args.max_rotation_rate)) + m_args.max_rotation_rate = Angles::radians(m_args.max_rotation_rate); + + if (paramChanged(m_args.pos_error_threshold)) + m_args.pos_error_threshold = Angles::radians(m_args.pos_error_threshold); + if (!m_args.pos_fault_detect && !m_args.curr_fault_detect) requestDeactivation(); } void - onEntityResolution() override + onEntityResolution(void) { for (unsigned i = 0; i < c_servo_count; ++i) { @@ -210,7 +253,7 @@ namespace Monitors } void - onResourceRelease() override + onResourceRelease(void) { for (unsigned i = 0; i < c_servo_count; ++i) { @@ -220,7 +263,7 @@ namespace Monitors } void - onResourceAcquisition() override + onResourceAcquisition(void) { if (m_args.curr_fault_detect) { @@ -237,7 +280,7 @@ namespace Monitors } void - onResourceInitialization() override + onResourceInitialization(void) { for (unsigned i = 0; i < c_servo_count; ++i) { @@ -299,9 +342,13 @@ namespace Monitors { unsigned i = msg->id; - if (m_pos_monitor[i] == nullptr) + if (m_pos_monitor[i] == NULL) { - m_pos_monitor[i] = new ServoPositionMonitor(); + m_pos_monitor[i] = new ServoPositionMonitor(m_args.pos_error_threshold, + m_args.rate_factor, + m_args.pos_error_delay, + m_args.pos_error_samples, + m_args.max_rotation_rate); } else { @@ -400,7 +447,7 @@ namespace Monitors } void - onMain() override + onMain(void) { while (!stopping()) {