forked from Archive/PX4-Autopilot
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:
parent
5d110101b5
commit
d08ec05bab
|
@ -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
|
||||
|
|
|
@ -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.
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
|
@ -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
|
||||
|
|
|
@ -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");
|
||||
|
|
|
@ -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
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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)};
|
||||
|
||||
};
|
|
@ -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;
|
||||
}
|
|
@ -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();
|
||||
};
|
||||
|
|
|
@ -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);
|
||||
|
|
Loading…
Reference in New Issue