/core/src/aos_main.cpp - AMiRo-OS - Research for Cognitive Interaction

amiro-os / core / src / aos_main.cpp @ 0963cf6c

History | View | Annotate | Download (44.806 KB)

       /*
       AMiRo-OS is an operating system designed for the Autonomous Mini Robot (AMiRo) platform.
       Copyright (C) 2016..2019  Thomas Schöpping et al.
       This program is free software: you can redistribute it and/or modify
       it under the terms of the GNU General Public License as published by
       the Free Software Foundation, either version 3 of the License, or
       (at your option) any later version.
       This program is distributed in the hope that it will be useful,
       but WITHOUT ANY WARRANTY; without even the implied warranty of
       MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
       GNU General Public License for more details.
       You should have received a copy of the GNU General Public License
       along with this program.  If not, see <http://www.gnu.org/licenses/>.
       */
       /**
        * @file    aos_main.cpp
        * @brief   Main function.
        * @details Main function with SSSP and initialization,
        *          extendable via hooks.
+       *
        * @addtogroup aos_system
        * @{
        */
       #include <amiroos.h>
       #include <module.h>
       /*
        * hook to add further includes
        */
       #if defined(AMIROOS_CFG_MAIN_EXTRA_INCLUDE_HEADER)
       #include AMIROOS_CFG_MAIN_EXTRA_INCLUDE_HEADER
       #endif
       /**
        * @brief   Event mask to identify I/O events.
        */
       #define IOEVENT_MASK                            EVENT_MASK(0)
       /**
        * @brief   Event mask to identify OS events.
        */
       #define OSEVENT_MASK                            EVENT_MASK(1)
       /**
        * @brief   Event mask to idetify CAN events.
        */
       #define CANEVENT_MASK                           EVENT_MASK(2)
       /**
        * @brief   Event mask to idetify timeout events.
        */
       #define TIMEOUTEVENT_MASK                       EVENT_MASK(3)
       /**
        * @brief   Event mask to idetify signal delay events.
        */
       #define DELAYEVENT_MASK                         EVENT_MASK(4)
       #if (AMIROOS_CFG_SSSP_ENABLE == true) || defined(__DOXYGEN__)
       /**
        * @brief   CAN message identifier for initialization of the SSSP stack initialization sequence.
        */
       #define SSSP_STACKINIT_CANMSGID_INIT            0x003
       /**
        * @brief   CAN message identifier for transmitting module IDs during the SSSP stack initialization sequence.
        */
       #define SSSP_STACKINIT_CANMSGID_MODULEID        0x002
       /**
        * @brief   CAN message identifier for abortion of the SSSP stack initialization sequence.
        */
       #define SSSP_STACKINIT_CANMSGID_ABORT           0x001
       #else /* AMIROOS_CFG_SSSP_ENABLE == false */
       /**
        * @brief   Default shutdown mode if SSSP is unavailable.
        */
       #define AOS_SHUTDOWN_DEFAULT                    AOS_SHUTDOWN_DEEPSLEEP
       #endif /* AMIROOS_CFG_SSSP_ENABLE */
       /**
        * @brief   CAN message identifier for calender synchronization message.
        */
       #define CALENDERSYNC_CANMSGID                   0x004
       /**
        * @brief   Listener object for I/O events.
        */
       static event_listener_t _eventListenerIO;
       /**
        * @brief   Listener object for OS events.
        */
       static event_listener_t _eventListenerOS;
       #if defined(MODULE_HAL_PROGIF) || defined(__DOXYGEN__)
       /**
        * @brief   I/O channel for the programmer interface.
        */
       static AosIOChannel _stdiochannel;
       #if (AMIROOS_CFG_SHELL_ENABLE == true) || defined(__DOXYGEN__)
       /**
        * @brief   I/O shell channel for the programmer interface.
        */
       static AosShellChannel _stdshellchannel;
       #endif /* AMIROOS_CFG_SHELL_ENABLE == true */
       #endif /* defined(MODULE_HAL_PROGIF) */
       /*
        * hook to add further static variables
        */
       #if defined(AMIROOS_CFG_MAIN_EXTRA_STATIC_VARIABLES)
       AMIROOS_CFG_MAIN_EXTRA_STATIC_VARIABLES
       #endif
       /**
        * @brief   Prints an error message about an unexpected event.
+       *
        * @param[in] mask    The event mask.
        * @param[in] flags   The event flags.
        */
       static inline void _unexpectedEventError(const eventmask_t mask, const eventflags_t flags)
