diff --git a/src/modules/local_position_estimator/local_position_estimator_main.cpp b/src/modules/local_position_estimator/local_position_estimator_main.cpp index 6343a1ef1e..5cdb7b3a5c 100644 --- a/src/modules/local_position_estimator/local_position_estimator_main.cpp +++ b/src/modules/local_position_estimator/local_position_estimator_main.cpp @@ -40,130 +40,101 @@ * Local position estimator */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include #include #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 { - if (reason) { - fprintf(stderr, "%s\n", reason); - } +public: + virtual ~LocalPositionEstimatorModule() = default; - fprintf(stderr, "usage: local_position_estimator {start|stop|status} [-p ]\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); }