diff --git a/src/drivers/pps_capture/PPSCapture.cpp b/src/drivers/pps_capture/PPSCapture.cpp index a221aaf345..3efea51f86 100644 --- a/src/drivers/pps_capture/PPSCapture.cpp +++ b/src/drivers/pps_capture/PPSCapture.cpp @@ -39,8 +39,15 @@ */ #include "PPSCapture.hpp" -#include "board_config.h" #include +#include +#include + +#ifdef BOARD_WITH_IO +# define PARAM_PREFIX "PWM_AUX" +#else +# define PARAM_PREFIX "PWM_MAIN" +#endif PPSCapture::PPSCapture() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) @@ -49,36 +56,66 @@ PPSCapture::PPSCapture() : PPSCapture::~PPSCapture() { -#if defined(PPS_CAPTURE_CHANNEL) - io_timer_unallocate_channel(PPS_CAPTURE_CHANNEL); - px4_arch_gpiosetevent(_pps_capture_gpio, false, false, false, nullptr, nullptr); -#endif + if (_channel >= 0) { + io_timer_unallocate_channel(_channel); + px4_arch_gpiosetevent(_pps_capture_gpio, false, false, false, nullptr, nullptr); + } } bool PPSCapture::init() { bool success = false; + param_t p_ctrl_alloc = param_find("SYS_CTRL_ALLOC"); + int32_t ctrl_alloc = 0; + + if (p_ctrl_alloc != PARAM_INVALID) { + param_get(p_ctrl_alloc, &ctrl_alloc); + } + + if (ctrl_alloc == 1) { + + for (unsigned i = 0; i < 16; ++i) { + char param_name[17]; + snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1); + param_t function_handle = param_find(param_name); + int32_t function; + + if (function_handle != PARAM_INVALID && param_get(function_handle, &function) == 0) { + if (function == 2064) { // PPS_Input + _channel = i; + } + } + } + } + #if defined(PPS_CAPTURE_CHANNEL) - int ret = io_timer_allocate_channel(PPS_CAPTURE_CHANNEL, IOTimerChanMode_PPS); + if (_channel == -1) { + _channel = PPS_CAPTURE_CHANNEL; + } - if (ret != PX4_OK) { - PX4_ERR("gpio alloc failed (%i) for PPS at channel (%d)", ret, PPS_CAPTURE_CHANNEL); +#endif + + if (_channel == -1) { + PX4_WARN("No pps channel configured"); return false; } - _pps_capture_gpio = PX4_MAKE_GPIO_EXTI(io_timer_channel_get_as_pwm_input(PPS_CAPTURE_CHANNEL)); + int ret = io_timer_allocate_channel(_channel, IOTimerChanMode_PPS); + + if (ret != PX4_OK) { + PX4_ERR("gpio alloc failed (%i) for PPS at channel (%d)", ret, _channel); + return false; + } + + _pps_capture_gpio = PX4_MAKE_GPIO_EXTI(io_timer_channel_get_as_pwm_input(_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 d6545d9516..cb29c842da 100644 --- a/src/drivers/pps_capture/PPSCapture.hpp +++ b/src/drivers/pps_capture/PPSCapture.hpp @@ -69,6 +69,7 @@ public: private: void Run() override; + int _channel{-1}; uint32_t _pps_capture_gpio{0}; uORB::Publication _pps_capture_pub{ORB_ID(pps_capture)}; uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps)}; diff --git a/src/lib/mixer_module/output_functions.yaml b/src/lib/mixer_module/output_functions.yaml index 2022eecee3..32a4bd684e 100644 --- a/src/lib/mixer_module/output_functions.yaml +++ b/src/lib/mixer_module/output_functions.yaml @@ -50,3 +50,9 @@ functions: condition: "CAM_CAP_FBACK==0" text: "Camera feedback needs to be enabled and configured via CAM_CAP_* parameters." exclude_from_actuator_testing: true + PPS_Input: + start: 2064 + note: + condition: "PPS_CAP_ENABLE==0" + text: "PPS needs to be enabled via PPS_CAP_ENABLE parameter." + exclude_from_actuator_testing: true