+      {
       #if (AMIROOS_CFG_DBG == true)
         aosprintf("CTRL: unexpected/unknown event received. mask: 0x%08X; flags: 0x%08X\n", mask, flags);
       #else
         (void)(mask);
         (void)(flags);
       #endif
         return;
+      }
       #if (AMIROOS_CFG_SSSP_ENABLE == true) || defined(__DOXYGEN__)
       /**
        * @brief   Callback function to be used during SSSP stack initialization sequence.
+       *
        * @param[in] par   A pointer to an @p event_source_t to be fired.
        */
       static void _ssspTimerCallback(void* par)
+      {
         aosDbgCheck(par != NULL);
         chSysLockFromISR();
         chEvtBroadcastI((event_source_t*)par);
         chSysUnlockFromISR();
         return;
+      }
       #endif /* AMIROOS_CFG_SSSP_ENABLE == true */
       /**
        * @brief   Helper function to serialize data.
+       *
        * @param[out]  dst   Pointer to the output buffer.
        * @param[in]   src   Data to be serialized.
        * @param[in]   n     Number of bytes to serialize.
        */
       inline void _serialize(uint8_t* dst, const uint64_t src, const uint8_t n)
+      {
         aosDbgCheck(dst != NULL);
         aosDbgCheck(n > 0 && n <= 8);
         for (uint8_t byte = 0; byte < n; ++byte) {
           dst[byte] = (uint8_t)((src >> (byte * 8)) & 0xFF);
+        }
         return;
+      }
       /**
        * @brief   Helper function to deserialize data.
+       *
        * @param[in] src   Pointer to the buffer of data to be deserialzed.
        * @param[in] n     Number of bytes to deserialize.
+       *
        * @return    The deserialized 32 bit data.
        */
       inline uint64_t _deserialize(uint8_t* src, const uint8_t n)
+      {
         aosDbgCheck(src != NULL);
         aosDbgCheck(n > 0 && n <= 8);
         uint64_t result = 0;
         for (uint8_t byte = 0; byte < n; ++byte) {
           result |= ((uint64_t)src[byte]) << (byte * 8);
+        }
         return result;
+      }
       /**
        * @brief   Converter function to encode a TM value to a single unsigned 64 bit integer.
+       *
        * @details Contents of the TM struct are mapped as follows:
        *            bits  |63     62|61      53|52    50|49         26|25     22|21     17|16     12|11      6|5       0|
        *            #bits |       2 |        9 |      3 |          24 |       4 |       5 |       5 |       6 |       6 |
        *            value |   isdst |     yday |   wday |        year |     mon |    mday |    hour |     min |     sec |
        *            range | special | [0, 365] | [0, 6] | [1900, ...] | [0, 11] | [1, 31] | [0, 23] | [0, 59] | [0, 61] |
        *          The Daylight Saving Time Flag (isdsst) is encoded as follows:
        *            DST not in effect         -> 0
        *            DST in effect             -> 1
        *            no information available  -> 2
+       *
        * @param[in] src   Pointer to the TM struct to encode.
+       *
        * @return  An unsigned 64 bit integer, which holds the encoded time value.
        */
       inline uint64_t _TM2U64(struct tm* src)
