LPE: refactor to use ModuleBase

src/modules/local_position_estimator/local_position_estimator_main.cpp

@@ -40,130 +40,101 @@
  * Local position estimator
  */
 
-#include <unistd.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <systemlib/param/param.h>
-#include <systemlib/err.h>
-#include <drivers/drv_hrt.h>
-#include <math.h>
-#include <fcntl.h>
-#include <px4_posix.h>
+#include <px4_log.h>
+#include <px4_module.h>
 #include <px4_tasks.h>
 
 #include "BlockLocalPositionEstimator.hpp"
 
-static volatile bool thread_should_exit = false;     /**< Deamon exit flag */
-static volatile bool thread_running = false;     /**< Deamon status flag */
-static int deamon_task;             /**< Handle of deamon task / thread */
-
-/**
- * Deamon management function.
- */
 extern "C" __EXPORT int local_position_estimator_main(int argc, char *argv[]);
 
-/**
- * Mainloop of deamon.
- */
-int local_position_estimator_thread_main(int argc, char *argv[]);
+class LocalPositionEstimatorModule : public ModuleBase<LocalPositionEstimatorModule>
+{
+public:
+	virtual ~LocalPositionEstimatorModule() = default;
 
-/**
- * Print the correct usage.
- */
-static int usage(const char *reason);
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
 
-static int
-usage(const char *reason)
+	/** @see ModuleBase */
+	static LocalPositionEstimatorModule *instantiate(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::run() */
+	void run() override;
+
+private:
+	BlockLocalPositionEstimator _estimator;
+};
+
+int LocalPositionEstimatorModule::print_usage(const char *reason)
 {
 	if (reason) {
-		fprintf(stderr, "%s\n", reason);
+		PX4_WARN("%s\n", reason);
 	}
 
-	fprintf(stderr, "usage: local_position_estimator {start|stop|status} [-p <additional params>]\n\n");
-	return 1;
-}
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Attitude and position estimator using an Extended Kalman Filter.
 
-/**
- * The deamon app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- *
- * The actual stack size should be set in the call
- * to task_create().
- */
-int local_position_estimator_main(int argc, char *argv[])
-{
+)DESCR_STR");
 
-	if (argc < 2) {
-		usage("missing command");
-		return 1;
-	}
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			PX4_INFO("already running");
-			/* this is not an error */
-			return 0;
-		}
-
-		thread_should_exit = false;
-
-		deamon_task = px4_task_spawn_cmd("lp_estimator",
-						 SCHED_DEFAULT,
-						 SCHED_PRIORITY_MAX - 5,
-						 13500,
-						 local_position_estimator_thread_main,
-						 (argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) nullptr);
-		return 0;
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		if (thread_running) {
-			PX4_DEBUG("stop");
-			thread_should_exit = true;
-
-		} else {
-			PX4_WARN("not started");
-		}
-
-		return 0;
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			PX4_INFO("is running");
-
-		} else {
-			PX4_INFO("not started");
-		}
-
-		return 0;
-	}
-
-	usage("unrecognized command");
-	return 1;
-}
-
-int local_position_estimator_thread_main(int argc, char *argv[])
-{
-
-	PX4_DEBUG("starting");
-
-	using namespace control;
-
-	BlockLocalPositionEstimator est;
-
-	thread_running = true;
-
-	while (!thread_should_exit) {
-		est.update();
-	}
-
-	PX4_DEBUG("exiting.");
-
-	thread_running = false;
+	PRINT_MODULE_USAGE_NAME("local_position_estimator", "estimator");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 
 	return 0;
 }
+
+int LocalPositionEstimatorModule::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+
+int LocalPositionEstimatorModule::task_spawn(int argc, char *argv[])
+{
+		_task_id = px4_task_spawn_cmd("lp_estimator",
+						 SCHED_DEFAULT,
+						 SCHED_PRIORITY_ESTIMATOR,
+						 7900,
+						 (px4_main_t)&run_trampoline,
+						 (char *const *)argv);
+
+	if (_task_id < 0) {
+		_task_id = -1;
+		return -errno;
+	}
+
+	return 0;
+}
+
+LocalPositionEstimatorModule *LocalPositionEstimatorModule::instantiate(int argc, char *argv[])
+{
+	LocalPositionEstimatorModule *instance = new LocalPositionEstimatorModule();
+
+	if (instance == nullptr) {
+		PX4_ERR("alloc failed");
+	}
+
+	return instance;
+}
+
+void LocalPositionEstimatorModule::run()
+{
+	while (!should_exit()) {
+		_estimator.update();
+	}
+}
+
+
+int local_position_estimator_main(int argc, char *argv[])
+{
+	return LocalPositionEstimatorModule::main(argc, argv);
+}