From 41cc04c06485f40c2219b8585d50138cd2e5fe37 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 21 Feb 2015 18:15:57 +1100 Subject: [PATCH] pwm_input: added PWM input driver this allows for input of PWM signals on the FMUv2 aux5 pins --- src/drivers/pwm_input/module.mk | 41 ++ src/drivers/pwm_input/pwm_input.cpp | 603 ++++++++++++++++++++++++++++ 2 files changed, 644 insertions(+) create mode 100644 src/drivers/pwm_input/module.mk create mode 100644 src/drivers/pwm_input/pwm_input.cpp diff --git a/src/drivers/pwm_input/module.mk b/src/drivers/pwm_input/module.mk new file mode 100644 index 0000000000..04f04d6cbc --- /dev/null +++ b/src/drivers/pwm_input/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# 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 PX4 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. +# +############################################################################ + +# +# Makefile to build the PWM input driver. +# + +MODULE_COMMAND = pwm_input + +SRCS = pwm_input.cpp + diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp new file mode 100644 index 0000000000..2d771266c0 --- /dev/null +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -0,0 +1,603 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Andrew Tridgell. All rights reserved. + * + * 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 PX4 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. + * + ****************************************************************************/ + +/** + * @file pwm_input.cpp + * + * PWM input driver based on earlier driver from Evan Slatyer, + * which in turn was based on drv_hrt.c + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "chip.h" +#include "up_internal.h" +#include "up_arch.h" + +#include "stm32.h" +#include "stm32_gpio.h" +#include "stm32_tim.h" +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#if HRT_TIMER == PWMIN_TIMER +#error cannot share timer between HRT and PWMIN +#endif + +#if !defined(GPIO_PWM_IN) || !defined(PWMIN_TIMER) || !defined(PWMIN_TIMER_CHANNEL) +#error PWMIN defines are needed in board_config.h for this board +#endif + +/* PWMIN configuration */ +#if PWMIN_TIMER == 1 +# define PWMIN_TIMER_BASE STM32_TIM1_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB2ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM1EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM1CC +# define PWMIN_TIMER_CLOCK STM32_APB2_TIM1_CLKIN +#elif PWMIN_TIMER == 2 +# define PWMIN_TIMER_BASE STM32_TIM2_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM2EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM2 +# define PWMIN_TIMER_CLOCK STM32_APB1_TIM2_CLKIN +#elif PWMIN_TIMER == 3 +# define PWMIN_TIMER_BASE STM32_TIM3_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB1ENR_TIM3EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM3 +# define PWMIN_TIMER_CLOCK STM32_APB1_TIM3_CLKIN +#elif PWMIN_TIMER == 4 +# define PWMIN_TIMER_BASE STM32_TIM4_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB1ENR_TIM4EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM4 +# define PWMIN_TIMER_CLOCK STM32_APB1_TIM4_CLKIN +#elif PWMIN_TIMER == 5 +# define PWMIN_TIMER_BASE STM32_TIM5_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM5EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM5 +# define PWMIN_TIMER_CLOCK STM32_APB1_TIM5_CLKIN +#elif PWMIN_TIMER == 8 +# define PWMIN_TIMER_BASE STM32_TIM8_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB2ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM8EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM8CC +# define PWMIN_TIMER_CLOCK STM32_APB2_TIM8_CLKIN +#elif PWMIN_TIMER == 9 +# define PWMIN_TIMER_BASE STM32_TIM9_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM9EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM1BRK +# define PWMIN_TIMER_CLOCK STM32_APB1_TIM9_CLKIN +#elif PWMIN_TIMER == 10 +# define PWMIN_TIMER_BASE STM32_TIM10_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM10EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM1UP +# define PWMIN_TIMER_CLOCK STM32_APB2_TIM10_CLKIN +#elif PWMIN_TIMER == 11 +# define PWMIN_TIMER_BASE STM32_TIM11_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM11EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM +# define PWMIN_TIMER_CLOCK STM32_APB2_TIM11_CLKIN +#elif PWMIN_TIMER == 12 +# define PWMIN_TIMER_BASE STM32_TIM12_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM12EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM +# define PWMIN_TIMER_CLOCK STM32_APB2_TIM12_CLKIN +#else +# error PWMIN_TIMER must be a value between 1 and 12 +#endif + +/* + * HRT clock must be at least 1MHz + */ +#if PWMIN_TIMER_CLOCK <= 1000000 +# error PWMIN_TIMER_CLOCK must be greater than 1MHz +#endif + +/* + * Timer register accessors + */ +#define REG(_reg) (*(volatile uint32_t *)(PWMIN_TIMER_BASE + _reg)) + +#define rCR1 REG(STM32_GTIM_CR1_OFFSET) +#define rCR2 REG(STM32_GTIM_CR2_OFFSET) +#define rSMCR REG(STM32_GTIM_SMCR_OFFSET) +#define rDIER REG(STM32_GTIM_DIER_OFFSET) +#define rSR REG(STM32_GTIM_SR_OFFSET) +#define rEGR REG(STM32_GTIM_EGR_OFFSET) +#define rCCMR1 REG(STM32_GTIM_CCMR1_OFFSET) +#define rCCMR2 REG(STM32_GTIM_CCMR2_OFFSET) +#define rCCER REG(STM32_GTIM_CCER_OFFSET) +#define rCNT REG(STM32_GTIM_CNT_OFFSET) +#define rPSC REG(STM32_GTIM_PSC_OFFSET) +#define rARR REG(STM32_GTIM_ARR_OFFSET) +#define rCCR1 REG(STM32_GTIM_CCR1_OFFSET) +#define rCCR2 REG(STM32_GTIM_CCR2_OFFSET) +#define rCCR3 REG(STM32_GTIM_CCR3_OFFSET) +#define rCCR4 REG(STM32_GTIM_CCR4_OFFSET) +#define rDCR REG(STM32_GTIM_DCR_OFFSET) +#define rDMAR REG(STM32_GTIM_DMAR_OFFSET) + +/* + * Specific registers and bits used by HRT sub-functions + */ +#if PWMIN_TIMER_CHANNEL == 1 + #define rCCR_PWMIN_A rCCR1 /* compare register for PWMIN */ + #define DIER_PWMIN_A (GTIM_DIER_CC1IE) /* interrupt enable for PWMIN */ + #define SR_INT_PWMIN_A GTIM_SR_CC1IF /* interrupt status for PWMIN */ + #define rCCR_PWMIN_B rCCR2 /* compare register for PWMIN */ + #define SR_INT_PWMIN_B GTIM_SR_CC2IF /* interrupt status for PWMIN */ + #define CCMR1_PWMIN ((0x02 << GTIM_CCMR1_CC2S_SHIFT) | (0x01 << GTIM_CCMR1_CC1S_SHIFT)) + #define CCMR2_PWMIN 0 + #define CCER_PWMIN (GTIM_CCER_CC2P | GTIM_CCER_CC1E | GTIM_CCER_CC2E) + #define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF) + #define SMCR_PWMIN_1 (0x05 << GTIM_SMCR_TS_SHIFT) + #define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1) +#elif PWMIN_TIMER_CHANNEL == 2 + #define rCCR_PWMIN_A rCCR2 /* compare register for PWMIN */ + #define DIER_PWMIN_A (GTIM_DIER_CC2IE) /* interrupt enable for PWMIN */ + #define SR_INT_PWMIN_A GTIM_SR_CC2IF /* interrupt status for PWMIN */ + #define rCCR_PWMIN_B rCCR1 /* compare register for PWMIN */ + #define DIER_PWMIN_B GTIM_DIER_CC1IE /* interrupt enable for PWMIN */ + #define SR_INT_PWMIN_B GTIM_SR_CC1IF /* interrupt status for PWMIN */ + #define CCMR1_PWMIN ((0x01 << GTIM_CCMR1_CC2S_SHIFT) | (0x02 << GTIM_CCMR1_CC1S_SHIFT)) + #define CCMR2_PWMIN 0 + #define CCER_PWMIN (GTIM_CCER_CC1P | GTIM_CCER_CC1E | GTIM_CCER_CC2E) + #define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF) + #define SMCR_PWMIN_1 (0x06 << GTIM_SMCR_TS_SHIFT) + #define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1) +#else + #error PWMIN_TIMER_CHANNEL must be either 1 and 2. +#endif + + +class PWMIN : device::CDev +{ +public: + PWMIN(); + virtual ~PWMIN(); + + virtual int init(); + virtual int open(struct file *filp); + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + void _publish(uint16_t status, uint32_t period, uint32_t pulse_width); + void _print_info(void); + +private: + uint32_t error_count; + uint32_t pulses_captured; + uint32_t last_period; + uint32_t last_width; + RingBuffer *reports; + bool timer_started; + + void timer_init(void); +}; + +static int pwmin_tim_isr(int irq, void *context); +static void pwmin_start(void); +static void pwmin_info(void); +static void pwmin_test(void); +static void pwmin_reset(void); + +static PWMIN *g_dev; + +PWMIN::PWMIN() : + CDev("pwmin", PWMIN0_DEVICE_PATH), + error_count(0), + pulses_captured(0), + last_period(0), + last_width(0), + reports(nullptr), + timer_started(false) +{ +} + +PWMIN::~PWMIN() +{ + if (reports != nullptr) + delete reports; +} + +/* + initialise the driver. This doesn't actually start the timer (that + is done on open). We don't start the timer to allow for this driver + to be started in init scripts when the user may be using the input + pin as PWM output + */ +int +PWMIN::init() +{ + // we just register the device in /dev, and only actually + // activate the timer when requested to when the device is opened + CDev::init(); + + reports = new RingBuffer(2, sizeof(struct pwm_input_s)); + if (reports == nullptr) { + return -ENOMEM; + } + + return OK; +} + +/* + * Initialise the timer we are going to use. + */ +void PWMIN::timer_init(void) +{ + // run with interrupts disabled in case the timer is already + // setup. We don't want it firing while we are doing the setup + irqstate_t flags = irqsave(); + stm32_configgpio(GPIO_PWM_IN); + + /* claim our interrupt vector */ + irq_attach(PWMIN_TIMER_VECTOR, pwmin_tim_isr); + + /* Clear no bits, set timer enable bit.*/ + modifyreg32(PWMIN_TIMER_POWER_REG, 0, PWMIN_TIMER_POWER_BIT); + + /* disable and configure the timer */ + rCR1 = 0; + rCR2 = 0; + rSMCR = 0; + rDIER = DIER_PWMIN_A; + rCCER = 0; /* unlock CCMR* registers */ + rCCMR1 = CCMR1_PWMIN; + rCCMR2 = CCMR2_PWMIN; + rSMCR = SMCR_PWMIN_1; /* Set up mode */ + rSMCR = SMCR_PWMIN_2; /* Enable slave mode controller */ + rCCER = CCER_PWMIN; + rDCR = 0; + + // for simplicity scale by the clock in MHz. This gives us + // readings in microseconds which is typically what is needed + // for a PWM input driver + uint32_t prescaler = PWMIN_TIMER_CLOCK/1000000UL; + + /* + * define the clock speed. We want the highest possible clock + * speed that avoids overflows. + */ + rPSC = prescaler - 1; + + /* run the full span of the counter. All timers can handle + * uint16 */ + rARR = UINT16_MAX; + + /* generate an update event; reloads the counter, all registers */ + rEGR = GTIM_EGR_UG; + + /* enable the timer */ + rCR1 = GTIM_CR1_CEN; + + /* enable interrupts */ + up_enable_irq(PWMIN_TIMER_VECTOR); + + irqrestore(flags); + + timer_started = true; +} + +/* + hook for open of the driver. We start the timer at this point, then + leave it running + */ +int +PWMIN::open(struct file *filp) +{ + if (g_dev == nullptr) { + return -EIO; + } + int ret = CDev::open(filp); + if (ret == OK && !timer_started) { + g_dev->timer_init(); + } + return ret; +} + + +/* + handle ioctl requests + */ +int +PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 500)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return reports->size(); + + case SENSORIOCRESET: + /* user has asked for the timer to be reset. This may + be needed if the pin was used for a different + purpose (such as PWM output) + */ + timer_init(); + return OK; + + default: + /* give it to the superclass */ + return CDev::ioctl(filp, cmd, arg); + } +} + + +/* + read some samples from the device + */ +ssize_t +PWMIN::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct pwm_input_s); + struct pwm_input_s *buf = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + while (count--) { + if (reports->get(buf)) { + ret += sizeof(struct pwm_input_s); + buf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; +} + +/* + publish some data from the ISR in the ring buffer + */ +void PWMIN::_publish(uint16_t status, uint32_t period, uint32_t pulse_width) +{ + /* if we missed an edge, we have to give up */ + if (status & SR_OVF_PWMIN) { + error_count++; + return; + } + + struct pwm_input_s pwmin_report; + pwmin_report.timestamp = hrt_absolute_time(); + pwmin_report.error_count = error_count; + pwmin_report.period = period; + pwmin_report.pulse_width = pulse_width; + + reports->force(&pwmin_report); + + last_period = period; + last_width = pulse_width; + pulses_captured++; +} + +/* + print information on the last captured + */ +void PWMIN::_print_info(void) +{ + if (!timer_started) { + printf("timer not started - try the 'test' command\n"); + } else { + printf("count=%u period=%u width=%u\n", + (unsigned)pulses_captured, + (unsigned)last_period, + (unsigned)last_width); + } +} + + +/* + Handle the interupt, gathering pulse data + */ +static int pwmin_tim_isr(int irq, void *context) +{ + uint16_t status = rSR; + uint32_t period = rCCR_PWMIN_A; + uint32_t pulse_width = rCCR_PWMIN_B; + + /* ack the interrupts we just read */ + rSR = 0; + + if (g_dev != nullptr) { + g_dev->_publish(status, period, pulse_width); + } + + return OK; +} + +/* + start the driver + */ +static void pwmin_start(void) +{ + if (g_dev != nullptr) { + printf("driver already started\n"); + exit(1); + } + g_dev = new PWMIN(); + if (g_dev == nullptr) { + errx(1, "driver allocation failed"); + } + if (g_dev->init() != OK) { + errx(1, "driver init failed"); + } + exit(0); +} + +/* + test the driver + */ +static void pwmin_test(void) +{ + int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); + if (fd == -1) { + errx(1, "Failed to open device"); + } + uint64_t start_time = hrt_absolute_time(); + + printf("Showing samples for 5 seconds\n"); + + while (hrt_absolute_time() < start_time+5U*1000UL*1000UL) { + struct pwm_input_s buf; + if (::read(fd, &buf, sizeof(buf)) == sizeof(buf)) { + printf("period=%u width=%u error_count=%u\n", + (unsigned)buf.period, + (unsigned)buf.pulse_width, + (unsigned)buf.error_count); + } + } + close(fd); + exit(0); +} + +/* + reset the timer + */ +static void pwmin_reset(void) +{ + int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); + if (fd == -1) { + errx(1, "Failed to open device"); + } + if (ioctl(fd, SENSORIOCRESET, 0) != OK) { + errx(1, "reset failed"); + } + close(fd); + exit(0); +} + +/* + show some information on the driver + */ +static void pwmin_info(void) +{ + if (g_dev == nullptr) { + printf("driver not started\n"); + exit(1); + } + g_dev->_print_info(); + exit(0); +} + + +/* + driver entry point + */ +int pwm_input_main(int argc, char * argv[]) +{ + const char *verb = argv[1]; + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) { + pwmin_start(); + } + + /* + * Print driver information. + */ + if (!strcmp(verb, "info")) { + pwmin_info(); + } + + /* + * print test results + */ + if (!strcmp(verb, "test")) { + pwmin_test(); + } + + /* + * reset the timer + */ + if (!strcmp(verb, "reset")) { + pwmin_reset(); + } + + errx(1, "unrecognized command, try 'start', 'info', 'reset' or 'test'"); + return 0; +}