AMiRo-OS

/*

AMiRo-OS is an operating system designed for the Autonomous Mini Robot (AMiRo) platform.

Copyright (C) 2016..2020  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.

 */