diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 1de3c2f4a0..ae6a1b5037 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -50,59 +50,69 @@ LandDetector::LandDetector() : _landDetected({0, false}), _arming_time(0), _taskShouldExit(false), - _taskIsRunning(false) + _taskIsRunning(false), + _work{} { // ctor } LandDetector::~LandDetector() { + work_cancel(LPWORK, &_work); _taskShouldExit = true; } +int LandDetector::start() +{ + /* schedule a cycle to start things */ + work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0); + + return 0; +} + void LandDetector::shutdown() { _taskShouldExit = true; } -void LandDetector::start() +void +LandDetector::cycle_trampoline(void *arg) { - // make sure this method has not already been called by another thread - if (isRunning()) { - return; + LandDetector *dev = reinterpret_cast(arg); + + dev->cycle(); +} + +void LandDetector::cycle() +{ + if (!_taskIsRunning) { + // advertise the first land detected uORB + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = false; + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); + + // initialize land detection algorithm + initialize(); + + // task is now running, keep doing so until shutdown() has been called + _taskIsRunning = true; + _taskShouldExit = false; } - // advertise the first land detected uORB - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = false; - _landDetectedPub = (uintptr_t)orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); + bool landDetected = update(); - // initialize land detection algorithm - initialize(); + // publish if land detection state has changed + if (_landDetected.landed != landDetected) { + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = landDetected; - // task is now running, keep doing so until shutdown() has been called - _taskIsRunning = true; - _taskShouldExit = false; - - while (isRunning()) { - - bool landDetected = update(); - - // publish if land detection state has changed - if (_landDetected.landed != landDetected) { - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = landDetected; - - // publish the land detected broadcast - orb_publish(ORB_ID(vehicle_land_detected), (orb_advert_t)_landDetectedPub, &_landDetected); - } - - // limit loop rate - usleep(1000000 / LAND_DETECTOR_UPDATE_RATE); + // publish the land detected broadcast + orb_publish(ORB_ID(vehicle_land_detected), (orb_advert_t)_landDetectedPub, &_landDetected); } - _taskIsRunning = false; - _exit(0); + if (!_taskShouldExit) { + work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE)); + } } bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void *buffer) diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 2ade4ac8c0..d28863b499 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -41,6 +41,7 @@ #ifndef __LAND_DETECTOR_H__ #define __LAND_DETECTOR_H__ +#include #include #include @@ -70,7 +71,9 @@ public: * @brief Blocking function that should be called from it's own task thread. This method will * run the underlying algorithm at the desired update rate and publish if the landing state changes. **/ - void start(); + int start(); + + static void cycle_trampoline(void *arg); protected: @@ -99,13 +102,16 @@ protected: static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME = 1000000; /**< time interval in which wider acceptance thresholds are used after arming */ protected: - uintptr_t _landDetectedPub; /**< publisher for position in local frame */ + orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ uint64_t _arming_time; /**< timestamp of arming time */ private: bool _taskShouldExit; /**< true if it is requested that this task should exit */ bool _taskIsRunning; /**< task has reached main loop and is currently running */ + struct work_s _work; + + void cycle(); }; #endif //__LAND_DETECTOR_H__ diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 34355c6b11..c4d0e5194d 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -67,47 +67,32 @@ extern "C" __EXPORT int land_detector_main(int argc, char *argv[]); //Private variables static LandDetector *land_detector_task = nullptr; -static int _landDetectorTaskID = -1; static char _currentMode[12]; -/** -* Deamon thread function -**/ -static void land_detector_deamon_thread(int argc, char *argv[]) -{ - land_detector_task->start(); -} - /** * Stop the task, force killing it if it doesn't stop by itself **/ static void land_detector_stop() { - if (land_detector_task == nullptr || _landDetectorTaskID == -1) { + if (land_detector_task == nullptr) { warnx("not running"); return; } land_detector_task->shutdown(); - //Wait for task to die + // Wait for task to die int i = 0; do { /* wait 20ms */ usleep(20000); - /* if we have given up, kill it */ - if (++i > 50) { - px4_task_delete(_landDetectorTaskID); - break; - } - } while (land_detector_task->isRunning()); + } while (land_detector_task->isRunning() && ++i < 50); delete land_detector_task; land_detector_task = nullptr; - _landDetectorTaskID = -1; warnx("land_detector has been stopped"); } @@ -116,7 +101,7 @@ static void land_detector_stop() **/ static int land_detector_start(const char *mode) { - if (land_detector_task != nullptr || _landDetectorTaskID != -1) { + if (land_detector_task != nullptr) { warnx("already running"); return -1; } @@ -140,14 +125,9 @@ static int land_detector_start(const char *mode) } //Start new thread task - _landDetectorTaskID = px4_task_spawn_cmd("land_detector", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1000, - (px4_main_t)&land_detector_deamon_thread, - nullptr); + int ret = land_detector_task->start(); - if (_landDetectorTaskID < 0) { + if (ret) { warnx("task start failed: %d", -errno); return -1; }