land_detector: use ModuleBase & add module documentation

src/modules/land_detector/FixedwingLandDetector.cpp

@@ -31,7 +31,7 @@
  *
  ****************************************************************************/
 
-/*
+/**
  * @file FixedwingLandDetector.cpp
  *
  * @author Johan Jansen <jnsn.johan@gmail.com>

src/modules/land_detector/LandDetector.cpp

@@ -58,8 +58,6 @@ LandDetector::LandDetector() :
 	_freefall_hysteresis(false),
 	_landed_hysteresis(true),
 	_ground_contact_hysteresis(true),
-	_taskShouldExit(false),
-	_taskIsRunning(false),
 	_total_flight_time{0},
 	_takeoff_time{0},
 	_work{}
@@ -71,23 +69,12 @@ LandDetector::LandDetector() :
 
 LandDetector::~LandDetector()
 {
-	work_cancel(HPWORK, &_work);
-	_taskShouldExit = true;
 }
 
 int LandDetector::start()
 {
-	_taskShouldExit = false;
-
 	/* schedule a cycle to start things */
-	work_queue(HPWORK, &_work, (worker_t)&LandDetector::_cycle_trampoline, this, 0);
-
-	return 0;
-}
-
-void LandDetector::stop()
-{
-	_taskShouldExit = true;
+	return work_queue(HPWORK, &_work, (worker_t)&LandDetector::_cycle_trampoline, this, 0);
 }
 
 void
@@ -100,7 +87,7 @@ LandDetector::_cycle_trampoline(void *arg)
 
 void LandDetector::_cycle()
 {
-	if (!_taskIsRunning) {
+	if (!_object) { // not initialized yet
 		// Advertise the first land detected uORB.
 		_landDetected.timestamp = hrt_absolute_time();
 		_landDetected.freefall = false;
@@ -114,8 +101,7 @@ void LandDetector::_cycle()
 
 		_check_params(true);
 
-		// Task is now running, keep doing so until we need to stop.
-		_taskIsRunning = true;
+		_object = this;
 	}
 
 	_check_params(false);
@@ -165,14 +151,14 @@ void LandDetector::_cycle()
 				 &instance, ORB_PRIO_DEFAULT);
 	}
 
-	if (!_taskShouldExit) {
+	if (!should_exit()) {
 
 		// Schedule next cycle.
 		work_queue(HPWORK, &_work, (worker_t)&LandDetector::_cycle_trampoline, this,
 			   USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE_HZ));
 
 	} else {
-		_taskIsRunning = false;
+		exit_and_cleanup();
 	}
 }
 void LandDetector::_check_params(const bool force)

src/modules/land_detector/LandDetector.h

@@ -33,7 +33,7 @@
 
 /**
  * @file LandDetector.h
-Land detector interface for multicopter, fixedwing and VTOL implementations.
+ * Land detector interface for multicopter, fixedwing and VTOL implementations.
  *
  * @author Johan Jansen <jnsn.johan@gmail.com>
  * @author Julian Oes <julian@oes.ch>
@@ -43,6 +43,7 @@ Land detector interface for multicopter, fixedwing and VTOL implementations.
 #pragma once
 
 #include <px4_workqueue.h>
+#include <px4_module.h>
 #include <systemlib/hysteresis/hysteresis.h>
 #include <systemlib/param/param.h>
 #include <uORB/uORB.h>
@@ -52,7 +53,7 @@ namespace land_detector
 {
 
 
-class LandDetector
+class LandDetector : public ModuleBase<LandDetector>
 {
 public:
 	enum class LandDetectionState {
@@ -65,14 +66,19 @@ public:
 	LandDetector();
 	virtual ~LandDetector();
 
-	/**
-	 * @return true if this task is currently running.
-	 */
-	inline bool is_running() const
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[])
 	{
-		return _taskIsRunning;
+		return print_usage("unknown command");
 	}
 
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
 
 	/**
 	 * @return current state.
@@ -82,11 +88,6 @@ public:
 		return _state;
 	}
 
-	/**
-	 * Tells the task that it should exit.
-	 */
-	void stop();
-
 	/**
 	 * Get the work queue going.
 	 */
