From f7b65c577b80f912be7e79d0a8ddbe4057e71220 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 27 Sep 2018 13:10:07 +0200 Subject: [PATCH] mc_pos_control: refactor to use ModuleBase --- .../mc_pos_control/mc_pos_control_main.cpp | 204 +++++++----------- 1 file changed, 72 insertions(+), 132 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index ecefbd49e4..e7d41cf197 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -34,17 +34,13 @@ /** * @file mc_pos_control_main.cpp * Multicopter position controller. - * - * The controller has two loops: P loop for position error and PID loop for velocity error. - * Output of velocity controller is thrust vector that is split to thrust direction - * (i.e. rotation matrix for multicopter orientation) and thrust scalar (i.e. multicopter thrust itself). - * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. */ #include #include #include #include +#include #include #include #include @@ -73,34 +69,34 @@ /** * Multicopter position control app start / stop handling function - * - * @ingroup apps */ extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]); -class MulticopterPositionControl : public control::SuperBlock, public ModuleParams +class MulticopterPositionControl : public ModuleBase, public control::SuperBlock, + public ModuleParams { public: - /** - * Constructor - */ MulticopterPositionControl(); - /** - * Destructor, also kills task. - */ ~MulticopterPositionControl(); - /** - * Start task. - * - * @return OK on success. - */ - int start(); + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static MulticopterPositionControl *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: - bool _task_should_exit = false; /** 50) { - px4_task_delete(_control_task); - break; - } - } while (_control_task != -1); - } - - pos_control::g_control = nullptr; } void @@ -444,13 +439,6 @@ MulticopterPositionControl::poll_subscriptions() } } -int -MulticopterPositionControl::task_main_trampoline(int argc, char *argv[]) -{ - pos_control::g_control->task_main(); - return 0; -} - void MulticopterPositionControl::limit_altitude(vehicle_local_position_setpoint_s &setpoint) { @@ -547,7 +535,7 @@ MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z) } void -MulticopterPositionControl::task_main() +MulticopterPositionControl::run() { // do subscriptions _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -577,18 +565,15 @@ MulticopterPositionControl::task_main() fds[0].fd = _local_pos_sub; fds[0].events = POLLIN; - while (!_task_should_exit) { + while (!should_exit()) { // wait for up to 20ms for data int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 20); - // timed out - periodic check for _task_should_exit - if (pret == 0) { - // Go through the loop anyway to copy manual input at 50 Hz. - } + // pret == 0: go through the loop anyway to copy manual input at 50 Hz. // this is undesirable but not much we can do if (pret < 0) { - warn("poll error %d, %d", pret, errno); + PX4_ERR("poll error %d, %d", pret, errno); continue; } @@ -785,7 +770,6 @@ MulticopterPositionControl::task_main() } } - _control_task = -1; } void @@ -1135,25 +1119,6 @@ MulticopterPositionControl::publish_local_pos_sp(const vehicle_local_position_se } } -int -MulticopterPositionControl::start() -{ - // start the task - _control_task = px4_task_spawn_cmd("mc_pos_control", - SCHED_DEFAULT, - SCHED_PRIORITY_POSITION_CONTROL, - 1900, - (px4_main_t)&MulticopterPositionControl::task_main_trampoline, - nullptr); - - if (_control_task < 0) { - warn("task start failed"); - return -errno; - } - - return OK; -} - void MulticopterPositionControl::check_failure(bool task_failure, uint8_t nav_state) { if (!task_failure) { @@ -1205,59 +1170,34 @@ void MulticopterPositionControl::send_vehicle_cmd_do(uint8_t nav_state) } } +int MulticopterPositionControl::task_spawn(int argc, char *argv[]) +{ + _task_id = px4_task_spawn_cmd("mc_pos_control", + SCHED_DEFAULT, + SCHED_PRIORITY_POSITION_CONTROL, + 1900, + (px4_main_t)&run_trampoline, + (char *const *)argv); + + if (_task_id < 0) { + _task_id = -1; + return -errno; + } + + return 0; +} + +MulticopterPositionControl *MulticopterPositionControl::instantiate(int argc, char *argv[]) +{ + return new MulticopterPositionControl(); +} + +int MulticopterPositionControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + int mc_pos_control_main(int argc, char *argv[]) { - if (argc < 2) { - warnx("usage: mc_pos_control {start|stop|status}"); - return 1; - } - - if (!strcmp(argv[1], "start")) { - - if (pos_control::g_control != nullptr) { - warnx("already running"); - return 1; - } - - pos_control::g_control = new MulticopterPositionControl; - - if (pos_control::g_control == nullptr) { - warnx("alloc failed"); - return 1; - } - - if (OK != pos_control::g_control->start()) { - delete pos_control::g_control; - pos_control::g_control = nullptr; - warnx("start failed"); - return 1; - } - - return 0; - } - - if (!strcmp(argv[1], "stop")) { - if (pos_control::g_control == nullptr) { - warnx("not running"); - return 1; - } - - delete pos_control::g_control; - pos_control::g_control = nullptr; - return 0; - } - - if (!strcmp(argv[1], "status")) { - if (pos_control::g_control) { - warnx("running"); - return 0; - - } else { - warnx("not running"); - return 1; - } - } - - warnx("unrecognized command"); - return 1; + return MulticopterPositionControl::main(argc, argv); }