From 662398d6b4a6da049f3e873b600edb940ab31eb5 Mon Sep 17 00:00:00 2001
From: Debora <debora.tahara@tidewise.io>
Date: Fri, 27 Sep 2024 17:48:24 -0300
Subject: [PATCH] feat:Implement automatic fault reset

---
 motors_weg_cvw300.orogen   |  4 +-
 motors_weg_cvw300Types.hpp |  1 +
 tasks/Task.cpp             | 86 +++++++++++++++++++++++---------------
 tasks/Task.hpp             |  2 +
 4 files changed, 58 insertions(+), 35 deletions(-)

diff --git a/motors_weg_cvw300.orogen b/motors_weg_cvw300.orogen
index d6fb75c..38a7a2f 100644
--- a/motors_weg_cvw300.orogen
+++ b/motors_weg_cvw300.orogen
@@ -87,8 +87,8 @@ task_context "Task", subclasses: "iodrivers_base::Task" do
 
     output_port "saturation_signal", "/control_base/SaturationSignal"
 
-    exception_states "INVALID_COMMAND_SIZE", "INVALID_COMMAND_PARAMETER",
-                     "CONTROLLER_FAULT", "CONTROLLER_UNDER_VOLTAGE"
+    exception_states "INVALID_COMMAND_SIZE", "INVALID_COMMAND_PARAMETER"
+    error_states "CONTROLLER_FAULT", "CONTROLLER_UNDER_VOLTAGE"
 
     # There's no data coming from the controller without the driver requesting
     # it first ... Must be periodic.
diff --git a/motors_weg_cvw300Types.hpp b/motors_weg_cvw300Types.hpp
index ac3a3e7..5d91e5d 100644
--- a/motors_weg_cvw300Types.hpp
+++ b/motors_weg_cvw300Types.hpp
@@ -5,6 +5,7 @@
 #include <motors_weg_cvw300/Configuration.hpp>
 #include <motors_weg_cvw300/FaultState.hpp>
 #include <motors_weg_cvw300/InverterStatus.hpp>
+#include <motors_weg_cvw300/CurrentState.hpp>
 
 namespace motors_weg_cvw300 {
     namespace configuration {
diff --git a/tasks/Task.cpp b/tasks/Task.cpp
index 4615eaa..3460980 100644
--- a/tasks/Task.cpp
+++ b/tasks/Task.cpp
@@ -110,31 +110,8 @@ bool Task::checkSpeedSaturation(base::commands::Joints const& cmd)
     return cmd.elements[0].speed >= m_limits.elements[0].max.speed ||
            cmd.elements[0].speed <= m_limits.elements[0].min.speed;
 }
-
-void Task::updateHook()
+CurrentState Task::readAndPublishControllerStates()
 {
-    while (_cmd_in.read(m_cmd_in) == RTT::NewData) {
-        if (m_cmd_in.elements.size() != 1) {
-            return exception(INVALID_COMMAND_SIZE);
-        }
-
-        auto joint = m_cmd_in.elements.at(0);
-        if (!joint.isSpeed()) {
-            return exception(INVALID_COMMAND_PARAMETER);
-        }
-
-        writeSpeedCommand(joint.speed);
-
-        SaturationSignal saturation_signal;
-        saturation_signal.value = checkSpeedSaturation(m_cmd_in);
-        saturation_signal.time = m_cmd_in.time;
-        _saturation_signal.write(saturation_signal);
-    }
-
-    if (commandTimedOut()) {
-        writeSpeedCommand(0);
-    }
-
     auto now = Time::now();
     CurrentState state = m_driver->readCurrentState();
     m_sample.time = now;
@@ -166,29 +143,72 @@ void Task::updateHook()
         _temperatures.write(m_driver->readTemperatures());
         m_last_temperature_update = now;
     }
-
-    if (state.inverter_status == STATUS_FAULT) {
-        publishFault();
-        return exception(CONTROLLER_FAULT);
+    return state;
+}
+void Task::evaluateInverterStatus(InverterStatus const& inverter_status)
+{
+    if (inverter_status == STATUS_FAULT) {
+        error(CONTROLLER_FAULT);
     }
-    else if (state.inverter_status == STATUS_UNDERVOLTAGE) {
-        publishFault();
-        return exception(CONTROLLER_UNDER_VOLTAGE);
+    else if (inverter_status == STATUS_UNDERVOLTAGE) {
+        error(CONTROLLER_UNDER_VOLTAGE);
     }
-
-    TaskBase::updateHook();
 }
 void Task::publishFault()
 {
     auto fault_state = m_driver->readFaultState();
     _fault_state.write(fault_state);
 }
+void Task::updateHook()
+{
+    while (_cmd_in.read(m_cmd_in) == RTT::NewData) {
+        if (m_cmd_in.elements.size() != 1) {
+            return exception(INVALID_COMMAND_SIZE);
+        }
+
+        auto joint = m_cmd_in.elements.at(0);
+        if (!joint.isSpeed()) {
+            return exception(INVALID_COMMAND_PARAMETER);
+        }
+
+        writeSpeedCommand(joint.speed);
+
+        SaturationSignal saturation_signal;
+        saturation_signal.value = checkSpeedSaturation(m_cmd_in);
+        saturation_signal.time = m_cmd_in.time;
+        _saturation_signal.write(saturation_signal);
+    }
+
+    if (commandTimedOut()) {
+        writeSpeedCommand(0);
+    }
+
+    auto state = readAndPublishControllerStates();
+    evaluateInverterStatus(state.inverter_status);
+
+    TaskBase::updateHook();
+}
 void Task::processIO()
 {
 }
 void Task::errorHook()
 {
     TaskBase::errorHook();
+
+    publishFault();
+
+    // Try to reset the faults
+    m_driver->prepare();
+    // Verify if the controller status is not in FAULT state
+    auto state = readAndPublishControllerStates();
+    if (state.inverter_status != STATUS_FAULT &&
+        state.inverter_status != STATUS_UNDERVOLTAGE) {
+        recover();
+    }
+
+    if (_cmd_in.read(m_cmd_in) == RTT::NewData) {
+        evaluateInverterStatus(state.inverter_status);
+    }
 }
 void Task::stopHook()
 {
diff --git a/tasks/Task.hpp b/tasks/Task.hpp
index 5fd83ad..7ca7d9c 100644
--- a/tasks/Task.hpp
+++ b/tasks/Task.hpp
@@ -46,6 +46,8 @@ namespace motors_weg_cvw300 {
         bool commandTimedOut() const;
         void publishFault();
         bool checkSpeedSaturation(base::commands::Joints const& cmd);
+        void evaluateInverterStatus(InverterStatus const& inverter_status);
+        CurrentState readAndPublishControllerStates();
 
     public:
         /** TaskContext constructor for Task