From 298a8c4637dd29b12da1f7a204bf21db1ebcfe44 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Igor=20Mi=C5=A1i=C4=87?= Date: Thu, 18 Nov 2021 12:55:41 +0100 Subject: [PATCH] boards/fmu-v5x: allocating PPS pin - start pps_capture before pwm_out --- ROMFS/px4fmu_common/init.d/rcS | 3 +++ .../px4/nxp/imxrt/include/px4_arch/io_timer.h | 3 ++- .../src/px4/nxp/imxrt/io_pins/io_timer.c | 4 ++-- .../nxp/kinetis/include/px4_arch/io_timer.h | 3 ++- .../src/px4/nxp/kinetis/io_pins/io_timer.c | 4 ++-- .../nxp/s32k1xx/include/px4_arch/io_timer.h | 3 ++- .../src/px4/nxp/s32k1xx/io_pins/io_timer.c | 4 ++-- .../stm32_common/include/px4_arch/io_timer.h | 3 ++- .../px4/stm/stm32_common/io_pins/io_timer.c | 4 ++-- src/drivers/pps_capture/PPSCapture.cpp | 22 +++++++++++++++++-- src/drivers/pps_capture/PPSCapture.hpp | 1 + 11 files changed, 40 insertions(+), 14 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 2b57ac7a6b..c469469236 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -477,6 +477,9 @@ else rc_input start $RC_INPUT_ARGS fi + # PPS capture driver (before pwm_out) + pps_capture start + # Camera capture driver (before pwm_out) if param greater -s CAM_CAP_FBACK 0 then diff --git a/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h index 4e5c552b4e..f54bc9bc0a 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h @@ -62,7 +62,8 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_Trigger = 5, IOTimerChanMode_Dshot = 6, IOTimerChanMode_LED = 7, - IOTimerChanMode_Other = 8, + IOTimerChanMode_PPS = 8, + IOTimerChanMode_Other = 9, IOTimerChanModeSize } io_timer_channel_mode_t; diff --git a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c index 5b0a316a47..b91f011373 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c @@ -149,8 +149,8 @@ static int io_timer_handler7(int irq, void *context, void *arg); #define rFCTRL20(_tim) REG(_tim, 0, IMXRT_FLEXPWM_FCTRL20_OFFSET) /* Fault Control 2 Register */ -// NotUsed PWMOut PWMIn Capture OneShot Trigger Dshot LED Other -io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0, 0, 0, 0 }; +// NotUsed PWMOut PWMIn Capture OneShot Trigger Dshot LED PPS Other +io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */ diff --git a/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h index a6e11b9ad8..8d24a75e3f 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h @@ -66,7 +66,8 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_Trigger = 5, IOTimerChanMode_Dshot = 6, IOTimerChanMode_LED = 7, - IOTimerChanMode_Other = 8, + IOTimerChanMode_PPS = 8, + IOTimerChanMode_Other = 9, IOTimerChanModeSize } io_timer_channel_mode_t; diff --git a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c index 2ab103ee84..dd5968db29 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c @@ -152,8 +152,8 @@ static int io_timer_handler7(int irq, void *context, void *arg); #define CnSC_PWMIN_INIT 0 // TBD -// NotUsed PWMOut PWMIn Capture OneShot Trigger Dshot LED Other -io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0, 0, 0, 0 }; +// NotUsed PWMOut PWMIn Capture OneShot Trigger Dshot LED PPS Other +io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */ diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h index 97d4198738..bc31b0bfd5 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h @@ -62,7 +62,8 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_Trigger = 5, IOTimerChanMode_Dshot = 6, IOTimerChanMode_LED = 7, - IOTimerChanMode_Other = 8, + IOTimerChanMode_PPS = 8, + IOTimerChanMode_Other = 9, IOTimerChanModeSize } io_timer_channel_mode_t; diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/io_timer.c b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/io_timer.c index 492e296115..ec3cdf15cd 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/io_timer.c @@ -150,8 +150,8 @@ static int io_timer_handler7(int irq, void *context, void *arg); #define CnSC_PWMIN_INIT 0 // TBD -// NotUsed PWMOut PWMIn Capture OneShot Trigger Dshot LED Other -io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0, 0, 0, 0 }; +// NotUsed PWMOut PWMIn Capture OneShot Trigger Dshot LED PPS Other +io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */ diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h index 163372c354..cbf69d432a 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h @@ -75,7 +75,8 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_Trigger = 5, IOTimerChanMode_Dshot = 6, IOTimerChanMode_LED = 7, - IOTimerChanMode_Other = 8, + IOTimerChanMode_PPS = 8, + IOTimerChanMode_Other = 9, IOTimerChanModeSize } io_timer_channel_mode_t; diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c index 5b44d70d6b..e27432f205 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c @@ -149,8 +149,8 @@ static int io_timer_handler7(int irq, void *context, void *arg); /* The transfer is done to 4 registers starting from TIMx_CR1 + TIMx_DCR.DBA */ #define TIM_DMABURSTLENGTH_4TRANSFERS 0x00000300U -// NotUsed PWMOut PWMIn Capture OneShot Trigger Dshot LED Other -io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0, 0, 0, 0 }; +// NotUsed PWMOut PWMIn Capture OneShot Trigger Dshot LED PPS Other +io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */ diff --git a/src/drivers/pps_capture/PPSCapture.cpp b/src/drivers/pps_capture/PPSCapture.cpp index cc33353821..db09dd031f 100644 --- a/src/drivers/pps_capture/PPSCapture.cpp +++ b/src/drivers/pps_capture/PPSCapture.cpp @@ -40,6 +40,7 @@ #include "PPSCapture.hpp" #include "board_config.h" +#include PPSCapture::PPSCapture() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) @@ -48,19 +49,36 @@ PPSCapture::PPSCapture() : PPSCapture::~PPSCapture() { - px4_arch_gpiosetevent(BOARD_CAPTURE_GPIO, false, false, false, nullptr, nullptr); +#if defined(PPS_CAPTURE_CHANNEL) + io_timer_unallocate_channel(PPS_CAPTURE_CHANNEL); + px4_arch_gpiosetevent(_pps_capture_gpio, false, false, false, nullptr, nullptr); +#endif } bool PPSCapture::init() { bool success = false; - int ret_val = px4_arch_gpiosetevent(BOARD_CAPTURE_GPIO, true, false, true, &PPSCapture::gpio_interrupt_callback, this); +#if defined(PPS_CAPTURE_CHANNEL) + + int ret = io_timer_allocate_channel(PPS_CAPTURE_CHANNEL, IOTimerChanMode_PPS); + + if (ret != PX4_OK) { + PX4_ERR("gpio alloc failed (%i) for PPS at channel (%d)", ret, PPS_CAPTURE_CHANNEL); + return false; + } + + _pps_capture_gpio = PX4_MAKE_GPIO_EXTI(io_timer_channel_get_as_pwm_input(PPS_CAPTURE_CHANNEL)); + int ret_val = px4_arch_gpiosetevent(_pps_capture_gpio, true, false, true, &PPSCapture::gpio_interrupt_callback, this); if (ret_val == PX4_OK) { success = true; } +#else +#error Driver requires PPS_CAPTURE_CHANNEL to be enabled +#endif + return success; } diff --git a/src/drivers/pps_capture/PPSCapture.hpp b/src/drivers/pps_capture/PPSCapture.hpp index 5f31aed670..3e46614166 100644 --- a/src/drivers/pps_capture/PPSCapture.hpp +++ b/src/drivers/pps_capture/PPSCapture.hpp @@ -67,6 +67,7 @@ public: private: void Run() override; + uint32_t _pps_capture_gpio{0}; uORB::Publication _pps_capture_pub{ORB_ID(pps_capture)}; static constexpr unsigned USEC_IN_1_SEC{1000000}; // microseconds in 1 second