AMiRo-OS

/*

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_sssp.c

 * @brief   SSSP related code.

 *

 * @addtogroup aos_sssp

 * @{

 */

#include <amiroos.h>

#if (AMIROOS_CFG_SSSP_ENABLE == true) || defined(__DOXYGEN__)

/******************************************************************************/

/* LOCAL DEFINITIONS                                                          */

/******************************************************************************/

#if ((AMIROOS_CFG_SSSP_MASTER != true) && (AMIROOS_CFG_PROFILE == true)) || defined(__DOXYGEN__)

/**

 * @brief   Weighting factor for smoothing the @p _syncskew value.

 */

#define SYNCSKEW_SMOOTHFACTOR                   (0.1f / AOS_SYSTEM_TIME_RESOLUTION)

#endif /* (AMIROOS_CFG_SSSP_MASTER != true) && (AMIROOS_CFG_PROFILE == true) */

/******************************************************************************/

/* EXPORTED VARIABLES                                                         */

/******************************************************************************/

/******************************************************************************/

/* LOCAL TYPES                                                                */

/******************************************************************************/

/******************************************************************************/

/* LOCAL VARIABLES                                                            */

/******************************************************************************/

/**

 * @brief   Pointer to the system uptime.

 */

static aos_timestamp_t* _uptime;

#if (AMIROOS_CFG_SSSP_MASTER == true) || defined(__DOXYGEN__)

/**

 * @brief   Timer to drive the S signal for system wide time synchronization during operation phase.

 */

static virtual_timer_t _synctimer;

/**

 * @brief   Last uptime of system wide time synchronization.

 */

static aos_timestamp_t _synctime;

#endif /* (AMIROOS_CFG_SSSP_MASTER == true) */

#if ((AMIROOS_CFG_SSSP_MASTER != true) && (AMIROOS_CFG_PROFILE == true)) || defined(__DOXYGEN__)

/**

 * @brief   Offset between local clock and system wide synchronization signal.

 */

static float _syncskew;

#endif /* (AMIROOS_CFG_SSSP_MASTER != true) && (AMIROOS_CFG_PROFILE == true) */

/**

 * @brief   A timer event based delays.

 * @details This timer must not be an AMiRo-OS timer, since delays occurr before system is initialized.

 */

static virtual_timer_t _delayTimer;

/**

 * @brief   Event source for the delay timer.

 */

static event_source_t _delayEventSource;

/**

 * @brief   Event listener for the delay event.

 */

static event_listener_t _delayEventListener;

/******************************************************************************/

/* LOCAL FUNCTIONS                                                            */

/******************************************************************************/

#if (AMIROOS_CFG_SSSP_MASTER != true) || defined(__DOXYGEN__)

/**

 * @brief   Callback function for the S signal interrupt.

 *

 * @param[in] args   Pointer to the GPIO line identifier.

 */

static void _gpioCallbackSSignal(void *args)

{

  aosDbgCheck((args != NULL) && (*((ioline_t*)args) != PAL_NOLINE) && (PAL_PAD(*((ioline_t*)args)) < sizeof(eventflags_t) * 8));

  apalControlGpioState_t s;

  aos_timestamp_t t;

  chSysLockFromISR();

  // if the system is in operation phase

  if (aos.sssp.stage == AOS_SSSP_STAGE_OPERATION) {

    // read signal S

    apalControlGpioGet(moduleSsspSignalS(), &s);

    // if S was toggled from on to off

    if (s == APAL_GPIO_OFF) {

      // get current uptime

      aosSysGetUptimeX(&t);

      // align the uptime with the synchronization period

      t %= AMIROOS_CFG_SSSP_SYSSYNCPERIOD;

      if (t < AMIROOS_CFG_SSSP_SYSSYNCPERIOD / 2) {

        *_uptime -= t;

#if (AMIROOS_CFG_PROFILE == true)

        _syncskew = ((1.0f - SYNCSKEW_SMOOTHFACTOR) * _syncskew) + (SYNCSKEW_SMOOTHFACTOR * t);

#endif /* (AMIROOS_CFG_PROFILE == true) */

      } else {

        t = AMIROOS_CFG_SSSP_SYSSYNCPERIOD - t;

        *_uptime += t;

#if (AMIROOS_CFG_PROFILE == true)

        _syncskew = ((1.0f - SYNCSKEW_SMOOTHFACTOR) * _syncskew) - (SYNCSKEW_SMOOTHFACTOR * t);

#endif /* (AMIROOS_CFG_PROFILE == true) */

      }

    }

  }

  // broadcast event

  chEvtBroadcastFlagsI(&aos.events.gpio, AOS_GPIOEVENT_FLAG(PAL_PAD(*((ioline_t*)args))));

  chSysUnlockFromISR();

  return;

}

