ekf2: use ModuleBase & add module documentation

posix-configs/SITL/init/ekf2/iris_replay

@@ -1,6 +1,6 @@
 uorb start
 
-ekf2 start --replay
+ekf2 start -r
 logger start -e -t -b 1000 -p vehicle_attitude
 sleep 0.2
 replay start

posix-configs/SITL/init/replay/iris

@@ -1,4 +1,4 @@
 uorb start
-ekf2 start --replay
+ekf2 start -r
 sleep 0.2
 ekf2_replay start replay.px4log

src/modules/ekf2/ekf2_main.cpp

@@ -40,6 +40,7 @@
 
 #include <px4_config.h>
 #include <px4_defines.h>
+#include <px4_module.h>
 #include <px4_tasks.h>
 #include <px4_posix.h>
 #include <px4_time.h>
@@ -88,49 +89,36 @@
 extern "C" __EXPORT int ekf2_main(int argc, char *argv[]);
 
 
-class Ekf2;
-
-namespace ekf2
-{
-Ekf2 *instance = nullptr;
-}
-
-
-class Ekf2 : public control::SuperBlock
+class Ekf2 : public control::SuperBlock, public ModuleBase<Ekf2>
 {
 public:
-	/**
-	 * Constructor
-	 */
 	Ekf2();
 
-	/**
-	 * Destructor, also kills task.
-	 */
 	~Ekf2();
 
-	/**
-	 * Start task.
-	 *
-	 * @return		OK on success.
-	 */
-	int		start();
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
 
-	void 	set_replay_mode(bool replay) {_replay_mode = replay;};
+	/** @see ModuleBase */
+	static Ekf2 *instantiate(int argc, char *argv[]);
 
-	static void	task_main_trampoline(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;
 
-	void		task_main();
+	void 	set_replay_mode(bool replay) {_replay_mode = replay;};
 
-	void print_status();
+	static void	task_main_trampoline(int argc, char *argv[]);
 
-	void exit() { _task_should_exit = true; }
+	int print_status() override;
 
 private:
 	static constexpr float _dt_max = 0.02;
-	bool	_task_should_exit = false;
-	int	_control_task = -1;		// task handle for task
-
 	bool 	_replay_mode;			// should we use replay data from a log
 	int32_t _publish_replay_mode;		// defines if we should publish replay messages
 
@@ -450,14 +438,15 @@ Ekf2::~Ekf2()
 
 }
 
-void Ekf2::print_status()
+int Ekf2::print_status()
 {
-	PX4_INFO("local position OK %s", (_ekf.local_position_is_valid()) ? "[YES]" : "[NO]");
-	PX4_INFO("global position OK %s", (_ekf.global_position_is_valid()) ? "[YES]" : "[NO]");
+	PX4_INFO("local position OK %s", (_ekf.local_position_is_valid()) ? "yes" : "no");
+	PX4_INFO("global position OK %s", (_ekf.global_position_is_valid()) ? "yes" : "no");
 	PX4_INFO("time slip: %" PRIu64 " us", _last_time_slip_us);
+	return 0;
 }
 
-void Ekf2::task_main()
+void Ekf2::run()
 {
 	// subscribe to relevant topics
 	int sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
@@ -495,7 +484,7 @@ void Ekf2::task_main()
 	vehicle_status_s vehicle_status = {};
 	sensor_selection_s sensor_selection = {};
 
-	while (!_task_should_exit) {
+	while (!should_exit()) {
 		int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);
 
 		if (ret < 0) {
@@ -1378,110 +1367,70 @@ void Ekf2::task_main()
 	orb_unsubscribe(vehicle_land_detected_sub);
 	orb_unsubscribe(status_sub);
 	orb_unsubscribe(sensor_selection_sub);
-
-	delete ekf2::instance;
-	ekf2::instance = nullptr;
 }
 
-void Ekf2::task_main_trampoline(int argc, char *argv[])
+Ekf2 *Ekf2::instantiate(int argc, char *argv[])
 {
-	ekf2::instance->task_main();
-}
+	Ekf2 *instance = new Ekf2();
 
-int Ekf2::start()
-{
-	ASSERT(_control_task == -1);
-
-	/* start the task */
-	_control_task = px4_task_spawn_cmd("ekf2",
-					   SCHED_DEFAULT,
-					   SCHED_PRIORITY_MAX - 5,
-					   5900,
-					   (px4_main_t)&Ekf2::task_main_trampoline,
-					   nullptr);
-
-	if (_control_task < 0) {
-		PX4_WARN("task start failed");
-		return -errno;
+	if (instance) {
+		if (argc >= 2 && !strcmp(argv[1], "-r")) {
+			instance->set_replay_mode(true);
+		}
 	}
 
-	return OK;
+	return instance;
 }
 
-int ekf2_main(int argc, char *argv[])
+int Ekf2::custom_command(int argc, char *argv[])
 {
-	if (argc < 2) {
-		PX4_WARN("usage: ekf2 {start|stop|status}");
-		return 1;
-	}
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (ekf2::instance != nullptr) {
-			PX4_WARN("already running");
-			return 1;
-		}
-
-		ekf2::instance = new Ekf2();
-
-		if (ekf2::instance == nullptr) {
-			PX4_WARN("alloc failed");
-			return 1;
-		}
-
-		if (argc >= 3) {
-			if (!strcmp(argv[2], "--replay")) {
-				ekf2::instance->set_replay_mode(true);
-			}
-		}
-
-		if (OK != ekf2::instance->start()) {
-			delete ekf2::instance;
-			ekf2::instance = nullptr;
-			PX4_WARN("start failed");
-			return 1;
-		}
+	return print_usage("unknown command");
+}
 
-		return 0;
+int Ekf2::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
 	}
 
-	if (!strcmp(argv[1], "stop")) {
-		if (ekf2::instance == nullptr) {
-			PX4_WARN("not running");
-			return 1;
-		}
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Attitude and position estimator using an Extended Kalman Filter. It is used for Multirotors and Fixed-Wing.
 
-		ekf2::instance->exit();
+The documentation can be found on the [tuning_the_ecl_ekf](https://dev.px4.io/en/tutorials/tuning_the_ecl_ekf.html) page.
 
-		// wait for the destruction of the instance
-		while (ekf2::instance != nullptr) {
-			usleep(50000);
-		}
+ekf2 can be started in replay mode (`-r`): in this mode it does not access the system time, but only uses the
+timestamps from the sensor topics.
 
-		return 0;
-	}
+)DESCR_STR");
 
-	if (!strcmp(argv[1], "print")) {
-		if (ekf2::instance != nullptr) {
+	PRINT_MODULE_USAGE_NAME("ekf2", "estimator");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAM_FLAG('r', "Enable replay mode", true);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 
-			return 0;
-		}
+	return 0;
+}
 
-		return 1;
+int Ekf2::task_spawn(int argc, char *argv[])
+{
+	_task_id = px4_task_spawn_cmd("ekf2",
+				      SCHED_DEFAULT,
+				      SCHED_PRIORITY_MAX - 5,
+				      5900,
+				      (px4_main_t)&run_trampoline,
+				      (char *const *)argv);
+
+	if (_task_id < 0) {
+		_task_id = -1;
+		return -errno;
 	}
 
-	if (!strcmp(argv[1], "status")) {
-		if (ekf2::instance) {
-			PX4_WARN("running");
-			ekf2::instance->print_status();
-			return 0;
-
-		} else {
-			PX4_WARN("not running");
-			return 1;
-		}
-	}
+	return 0;
+}
 
-	PX4_WARN("unrecognized command");
-	return 1;
+int ekf2_main(int argc, char *argv[])
+{
+	return Ekf2::main(argc, argv);
 }