PWM automatic trigger system (ATS) for parachutes (#13726)

* parameter and logic to commander for triggering failsafe from external automatic trigger system.
* logic to startup script for enabling ATS. Added uORB publishing to pwm_input module.
* Refactored out CDev usage from pwm_input and ll40ls. Refactored out ll40ls specifics from pwm_input and cleaned up dead code.
This commit is contained in:
Jacob Dahl 2020-01-06 18:14:06 -07:00 committed by Daniel Agar
parent 5d110101b5
commit d08ec05bab
13 changed files with 350 additions and 566 deletions

View File

@ -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

View File

@ -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.

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -45,14 +45,15 @@
#include "LidarLite.h"
#include <stdio.h>
#include <string.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_input.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/topics/pwm_input.h>
#include <uORB/Subscription.hpp>
#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

View File

@ -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");

View File

@ -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 <sys/types.h>
#include <stdbool.h>
#include <time.h>
#include <queue.h>
#define PWMIN0_DEVICE_PATH "/dev/pwmin0"
__BEGIN_DECLS
/*
* Initialise the timer
*/
__EXPORT extern int pwm_input_main(int argc, char *argv[]);
__END_DECLS

View File

@ -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 <bansiesta@gmail.com>
*/
#include "pwm_input.h"
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/time.h>
#include <nuttx/arch.h>
#include <nuttx/irq.h>
#include <sys/types.h>
#include <stdbool.h>
#include <assert.h>
#include <debug.h>
#include <time.h>
#include <queue.h>
#include <errno.h>
#include <string.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <board_config.h>
#include <drivers/drv_pwm_input.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include "chip.h"
#include "up_internal.h"
#include "up_arch.h"
#include "stm32_gpio.h"
#include "stm32_tim.h"
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/pwm_input.h>
#include <drivers/drv_device.h>
#include <drivers/device/device.h>
#include <drivers/device/ringbuffer.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
/* 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<hrt_callout>(&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<hrt_callout>(&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<struct pwm_input_s *>(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);
// update statistics
_last_period = period;
_last_width = pulse_width;
_pulses_captured++;
}
if (g_dev->init() != OK) {
errx(1, "driver init failed");
}
exit(0);
}
/*
* 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");
PX4_INFO("count=%u period=%u width=%u\n",
static_cast<unsigned>(_pulses_captured),
static_cast<unsigned>(_last_period),
static_cast<unsigned>(_last_width));
}
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);
}
/*
* 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.
)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;
}
close(fd);
exit(0);
}
/*
* 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;
}
static void pwmin_usage()
print_usage();
return PX4_ERROR;
}
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);
}

View File

@ -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 <drivers/drv_hrt.h>
#include <drivers/device/device.h>
#include <px4_platform_common/module.h>
#include <uORB/uORB.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/pwm_input.h>
#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<PWMIN>
{
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_s> _pwm_input_pub{ORB_ID(pwm_input)};
};

View File

@ -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;
}

View File

@ -48,17 +48,20 @@
#include <px4_platform_common/module_params.h>
#include <hysteresis/hysteresis.h>
// subscriptions
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/pwm_input.h>
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<px4::params::FD_FAIL_P>) _param_fd_fail_p,
(ParamInt<px4::params::FD_FAIL_R>) _param_fd_fail_r,
(ParamFloat<px4::params::FD_FAIL_R_TTRI>) _param_fd_fail_r_ttri,
(ParamFloat<px4::params::FD_FAIL_P_TTRI>) _param_fd_fail_p_ttri
(ParamFloat<px4::params::FD_FAIL_P_TTRI>) _param_fd_fail_p_ttri,
(ParamBool<px4::params::FD_EXT_ATS_EN>) _param_fd_ext_ats_en,
(ParamInt<px4::params::FD_EXT_ATS_TRIG>) _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();
};

View File

@ -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);