From 5abee359d6f917ade2edf7c18a4b090a75c3f0c8 Mon Sep 17 00:00:00 2001 From: Michael Schaeuble Date: Fri, 22 Apr 2022 11:59:06 +0200 Subject: [PATCH] Remove PPS GPIO interrupt when the rate is higher than 20Hz If the PPS GPIO is exposed to a signal with high frequency changes, a lot of interrupts are scheduled and the handling of these calls can worst-case starve flight critical processes leading to a loss of control. Since PPS is not flight critical, we now give up the PPS functionality and stop the interrupts to prevent the starvation of other processes. --- msg/pps_capture.msg | 3 ++- src/drivers/pps_capture/PPSCapture.cpp | 24 +++++++++++++++++++++++- src/drivers/pps_capture/PPSCapture.hpp | 4 +++- 3 files changed, 28 insertions(+), 3 deletions(-) diff --git a/msg/pps_capture.msg b/msg/pps_capture.msg index b9ec949a9c..c6fa2cb979 100644 --- a/msg/pps_capture.msg +++ b/msg/pps_capture.msg @@ -1,2 +1,3 @@ uint64 timestamp # time since system start (microseconds) at PPS capture event -uint64 rtc_timestamp # Corrected GPS UTC timestamp at PPS capture event \ No newline at end of file +uint64 rtc_timestamp # Corrected GPS UTC timestamp at PPS capture event +uint8 pps_rate_exceeded_counter # Increments when PPS dt < 50ms diff --git a/src/drivers/pps_capture/PPSCapture.cpp b/src/drivers/pps_capture/PPSCapture.cpp index aeb8eb0e8d..9d2a3fa1bd 100644 --- a/src/drivers/pps_capture/PPSCapture.cpp +++ b/src/drivers/pps_capture/PPSCapture.cpp @@ -42,6 +42,8 @@ #include #include #include +#include +#include PPSCapture::PPSCapture() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) @@ -130,6 +132,7 @@ void PPSCapture::Run() pps_capture_s pps_capture; pps_capture.timestamp = _hrt_timestamp; + pps_capture.pps_rate_exceeded_counter = _pps_rate_exceeded_counter; // GPS UTC time when the GPIO interrupt was triggered // Last UTC time received from the GPS + elapsed time to the PPS interrupt uint64_t gps_utc_time = _last_gps_utc_timestamp + (_hrt_timestamp - _last_gps_timestamp); @@ -141,13 +144,32 @@ void PPSCapture::Run() pps_capture.rtc_timestamp = gps_utc_time - (gps_utc_time % USEC_PER_SEC) + USEC_PER_SEC; _pps_capture_pub.publish(pps_capture); + + if (_pps_rate_failure.load()) { + mavlink_log_warning(&_mavlink_log_pub, "Hardware fault: GPS PPS disabled\t"); + events::send(events::ID("pps_capture_pps_rate_exceeded"), + events::Log::Error, "Hardware fault: GPS PPS disabled"); + _pps_rate_failure.store(false); + } } int PPSCapture::gpio_interrupt_callback(int irq, void *context, void *arg) { PPSCapture *instance = static_cast(arg); - instance->_hrt_timestamp = hrt_absolute_time(); + hrt_abstime interrupt_time = hrt_absolute_time(); + + if ((interrupt_time - instance->_hrt_timestamp) < 50_ms) { + ++(instance->_pps_rate_exceeded_counter); + + if (instance->_pps_rate_exceeded_counter >= 10) { + // Trigger rate too high, stop future interrupts + px4_arch_gpiosetevent(instance->_pps_capture_gpio, false, false, false, nullptr, nullptr); + instance->_pps_rate_failure.store(true); + } + } + + instance->_hrt_timestamp = interrupt_time; instance->ScheduleNow(); // schedule work queue to publish PPS captured time return PX4_OK; diff --git a/src/drivers/pps_capture/PPSCapture.hpp b/src/drivers/pps_capture/PPSCapture.hpp index cb29c842da..48a0774495 100644 --- a/src/drivers/pps_capture/PPSCapture.hpp +++ b/src/drivers/pps_capture/PPSCapture.hpp @@ -73,10 +73,12 @@ private: uint32_t _pps_capture_gpio{0}; uORB::Publication _pps_capture_pub{ORB_ID(pps_capture)}; uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps)}; + orb_advert_t _mavlink_log_pub{nullptr}; hrt_abstime _hrt_timestamp{0}; hrt_abstime _last_gps_timestamp{0}; uint64_t _last_gps_utc_timestamp{0}; - + uint8_t _pps_rate_exceeded_counter{0}; + px4::atomic _pps_rate_failure{false}; };