+      {
         aosDbgCheck(src != NULL);
         return (((uint64_t)(src->tm_sec  & 0x0000003F) << (0))               |
                 ((uint64_t)(src->tm_min  & 0x0000003F) << (6))               |
                 ((uint64_t)(src->tm_hour & 0x0000001F) << (12))              |
                 ((uint64_t)(src->tm_mday & 0x0000001F) << (17))              |
                 ((uint64_t)(src->tm_mon  & 0x0000000F) << (22))              |
                 ((uint64_t)(src->tm_year & 0x00FFFFFF) << (26))              |
                 ((uint64_t)(src->tm_wday & 0x00000007) << (50))              |
                 ((uint64_t)(src->tm_yday & 0x000001FF) << (53))              |
                 ((uint64_t)((src->tm_isdst == 0) ? 0 : (src->tm_isdst > 0) ? 1 : 2) << (62)));
+      }
       /**
        * @brief   Converter functiomn to retrieve the encoded TM value from an unsigned 64 bit integer.
+       *
        * @details For information on the encoding, please refer to @p _TM2U64 function.
+       *
        * @param[out] dst  The TM struct to fill with the decoded values.
        * @param[in]  src  Unsigned 64 bit integer holding the encoded TM value.
        */
       inline void _U642TM(struct tm* dst, const uint64_t src)
+      {
         aosDbgCheck(dst != NULL);
         dst->tm_sec  = (src >> 0)  & 0x0000003F;
         dst->tm_min  = (src >> 6)  & 0x0000003F;
         dst->tm_hour = (src >> 12) & 0x0000001F;
         dst->tm_mday = (src >> 17) & 0x0000001F;
         dst->tm_mon  = (src >> 22) & 0x0000000F;
         dst->tm_year = (src >> 26) & 0x00FFFFFF;
         dst->tm_wday = (src >> 50) & 0x00000007;
         dst->tm_yday = (src >> 53) & 0x000001FF;
         dst->tm_isdst = (((src >> 62) & 0x03) == 0) ? 0 : (((src >> 62) & 0x03) > 0) ? 1 : -1;
         return;
+      }
       #if (AMIROOS_CFG_SSSP_ENABLE == true) || defined(__DOXYGEN__)
       /**
        * @brief   Implementation of the SSSP module stack initialization sequence (startup phase 3).
+       *
        * @return Shutdown value.
        * @retval AOS_SHUTDOWN_NONE      No shutdown signal received
        * @retval AOS_SHUTDOWN_PASSIVE   Shutdown signal received.
        */
       aos_shutdown_t _ssspModuleStackInitialization(void)
