Land detector: Run in work queue

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<LandDetector *>(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)

src/modules/land_detector/LandDetector.h

@@ -41,6 +41,7 @@
 #ifndef __LAND_DETECTOR_H__
 #define __LAND_DETECTOR_H__
 
+#include <px4_workqueue.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_land_detected.h>
 
@@ -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__

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;
 	}