diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index e34c4a8d2d..599628c72c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -58,6 +58,12 @@ then fi fi +# External automatic trigger system +if param compare FD_EXT_ATS_EN 1 +then + pwm_input start +fi + # Lidar-Lite on I2C if param compare -s SENS_EN_LL40LS 2 then diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6a84c735f5..6b33ec4897 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -360,6 +360,15 @@ else set AUX_MODE pwm4 fi + + # Check if ATS is enabled + if param compare FD_EXT_ATS_EN 1 + then + # Clear pins 5 and 6. + set FMU_MODE pwm4 + set AUX_MODE pwm4 + fi + if param greater TRIG_MODE 0 then # We ONLY support trigger on pins 5 and 6 when simultanously using AUX for actuator output. diff --git a/msg/pwm_input.msg b/msg/pwm_input.msg index 40b5429b79..fcc7dbe4ac 100644 --- a/msg/pwm_input.msg +++ b/msg/pwm_input.msg @@ -1,4 +1,4 @@ -uint64 timestamp # time since system start (microseconds) -uint64 error_count +uint64 timestamp # Time since system start (microseconds) +uint64 error_count # Timer overcapture error flag (AUX5 or MAIN5) uint32 pulse_width # Pulse width, timer counts uint32 period # Period, timer counts diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index e73c0dcb92..9c60d76ac3 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -15,6 +15,7 @@ uint8 FAILURE_NONE = 0 uint8 FAILURE_ROLL = 1 # (1 << 0) uint8 FAILURE_PITCH = 2 # (1 << 1) uint8 FAILURE_ALT = 4 # (1 << 2) +uint8 FAILURE_EXT = 8 # (1 << 3) # HIL uint8 HIL_STATE_OFF = 0 diff --git a/src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp b/src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp index 6184a02f21..5d5e022286 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp @@ -44,10 +44,13 @@ #include "LidarLitePWM.h" +#ifdef LIDAR_LITE_PWM_SUPPORTED + LidarLitePWM::LidarLitePWM(const uint8_t rotation) : LidarLite(rotation), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { + px4_arch_configgpio(GPIO_VDD_RANGEFINDER_EN); } LidarLitePWM::~LidarLitePWM() @@ -113,17 +116,16 @@ LidarLitePWM::measure() int LidarLitePWM::collect() { - int fd = ::open(PWMIN0_DEVICE_PATH, O_RDONLY); + pwm_input_s pwm_input; - if (fd == -1) { - return PX4_ERROR; - } + if (_sub_pwm_input.update(&pwm_input)) { + + _pwm = pwm_input; - if (::read(fd, &_pwm, sizeof(_pwm)) == sizeof(_pwm)) { - ::close(fd); return PX4_OK; } - ::close(fd); return EAGAIN; } + +#endif \ No newline at end of file diff --git a/src/drivers/distance_sensor/ll40ls/LidarLitePWM.h b/src/drivers/distance_sensor/ll40ls/LidarLitePWM.h index 32734fbdfd..511ea3c4a1 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLitePWM.h +++ b/src/drivers/distance_sensor/ll40ls/LidarLitePWM.h @@ -45,14 +45,15 @@ #include "LidarLite.h" -#include -#include - -#include -#include -#include #include + #include +#include + +#ifdef GPIO_GPIO5_OUTPUT + +#define LIDAR_LITE_PWM_SUPPORTED +#define GPIO_VDD_RANGEFINDER_EN GPIO_GPIO5_OUTPUT class LidarLitePWM : public LidarLite, public px4::ScheduledWorkItem { @@ -73,7 +74,9 @@ protected: private: - int _pwmSub{-1}; + uORB::Subscription _sub_pwm_input{ORB_ID(pwm_input)}; pwm_input_s _pwm{}; }; + +#endif diff --git a/src/drivers/distance_sensor/ll40ls/ll40ls.cpp b/src/drivers/distance_sensor/ll40ls/ll40ls.cpp index c1ab519f8c..6ecabe591a 100644 --- a/src/drivers/distance_sensor/ll40ls/ll40ls.cpp +++ b/src/drivers/distance_sensor/ll40ls/ll40ls.cpp @@ -164,7 +164,12 @@ start_pwm(const uint8_t rotation) return PX4_OK; } +#ifdef LIDAR_LITE_PWM_SUPPORTED instance = new LidarLitePWM(rotation); +#else + instance = nullptr; + PX4_ERR("PWM input not supported."); +#endif if (instance == nullptr) { PX4_ERR("Failed to instantiate the driver"); diff --git a/src/drivers/drv_pwm_input.h b/src/drivers/drv_pwm_input.h deleted file mode 100644 index 47b84e6e1e..0000000000 --- a/src/drivers/drv_pwm_input.h +++ /dev/null @@ -1,51 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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. - * - ****************************************************************************/ - -#pragma once - -#include -#include - -#include -#include - -#define PWMIN0_DEVICE_PATH "/dev/pwmin0" - -__BEGIN_DECLS - -/* - * Initialise the timer - */ -__EXPORT extern int pwm_input_main(int argc, char *argv[]); - -__END_DECLS diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp index a1ee7012f9..bdb798c291 100644 --- a/src/drivers/pwm_input/pwm_input.cpp +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Andrew Tridgell. All rights reserved. + * Copyright (c) 2019 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 @@ -31,237 +31,39 @@ * ****************************************************************************/ -/** - * @file pwm_input.cpp - * - * PWM input driver based on earlier driver from Evan Slatyer, - * which in turn was based on drv_hrt.c - * - * @author: Andrew Tridgell - * @author: Ban Siesta - */ +#include "pwm_input.h" -#include -#include -#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_gpio.h" -#include "stm32_tim.h" -#include - -#include -#include - -#include -#include -#include - -#include -#include -#include - -/* Reset pin define */ -#define GPIO_VDD_RANGEFINDER_EN GPIO_GPIO5_OUTPUT - -#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 - -/* Get the timer defines */ -#define INPUT_TIMER PWMIN_TIMER -#include "timer_registers.h" -#define PWMIN_TIMER_BASE TIMER_BASE -#define PWMIN_TIMER_CLOCK TIMER_CLOCK -#define PWMIN_TIMER_POWER_REG TIMER_CLOCK_POWER_REG -#define PWMIN_TIMER_POWER_BIT TIMER_CLOCK_POWER_BIT -#define PWMIN_TIMER_VECTOR TIMER_IRQ_REG - -/* - * 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 - -// XXX refactor this out of this driver -#define TIMEOUT_POLL 300000 /* reset after no response over this time in microseconds [0.3s] */ -#define TIMEOUT_READ 200000 /* don't reset if the last read is back more than this time in microseconds [0.2s] */ - -class PWMIN : cdev::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); - void hard_reset(); - -private: - uint32_t _error_count; - uint32_t _pulses_captured; - uint32_t _last_period; - uint32_t _last_width; - hrt_abstime _last_poll_time; - hrt_abstime _last_read_time; - ringbuffer::RingBuffer *_reports; - bool _timer_started; - - hrt_call _hard_reset_call; /* HRT callout for note completion */ - hrt_call _freeze_test_call; /* HRT callout for note completion */ - - void _timer_init(void); - - void _turn_on(); - void _turn_off(); - void _freeze_test(); - -}; - -static int pwmin_tim_isr(int irq, void *context, void *arg); -static void pwmin_start(); -static void pwmin_info(void); -static void pwmin_test(void); -static void pwmin_reset(void); -static void pwmin_usage(void); - -static PWMIN *g_dev; - -PWMIN::PWMIN() : - CDev(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() +PWMIN::task_spawn(int argc, char *argv[]) { - /* we just register the device in /dev, and only actually - * activate the timer when requested to when the device is opened */ - CDev::init(); + auto *pwmin = new PWMIN(); - _reports = new ringbuffer::RingBuffer(2, sizeof(struct pwm_input_s)); - - if (_reports == nullptr) { - return -ENOMEM; + if (!pwmin) { + PX4_ERR("driver allocation failed"); + return PX4_ERROR; } - /* Schedule freeze check to invoke periodically */ - hrt_call_every(&_freeze_test_call, 0, TIMEOUT_POLL, reinterpret_cast(&PWMIN::_freeze_test), this); + _object.store(pwmin); + _task_id = task_id_is_work_queue; - return OK; + pwmin->start(); + + return PX4_OK; } -/* - * Initialise the timer we are going to use. - */ -void PWMIN::_timer_init(void) +void +PWMIN::start() +{ + // NOTE: must first publish here, first publication cannot be in interrupt context + _pwm_input_pub.update(); + + // Initialize the timer isr for measuring pulse widths. Publishing is done inside the isr. + timer_init(); +} + + +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 */ @@ -270,12 +72,8 @@ void PWMIN::_timer_init(void) /* configure input pin */ px4_arch_configgpio(GPIO_PWM_IN); - // XXX refactor this out of this driver - /* configure reset pin */ - px4_arch_configgpio(GPIO_VDD_RANGEFINDER_EN); - /* claim our interrupt vector */ - irq_attach(PWMIN_TIMER_VECTOR, pwmin_tim_isr, NULL); + irq_attach(PWMIN_TIMER_VECTOR, PWMIN::pwmin_tim_isr, NULL); /* Clear no bits, set timer enable bit.*/ modifyreg32(PWMIN_TIMER_POWER_REG, 0, PWMIN_TIMER_POWER_BIT); @@ -314,161 +112,14 @@ void PWMIN::_timer_init(void) /* enable the timer */ rCR1 = GTIM_CR1_CEN; - /* enable interrupts */ - up_enable_irq(PWMIN_TIMER_VECTOR); - px4_leave_critical_section(flags); - _timer_started = true; + /* enable interrupts */ + up_enable_irq(PWMIN_TIMER_VECTOR); } -// XXX refactor this out of this driver -void -PWMIN::_freeze_test() -{ - /* reset if last poll time was way back and a read was recently requested */ - if (hrt_elapsed_time(&_last_poll_time) > TIMEOUT_POLL && hrt_elapsed_time(&_last_read_time) < TIMEOUT_READ) { - hard_reset(); - } -} - -// XXX refactor this out of this driver -void -PWMIN::_turn_on() -{ - px4_arch_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 1); -} - -// XXX refactor this out of this driver -void -PWMIN::_turn_off() -{ - px4_arch_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 0); -} - -// XXX refactor this out of this driver -void -PWMIN::hard_reset() -{ - _turn_off(); - hrt_call_after(&_hard_reset_call, 9000, reinterpret_cast(&PWMIN::_turn_on), this); -} - -/* - * 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 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(); - /* also reset the sensor */ - hard_reset(); - 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) -{ - _last_read_time = hrt_absolute_time(); - - 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; - } - - _last_poll_time = hrt_absolute_time(); - - struct pwm_input_s pwmin_report; - pwmin_report.timestamp = _last_poll_time; - pwmin_report.error_count = _error_count; - pwmin_report.period = period; - pwmin_report.pulse_width = pulse_width; - - _reports->force(&pwmin_report); -} - -/* - * 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 interrupt, gathering pulse data - */ -static int pwmin_tim_isr(int irq, void *context, void *arg) +PWMIN::pwmin_tim_isr(int irq, void *context, void *arg) { uint16_t status = rSR; uint32_t period = rCCR_PWMIN_A; @@ -477,148 +128,89 @@ static int pwmin_tim_isr(int irq, void *context, void *arg) /* ack the interrupts we just read */ rSR = 0; - if (g_dev != nullptr) { - g_dev->publish(status, period, pulse_width); + auto obj = get_instance(); + + if (obj != nullptr) { + obj->publish(status, period, pulse_width); } - return OK; + return PX4_OK; } -/* - * start the driver - */ -static void pwmin_start() +void +PWMIN::publish(uint16_t status, uint32_t period, uint32_t pulse_width) { - if (g_dev != nullptr) { - errx(1, "driver already started"); + // if we missed an edge, we have to give up + if (status & SR_OVF_PWMIN) { + _error_count++; + return; } - g_dev = new PWMIN(); + _pwm.timestamp = hrt_absolute_time(); + _pwm.error_count = _error_count; + _pwm.period = period; + _pwm.pulse_width = pulse_width; - if (g_dev == nullptr) { - errx(1, "driver allocation failed"); - } + _pwm_input_pub.publish(_pwm); - if (g_dev->init() != OK) { - errx(1, "driver init failed"); - } - - exit(0); + // update statistics + _last_period = period; + _last_width = pulse_width; + _pulses_captured++; } -/* - * test the driver - */ -static void pwmin_test(void) +void +PWMIN::print_info(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); - - } else { - /* no data, retry in 2 ms */ - px4_usleep(2000); - } - } - - close(fd); - exit(0); + PX4_INFO("count=%u period=%u width=%u\n", + static_cast(_pulses_captured), + static_cast(_last_period), + static_cast(_last_width)); } -/* - * reset the timer - */ -static void pwmin_reset(void) +int +PWMIN::print_usage(const char *reason) { - g_dev->hard_reset(); - int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); - - if (fd == -1) { - errx(1, "Failed to open device"); + if (reason) { + printf("%s\n\n", reason); } - if (ioctl(fd, SENSORIOCRESET, 0) != OK) { - errx(1, "reset failed"); - } + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Measures the PWM input on AUX5 (or MAIN5) via a timer capture ISR and publishes via the uORB 'pwm_input` message. - close(fd); - exit(0); +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("pwm_input", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_COMMAND_DESCR("test", "prints PWM capture info."); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return PX4_OK; } -/* - * show some information on the driver - */ -static void pwmin_info(void) +int +PWMIN::custom_command(int argc, char *argv[]) { - if (g_dev == nullptr) { - printf("driver not started\n"); - exit(1); + const char *input = argv[0]; + auto *obj = get_instance(); + + if (!is_running() || !obj) { + PX4_ERR("not running"); + return PX4_ERROR; } - g_dev->print_info(); - exit(0); + if (!strcmp(input, "test")) { + obj->print_info(); + return PX4_OK; + } + + print_usage(); + return PX4_ERROR; } -static void pwmin_usage() +extern "C" __EXPORT int pwm_input_main(int argc, char *argv[]) { - PX4_ERR("unrecognized command, try 'start', 'info', 'reset' or 'test'"); -} - -/* - * driver entry point - */ -int pwm_input_main(int argc, char *argv[]) -{ - if (argc < 2) { - pwmin_usage(); - return -1; - } - - 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(); - } - - pwmin_usage(); - return -1; + return PWMIN::main(argc, argv); } diff --git a/src/drivers/pwm_input/pwm_input.h b/src/drivers/pwm_input/pwm_input.h new file mode 100644 index 0000000000..469b2ea8d5 --- /dev/null +++ b/src/drivers/pwm_input/pwm_input.h @@ -0,0 +1,151 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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. + * + ****************************************************************************/ + +#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 + +/* Get the timer defines */ +#define INPUT_TIMER PWMIN_TIMER +#include "timer_registers.h" +#define PWMIN_TIMER_BASE TIMER_BASE +#define PWMIN_TIMER_CLOCK TIMER_CLOCK +#define PWMIN_TIMER_POWER_REG TIMER_CLOCK_POWER_REG +#define PWMIN_TIMER_POWER_BIT TIMER_CLOCK_POWER_BIT +#define PWMIN_TIMER_VECTOR TIMER_IRQ_REG + +/* + * 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 : public ModuleBase +{ +public: + void start(); + void publish(uint16_t status, uint32_t period, uint32_t pulse_width); + void print_info(void); + + static int pwmin_tim_isr(int irq, void *context, void *arg); + + static int custom_command(int argc, char *argv[]); + static int print_usage(const char *reason = nullptr); + static int task_spawn(int argc, char *argv[]); + + +private: + void timer_init(void); + + uint32_t _error_count {}; + uint32_t _pulses_captured {}; + uint32_t _last_period {}; + uint32_t _last_width {}; + + bool _timer_started {}; + + pwm_input_s _pwm {}; + + uORB::PublicationData _pwm_input_pub{ORB_ID(pwm_input)}; + +}; diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index c096b475a3..16aae181c8 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -40,6 +40,8 @@ #include "FailureDetector.hpp" +using namespace time_literals; + FailureDetector::FailureDetector(ModuleParams *parent) : ModuleParams(parent) { @@ -61,6 +63,10 @@ FailureDetector::update(const vehicle_status_s &vehicle_status) if (isAttitudeStabilized(vehicle_status)) { updated = updateAttitudeStatus(); + if (_param_fd_ext_ats_en.get()) { + updated |= updateExternalAtsStatus(); + } + } else { updated = resetStatus(); } @@ -111,8 +117,8 @@ FailureDetector::updateAttitudeStatus() hrt_abstime time_now = hrt_absolute_time(); // Update hysteresis - _roll_failure_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _param_fd_fail_r_ttri.get())); - _pitch_failure_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _param_fd_fail_p_ttri.get())); + _roll_failure_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1_s * _param_fd_fail_r_ttri.get())); + _pitch_failure_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1_s * _param_fd_fail_p_ttri.get())); _roll_failure_hysteresis.set_state_and_update(roll_status, time_now); _pitch_failure_hysteresis.set_state_and_update(pitch_status, time_now); @@ -132,3 +138,30 @@ FailureDetector::updateAttitudeStatus() return updated; } + +bool +FailureDetector::updateExternalAtsStatus() +{ + pwm_input_s pwm_input; + bool updated = _sub_pwm_input.update(&pwm_input); + + if (updated) { + + uint32_t pulse_width = pwm_input.pulse_width; + bool ats_trigger_status = (pulse_width >= (uint32_t)_param_fd_ext_ats_trig.get()) && (pulse_width < 3_ms); + + hrt_abstime time_now = hrt_absolute_time(); + + // Update hysteresis + _ext_ats_failure_hysteresis.set_hysteresis_time_from(false, 100_ms); // 5 consecutive pulses at 50hz + _ext_ats_failure_hysteresis.set_state_and_update(ats_trigger_status, time_now); + + _status &= ~FAILURE_EXT; + + if (_ext_ats_failure_hysteresis.get_state()) { + _status |= FAILURE_EXT; + } + } + + return updated; +} \ No newline at end of file diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp index 57737f7b71..5463b5632a 100644 --- a/src/modules/commander/failure_detector/FailureDetector.hpp +++ b/src/modules/commander/failure_detector/FailureDetector.hpp @@ -48,17 +48,20 @@ #include #include + // subscriptions #include #include #include #include +#include typedef enum { FAILURE_NONE = vehicle_status_s::FAILURE_NONE, FAILURE_ROLL = vehicle_status_s::FAILURE_ROLL, FAILURE_PITCH = vehicle_status_s::FAILURE_PITCH, FAILURE_ALT = vehicle_status_s::FAILURE_ALT, + FAILURE_EXT = vehicle_status_s::FAILURE_EXT, } failure_detector_bitmak; using uORB::SubscriptionData; @@ -79,18 +82,24 @@ private: (ParamInt) _param_fd_fail_p, (ParamInt) _param_fd_fail_r, (ParamFloat) _param_fd_fail_r_ttri, - (ParamFloat) _param_fd_fail_p_ttri + (ParamFloat) _param_fd_fail_p_ttri, + (ParamBool) _param_fd_ext_ats_en, + (ParamInt) _param_fd_ext_ats_trig ) // Subscriptions uORB::Subscription _sub_vehicule_attitude{ORB_ID(vehicle_attitude)}; + uORB::Subscription _sub_pwm_input{ORB_ID(pwm_input)}; + uint8_t _status{FAILURE_NONE}; systemlib::Hysteresis _roll_failure_hysteresis{false}; systemlib::Hysteresis _pitch_failure_hysteresis{false}; + systemlib::Hysteresis _ext_ats_failure_hysteresis{false}; bool resetStatus(); bool isAttitudeStabilized(const vehicle_status_s &vehicle_status); bool updateAttitudeStatus(); + bool updateExternalAtsStatus(); }; diff --git a/src/modules/commander/failure_detector/failure_detector_params.c b/src/modules/commander/failure_detector/failure_detector_params.c index c33afcdcf5..dbc024ae72 100644 --- a/src/modules/commander/failure_detector/failure_detector_params.c +++ b/src/modules/commander/failure_detector/failure_detector_params.c @@ -103,3 +103,27 @@ PARAM_DEFINE_FLOAT(FD_FAIL_R_TTRI, 0.3); * @group Failure Detector */ PARAM_DEFINE_FLOAT(FD_FAIL_P_TTRI, 0.3); + +/** + * Enable PWM input on AUX5 or MAIN5 (depending on board) for engaging failsafe from an external + * automatic trigger system (ATS). + * + * External ATS is required by ASTM F3322-18. + * + * @boolean + * @reboot_required true + * @group Failure Detector + */ +PARAM_DEFINE_INT32(FD_EXT_ATS_EN, 0); + +/** + * The PWM threshold from external automatic trigger system for engaging failsafe. + * + * External ATS is required by ASTM F3322-18. + * + * @unit microseconds + * @decimal 2 + * + * @group Failure Detector + */ +PARAM_DEFINE_INT32(FD_EXT_ATS_TRIG, 1900);