From 662398d6b4a6da049f3e873b600edb940ab31eb5 Mon Sep 17 00:00:00 2001 From: Debora Date: Fri, 27 Sep 2024 17:48:24 -0300 Subject: [PATCH 1/3] 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 #include #include +#include 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 From a35667c017049a08b7604e9d412957452b6bf400 Mon Sep 17 00:00:00 2001 From: Debora Date: Fri, 27 Sep 2024 17:49:08 -0300 Subject: [PATCH 2/3] feat: Unit test for automatic fault reset --- test/task_test.rb | 156 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 156 insertions(+) diff --git a/test/task_test.rb b/test/task_test.rb index fc53295..e49d73b 100644 --- a/test/task_test.rb +++ b/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 From d8a963ba9adcaa69ff1ca8a43661ec0bfa9bc19b Mon Sep 17 00:00:00 2001 From: Jhonas Date: Wed, 2 Oct 2024 17:17:48 -0300 Subject: [PATCH 3/3] feat: reset Fault on error Hook and then reenables before recovery Instead of using prepare, which does a bunch of stuff, we took the important bit from it and create the resetFault method, toggles the reset fault bit on and off. Also, after it was reset the controller must go through the enable control method before recovery. --- tasks/Task.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tasks/Task.cpp b/tasks/Task.cpp index 3460980..70cc056 100644 --- a/tasks/Task.cpp +++ b/tasks/Task.cpp @@ -198,11 +198,12 @@ void Task::errorHook() publishFault(); // Try to reset the faults - m_driver->prepare(); + 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(); }