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[]);
-
-/**
- * Print the correct usage.
- */
-static int usage(const char *reason);
-
-static int
-usage(const char *reason)
+class LocalPositionEstimatorModule : public ModuleBase<LocalPositionEstimatorModule>
 {
-	if (reason) {
-		fprintf(stderr, "%s\n", reason);
-	}
+public:
+	virtual ~LocalPositionEstimatorModule() = default;
 
-	fprintf(stderr, "usage: local_position_estimator {start|stop|status} [-p <additional params>]\n\n");
-	return 1;
-}
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
 
-/**
- * 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[])
-{
+	/** @see ModuleBase */
+	static LocalPositionEstimatorModule *instantiate(int argc, char *argv[]);
 
-	if (argc < 2) {
-		usage("missing command");
-		return 1;
-	}
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
 
-	if (!strcmp(argv[1], "start")) {
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
 
-		if (thread_running) {
-			PX4_INFO("already running");
-			/* this is not an error */
-			return 0;
-		}
+	/** @see ModuleBase::run() */
+	void run() override;
 
-		thread_should_exit = false;
+private:
+	BlockLocalPositionEstimator _estimator;
+};
 
-		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;
+int LocalPositionEstimatorModule::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
 	}
 
-	if (!strcmp(argv[1], "stop")) {
-		if (thread_running) {
-			PX4_DEBUG("stop");
-			thread_should_exit = true;
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Attitude and position estimator using an Extended Kalman Filter.
 
-		} else {
-			PX4_WARN("not started");
-		}
+)DESCR_STR");
 
-		return 0;
-	}
+	PRINT_MODULE_USAGE_NAME("local_position_estimator", "estimator");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
 
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			PX4_INFO("is running");
+int LocalPositionEstimatorModule::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
 
-		} else {
-			PX4_INFO("not started");
-		}
 
-		return 0;
+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;
 	}
 
-	usage("unrecognized command");
-	return 1;
+	return 0;
 }
 
-int local_position_estimator_thread_main(int argc, char *argv[])
+LocalPositionEstimatorModule *LocalPositionEstimatorModule::instantiate(int argc, char *argv[])
 {
+	LocalPositionEstimatorModule *instance = new LocalPositionEstimatorModule();
 
-	PX4_DEBUG("starting");
-
-	using namespace control;
-
-	BlockLocalPositionEstimator est;
+	if (instance == nullptr) {
+		PX4_ERR("alloc failed");
+	}
 
-	thread_running = true;
+	return instance;
+}
 
-	while (!thread_should_exit) {
-		est.update();
+void LocalPositionEstimatorModule::run()
+{
+	while (!should_exit()) {
+		_estimator.update();
 	}
+}
 
-	PX4_DEBUG("exiting.");
-
-	thread_running = false;
 
-	return 0;
+int local_position_estimator_main(int argc, char *argv[])
+{
+	return LocalPositionEstimatorModule::main(argc, argv);
 }