@@ -170,15 +171,11 @@ private:
 
 	void _update_state();
 
-	bool _taskShouldExit;
-	bool _taskIsRunning;
-
 	param_t _p_total_flight_time_high;
 	param_t _p_total_flight_time_low;
 	uint64_t _total_flight_time; ///< in microseconds
 	hrt_abstime _takeoff_time;
 
-
 	struct work_s	_work;
 };
 

src/modules/land_detector/land_detector_main.cpp

@@ -50,7 +50,6 @@
 #include <errno.h>
 #include <drivers/drv_hrt.h>
 #include <systemlib/systemlib.h>	//Scheduler
-#include <systemlib/err.h>			//print to console
 
 #include "FixedwingLandDetector.h"
 #include "MulticopterLandDetector.h"
@@ -61,180 +60,115 @@
 namespace land_detector
 {
 
-// Function prototypes
-static int land_detector_start(const char *mode);
-static void land_detector_stop();
-
-/**
- * land detector app start / stop handling function
- * This makes the land detector module accessible from the nuttx shell
- * @ingroup apps
- */
 extern "C" __EXPORT int land_detector_main(int argc, char *argv[]);
 
-// Private variables
-static LandDetector *land_detector_task = nullptr;
 static char _currentMode[12];
 
-/**
- * Stop the task, force killing it if it doesn't stop by itself
- */
-static void land_detector_stop()
+int LandDetector::task_spawn(int argc, char *argv[])
 {
-	if (land_detector_task == nullptr) {
-		PX4_WARN("not running");
-		return;
-	}
-
-	land_detector_task->stop();
-
-	// Wait for task to die
-	int i = 0;
-
-	do {
-		// wait 20ms at a time
-		usleep(20000);
-
-	} while (land_detector_task->is_running() && ++i < 50);
-
-
-	delete land_detector_task;
-	land_detector_task = nullptr;
-	PX4_WARN("land_detector has been stopped");
-}
-
-/**
- * Start new task, fails if it is already running. Returns OK if successful
- */
-static int land_detector_start(const char *mode)
-{
-	if (land_detector_task != nullptr) {
-		PX4_WARN("already running");
+	if (argc < 2) {
+		print_usage();
 		return -1;
 	}
 
-	//Allocate memory
-	if (!strcmp(mode, "fixedwing")) {
-		land_detector_task = new FixedwingLandDetector();
+	LandDetector *obj;
 
-	} else if (!strcmp(mode, "multicopter")) {
-		land_detector_task = new MulticopterLandDetector();
+	if (!strcmp(argv[1], "fixedwing")) {
+		obj = new FixedwingLandDetector();
 
-	} else if (!strcmp(mode, "vtol")) {
-		land_detector_task = new VtolLandDetector();
+	} else if (!strcmp(argv[1], "multicopter")) {
+		obj = new MulticopterLandDetector();
 
-	} else if (!strcmp(mode, "ugv")) {
-		land_detector_task = new RoverLandDetector();
+	} else if (!strcmp(argv[1], "vtol")) {
+		obj = new VtolLandDetector();
+
+	} else if (!strcmp(argv[1], "ugv")) {
+		obj = new RoverLandDetector();
 
 	} else {
-		PX4_WARN("[mode] must be either 'fixedwing', 'multicopter', or 'vtol'");
+		print_usage("unknown mode");
 		return -1;
 	}
 
-	// Check if alloc worked
-	if (land_detector_task == nullptr) {
-		PX4_WARN("alloc failed");
+	if (!obj) {
+		PX4_ERR("alloc failed");
 		return -1;
 	}
 
-	// Start new thread task
-	int ret = land_detector_task->start();
+	int ret = obj->start();
 
-	if (ret) {
-		PX4_WARN("task start failed: %d", -errno);
-		return -1;
-	}
-
-	// Avoid memory fragmentation by not exiting start handler until the task has fully started
-	const uint64_t timeout = hrt_absolute_time() + 5000000; // 5 second timeout
-
-	// Do one sleep before the first check
-	usleep(10000);
-
-	if (!land_detector_task->is_running()) {
-		while (!land_detector_task->is_running()) {
-			usleep(50000);
-
-			if (hrt_absolute_time() > timeout) {
-				PX4_WARN("start failed - timeout");
-				land_detector_stop();
-				return 1;
-			}
-		}
+	if (ret < 0) {
+		delete obj;
+		return ret;
 	}
 
 	// Remember current active mode
-	strncpy(_currentMode, mode, sizeof(_currentMode) - 1);
+	strncpy(_currentMode, argv[1], sizeof(_currentMode) - 1);
 	_currentMode[sizeof(_currentMode) - 1] = '\0';
 
+	wait_until_running(); // this will wait until _object is set from the cycle method
+	_task_id = task_id_is_work_queue;
+	return 0;
+}
+
+int LandDetector::print_status()
+{
+	PX4_INFO("running (%s)", _currentMode);
+	LandDetector::LandDetectionState state = get_state();
+
+	switch (state) {
+	case LandDetector::LandDetectionState::FLYING:
+		PX4_INFO("State: Flying");
+		break;
+
+	case LandDetector::LandDetectionState::LANDED:
+		PX4_INFO("State: Landed");
+		break;
+
+	case LandDetector::LandDetectionState::FREEFALL:
+		PX4_INFO("State: Freefall");
+		break;
+
+	default:
+		PX4_ERR("State: unknown");
+		break;
+	}
+
 	return 0;
 }
 
-/**
- * Main entry point for this module
- */
+int LandDetector::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_ERR("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Module to detect the freefall and landed state of the vehicle, and publishing the `vehicle_land_detected` topic.
+Each vehicle type (multirotor, fixedwing, vtol, ...) provides its own algorithm, taking into account various
+states, such as commanded thrust, arming state and vehicle motion.
+
+### Implementation
+Every type is implemented in its own class with a common base class. The base class maintains a state (landed, ground
+contact, ...). Each possible state is implemented in the derived classes. A hysteresis and a fixed priority of each
+state determines the current state.
+
+The module runs periodically on the HP work queue.
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("land_detector", "system");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task");
+	PRINT_MODULE_USAGE_ARG("fixedwing|multicopter|vtol|rover", "Select vehicle type", false);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+	return 0;
+}
+
+
 int land_detector_main(int argc, char *argv[])
 {
-
-	if (argc < 2) {
-		goto exiterr;
-	}
-
-	if (argc >= 2 && !strcmp(argv[1], "start")) {
-		if (land_detector_start(argv[2]) != 0) {
-			PX4_WARN("land_detector start failed");
-			return 1;
-		}
-
-		return 0;
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		land_detector_stop();
-		return 0;
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (land_detector_task) {
-
-			if (land_detector_task->is_running()) {
-				PX4_INFO("running (%s)", _currentMode);
-				LandDetector::LandDetectionState state = land_detector_task->get_state();
-
-				switch (state) {
-				case LandDetector::LandDetectionState::FLYING:
-					PX4_INFO("State: Flying");
-					break;
-
-				case LandDetector::LandDetectionState::LANDED:
-					PX4_INFO("State: Landed");
-					break;
-
-				case LandDetector::LandDetectionState::FREEFALL:
-					PX4_INFO("State: Freefall");
-					break;
-
-				default:
-					PX4_ERR("State: unknown");
-					break;
-				}
-
-			} else {
-				PX4_WARN("exists, but not running (%s)", _currentMode);
-			}
-
-			return 0;
-
-		} else {
-			PX4_WARN("not running");
-			return 1;
-		}
-	}
-
-exiterr:
-	PX4_WARN("usage: land_detector {start|stop|status} [mode]");
-	PX4_WARN("mode can either be 'fixedwing' or 'multicopter'");
-	return 1;
+	return LandDetector::main(argc, argv);
 }
 
 }