#endif /* (AMIROOS_CFG_SSSP_MASTER != true) */

#if (AMIROOS_CFG_SSSP_MASTER == true) || defined (__DOXYGEN__)

/**

 * @brief   Periodic system synchronization callback function.

 * @details Toggles the S signal and reconfigures the system synchronization timer.

 *

 * @param[in] par   Unused parameters.

 */

static void _syncTimerCallback(void* par)

{

  (void)par;

  // local variables

  apalControlGpioState_t state;

  aos_timestamp_t uptime;

  chSysLockFromISR();

  // toggle and read signal S

  apalGpioToggle(moduleSsspSignalS()->gpio);

  apalControlGpioGet(moduleSsspSignalS(), &state);

  // if S was toggled from off to on

  if (state == APAL_GPIO_ON) {

    // reconfigure the timer precisely, because the logically falling edge (next interrupt) snychronizes the system time

    _synctime += AMIROOS_CFG_SSSP_SYSSYNCPERIOD;

    aosSysGetUptimeX(&uptime);

    chVTSetI(&_synctimer, chTimeUS2I((time_usecs_t)(_synctime - uptime)), _syncTimerCallback, NULL);

  }

  // if S was toggled from on to off

  else /* if (state == APAL_GPIO_OFF) */ {

    // reconfigure the timer (lazy)

    chVTSetI(&_synctimer, chTimeUS2I(AMIROOS_CFG_SSSP_SYSSYNCPERIOD / 2), _syncTimerCallback, NULL);

  }

  chSysUnlockFromISR();

  return;

}

/**

 * @brief   Start the timer that toggles S for synchronization of the system.

 * @note    Must be called from a locked context.

 */

static inline void _syncTimerStartS(void)

{

  chDbgCheckClassS();

  // start synchronization timer

  // The first iteration of the timer is set to the next 'center' of a 'slice'.

  aos_timestamp_t t;

  aosSysGetUptimeX(&t);

  t = AMIROOS_CFG_SSSP_SYSSYNCPERIOD - (t % AMIROOS_CFG_SSSP_SYSSYNCPERIOD);

  chVTSetI(&_synctimer, chTimeUS2I((time_usecs_t)((t > (AMIROOS_CFG_SSSP_SYSSYNCPERIOD / 2)) ? (t - (AMIROOS_CFG_SSSP_SYSSYNCPERIOD / 2)) : (t + (AMIROOS_CFG_SSSP_SYSSYNCPERIOD / 2)))), _syncTimerCallback, NULL);

  return;

}

#endif /* (AMIROOS_CFG_SSSP_MASTER == true) */

/**

 * @brief   General callback function to be used for any local timers.

 *

 * @param[in] par   A pointer to an @p event_source_t to be fired.

 */

static void _timerCallback(void* par)

{

  aosDbgCheck(par != NULL);

  chSysLockFromISR();

  chEvtBroadcastI((event_source_t*)par);

  chSysUnlockFromISR();

  return;

}

/**

 * @brief   Waits for the S signal to switch to the desired state.

 *

 * @param[in]   listener      Pointer to the GPIO event listener to be used.

 * @param[in]   signalstate   Desired state of the S signal ti be waited for.

 * @param[out]  received      Output variable to store the received event mask to.

 *

 * @return  Status, indicating whether the expected event was received.

 */

static inline aos_status_t _waitForS(event_listener_t* listener, apalControlGpioState_t signalstate, eventmask_t* received)

{

  aosDbgCheck(listener != NULL);

  aosDbgCheck(received != NULL);

  // local variables

  aos_status_t status = AOS_FAILURE;

  // wait for the next event (do not apply any filters in order not to miss any events)

  *received = chEvtWaitOne(ALL_EVENTS);

  // if the correct event was triggered

  if (*received & listener->events) {

    apalControlGpioState_t s;

    apalControlGpioGet(moduleSsspSignalS(), &s);

    chSysLock();

    // only check for the expected event

    if ((listener->flags & moduleSsspEventflagS()) && (s == signalstate)) {

      // unset the expected flags but keep any other ones

      listener->flags &= ~moduleSsspEventflagS();

      status = AOS_SUCCESS;

    }

    // if no further flags are set

    if (listener->flags == 0) {

      // clear event

      *received &= ~(listener->events);

    }

    chSysUnlock();

  }

  return status;

}

