From f02725e00850488fbd53811cd861250a3a44534b Mon Sep 17 00:00:00 2001 From: anaezes Date: Wed, 24 Jul 2019 18:02:27 +0100 Subject: [PATCH 1/3] Sensors/Imagenex872: Added state machine to handling errors. --- src/Sensors/Imagenex872/Task.cpp | 170 +++++++++++++++++++++++++++---- 1 file changed, 149 insertions(+), 21 deletions(-) diff --git a/src/Sensors/Imagenex872/Task.cpp b/src/Sensors/Imagenex872/Task.cpp index 888c2e5e86..7374720dcd 100644 --- a/src/Sensors/Imagenex872/Task.cpp +++ b/src/Sensors/Imagenex872/Task.cpp @@ -40,6 +40,19 @@ namespace Sensors { using DUNE_NAMESPACES; + //! Finite state machine states. + enum StateMachineStates + { + //! Waiting for activation. + SM_IDLE, + //! Start activation sequence + SM_BEGIN, + //! Connect with sidescan + SM_ACTIVE, + //! Wait for connection + SM_WAIT + }; + enum Side { SIDE_PORT, @@ -74,6 +87,10 @@ namespace Sensors unsigned frequency; // Default range. unsigned range; + // Error case timer + uint16_t wait_time; + // Time between attempts. + uint16_t wait_attempts; }; // List of available ranges. @@ -101,19 +118,26 @@ namespace Sensors struct Task: public Tasks::Periodic { - // TCP socket. + //! TCP socket. TCPSocket* m_sock; - // Output switch data. + //! Output switch data. uint8_t m_sdata[c_sdata_size]; - // Return data. + //! Return data. uint8_t m_rdata_hdr[c_rdata_hdr_size]; - // Return data. + //! Return data. uint8_t m_rdata_ftr[c_rdata_ftr_size]; - // Single sidescan ping. + //! Single sidescan ping. IMC::SonarData m_ping; - // Configuration parameters. + //! Current state machine state. + StateMachineStates m_sm_state; + //! Watchdog timer to idle task. + Counter m_wdog; + //! Timer between attempts. + Counter m_timer_attempts; + //! Configuration parameters. Arguments m_args; + Task(const std::string& name, Tasks::Context& ctx): Tasks::Periodic(name, ctx), m_sock(NULL) @@ -163,6 +187,18 @@ namespace Sensors .valuesIf("Frequency", "770", "10, 20, 30, 40, 50") .description(DTR("Operating range")); + param("Wait timer", m_args.wait_time) + .defaultValue("20") + .minimumValue("0") + .maximumValue("60") + .description("Error case timer"); + + param("Wait attempts", m_args.wait_attempts) + .defaultValue("5") + .minimumValue("0") + .maximumValue("60") + .description("Error case timer"); + // Initialize switch data. std::memset(m_sdata, 0, sizeof(m_sdata)); m_sdata[0] = 0xfe; @@ -194,8 +230,10 @@ namespace Sensors void onResourceAcquisition(void) { - m_sock = new TCPSocket(); - m_sock->setNoDelay(true); + //setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE); + + //m_sock = new TCPSocket(); + //m_sock->setNoDelay(true); } void @@ -206,22 +244,50 @@ namespace Sensors void onResourceInitialization(void) + { + + setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE); + + } + + //! Set current state of task + //! @param[in] state state machine state. + void + setState(StateMachineStates state) + { + m_sm_state = state; + } + + void + onActivation() { + setState(SM_BEGIN); + } + + void + activation(void) { try { - m_sock->connect(m_args.addr, m_args.port); - pingBoth(); - setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE); + connect(); } catch (std::runtime_error& e) { - throw RestartNeeded(e.what(), 10.0, false); + debug("deu erro!!! state wait!"); + m_wdog.setTop(m_args.wait_time); + m_timer_attempts.setTop(m_args.wait_attempts); + setState(SM_WAIT); + } } - void - onActivation(void) - { + void connect() { + m_sock = new TCPSocket(); + m_sock->setNoDelay(true); + m_sock->setReceiveTimeout(5); + m_sock->setSendTimeout(5); + m_sock->connect(m_args.addr, m_args.port); + pingBoth(); + setState(SM_ACTIVE); setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE); } @@ -229,8 +295,72 @@ namespace Sensors onDeactivation(void) { setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE); + debug("Deactivation Sidescan: error to connect."); } + void + handleSonarData(void) + { + try + { + pingBoth(); + dispatch(m_ping); + } + catch (std::runtime_error& e) + { + m_wdog.setTop(m_args.wait_time); + m_timer_attempts.setTop(m_args.wait_attempts); + setState(SM_WAIT); + } + } + + void tryConnectAgain() { + try + { + connect(); + } + catch (std::runtime_error& e) + { + m_timer_attempts.reset(); + } + } + + void + updateStateMachine(void) + { + switch(m_sm_state) { + // Wait for activation. + case SM_IDLE: + break; + + // Begin activation + case SM_BEGIN: + setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVATING); + activation(); + break; + + // Works + case SM_ACTIVE: + handleSonarData(); + debug("Ping and send data!"); + break; + + // Wait + case SM_WAIT: + if (m_wdog.overflow()) { + setState(SM_IDLE); + requestDeactivation(); + } + else if(m_timer_attempts.overflow()) { + debug("Problem to connect. Try again..."); + tryConnectAgain(); + } + break; + } + + } + + unsigned getIndex(unsigned value, const unsigned* table, unsigned table_size) { @@ -326,15 +456,13 @@ namespace Sensors try { - pingBoth(); - dispatch(m_ping); + updateStateMachine(); } - catch (std::exception& e) + catch (std::runtime_error& e) { - err("%s", e.what()); - setEntityState(IMC::EntityState::ESTA_ERROR, Status::CODE_COM_ERROR); - throw RestartNeeded(DTR(Status::getString(CODE_COM_ERROR)), 5); + throw RestartNeeded(e.what(), 5); } + } }; } From 4dd2fc8183c9e2efe28d86b02438dbc9c7732ea9 Mon Sep 17 00:00:00 2001 From: anaezes Date: Thu, 25 Jul 2019 17:12:33 +0100 Subject: [PATCH 2/3] Sensors/Imagenex872: Task into error if unable to connect. --- src/Sensors/Imagenex872/Task.cpp | 98 ++++++++++++++------------------ 1 file changed, 43 insertions(+), 55 deletions(-) diff --git a/src/Sensors/Imagenex872/Task.cpp b/src/Sensors/Imagenex872/Task.cpp index 7374720dcd..f382819d74 100644 --- a/src/Sensors/Imagenex872/Task.cpp +++ b/src/Sensors/Imagenex872/Task.cpp @@ -24,7 +24,7 @@ // https://github.com/LSTS/dune/blob/master/LICENCE.md and * // http://ec.europa.eu/idabc/eupl.html. * //*************************************************************************** -// Author: Ricardo Martins * +// Author: Ricardo Martins, Ana Santos * //*************************************************************************** // ISO C++ 98 headers. @@ -43,14 +43,14 @@ namespace Sensors //! Finite state machine states. enum StateMachineStates { - //! Waiting for activation. - SM_IDLE, - //! Start activation sequence - SM_BEGIN, - //! Connect with sidescan - SM_ACTIVE, - //! Wait for connection - SM_WAIT + //! Waiting for activation. + SM_IDLE, + //! Start activation sequence + SM_BEGIN, + //! Connect with sidescan + SM_ACTIVE, + //! Wait for connection + SM_WAIT }; enum Side @@ -227,15 +227,6 @@ namespace Sensors throw RestartNeeded(DTR("restarting to change TCP port"), 1); } - void - onResourceAcquisition(void) - { - //setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE); - - //m_sock = new TCPSocket(); - //m_sock->setNoDelay(true); - } - void onResourceRelease(void) { @@ -245,9 +236,7 @@ namespace Sensors void onResourceInitialization(void) { - setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE); - } //! Set current state of task @@ -272,15 +261,14 @@ namespace Sensors } catch (std::runtime_error& e) { - debug("deu erro!!! state wait!"); m_wdog.setTop(m_args.wait_time); m_timer_attempts.setTop(m_args.wait_attempts); setState(SM_WAIT); - } } - void connect() { + void + connect(void) { m_sock = new TCPSocket(); m_sock->setNoDelay(true); m_sock->setReceiveTimeout(5); @@ -295,7 +283,6 @@ namespace Sensors onDeactivation(void) { setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE); - debug("Deactivation Sidescan: error to connect."); } void @@ -325,39 +312,40 @@ namespace Sensors } } - void + void updateStateMachine(void) { - switch(m_sm_state) { - // Wait for activation. - case SM_IDLE: - break; - - // Begin activation - case SM_BEGIN: - setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVATING); - activation(); - break; - - // Works - case SM_ACTIVE: - handleSonarData(); - debug("Ping and send data!"); - break; - - // Wait - case SM_WAIT: - if (m_wdog.overflow()) { - setState(SM_IDLE); - requestDeactivation(); - } - else if(m_timer_attempts.overflow()) { - debug("Problem to connect. Try again..."); - tryConnectAgain(); - } - break; - } - + switch(m_sm_state) { + + // Wait for activation. + case SM_IDLE: + break; + + // Begin activation + case SM_BEGIN: + setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVATING); + activation(); + break; + + // Works + case SM_ACTIVE: + handleSonarData(); + debug("Ping and send data!"); + break; + + // Wait + case SM_WAIT: + if (m_wdog.overflow()) { + setState(SM_IDLE); + setEntityState(IMC::EntityState::ESTA_ERROR, Status::CODE_COM_ERROR); + throw RestartNeeded(DTR(Status::getString(CODE_COM_ERROR)), 5); + } + else if(m_timer_attempts.overflow()) { + debug("Problem to connect. Try again..."); + tryConnectAgain(); + } + break; + } } From 09f6531a47df10af6e4dea5d227684b677e57420 Mon Sep 17 00:00:00 2001 From: anaezes Date: Thu, 1 Aug 2019 15:13:14 +0100 Subject: [PATCH 3/3] Sensors/Imagenex872: Code cleanup. --- src/Sensors/Imagenex872/Task.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/Sensors/Imagenex872/Task.cpp b/src/Sensors/Imagenex872/Task.cpp index f382819d74..78aec3c907 100644 --- a/src/Sensors/Imagenex872/Task.cpp +++ b/src/Sensors/Imagenex872/Task.cpp @@ -330,7 +330,6 @@ namespace Sensors // Works case SM_ACTIVE: handleSonarData(); - debug("Ping and send data!"); break; // Wait