diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index b0a9c11f5b..274e5f90c0 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -54,6 +54,7 @@ namespace land_detector { LandDetector::LandDetector() : + ScheduledWorkItem(px4::wq_configurations::hp_default), _cycle_perf(perf_alloc(PC_ELAPSED, "land_detector_cycle")) { } @@ -65,19 +66,12 @@ LandDetector::~LandDetector() int LandDetector::start() { - /* schedule a cycle to start things */ - return work_queue(HPWORK, &_work, (worker_t)&LandDetector::_cycle_trampoline, this, 0); + ScheduleOnInterval(LAND_DETECTOR_UPDATE_INTERVAL, 10000); + + return PX4_OK; } -void -LandDetector::_cycle_trampoline(void *arg) -{ - LandDetector *dev = reinterpret_cast(arg); - - dev->_cycle(); -} - -void LandDetector::_cycle() +void LandDetector::Run() { perf_begin(_cycle_perf); @@ -155,13 +149,8 @@ void LandDetector::_cycle() perf_end(_cycle_perf); - if (!should_exit()) { - - // Schedule next cycle. - work_queue(HPWORK, &_work, (worker_t)&LandDetector::_cycle_trampoline, this, - USEC2TICK(1_s / LAND_DETECTOR_UPDATE_RATE_HZ)); - - } else { + if (should_exit()) { + ScheduleClear(); exit_and_cleanup(); } } diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 205ae7a0dd..0db422fbb2 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -42,7 +42,6 @@ #pragma once -#include #include #include #include @@ -51,12 +50,14 @@ #include #include #include +#include + +using namespace time_literals; namespace land_detector { - -class LandDetector : public ModuleBase +class LandDetector : public ModuleBase, px4::ScheduledWorkItem { public: enum class LandDetectionState { @@ -139,8 +140,8 @@ protected: */ virtual bool _get_ground_effect_state() { return false; } - /** Run main land detector loop at this rate in Hz. */ - static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE_HZ = 50; + /** Run main land detector loop at this interval. */ + static constexpr uint32_t LAND_DETECTOR_UPDATE_INTERVAL = 20_ms; orb_advert_t _landDetectedPub{nullptr}; vehicle_land_detected_s _landDetected{}; @@ -156,9 +157,7 @@ protected: struct actuator_armed_s _arming {}; private: - static void _cycle_trampoline(void *arg); - - void _cycle(); + void Run() override; void _check_params(bool force = false); @@ -169,8 +168,6 @@ private: uint64_t _total_flight_time{0}; ///< in microseconds hrt_abstime _takeoff_time{0}; - struct work_s _work {}; - perf_counter_t _cycle_perf; bool _previous_arming_state{false}; ///< stores the previous _arming.armed state