/**

 * @brief   Uses the local delay timer to setup a timed event and waits for it to fire.

 *

 * @param[in]   dt          Delay time in microseconds to be set to the timer.

 * @param[in]   mask        Event mask to be used for the delay timer.

 * @param[out]  received    Output variable to store the received event mask to.

 *

 * @return  Status, indicating whether the expected event (delay timer) was received.

 */

static inline aos_status_t _delay(uint32_t dt, eventmask_t mask, eventmask_t* received)

{

  aosDbgCheck(dt != TIME_IMMEDIATE);

  aosDbgCheck(mask != 0);

  aosDbgCheck(received != NULL);

  // arm the delay timer once

  if (!chVTIsArmed(&_delayTimer)) {

    chEvtRegisterMask(&_delayEventSource, &_delayEventListener, mask);

    chVTSet(&_delayTimer, chTimeUS2I(dt), _timerCallback, &_delayEventSource);

  }

  // wait for any event to occur (do not apply any filters in order not to miss any events)

  *received = chEvtWaitOne(ALL_EVENTS);

  // if the timer event was received, cleanup

  if (*received & _delayEventListener.events) {

    chEvtUnregister(&_delayEventSource, &_delayEventListener);

    *received &= ~(_delayEventListener.events);

    return AOS_SUCCESS;

  }

  return AOS_FAILURE;

}

#if (AMIROOS_CFG_SSSP_MSI == true) || defined(__DOXYGEN__)

/**

 * @brief   Serialize a BCB message.

 * @details The individual primitives are serialized in big-endian fashion.

 *

 * @param[out]  buffer    Buffer to write the serialized message to.

 * @param[in]   message   The message to be serialized.

 */

aos_status_t _serializeBcbMessage(uint8_t* buffer, aos_ssspbcbmessage_t message)

{

  aosDbgCheck(buffer != NULL);

  // encode the message depending on its type

  switch (message.type) {

    case AOS_SSSP_BCBMESSAGE_MSIINIT:

    case AOS_SSSP_BCBMESSAGE_MSIABORT:

    {

      buffer[0] = message.type;

      return AOS_SUCCESS;

    }

    case AOS_SSSP_BCBMESSAGE_MSIID:

    {

      buffer[0] = message.type;

      for (size_t byte = 0; byte < sizeof(aos_ssspmoduleid_t); ++byte) {

        buffer[byte+1] = (message.payload.id >> ((sizeof(aos_ssspmoduleid_t) - (byte+1)) * 8)) & 0xFF;

      }

      return AOS_SUCCESS;

    }

    default:

    {

      return AOS_FAILURE;

    }

  }

}

/**

 * @brief   Deserialize a BCB message.

 * @details The individual primitives must be serialized in big-endian fashion.

 *

 * @param[out]  message   Message object to be filled.

 * @param[in]   buffer    Buffer holding the serialized message.

 */

aos_status_t _deserializeBcbMessage(aos_ssspbcbmessage_t* message, uint8_t* buffer)

{

  aosDbgCheck(message != NULL);

  aosDbgCheck(buffer != NULL);

  // only decode the first byte, which contains the message type

  message->type = buffer[0];

  // decode the message depedning on its type

  switch (message->type) {

    case AOS_SSSP_BCBMESSAGE_MSIINIT:

    case AOS_SSSP_BCBMESSAGE_MSIABORT:

    {

      return AOS_SUCCESS;

    }

    case AOS_SSSP_BCBMESSAGE_MSIID:

    {

      message->payload.id = 0;

      for (size_t byte = 0; byte < sizeof(aos_ssspmoduleid_t); ++byte) {

        message->payload.id |= (aos_ssspmoduleid_t)(buffer[byte+1] << ((sizeof(aos_ssspmoduleid_t) - (byte+1)) * 8));

      }

      return AOS_SUCCESS;

    }

    default:

    {

      message->type = AOS_SSSP_BCBMESSAGE_INVALID;

      return AOS_FAILURE;