dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ROMFS/px4fmu_common/init.d/rc.sensors
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

9
ROMFS/px4fmu_common/init.d/rcS
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.

4
msg/pwm_input.msg
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

1
msg/vehicle_status.msg
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

16
src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp
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

17
src/drivers/distance_sensor/ll40ls/LidarLitePWM.h
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

5
src/drivers/distance_sensor/ll40ls/ll40ls.cpp
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");

51
src/drivers/drv_pwm_input.h
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

584
src/drivers/pwm_input/pwm_input.cpp
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);
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<unsigned>(_pulses_captured),
static_cast<unsigned>(_last_period),
static_cast<unsigned>(_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);
}

151
src/drivers/pwm_input/pwm_input.h Normal file
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)};
};

37
src/modules/commander/failure_detector/FailureDetector.cpp
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;
}

11
src/modules/commander/failure_detector/FailureDetector.hpp
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();
};

24
src/modules/commander/failure_detector/failure_detector_params.c
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);