flight/pios/stm32f4xx/pios_delay.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup PIOS PIOS Core hardware abstraction layer
   4  * @{
   5  * @addtogroup PIOS_DELAY Delay Functions
   6  * @brief PiOS Delay functionality
   7  * @{
   8  *
   9  * @file       pios_delay.c
  10  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
  11  *             Michael Smith Copyright (C) 2012
  12  * @brief      Delay Functions
  13  *                 - Provides a micro-second granular delay using the CPU
  14  *                   cycle counter.
  15  * @see        The GNU Public License (GPL) Version 3
  16  *
  17  *****************************************************************************/
  18 /*
  19  * This program is free software; you can redistribute it and/or modify
  20  * it under the terms of the GNU General Public License as published by
  21  * the Free Software Foundation; either version 3 of the License, or
  22  * (at your option) any later version.
  23  *
  24  * This program is distributed in the hope that it will be useful, but
  25  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  26  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  27  * for more details.
  28  *
  29  * You should have received a copy of the GNU General Public License along
  30  * with this program; if not, write to the Free Software Foundation, Inc.,
  31  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  32  */
  33
  34 #include <pios.h>
  35
  36 #ifdef PIOS_INCLUDE_DELAY
  37
  38 /* cycles per microsecond */
  39 static uint32_t us_ticks;
  40 static uint32_t raw_hz;
  41
  42 /**
  43  * Initialises the Timer used by PIOS_DELAY functions.
  44  *
  45  * \return always zero (success)
  46  */
  47
  48 int32_t PIOS_DELAY_Init(void)
  49 {
  50     RCC_ClocksTypeDef clocks;
  51
  52     /* compute the number of system clocks per microsecond */
  53     RCC_GetClocksFreq(&clocks);
  54     us_ticks = clocks.SYSCLK_Frequency / 1000000;
  55     PIOS_DEBUG_Assert(us_ticks > 1);
  56     raw_hz   = clocks.SYSCLK_Frequency;
  57
  58     /* turn on access to the DWT registers */
  59     CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
  60
  61     /* enable the CPU cycle counter */
  62     DWT_CTRL |= CYCCNTENA;
  63
  64     return 0;
  65 }
  66
  67 /**
  68  * Waits for a specific number of uS
  69  *
  70  * Example:<BR>
  71  * \code
  72  *   // Wait for 500 uS
  73  *   PIOS_DELAY_Wait_uS(500);
  74  * \endcode
  75  * \param[in] uS delay
  76  * \return < 0 on errors
  77  */
  78 int32_t PIOS_DELAY_WaituS(uint32_t uS)
  79 {
  80     uint32_t elapsed    = 0;
  81     uint32_t last_count = PIOS_DELAY_GetRaw();
  82
  83     for (;;) {
  84         uint32_t current_count = PIOS_DELAY_GetRaw();
  85         uint32_t elapsed_uS;
  86
  87         /* measure the time elapsed since the last time we checked */
  88         elapsed   += current_count - last_count;
  89         last_count = current_count;
  90
  91         /* convert to microseconds */
  92         elapsed_uS = elapsed / us_ticks;
  93         if (elapsed_uS >= uS) {
  94             break;
  95         }
  96
  97         /* reduce the delay by the elapsed time */
  98         uS -= elapsed_uS;
  99
 100         /* keep fractional microseconds for the next iteration */
 101         elapsed %= us_ticks;
 102     }
 103
 104     /* No error */
 105     return 0;
 106 }
 107
 108 /**
 109  * Waits for a specific number of mS
 110  *
 111  * Example:<BR>
 112  * \code
 113  *   // Wait for 500 mS
 114  *   PIOS_DELAY_Wait_mS(500);
 115  * \endcode
 116  * \param[in] mS delay (1..65535 milliseconds)
 117  * \return < 0 on errors
 118  */
 119 int32_t PIOS_DELAY_WaitmS(uint32_t mS)
 120 {
 121     while (mS--) {
 122         PIOS_DELAY_WaituS(1000);
 123     }
 124
 125     /* No error */
 126     return 0;
 127 }
 128
 129 /**
 130  * @brief Query the Delay timer for the current uS
 131  * @return A microsecond value
 132  */
 133 uint32_t PIOS_DELAY_GetuS()
 134 {
 135     return PIOS_DELAY_GetRaw() / us_ticks;
 136 }
 137
 138 /**
 139  * @brief Calculate time in microseconds since a previous time
 140  * @param[in] t previous time
 141  * @return time in us since previous time t.
 142  */
 143 uint32_t PIOS_DELAY_GetuSSince(uint32_t t)
 144 {
 145     return PIOS_DELAY_GetuS() - t;
 146 }
 147
 148 /**
 149  * @brief Get the raw delay timer frequency
 150  * @return raw delay timer frequency in Hz
 151  */
 152 uint32_t PIOS_DELAY_GetRawHz()
 153 {
 154     return raw_hz;
 155 }
 156
 157 /**
 158  * @brief Compare to raw times to and convert to us
 159  * @return A microsecond value
 160  */
 161 uint32_t PIOS_DELAY_DiffuS(uint32_t raw)
 162 {
 163     uint32_t diff = PIOS_DELAY_GetRaw() - raw;
 164
 165     return diff / us_ticks;
 166 }
 167
 168 #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
 169 /**
 170  * @brief Subtract two raw times and convert to us.
 171  * @return Interval between raw times in microseconds
 172  */
 173 uint32_t PIOS_DELAY_DiffuS2(uint32_t raw, uint32_t later)
 174 {
 175     return (later - raw) / us_ticks;
 176 }
 177 #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
 178
 179 #endif /* PIOS_INCLUDE_DELAY */
 180
 181 /**
 182  * @}
 183  * @}
 184  */