diff --git a/configs/stm32f103-minimum/src/Makefile b/configs/stm32f103-minimum/src/Makefile index c399b10fbf..360aa0a08e 100644 --- a/configs/stm32f103-minimum/src/Makefile +++ b/configs/stm32f103-minimum/src/Makefile @@ -40,73 +40,77 @@ ASRCS = CSRCS = stm32_boot.c stm32_bringup.c stm32_spi.c ifeq ($(CONFIG_LIB_BOARDCTL),y) -CSRCS += stm32_appinit.c + CSRCS += stm32_appinit.c endif ifeq ($(CONFIG_ARCH_BUTTONS),y) -CSRCS += stm32_buttons.c + CSRCS += stm32_buttons.c endif ifeq ($(CONFIG_ARCH_LEDS),y) -CSRCS += stm32_autoleds.c + CSRCS += stm32_autoleds.c else -CSRCS += stm32_userleds.c + CSRCS += stm32_userleds.c endif ifeq ($(CONFIG_DEV_GPIO),y) -CSRCS += stm32_gpio.c + CSRCS += stm32_gpio.c endif ifeq ($(CONFIG_PWM),y) -CSRCS += stm32_pwm.c + CSRCS += stm32_pwm.c endif ifeq ($(CONFIG_RGBLED),y) -CSRCS += stm32_rgbled.c + CSRCS += stm32_rgbled.c endif ifeq ($(CONFIG_MMCSD),y) -CSRCS += stm32_mmcsd.c + CSRCS += stm32_mmcsd.c endif ifeq ($(CONFIG_MTD_W25),y) -CSRCS += stm32_w25.c + CSRCS += stm32_w25.c endif ifeq ($(CONFIG_AUDIO_TONE),y) -CSRCS += stm32_tone.c + CSRCS += stm32_tone.c endif ifeq ($(CONFIG_CAN_MCP2515),y) -CSRCS += stm32_mcp2515.c + CSRCS += stm32_mcp2515.c endif ifeq ($(CONFIG_CL_MFRC522),y) -CSRCS += stm32_mfrc522.c + CSRCS += stm32_mfrc522.c +endif + +ifeq ($(CONFIG_SENSORS_HCSR04),y) + CSRCS += stm32_hcsr04.c endif ifeq ($(CONFIG_LCD_ST7567),y) -CSRCS += stm32_lcd.c + CSRCS += stm32_lcd.c endif ifeq ($(CONFIG_LCD_PCD8544),y) -CSRCS += stm32_pcd8544.c + CSRCS += stm32_pcd8544.c endif ifeq ($(CONFIG_QENCODER),y) -CSRCS += stm32_qencoder.c + CSRCS += stm32_qencoder.c endif ifeq ($(CONFIG_VEML6070),y) -CSRCS += stm32_veml6070.c + CSRCS += stm32_veml6070.c endif ifeq ($(CONFIG_WL_NRF24L01),y) -CSRCS += stm32_nrf24l01.c + CSRCS += stm32_nrf24l01.c endif ifeq ($(CONFIG_USBDEV),y) -CSRCS += stm32_usbdev.c + CSRCS += stm32_usbdev.c endif include $(TOPDIR)/configs/Board.mk diff --git a/configs/stm32f103-minimum/src/stm32_bringup.c b/configs/stm32f103-minimum/src/stm32_bringup.c index 95c5cc4654..0c5aaa1597 100644 --- a/configs/stm32f103-minimum/src/stm32_bringup.c +++ b/configs/stm32f103-minimum/src/stm32_bringup.c @@ -69,6 +69,10 @@ # include #endif +#ifdef CONFIG_USERLED +# include +#endif + #include "stm32f103_minimum.h" /* Conditional logic in stm32f103_minimum.h will determine if certain features @@ -81,6 +85,10 @@ # include "stm32_rtc.h" #endif +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + /* Checking needed by W25 Flash */ #define HAVE_W25 1 @@ -196,6 +204,16 @@ int stm32_bringup(void) } #endif +#ifdef CONFIG_SENSORS_HCSR04 + /* Configure and initialize the HC-SR04 distance sensor */ + + ret = stm32_hcsr04_initialize("/dev/dist0"); + if (ret < 0) + { + syslog(LOG_ERR, "ERROR: stm32_hcsr04_initialize() failed: %d\n", ret); + } +#endif + #ifdef CONFIG_CAN_MCP2515 /* Configure and initialize the MCP2515 CAN device */ diff --git a/configs/stm32f103-minimum/src/stm32_hcsr04.c b/configs/stm32f103-minimum/src/stm32_hcsr04.c new file mode 100644 index 0000000000..ed8edfc1eb --- /dev/null +++ b/configs/stm32f103-minimum/src/stm32_hcsr04.c @@ -0,0 +1,268 @@ +/************************************************************************************ + * configs/stm32f103-minimum/src/stm32_hcsr04.c + * + * Copyright (C) 2017 Gregory Nutt. All rights reserved. + * Copyright (C) 2017 Alan Carvalho de Assis. All rights reserved. + * Author: Alan Carvalho de Assis + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#include + +#include +#include + +#include +#include + +#include "stm32.h" +#include "stm32_freerun.h" +#include "stm32f103_minimum.h" + +#if defined(CONFIG_STM32_FREERUN) && defined (CONFIG_SENSORS_HCSR04) + +#if !defined(CONFIG_STM32_TIM1) +# error STM32 TIM1 is not defined +#endif + +/************************************************************************************ + * Pre-processor Defintions + ************************************************************************************/ + +/* Use TIM1 as free running timer for HC-SR04 sensor */ + +#define HCSR04_FRTIMER 1 + +/************************************************************************************ + * Private Types + ************************************************************************************/ + +struct stm32_hcsr04config_s +{ + /* Configuration structure as seen by the HC-SR04 driver */ + + struct hcsr04_config_s config; + + /* Additional private definitions only known to this driver */ + + FAR void *arg; /* Argument to pass to the interrupt handler */ + FAR xcpt_t isr; /* ISR Handler */ + bool rising; /* Rising edge enabled */ + bool falling; /* Falling edge enabled */ +}; + +/************************************************************************************ + * Private Function Prototypes + ************************************************************************************/ + +static int hcsr04_irq_attach(FAR struct hcsr04_config_s *state, xcpt_t isr, + FAR void *arg); +static void hcsr04_irq_enable(FAR const struct hcsr04_config_s *state, + bool enable); +static void hcsr04_irq_clear(FAR const struct hcsr04_config_s *state); +static void hcsr04_irq_setmode(FAR struct hcsr04_config_s *state, + bool rise_mode); +static void hcsr04_set_trigger(FAR const struct hcsr04_config_s *state, + bool on); +static int64_t hcsr04_get_clock(FAR const struct hcsr04_config_s *state); + +/************************************************************************************ + * Private Data + ************************************************************************************/ + +/* A reference to a structure of this type must be passed to the HC-SR04 + * driver. This structure provides information about the configuration + * of the HC-SR04 and provides some board-specific hooks. + * + * Memory for this structure is provided by the caller. It is not copied + * by the driver and is presumed to persist while the driver is active. The + * memory must be writable because, under certain circumstances, the driver + * may modify frequency or X plate resistance values. + */ + +static struct stm32_hcsr04config_s g_hcsr04config = +{ + .config = + { + .irq_attach = hcsr04_irq_attach, + .irq_enable = hcsr04_irq_enable, + .irq_clear = hcsr04_irq_clear, + .irq_setmode = hcsr04_irq_setmode, + .set_trigger = hcsr04_set_trigger, + .get_clock = hcsr04_get_clock, + }, +}; + +struct stm32_freerun_s g_freerun; +struct timespec ts; + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/* Attach the HC-SR04 interrupt handler to the GPIO interrupt */ + +static int hcsr04_irq_attach(FAR struct hcsr04_config_s *state, xcpt_t isr, + FAR void *arg) +{ + FAR struct stm32_hcsr04config_s *priv = + (FAR struct stm32_hcsr04config_s *)state; + irqstate_t flags; + + sinfo("hcsr04_irq_attach\n"); + + flags = enter_critical_section(); + + priv->rising = true; + priv->falling = false; + priv->isr = isr; + priv->arg = arg; + + (void)stm32_gpiosetevent(GPIO_HCSR04_INT, priv->rising, priv->falling, true, + isr, arg); + + leave_critical_section(flags); + + return OK; +} + +/* Setup the interruption mode: Rising or Falling */ + +static void hcsr04_irq_setmode(FAR struct hcsr04_config_s *state, bool rise_mode) +{ + FAR struct stm32_hcsr04config_s *priv = + (FAR struct stm32_hcsr04config_s *)state; + + if (rise_mode) + { + priv->rising = true; + priv->falling = false; + } + else + { + priv->rising = false; + priv->falling = true; + } +} + +/* Enable or disable the GPIO interrupt */ + +static void hcsr04_irq_enable(FAR const struct hcsr04_config_s *state, + bool enable) +{ + FAR struct stm32_hcsr04config_s *priv = + (FAR struct stm32_hcsr04config_s *)state; + + iinfo("%d\n", enable); + + (void)stm32_gpiosetevent(GPIO_HCSR04_INT, priv->rising, priv->falling, true, + enable ? priv->isr : NULL, priv->arg); +} + +/* Acknowledge/clear any pending GPIO interrupt */ + +static void hcsr04_irq_clear(FAR const struct hcsr04_config_s *state) +{ + // FIXME Nothing to do ? +} + +/* Set the Trigger pin state */ + +static void hcsr04_set_trigger(FAR const struct hcsr04_config_s *state, bool on) +{ + (void)stm32_gpiowrite(GPIO_HCSR04_TRIG, on); +} + +/* Return the current Free Running clock tick */ + +static int64_t hcsr04_get_clock(FAR const struct hcsr04_config_s *state) +{ + /* Get the time from free running timer */ + + stm32_freerun_counter(&g_freerun, &ts); + + /* Return time in microseconds */ + + return ((ts.tv_sec * 1000000) + (ts.tv_nsec / 1000)); +} + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/**************************************************************************** + * Name: stm32_hcsr04_initialize + * + * Description: + * This function is called by application-specific, setup logic to + * configure the HC-SR04 sensor. This function will register the driver + * as /dev/dist0 or any other name passed at *devname. + * + * Input Parameters: + * devname - The device name to register (i.e. "/dev/dist0"). + * + * Returned Value: + * Zero is returned on success. Otherwise, a negated errno value is + * returned to indicate the nature of the failure. + * + ****************************************************************************/ + +int stm32_hcsr04_initialize(FAR const char *devname) +{ + int ret; + + sinfo("devname = %s\n", devname); + + /* Configure the PIO interrupt */ + + stm32_configgpio(GPIO_HCSR04_INT); + + /* Configure the Trigger pin */ + + stm32_configgpio(GPIO_HCSR04_TRIG); + + /* Initialize the free-running timer with 1uS resolution */ + + ret = stm32_freerun_initialize(&g_freerun, HCSR04_FRTIMER, 1); + if (ret < 0) + { + serr("Failed to initialize the free running timer! Err = %d\n", ret); + return -ENODEV; + } + + return hcsr04_register(devname, &g_hcsr04config.config); +} + +#endif /* CONFIG_STM32_FREERUN and CONFIG_SENSORS_HCSR04 */ diff --git a/configs/stm32f103-minimum/src/stm32f103_minimum.h b/configs/stm32f103-minimum/src/stm32f103_minimum.h index b2cea45226..9c3e69b726 100644 --- a/configs/stm32f103-minimum/src/stm32f103_minimum.h +++ b/configs/stm32f103-minimum/src/stm32f103_minimum.h @@ -77,6 +77,11 @@ #define MAX_IRQBUTTON BUTTON_USER2 #define NUM_IRQBUTTONS (BUTTON_USER1 - BUTTON_USER2 + 1) +/* Pins config to use with HC-SR04 sensor */ + +#define GPIO_HCSR04_INT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_PORTA|GPIO_PIN0) +#define GPIO_HCSR04_TRIG (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) /* SPI chip selects */ @@ -192,6 +197,16 @@ int stm32_gpio_initialize(void); void stm32_spidev_initialize(void); +/************************************************************************************ + * Name: stm32_hcsr04_initialize + * + * Description: + * Called to initialize the HC-SR04 sensor + * + ************************************************************************************/ + +int stm32_hcsr04_initialize(FAR const char *devname); + /************************************************************************************ * Name: stm32_w25initialize * diff --git a/drivers/sensors/Kconfig b/drivers/sensors/Kconfig index 36ccee082c..898db5f0ce 100644 --- a/drivers/sensors/Kconfig +++ b/drivers/sensors/Kconfig @@ -32,6 +32,28 @@ config BMP180 ---help--- Enable driver support for the Bosch BMP180 barometer sensor. +config SENSORS_HCSR04 + bool "HC-SR04 Distance Measurement Sensor" + default n + ---help--- + Enable driver support for the HC-SR04 Distance Sensor. + +if SENSORS_HCSR04 + +config HCSR04_DEBUG + bool "Debug support for the HC-SR04" + default n + ---help--- + Enables debug features for the HC-SR04 + +config HCSR04_NPOLLWAITERS + int "Number of waiters to poll" + default 1 + ---help--- + Number of waiters to poll + +endif # SENSORS_HCSR04 + config HTS221 bool "STMicro HTS221 humidity sensor" default n @@ -39,7 +61,7 @@ config HTS221 ---help--- Enable driver support for the STMicro HTS221 humidity sensor. -if HTS221 +if SENSORS_HTS221 config HTS221_I2C_FREQUENCY int "HTS221 I2C frequency" diff --git a/drivers/sensors/Make.defs b/drivers/sensors/Make.defs index 1f6affc225..0623eaa689 100644 --- a/drivers/sensors/Make.defs +++ b/drivers/sensors/Make.defs @@ -37,6 +37,10 @@ ifeq ($(CONFIG_SENSORS),y) +ifeq ($(CONFIG_SENSORS_HCSR04),y) + CSRCS += hc_sr04.c +endif + ifeq ($(CONFIG_SENSORS_ADXL345),y) CSRCS += adxl345_base.c endif diff --git a/drivers/sensors/hc_sr04.c b/drivers/sensors/hc_sr04.c new file mode 100644 index 0000000000..097cb29e1f --- /dev/null +++ b/drivers/sensors/hc_sr04.c @@ -0,0 +1,464 @@ +/**************************************************************************** + * drivers/sensors/hc_sr04.c + * + * Copyright (C) 2017 Alan Carvalho de Assis + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +#ifdef CONFIG_HCSR04_DEBUG +# define hcsr04_dbg(x, ...) _info(x, ##__VA_ARGS__) +#else +# define hcsr04_dbg(x, ...) sninfo(x, ##__VA_ARGS__) +#endif + +/**************************************************************************** + * Private Function Prototypes + *****************************************************************************/ + +static int hcsr04_open(FAR struct file *filep); +static int hcsr04_close(FAR struct file *filep); +static ssize_t hcsr04_read(FAR struct file *filep, FAR char *buffer, + size_t buflen); +static ssize_t hcsr04_write(FAR struct file *filep, FAR const char *buffer, + size_t buflen); +static int hcsr04_ioctl(FAR struct file *filep, int cmd, unsigned long arg); +#ifndef CONFIG_DISABLE_POLL +static int hcsr04_poll(FAR struct file *filep, FAR struct pollfd *fds, + bool setup); +#endif + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +struct hcsr04_dev_s +{ + FAR struct hcsr04_config_s *config; + sem_t devsem; + int time_start_pulse; + int time_finish_pulse; + volatile bool conv_done; + volatile bool rising; +#ifndef CONFIG_DISABLE_POLL + struct pollfd *fds[CONFIG_HCSR04_NPOLLWAITERS]; +#endif +}; + +/**************************************************************************** +* Private Data +****************************************************************************/ + +static const struct file_operations g_hcsr04ops = +{ + hcsr04_open, /* open */ + hcsr04_close, /* close */ + hcsr04_read, /* read */ + hcsr04_write, /* write */ + NULL, /* seek */ + hcsr04_ioctl /* ioctl */ +#ifndef CONFIG_DISABLE_POLL + , hcsr04_poll /* poll */ +#endif +#ifndef CONFIG_DISABLE_PSEUDOFS_OPERATIONS + , NULL /* unlink */ +#endif +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +static int hcsr04_read_distance(FAR struct hcsr04_dev_s *priv) +{ + if (priv->conv_done) + { + return (priv->time_finish_pulse - priv->time_start_pulse); + } + else + { + return -EAGAIN; + } +} + +static int hcsr04_start_measuring(FAR struct hcsr04_dev_s *priv) +{ + /* Delay 60ms between readings */ + + usleep(60000); + + /* Configure the interruption */ + + priv->conv_done = false; + priv->rising = true; + priv->config->irq_setmode(priv->config, priv->rising); + priv->config->irq_enable(priv->config, true); + + /* Send to 10uS trigger pulse */ + + priv->config->set_trigger(priv->config, true); + usleep(10); + priv->config->set_trigger(priv->config, false); + + return 0; +} + +static int hcsr04_open(FAR struct file *filep) +{ + FAR struct inode *inode = filep->f_inode; + FAR struct hcsr04_dev_s *priv = inode->i_private; + + while (sem_wait(&priv->devsem) != 0) + { + assert(errno == EINTR); + } + + sem_post(&priv->devsem); + hcsr04_dbg("OPENED\n"); + return OK; +} + +static int hcsr04_close(FAR struct file *filep) +{ + FAR struct inode *inode = filep->f_inode; + FAR struct hcsr04_dev_s *priv = inode->i_private; + int ret = OK; + + while (sem_wait(&priv->devsem) != 0) + { + assert(errno == EINTR); + } + + sem_post(&priv->devsem); + hcsr04_dbg("CLOSED\n"); + return ret; +} + +static ssize_t hcsr04_read(FAR struct file *filep, FAR char *buffer, + size_t buflen) +{ + FAR struct inode *inode = filep->f_inode; + FAR struct hcsr04_dev_s *priv = inode->i_private; + int distance = 0; + ssize_t length = 0; + + while (sem_wait(&priv->devsem) != 0) + { + assert(errno == EINTR); + } + + /* Setup and send a pulse to start measuring */ + + (void)hcsr04_start_measuring(priv); + + /* Wait the convertion to finish */ + + while (!priv->conv_done) + { + /* busy wait */ + } + + distance = hcsr04_read_distance(priv); + if (distance < 0) + { + hcsr04_dbg("failed to read the distance\n"); + } + else + { + /* This interface is mainly intended for easy debugging in nsh. */ + + length = snprintf(buffer, buflen, "%d\n", distance); + if (length > buflen) + { + length = buflen; + } + } + + sem_post(&priv->devsem); + return length; +} + +static ssize_t hcsr04_write(FAR struct file *filep, FAR const char *buffer, + size_t buflen) +{ + ssize_t length = 0; + return length; +} + +static int hcsr04_ioctl(FAR struct file *filep, int cmd, unsigned long arg) +{ + FAR struct inode *inode = filep->f_inode; + FAR struct hcsr04_dev_s *priv = inode->i_private; + int ret = OK; + + while (sem_wait(&priv->devsem) != 0) + { + assert(errno == EINTR); + } + + switch (cmd) + { + case SNIOC_START_CONVERSION: + ret = hcsr04_start_measuring(priv); + break; + + case SNIOC_READ_RAW_DATA: + //ret = hcsr04_read_raw_data(priv, (FAR hcsr04_raw_data_t *) arg); + break; + +#ifdef CONFIG_HCSR04_DEBUG + case SNIOC_DUMP_REGS: + ret = hcsr04_dump_registers(priv); + break; +#endif + + default: + ret = -ENOTTY; + break; + } + + sem_post(&priv->devsem); + return ret; +} + +#ifndef CONFIG_DISABLE_POLL +static bool hcsr04_sample(FAR struct hcsr04_dev_s *priv) +{ + return priv->conv_done; +} + +static void hcsr04_notify(FAR struct hcsr04_dev_s *priv) +{ + DEBUGASSERT(priv != NULL); + + int i; + + /* If there are threads waiting on poll() for data to become available, + * then wake them up now. NOTE: we wake up all waiting threads because we + * do not know that they are going to do. If they all try to read the data, + * then some make end up blocking after all. + */ + + for (i = 0; i < CONFIG_HCSR04_NPOLLWAITERS; i++) + { + FAR struct pollfd *fds = priv->fds[i]; + if (fds) + { + fds->revents |= POLLIN; + hcsr04_dbg("Report events: %02x\n", fds->revents); + sem_post(fds->sem); + } + } +} + +static int hcsr04_poll(FAR struct file *filep, FAR struct pollfd *fds, + bool setup) +{ + FAR struct inode *inode; + FAR struct hcsr04_dev_s *priv; + int ret = OK; + int i; + uint32_t flags; + + DEBUGASSERT(filep && fds); + inode = filep->f_inode; + + DEBUGASSERT(inode && inode->i_private); + priv = (FAR struct hcsr04_dev_s *)inode->i_private; + + while (sem_wait(&priv->devsem) != 0) + { + assert(errno == EINTR); + } + + if (setup) + { + /* Ignore waits that do not include POLLIN */ + + if ((fds->events & POLLIN) == 0) + { + ret = -EDEADLK; + goto out; + } + + /* This is a request to set up the poll. Find an available slot for + * the poll structure reference. + */ + + for (i = 0; i < CONFIG_HCSR04_NPOLLWAITERS; i++) + { + /* Find an available slot */ + + if (!priv->fds[i]) + { + /* Bind the poll structure and this slot */ + + priv->fds[i] = fds; + fds->priv = &priv->fds[i]; + break; + } + } + + if (i >= CONFIG_HCSR04_NPOLLWAITERS) + { + fds->priv = NULL; + ret = -EBUSY; + goto out; + } + + flags = enter_critical_section(); + if (hcsr04_sample(priv)) + { + hcsr04_notify(priv); + } + + leave_critical_section(flags); + } + else if (fds->priv) + { + /* This is a request to tear down the poll. */ + + struct pollfd **slot = (struct pollfd **)fds->priv; + DEBUGASSERT(slot != NULL); + + /* Remove all memory of the poll setup */ + + *slot = NULL; + fds->priv = NULL; + } + +out: + sem_post(&priv->devsem); + return ret; +} +#endif /* !CONFIG_DISABLE_POLL */ + +static int hcsr04_int_handler(int irq, FAR void *context, FAR void *arg) +{ + FAR struct hcsr04_dev_s *priv = (FAR struct hcsr04_dev_s *)arg; + + DEBUGASSERT(priv != NULL); + + /* Is this the start of the pulse used to encode the distance ? */ + + if (priv->rising) + { + /* Get the clock ticks from the free running timer */ + + priv->time_start_pulse = priv->config->get_clock(priv->config); + + /* Now we need to wait for the falling edge interruption */ + + priv->rising = false; + priv->config->irq_setmode(priv->config, priv->rising); + priv->config->irq_enable(priv->config, true); + } + else + { + /* Get the clock ticks from the free running timer */ + + priv->time_finish_pulse = priv->config->get_clock(priv->config); + + /* Disable interruptions */ + + priv->config->irq_enable(priv->config, false); + + /* Convertion is done */ + + priv->conv_done = true; + } + + hcsr04_dbg("HC-SR04 interrupt\n"); +#ifndef CONFIG_DISABLE_POLL + hcsr04_notify(priv); +#endif + + return OK; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +int hcsr04_register(FAR const char *devpath, + FAR struct hcsr04_config_s *config) +{ + int ret = 0; + FAR struct hcsr04_dev_s *priv; + + priv = (struct hcsr04_dev_s *)kmm_zalloc(sizeof(struct hcsr04_dev_s)); + + if (!priv) + { + hcsr04_dbg("Memory cannot be allocated for HC-SR04 sensor"); + return -ENOMEM; + } + + priv->config = config; + sem_init(&priv->devsem, 0, 1); + + ret = register_driver(devpath, &g_hcsr04ops, 0666, priv); + if (ret < 0) + { + kmm_free(priv); + hcsr04_dbg("Error occurred during the driver registering = %d\n", ret); + return ret; + } + + if (priv->config->irq_clear) + { + priv->config->irq_clear(priv->config); + } + + priv->config->irq_attach(priv->config, hcsr04_int_handler, priv); + priv->config->irq_enable(priv->config, false); + return OK; +} diff --git a/include/nuttx/sensors/hc_sr04.h b/include/nuttx/sensors/hc_sr04.h new file mode 100644 index 0000000000..9efd92028f --- /dev/null +++ b/include/nuttx/sensors/hc_sr04.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * include/nuttx/sensors/hc_sr04.h + * + * Copyright (C) 2017 Greg Nutt. All rights reserved. + * Author: Alan Carvalho de Assis + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef __INCLUDE_NUTT_SENSORS_HCSR04_H +#define __INCLUDE_NUTT_SENSORS_HCSR04_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/* Interrupt configuration data structure */ + +struct hcsr04_config_s +{ + CODE int (*irq_attach)(FAR struct hcsr04_config_s * state, xcpt_t isr, + FAR void *arg); + CODE void (*irq_enable)(FAR const struct hcsr04_config_s *state, + bool enable); + CODE void (*irq_clear)(FAR const struct hcsr04_config_s *state); + CODE void (*irq_setmode)(FAR struct hcsr04_config_s *state, bool risemode); + CODE void (*set_trigger)(FAR const struct hcsr04_config_s *state, bool on); + CODE int64_t (*get_clock)(FAR const struct hcsr04_config_s *state); +}; + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +int hcsr04_register(FAR const char *devpath, + FAR struct hcsr04_config_s *config); + +#endif /* __INCLUDE_NUTT_SENSORS_HCSR04_H */