Browse Source

LPE: refactor to use ModuleBase

sbg
Beat Küng 7 years ago
parent
commit
83d55773af
  1. 159
      src/modules/local_position_estimator/local_position_estimator_main.cpp

159
src/modules/local_position_estimator/local_position_estimator_main.cpp

@ -40,130 +40,101 @@
* Local position estimator * Local position estimator
*/ */
#include <unistd.h> #include <px4_log.h>
#include <stdio.h> #include <px4_module.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_tasks.h> #include <px4_tasks.h>
#include "BlockLocalPositionEstimator.hpp" #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[]); extern "C" __EXPORT int local_position_estimator_main(int argc, char *argv[]);
/** class LocalPositionEstimatorModule : public ModuleBase<LocalPositionEstimatorModule>
* 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)
{ {
if (reason) { public:
fprintf(stderr, "%s\n", reason); virtual ~LocalPositionEstimatorModule() = default;
}
fprintf(stderr, "usage: local_position_estimator {start|stop|status} [-p <additional params>]\n\n"); /** @see ModuleBase */
return 1; static int task_spawn(int argc, char *argv[]);
}
/** /** @see ModuleBase */
* The deamon app only briefly exists to start static LocalPositionEstimatorModule *instantiate(int argc, char *argv[]);
* 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[])
{
if (argc < 2) { /** @see ModuleBase */
usage("missing command"); static int custom_command(int argc, char *argv[]);
return 1;
}
if (!strcmp(argv[1], "start")) { /** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
if (thread_running) { /** @see ModuleBase::run() */
PX4_INFO("already running"); void run() override;
/* this is not an error */
return 0;
}
thread_should_exit = false; private:
BlockLocalPositionEstimator _estimator;
};
deamon_task = px4_task_spawn_cmd("lp_estimator", int LocalPositionEstimatorModule::print_usage(const char *reason)
SCHED_DEFAULT, {
SCHED_PRIORITY_MAX - 5, if (reason) {
13500, PX4_WARN("%s\n", reason);
local_position_estimator_thread_main,
(argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) nullptr);
return 0;
} }
if (!strcmp(argv[1], "stop")) { PRINT_MODULE_DESCRIPTION(
if (thread_running) { R"DESCR_STR(
PX4_DEBUG("stop"); ### Description
thread_should_exit = true; Attitude and position estimator using an Extended Kalman Filter.
} else { )DESCR_STR");
PX4_WARN("not started");
}
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")) { int LocalPositionEstimatorModule::custom_command(int argc, char *argv[])
if (thread_running) { {
PX4_INFO("is running"); 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 0;
return 1;
} }
int local_position_estimator_thread_main(int argc, char *argv[]) LocalPositionEstimatorModule *LocalPositionEstimatorModule::instantiate(int argc, char *argv[])
{ {
LocalPositionEstimatorModule *instance = new LocalPositionEstimatorModule();
PX4_DEBUG("starting"); if (instance == nullptr) {
PX4_ERR("alloc failed");
using namespace control; }
BlockLocalPositionEstimator est;
thread_running = true; return instance;
}
while (!thread_should_exit) { void LocalPositionEstimatorModule::run()
est.update(); {
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);
} }

Loading…
Cancel
Save