+      {
         // local types
         /**
          * @brief   States for the internal state machine to implement SSSP startup stage 3.
          */
         typedef enum {
           STAGE_3_1,                  /**< Initiation of SSSP startup stage 3. */
           STAGE_3_2,                  /**< Starting the sequence and broadcasting the first ID. */
           STAGE_3_3_WAITFORFIRSTID,   /**< Waiting for first ID after initiation. */
           STAGE_3_3_WAITFORIDORSIG,   /**< Waiting for next ID or activation of neighbor signal. */
           STAGE_3_3_WAITFORID,        /**< Waiting for next ID (after the module has set its own ID). */
           STAGE_3_4_FINISH,           /**< Successful finish of stage 3. */
           STAGE_3_4_ABORT_ACTIVE,     /**< Aborting stage 3 (active). */
           STAGE_3_4_ABORT,            /**< Aborting stage 3 (passive). */
         } sssp_modulestackinitstage_t;
         typedef struct {
           bool loop     : 1;
           bool wfe      : 1;
           bool wfe_next : 1;
         } flags_t;
         // local variables
         aos_shutdown_t shutdown = AOS_SHUTDOWN_NONE;
         sssp_modulestackinitstage_t stage = STAGE_3_1;
         eventmask_t eventmask = 0;
         eventflags_t ioflags = 0;
         event_source_t eventSourceTimeout;
         event_source_t eventSourceDelay;
         event_listener_t eventListenerTimeout;
         event_listener_t eventListenerDelay;
         event_listener_t eventListenerCan;
         virtual_timer_t timerTimeout;
         virtual_timer_t timerDelay;
         CANTxFrame canTxFrame;
         CANRxFrame canRxFrame;
       #if (AMIROOS_CFG_SSSP_STACK_START != true) || (AMIROOS_CFG_DBG == true)
         aos_ssspmoduleid_t lastid = 0;
       #endif
         flags_t flags;
         aos_timestamp_t uptime;
         // initialize local varibles
         chEvtObjectInit(&eventSourceTimeout);
         chEvtObjectInit(&eventSourceDelay);
         chVTObjectInit(&timerTimeout);
         chVTObjectInit(&timerDelay);
         canTxFrame.RTR = CAN_RTR_DATA;
         canTxFrame.IDE = CAN_IDE_STD;
         flags.loop = true;
         flags.wfe = false; // do not wait for events in the initial iteration of the FSM loop
         flags.wfe_next = true;
         // initialize system variables
         aos.sssp.stage = AOS_SSSP_STARTUP_3_1;
         aos.sssp.moduleId = 0;
         // listen to events (timout, delay, CAN receive)
         chEvtRegisterMask(&eventSourceTimeout, &eventListenerTimeout, TIMEOUTEVENT_MASK);
         chEvtRegisterMask(&eventSourceDelay, &eventListenerDelay, DELAYEVENT_MASK);
         chEvtRegisterMask(&MODULE_HAL_CAN.rxfull_event, &eventListenerCan, CANEVENT_MASK);
         /*
          * FSM in a loop.
+         *
          * This is a fully event-based FSM for the module stack initialization
          * sequence, defined by SSSP as startup stage 3. There are five different
          * events that can occur at this point:
          *  I/O events: The input level of an input pin has changed. Such events must
          *              be handled differently depending on the current state. Most
          *              of the time, however, such events can be ignored.
          *  OS events:  Such events are only available after this stage completed and
          *              thus should never occur. However, there is an optional hook
          *              to handle such events, nevertheless.
          *  CAN events: At least one CAN message was received. Note that this event
          *              will only fire again if all input buffers have been cleared.
          *  timeouts:   If some module does not support the sequence of there is any
          *              issue, such a case is detected via timeouts and must be
          *              handled accordingly (see abort state). In some cases, it is
          *              possible that a timeout event occurres 'simultaneously' with
          *              some other event. This can be caused by several timing issues
          *              and is a valid situation. As a result, any other events
          *              should be handled before the timeout event. If the other
          *              events are expected and valid, this implementation requires
          *              the timeout event flag to be cleared explicitely. Otherwise
          *              it is evaluated at the end of each iteration of the loop.
          *  delays:     Depending on the current state, delays are required by SSSP
          *              for timing of the sequential activation of signals.
          */
         aosDbgPrintf("SSSP stack initialization sequence:\n");
         while (flags.loop) {
       #if (AMIROOS_CFG_DBG == true)
           switch (stage) {
             case STAGE_3_1:
               aosDbgPrintf(">>> 3-1\n");
               break;
             case STAGE_3_2:
               aosDbgPrintf(">>> 3-2\n");
               break;
             case STAGE_3_3_WAITFORFIRSTID:
               aosDbgPrintf(">>> 3-3 (1st ID)\n");
               break;
             case STAGE_3_3_WAITFORIDORSIG:
               aosDbgPrintf(">>> 3-3 (ID/sig)\n");
               break;
             case STAGE_3_3_WAITFORID:
               aosDbgPrintf(">>> 3-3 (ID)\n");
               break;
             case STAGE_3_4_FINISH:
               aosDbgPrintf(">>> 3-4 (finish)\n");
               break;
             case STAGE_3_4_ABORT_ACTIVE:
               aosDbgPrintf(">>> 3-4 (active abort)\n");
               break;
             case STAGE_3_4_ABORT:
               aosDbgPrintf(">>> 3-4 (abort)\n");
               break;
+          }
       #endif
           // reset wfe flag for the next iteration
           flags.wfe_next = true;
           // waiting for events (may be skipped)
           if (flags.wfe) {
             // wait for any event to occur
             aosDbgPrintf("WFE...");
             eventmask = chEvtWaitAnyTimeout(ALL_EVENTS, chTimeUS2I(AOS_SYSTEM_SSSP_TIMEOUT));
             aosDbgPrintf("\t0x%08X", eventmask);
           } else {
             aosDbgPrintf("WFE skipped");
             eventmask = 0;
+          }
           aosSysGetUptime(&uptime);
           aosDbgPrintf("\t%04ums\n", (uint32_t)(uptime / MICROSECONDS_PER_MILLISECOND));
           /*
            * execute some general tasks and high priority events
            */
           // no event occurred at all
           if ((flags.wfe) && (eventmask == 0)) {
             aosDbgPrintf("ERR: no evt\n");
             // enforce timeout event
             chEvtBroadcast(&eventSourceTimeout);
             continue;
+          }
           // if an IO event occurred
           if (eventmask & _eventListenerIO.events) {
             ioflags = chEvtGetAndClearFlags(&_eventListenerIO);
             aosDbgPrintf("INFO: IO evt (0x%08X)\n", ioflags);
             // a power-down event occurred
             if (ioflags & MODULE_SSSP_EVENTFLAGS_PD) {
               aosDbgPrintf("PD evt\n");
               // deactivate S and UP
               aosDbgPrintf("disabling S\n");
               apalControlGpioSet(&moduleSsspGpioSync, APAL_GPIO_OFF);
       #if (AMIROOS_CFG_SSSP_STACK_END != true)
               aosDbgPrintf("disabling UP\n");
               apalControlGpioSet(&moduleSsspGpioUp, APAL_GPIO_OFF);
       #endif
               // set shutdown flag and exit the loop
               shutdown = AOS_SHUTDOWN_PASSIVE;
               break;
+            }
             // the S signal was deactivated
             if (ioflags & MODULE_SSSP_EVENTFLAGS_SYNC) {
               apalControlGpioState_t sstate;
               apalControlGpioGet(&moduleSsspGpioSync, &sstate);
               if (sstate == APAL_GPIO_ON) {
                 aosDbgPrintf("S evt (enabled)\n");
               } else {
                 aosDbgPrintf("S evt (disabled)\n");
                 // either finish or abort
                 if ((stage == STAGE_3_3_WAITFORID) && (aos.sssp.moduleId != 0)) {
                   stage = STAGE_3_4_FINISH;
                 } else if (stage != STAGE_3_4_ABORT) {
                   stage = STAGE_3_4_ABORT_ACTIVE;
+                }
+              }
+            }
+          }
           // an OS event occurred
           if (eventmask & _eventListenerOS.events) {
             aosDbgPrintf("WARN: OS evt\n");
             // get the flags
             eventflags_t oseventflags = chEvtGetAndClearFlags(&_eventListenerOS);
             // there should be no OS events at this point
       #ifdef MODULE_SSSP_STARTUP_3_OSEVENT_HOOK
             MODULE_SSSP_STARTUP_3_OSEVENT_HOOK(eventmask, eventflags);
       #else
             _unexpectedEventError(eventmask, oseventflags);
       #endif
+          }
           // if a CAN event occurred
           if ((eventmask & eventListenerCan.events)) {
             aosDbgPrintf("CAN evt\n");
             // fetch message
             if (flags.wfe) {
               canReceiveTimeout(&MODULE_HAL_CAN, CAN_ANY_MAILBOX, &canRxFrame, TIME_IMMEDIATE);
               aosDbgPrintf("CAN <- 0x%03X\n", canRxFrame.SID);
+            }
             // identify and handle abort messgaes
             if (canRxFrame.SID == SSSP_STACKINIT_CANMSGID_ABORT) {
               stage = STAGE_3_4_ABORT;
+            }
             // warn if a unexpected message was received
             else if ((canRxFrame.SID != SSSP_STACKINIT_CANMSGID_INIT) &&
                      (canRxFrame.SID != SSSP_STACKINIT_CANMSGID_MODULEID)) {
               aosDbgPrintf("WARN: unknown msg\n");
+            }
             // any further pending messages are fetched at the end of the loop
+          }
           // if a timeout event occurred
           if (eventmask & eventListenerTimeout.events) {
             aosDbgPrintf("timeout evt\n");
             // is handled at the end of the loop (or must be cleared by FSM)
+          }
           // if a delay event occurred
           if (eventmask & eventListenerDelay.events) {
             aosDbgPrintf("delay evt\n");
             // is handled by FSM
+          }
           /*
            * this is the actual FSM
            */
           switch (stage) {
             case STAGE_3_1:
+            {
               aos.sssp.stage = AOS_SSSP_STARTUP_3_1;
               // there was no event at all (skipped wfe)
               if (eventmask == 0 && flags.wfe == false) {
       #if (AMIROOS_CFG_SSSP_MASTER == true)
                 // initialize the stage by transmitting an according CAN message
                 aosDbgPrintf("CAN -> init\n");
                 canTxFrame.DLC = 0;
                 canTxFrame.SID = SSSP_STACKINIT_CANMSGID_INIT;
                 if (canTransmitTimeout(&MODULE_HAL_CAN, CAN_ANY_MAILBOX, &canTxFrame, TIME_IMMEDIATE) != MSG_OK) {
                   chEvtBroadcast(&eventSourceTimeout);
                   break;
+                }
                 // activate S
                 aosDbgPrintf("enabling S\n");
                 apalControlGpioSet(&moduleSsspGpioSync, APAL_GPIO_ON);
       #if (AMIROOS_CFG_SSSP_STACK_START == true)
                 // proceed immediately
                 stage = STAGE_3_2;
                 flags.wfe_next = false;
       #else
                 // set the timeout timer
                 chVTSet(&timerTimeout, chTimeUS2I(AOS_SYSTEM_SSSP_TIMEOUT), _ssspTimerCallback, &eventSourceTimeout);
                 // proceed
                 stage = STAGE_3_3_WAITFORFIRSTID;
       #endif
       #else
                 // set the timeout timer
                 chVTSet(&timerTimeout, chTimeUS2I(AOS_SYSTEM_SSSP_TIMEOUT), _ssspTimerCallback, &eventSourceTimeout);
       #endif
+              }
       #if (AMIROOS_CFG_SSSP_MASTER != true)
               // a CAN message was received
               else if (eventmask & eventListenerCan.events) {
                 // if an initiation message was received
                 if (canRxFrame.DLC == 0 &&
                     canRxFrame.RTR == CAN_RTR_DATA &&
                     canRxFrame.IDE == CAN_IDE_STD &&
                     canRxFrame.SID == SSSP_STACKINIT_CANMSGID_INIT) {
                   aosDbgPrintf("init msg\n");
                   // reset the timeout timer and clear pending flags
                   chVTReset(&timerTimeout);
                   chEvtWaitAnyTimeout(eventListenerTimeout.events, TIME_IMMEDIATE);
                   eventmask &= ~(eventListenerTimeout.events);
                   // activate S
                   aosDbgPrintf("enabling S\n");
                   apalControlGpioSet(&moduleSsspGpioSync, APAL_GPIO_ON);
       #if (AMIROOS_CFG_SSSP_STACK_START == true)
                   // proceed
                   stage = STAGE_3_2;
                   flags.wfe_next = false;
       #else
                   // set the timeout timer
                   chVTSet(&timerTimeout, chTimeUS2I(AOS_SYSTEM_SSSP_TIMEOUT), _ssspTimerCallback, &eventSourceTimeout);
                   // proceed
                   stage = STAGE_3_3_WAITFORFIRSTID;
       #endif
+                }
+              }
       #endif
               break;
             } /* end of STAGE_3_1 */
             case STAGE_3_2:
+            {
       #if (AMIROOS_CFG_SSSP_STACK_START == true)
               aos.sssp.stage = AOS_SSSP_STARTUP_3_2;
               // if this stage was just entered
               if (flags.wfe == false) {
                 // set the module ID
                 aos.sssp.moduleId = 1;
                 // broadcast module ID
                 aosDbgPrintf("CAN -> ID (%u)\n", aos.sssp.moduleId);
                 canTxFrame.DLC = 4;
                 canTxFrame.SID = SSSP_STACKINIT_CANMSGID_MODULEID;
                 _serialize(canTxFrame.data8, aos.sssp.moduleId, 4);
                 if (canTransmitTimeout(&MODULE_HAL_CAN, CAN_ANY_MAILBOX, &canTxFrame, TIME_IMMEDIATE) != MSG_OK) {
                   chEvtBroadcast(&eventSourceTimeout);
                   break;
+                }
       #if (AMIROOS_CFG_SSSP_STACK_START != true) || (AMIROOS_CFG_DBG == true)
                 lastid = aos.sssp.moduleId;
       #endif
       #if (AMIROOS_CFG_SSSP_STACK_END == true)
                 // sequence is already over
                 // deactivate S
                 aosDbgPrintf("disabling S\n");
                 apalControlGpioSet(&moduleSsspGpioSync, APAL_GPIO_OFF);
                 // proceed
                 stage = STAGE_3_3_WAITFORID;
       #else
                 // set the delay timer so the UP signal is activated later
                 chVTSet(&timerDelay, chTimeUS2I(AMIROOS_CFG_SSSP_SIGNALDELAY), _ssspTimerCallback, &eventSourceDelay);
       #endif
+              }
               // if a delay event occurred
               if (eventmask & eventListenerDelay.events) {
                 // activate UP
                 aosDbgPrintf("enabling UP\n");
                 apalControlGpioSet(&moduleSsspGpioUp, APAL_GPIO_ON);
                 // deactivate S
                 aosDbgPrintf("disabling S\n");
                 apalControlGpioSet(&moduleSsspGpioSync, APAL_GPIO_OFF);
                 // explicitely clear timeout event flag
                 chEvtWaitAnyTimeout(eventListenerTimeout.events, TIME_IMMEDIATE);
                 eventmask &= ~(eventListenerTimeout.events);
                 // proceed
                 stage = STAGE_3_3_WAITFORID;
+              }
       #endif
               break;
             } /* end of STAGE_3_2 */
             case STAGE_3_3_WAITFORFIRSTID:
+            {
       #if (AMIROOS_CFG_SSSP_STACK_START != true)
               aos.sssp.stage = AOS_SSSP_STARTUP_3_3;
               // a CAN message was received
               if (eventmask & eventListenerCan.events) {
                 // if an ID message was received
                 if (canRxFrame.DLC == 4 &&
                     canRxFrame.RTR == CAN_RTR_DATA &&
                     canRxFrame.IDE == CAN_IDE_STD &&
                     canRxFrame.SID == SSSP_STACKINIT_CANMSGID_MODULEID) {
                   aosDbgPrintf("ID (%u)\n", (uint32_t)_deserialize(canRxFrame.data8, 4));
                   // validate received ID
                   if (lastid < _deserialize(canRxFrame.data8, 4)) {
                     // store received ID
                     lastid = _deserialize(canRxFrame.data8, 4);
                     // restart timeout timer
                     chVTSet(&timerTimeout, chTimeUS2I(AOS_SYSTEM_SSSP_TIMEOUT), _ssspTimerCallback, &eventSourceTimeout);
                     // proceed
                     stage = STAGE_3_3_WAITFORIDORSIG;
                   } else {
                     aosDbgPrintf("ERR: invalid ID\n");
                     // abort
                     stage = STAGE_3_4_ABORT_ACTIVE;
                     flags.wfe_next = false;
+                  }
                   // explicitely clear timeout event flag
                   chEvtWaitAnyTimeout(eventListenerTimeout.events, TIME_IMMEDIATE);
                   eventmask &= ~(eventListenerTimeout.events);
+                }
+              }
       #endif
               break;
             } /* end of STAGE_3_3_WAITFORFIRSTID */
             case STAGE_3_3_WAITFORIDORSIG:
+            {
       #if (AMIROOS_CFG_SSSP_STACK_START != true)
               aos.sssp.stage = AOS_SSSP_STARTUP_3_3;
               // a CAN message was received
               if (eventmask & eventListenerCan.events) {
                 // if an ID message was received
                 if (canRxFrame.DLC == 4 &&
                     canRxFrame.RTR == CAN_RTR_DATA &&
                     canRxFrame.IDE == CAN_IDE_STD &&
                     canRxFrame.SID == SSSP_STACKINIT_CANMSGID_MODULEID) {
                   aosDbgPrintf("ID (%u)\n", (uint32_t)_deserialize(canRxFrame.data8, 4));
                   // validate received ID
                   if (lastid < _deserialize(canRxFrame.data8, 4)) {
                     // store received ID
                     lastid = _deserialize(canRxFrame.data8, 4);
                     // restart timeout timer
                     chVTSet(&timerTimeout, chTimeUS2I(AOS_SYSTEM_SSSP_TIMEOUT), _ssspTimerCallback, &eventSourceTimeout);
                   } else {
                     aosDbgPrintf("ERR: invalid ID\n");
                     // abort
                     stage = STAGE_3_4_ABORT_ACTIVE;
                     flags.wfe_next = false;
+                  }
                   // explicitely clear timeout event flag
                   chEvtWaitAnyTimeout(eventListenerTimeout.events, TIME_IMMEDIATE);
                   eventmask &= ~(eventListenerTimeout.events);
+                }
+              }
               // if an IO event was received (DN signal)
               if ((eventmask & _eventListenerIO.events) && (ioflags & MODULE_SSSP_EVENTFLAGS_DN)) {
                 aosDbgPrintf("DN evt\n");
                 // reset timeout timer
                 chVTReset(&timerTimeout);
                 chEvtWaitAnyTimeout(eventListenerTimeout.events, TIME_IMMEDIATE);
                 eventmask &= ~(eventListenerTimeout.events);
                 // increment and broadcast ID
                 aos.sssp.moduleId = lastid + 1;
                 aosDbgPrintf("CAN -> ID (%u)\n", aos.sssp.moduleId);
                 canTxFrame.DLC = 4;
                 canTxFrame.SID = SSSP_STACKINIT_CANMSGID_MODULEID;
                 _serialize(canTxFrame.data8, aos.sssp.moduleId, 4);
                 if (canTransmitTimeout(&MODULE_HAL_CAN, CAN_ANY_MAILBOX, &canTxFrame, TIME_IMMEDIATE) != MSG_OK) {
                   chEvtBroadcast(&eventSourceTimeout);
                   break;
+                }
                 // set delay timer
                 chVTSet(&timerDelay, chTimeUS2I(AMIROOS_CFG_SSSP_SIGNALDELAY), _ssspTimerCallback, &eventSourceDelay);
+              }
               // if a delay event occurred
               if (eventmask & eventListenerDelay.events) {
       #if (AMIROOS_CFG_SSSP_STACK_END != true)
                 // activate UP
                 aosDbgPrintf("enabling UP\n");
                 apalControlGpioSet(&moduleSsspGpioUp, APAL_GPIO_ON);
       #endif
                 // deactivate S
                 aosDbgPrintf("disabling S\n");
                 apalControlGpioSet(&moduleSsspGpioSync, APAL_GPIO_OFF);
                 // reset the timeout timer
                 chVTSet(&timerTimeout, chTimeUS2I(AOS_SYSTEM_SSSP_TIMEOUT), _ssspTimerCallback, &eventSourceTimeout);
                 chEvtWaitAnyTimeout(eventListenerTimeout.events, TIME_IMMEDIATE);
                 eventmask &= ~(eventListenerTimeout.events);
                 // proceed
                 stage = STAGE_3_3_WAITFORID;
+              }
       #endif
               break;