Merge pull request #17 from tidewise/reset_controller_faults

feat: Implements the automatic controller faults reset

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.

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 {

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,73 @@ 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->resetFault();
+    // Verify if the controller status is not in FAULT state
+    auto state = readAndPublishControllerStates();
+    if (state.inverter_status != STATUS_FAULT &&
+        state.inverter_status != STATUS_UNDERVOLTAGE) {
+        m_driver->enable();
+        recover();
+    }
+
+    if (_cmd_in.read(m_cmd_in) == RTT::NewData) {
+        evaluateInverterStatus(state.inverter_status);
+    }
 }
 void Task::stopHook()
 {

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

test/task_test.rb

@@ -59,6 +59,10 @@
         modbus_set(93, 14)
         modbus_set(94, 15)
         modbus_set(95, 16)
+
+        modbus_set(265, 11) # no external fault
+        modbus_set(266, 12) # reset
+        modbus_set(275, 21) # pre-charge ok
     end
 
     after do
@@ -236,6 +240,82 @@
             assert_in_delta 1.6, sample.inverter_output_voltage
         end
 
+        it "keeps writing on the output ports when it is in error state when the inverter is in under-voltage status" do
+            modbus_configure_and_start
+
+            now = Time.now
+            modbus_expect_execution(@writer, @reader) do
+                modbus_set(49, 1)
+                modbus_set(6, 2)
+            end.to do
+                emit task.controller_under_voltage_event
+                have_one_new_sample task.fault_state_port
+                have_one_new_sample task.joint_samples_port
+                have_one_new_sample task.inverter_state_port
+                have_one_new_sample task.alarm_state_port
+                have_one_new_sample task.temperatures_port
+            end
+
+            modbus_expect_execution(@writer, @reader).to do
+                have_one_new_sample task.fault_state_port
+                have_one_new_sample task.joint_samples_port
+                have_one_new_sample task.inverter_state_port
+                have_one_new_sample task.alarm_state_port
+                have_one_new_sample task.temperatures_port
+            end
+        end
+
+        it "transits between error state and running state when the inverter stops reporting the under-voltage status" do
+            modbus_configure_and_start
+
+            now = Time.now
+            modbus_expect_execution(@writer, @reader) do
+                modbus_set(49, 1)
+                modbus_set(6, 2)
+            end.to do
+                emit task.controller_under_voltage_event
+            end
+
+            modbus_expect_execution(@writer, @reader) do
+                modbus_set(49, 1)
+                modbus_set(6, 1)
+            end.to do
+                emit task.running_event
+            end
+
+            modbus_expect_execution(@writer, @reader) do
+                modbus_set(49, 1)
+                modbus_set(6, 2)
+            end.to do
+                emit task.controller_under_voltage_event
+            end
+        end
+
+        it "emits the last error state when receiving new input commands while the inverter is in under-voltage status" do
+            modbus_configure_and_start
+
+            now = Time.now
+            modbus_expect_execution(@writer, @reader) do
+                modbus_set(49, 1)
+                modbus_set(6, 2)
+            end.to do
+                emit task.controller_under_voltage_event
+            end
+
+            modbus_expect_execution(@writer, @reader).to do
+                not_emit task.controller_under_voltage_event, within: 0.2
+            end
+
+            cmd = Types.base.samples.Joints.new(
+                elements: [Types.base.JointState.Speed(1000 * 2 * Math::PI / 60)]
+            )
+            modbus_expect_execution(@writer, @reader) do
+                syskit_write task.cmd_in_port, cmd
+            end.to do
+                emit task.controller_under_voltage_event
+            end
+        end
+
         it "outputs a fault state structure and transitions to fault if there is a fault" do
             modbus_configure_and_start
 
@@ -257,6 +337,82 @@
             assert_in_delta 1.5, sample.inverter_output_frequency
             assert_in_delta 1.6, sample.inverter_output_voltage
         end
+
+        it "keeps writing on the output ports when it is in error state when the inverter is in fault status" do
+            modbus_configure_and_start
+
+            now = Time.now
+            modbus_expect_execution(@writer, @reader) do
+                modbus_set(49, 1)
+                modbus_set(6, 3)
+            end.to do
+                emit task.controller_fault_event
+                have_one_new_sample task.fault_state_port
+                have_one_new_sample task.joint_samples_port
+                have_one_new_sample task.inverter_state_port
+                have_one_new_sample task.alarm_state_port
+                have_one_new_sample task.temperatures_port
+            end
+
+            modbus_expect_execution(@writer, @reader).to do
+                have_one_new_sample task.fault_state_port
+                have_one_new_sample task.joint_samples_port
+                have_one_new_sample task.inverter_state_port
+                have_one_new_sample task.alarm_state_port
+                have_one_new_sample task.temperatures_port
+            end
+        end
+
+        it "transits between error state and running state when the inverter stops reporting the fault status" do
+            modbus_configure_and_start
+
+            now = Time.now
+            modbus_expect_execution(@writer, @reader) do
+                modbus_set(49, 1)
+                modbus_set(6, 3)
+            end.to do
+                emit task.controller_fault_event
+            end
+
+            modbus_expect_execution(@writer, @reader) do
+                modbus_set(49, 1)
+                modbus_set(6, 1)
+            end.to do
+                emit task.running_event
+            end
+
+            modbus_expect_execution(@writer, @reader) do
+                modbus_set(49, 1)
+                modbus_set(6, 3)
+            end.to do
+                emit task.controller_fault_event
+            end
+        end
+
+        it "emits the last error state when receiving new input commands while the inverter is in fault status" do
+            modbus_configure_and_start
+
+            now = Time.now
+            modbus_expect_execution(@writer, @reader) do
+                modbus_set(49, 1)
+                modbus_set(6, 3)
+            end.to do
+                emit task.controller_fault_event
+            end
+
+            modbus_expect_execution(@writer, @reader).to do
+                not_emit task.controller_fault_event, within: 0.2
+            end
+
+            cmd = Types.base.samples.Joints.new(
+                elements: [Types.base.JointState.Speed(1000 * 2 * Math::PI / 60)]
+            )
+            modbus_expect_execution(@writer, @reader) do
+                syskit_write task.cmd_in_port, cmd
+            end.to do
+                emit task.controller_fault_event
+            end
+        end
     end
 
     describe "inverted = false" do