flight/pios/common/pios_hcsr04.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup PIOS PIOS Core hardware abstraction layer
   4  * @{
   5  * @addtogroup PIOS_HCSR04 HCSR04 Functions
   6  * @brief Hardware functions to deal with the altitude pressure sensor
   7  * @{
   8  *
   9  * @file       pios_hcsr04.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
  11  * @brief      HCSR04 sonar Sensor Routines
  12  * @see        The GNU Public License (GPL) Version 3
  13  *
  14  ******************************************************************************/
  15 /*
  16  * This program is free software; you can redistribute it and/or modify
  17  * it under the terms of the GNU General Public License as published by
  18  * the Free Software Foundation; either version 3 of the License, or
  19  * (at your option) any later version.
  20  *
  21  * This program is distributed in the hope that it will be useful, but
  22  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  23  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  24  * for more details.
  25  *
  26  * You should have received a copy of the GNU General Public License along
  27  * with this program; if not, write to the Free Software Foundation, Inc.,
  28  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  29  */
  30
  31 #include "pios.h"
  32 #include "pios_hcsr04_priv.h"
  33
  34 #ifdef PIOS_INCLUDE_HCSR04
  35
  36 #if !(defined(PIOS_INCLUDE_DSM) || defined(PIOS_INCLUDE_SBUS))
  37 #error Only supported with Spektrum/JR DSM or S.Bus interface
  38 #endif
  39
  40 /* Local Variables */
  41 /* 100 ms timeout without updates on channels */
  42 const static uint32_t PWM_SUPERVISOR_TIMEOUT = 100000;
  43
  44 struct pios_hcsr04_dev *hcsr04_dev_loc;
  45
  46 enum pios_hcsr04_dev_magic {
  47     PIOS_HCSR04_DEV_MAGIC = 0xab3029AA,
  48 };
  49
  50 struct pios_hcsr04_dev {
  51     enum pios_hcsr04_dev_magic   magic;
  52     const struct pios_hcsr04_cfg *cfg;
  53
  54     uint8_t  CaptureState[PIOS_PWM_NUM_INPUTS];
  55     uint16_t RiseValue[PIOS_PWM_NUM_INPUTS];
  56     uint16_t FallValue[PIOS_PWM_NUM_INPUTS];
  57     uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS];
  58     uint32_t CapCounter[PIOS_PWM_NUM_INPUTS];
  59     uint32_t us_since_update[PIOS_PWM_NUM_INPUTS];
  60 };
  61
  62 static bool PIOS_HCSR04_validate(struct pios_hcsr04_dev *hcsr04_dev)
  63 {
  64     return hcsr04_dev->magic == PIOS_HCSR04_DEV_MAGIC;
  65 }
  66
  67 #if defined(PIOS_INCLUDE_FREERTOS)
  68 static struct pios_hcsr04_dev *PIOS_PWM_alloc(void)
  69 {
  70     struct pios_hcsr04_dev *hcsr04_dev;
  71
  72     hcsr04_dev = (struct pios_hcsr04_dev *)pios_malloc(sizeof(*hcsr04_dev));
  73     if (!hcsr04_dev) {
  74         return NULL;
  75     }
  76
  77     hcsr04_dev->magic = PIOS_HCSR04_DEV_MAGIC;
  78     return hcsr04_dev;
  79 }
  80 #else
  81 static struct pios_hcsr04_dev pios_hcsr04_devs[PIOS_PWM_MAX_DEVS];
  82 static uint8_t pios_hcsr04_num_devs;
  83 static struct pios_hcsr04_dev *PIOS_PWM_alloc(void)
  84 {
  85     struct pios_hcsr04_dev *hcsr04_dev;
  86
  87     if (pios_pwm_num_devs >= PIOS_PWM_MAX_DEVS) {
  88         return NULL;
  89     }
  90
  91     hcsr04_dev = &pios_hcsr04_devs[pios_hcsr04_num_devs++];
  92     hcsr04_dev->magic = PIOS_HCSR04_DEV_MAGIC;
  93
  94     return hcsr04_dev;
  95 }
  96 #endif /* if defined(PIOS_INCLUDE_FREERTOS) */
  97
  98 static void PIOS_HCSR04_tim_overflow_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
  99 static void PIOS_HCSR04_tim_edge_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
 100 const static struct pios_tim_callbacks tim_callbacks = {
 101     .overflow = PIOS_HCSR04_tim_overflow_cb,
 102     .edge     = PIOS_HCSR04_tim_edge_cb,
 103 };
 104
 105
 106 /**
 107  * Initialises all the pins
 108  */
 109 int32_t PIOS_HCSR04_Init(uint32_t *pwm_id, const struct pios_hcsr04_cfg *cfg)
 110 {
 111     PIOS_DEBUG_Assert(pwm_id);
 112     PIOS_DEBUG_Assert(cfg);
 113
 114     struct pios_hcsr04_dev *hcsr04_dev;
 115
 116     hcsr04_dev = (struct pios_hcsr04_dev *)PIOS_PWM_alloc();
 117     if (!hcsr04_dev) {
 118         goto out_fail;
 119     }
 120
 121     /* Bind the configuration to the device instance */
 122     hcsr04_dev->cfg = cfg;
 123     hcsr04_dev_loc  = hcsr04_dev;
 124
 125     for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) {
 126         /* Flush counter variables */
 127         hcsr04_dev->CaptureState[i] = 0;
 128         hcsr04_dev->RiseValue[i]    = 0;
 129         hcsr04_dev->FallValue[i]    = 0;
 130         hcsr04_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT;
 131     }
 132
 133     uint32_t tim_id;
 134     if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)hcsr04_dev)) {
 135         return -1;
 136     }
 137
 138     /* Configure the channels to be in capture/compare mode */
 139     for (uint8_t i = 0; i < cfg->num_channels; i++) {
 140         const struct pios_tim_channel *chan   = &cfg->channels[i];
 141
 142         /* Configure timer for input capture */
 143         TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init;
 144         TIM_ICInitStructure.TIM_Channel = chan->timer_chan;
 145         TIM_ICInit(chan->timer, &TIM_ICInitStructure);
 146
 147         /* Enable the Capture Compare Interrupt Request */
 148         switch (chan->timer_chan) {
 149         case TIM_Channel_1:
 150             TIM_ITConfig(chan->timer, TIM_IT_CC1, ENABLE);
 151             break;
 152         case TIM_Channel_2:
 153             TIM_ITConfig(chan->timer, TIM_IT_CC2, ENABLE);
 154             break;
 155         case TIM_Channel_3:
 156             TIM_ITConfig(chan->timer, TIM_IT_CC3, ENABLE);
 157             break;
 158         case TIM_Channel_4:
 159             TIM_ITConfig(chan->timer, TIM_IT_CC4, ENABLE);
 160             break;
 161         }
 162
 163         // Need the update event for that timer to detect timeouts
 164         TIM_ITConfig(chan->timer, TIM_IT_Update, ENABLE);
 165     }
 166
 167 #ifndef STM32F4XX
 168     /* Enable the peripheral clock for the GPIO */
 169     switch ((uint32_t)hcsr04_dev->cfg->trigger.gpio) {
 170     case (uint32_t)GPIOA:
 171         RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
 172         break;
 173     case (uint32_t)GPIOB:
 174         RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
 175         break;
 176     case (uint32_t)GPIOC:
 177         RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
 178         break;
 179     default:
 180         PIOS_Assert(0);
 181         break;
 182     }
 183 #endif
 184     GPIO_Init(hcsr04_dev->cfg->trigger.gpio, &hcsr04_dev->cfg->trigger.init);
 185
 186     *pwm_id = (uint32_t)hcsr04_dev;
 187
 188     return 0;
 189
 190 out_fail:
 191     return -1;
 192 }
 193
 194 void PIOS_HCSR04_Trigger(void)
 195 {
 196     GPIO_SetBits(hcsr04_dev_loc->cfg->trigger.gpio, hcsr04_dev_loc->cfg->trigger.init.GPIO_Pin);
 197     PIOS_DELAY_WaituS(15);
 198     GPIO_ResetBits(hcsr04_dev_loc->cfg->trigger.gpio, hcsr04_dev_loc->cfg->trigger.init.GPIO_Pin);
 199 }
 200
 201 /**
 202  * Get the value of an input channel
 203  * \param[in] Channel Number of the channel desired
 204  * \output -1 Channel not available
 205  * \output >0 Channel value
 206  */
 207 int32_t PIOS_HCSR04_Get(void)
 208 {
 209     return hcsr04_dev_loc->CaptureValue[0];
 210 }
 211
 212 int32_t PIOS_HCSR04_Completed(void)
 213 {
 214     return hcsr04_dev_loc->CapCounter[0];
 215 }
 216
 217 static void PIOS_HCSR04_tim_overflow_cb(uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count)
 218 {
 219     struct pios_hcsr04_dev *hcsr04_dev = (struct pios_hcsr04_dev *)context;
 220
 221     if (!PIOS_HCSR04_validate(hcsr04_dev)) {
 222         /* Invalid device specified */
 223         return;
 224     }
 225
 226     if (channel >= hcsr04_dev->cfg->num_channels) {
 227         /* Channel out of range */
 228         return;
 229     }
 230
 231     hcsr04_dev->us_since_update[channel] += count;
 232     if (hcsr04_dev->us_since_update[channel] >= PWM_SUPERVISOR_TIMEOUT) {
 233         hcsr04_dev->CaptureState[channel]    = 0;
 234         hcsr04_dev->RiseValue[channel]       = 0;
 235         hcsr04_dev->FallValue[channel]       = 0;
 236         hcsr04_dev->CaptureValue[channel]    = PIOS_RCVR_TIMEOUT;
 237         hcsr04_dev->us_since_update[channel] = 0;
 238     }
 239 }
 240
 241 static void PIOS_HCSR04_tim_edge_cb(uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count)
 242 {
 243     /* Recover our device context */
 244     struct pios_hcsr04_dev *hcsr04_dev = (struct pios_hcsr04_dev *)context;
 245
 246     if (!PIOS_HCSR04_validate(hcsr04_dev)) {
 247         /* Invalid device specified */
 248         return;
 249     }
 250
 251     if (chan_idx >= hcsr04_dev->cfg->num_channels) {
 252         /* Channel out of range */
 253         return;
 254     }
 255
 256     const struct pios_tim_channel *chan = &hcsr04_dev->cfg->channels[chan_idx];
 257
 258     if (hcsr04_dev->CaptureState[chan_idx] == 0) {
 259         hcsr04_dev->RiseValue[chan_idx] = count;
 260         hcsr04_dev->us_since_update[chan_idx] = 0;
 261     } else {
 262         hcsr04_dev->FallValue[chan_idx] = count;
 263     }
 264
 265     // flip state machine and capture value here
 266     /* Simple rise or fall state machine */
 267     TIM_ICInitTypeDef TIM_ICInitStructure = hcsr04_dev->cfg->tim_ic_init;
 268     if (hcsr04_dev->CaptureState[chan_idx] == 0) {
 269         /* Switch states */
 270         hcsr04_dev->CaptureState[chan_idx] = 1;
 271
 272         /* Switch polarity of input capture */
 273         TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
 274         TIM_ICInitStructure.TIM_Channel    = chan->timer_chan;
 275         TIM_ICInit(chan->timer, &TIM_ICInitStructure);
 276     } else {
 277         /* Capture computation */
 278         if (hcsr04_dev->FallValue[chan_idx] > hcsr04_dev->RiseValue[chan_idx]) {
 279             hcsr04_dev->CaptureValue[chan_idx] = (hcsr04_dev->FallValue[chan_idx] - hcsr04_dev->RiseValue[chan_idx]);
 280         } else {
 281             hcsr04_dev->CaptureValue[chan_idx] = ((chan->timer->ARR - hcsr04_dev->RiseValue[chan_idx]) + hcsr04_dev->FallValue[chan_idx]);
 282         }
 283
 284         /* Switch states */
 285         hcsr04_dev->CaptureState[chan_idx] = 0;
 286
 287         /* Increase supervisor counter */
 288         hcsr04_dev->CapCounter[chan_idx]++;
 289
 290         /* Switch polarity of input capture */
 291         TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
 292         TIM_ICInitStructure.TIM_Channel    = chan->timer_chan;
 293         TIM_ICInit(chan->timer, &TIM_ICInitStructure);
 294     }
 295 }
 296
 297 #endif /* PIOS_INCLUDE_HCSR04 */