From 03ba38d0a4e39dc513cae6b53ff7cf845c13161c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 12:42:57 +0100 Subject: very much WIP, start to make mc att control p4 and ros compatible --- src/modules/mc_att_control/mc_att_control_main.cpp | 418 +++++++++++---------- 1 file changed, 212 insertions(+), 206 deletions(-) (limited to 'src/modules/mc_att_control/mc_att_control_main.cpp') diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 631ca57e0..7a3b61482 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -71,17 +71,53 @@ #include "mc_att_control_base.h" +static bool thread_running = false; /**< Deamon status flag */ +static int daemon_task; /**< Handle of deamon task / thread */ + +using namespace px4; + /** * Multicopter attitude control app start / stop handling function * * @ingroup apps */ -extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); + +PX4_MAIN_FUNCTION(mc_att_control); + +int mc_attitude_thread_main(int argc, char *argv[]); #define YAW_DEADZONE 0.05f #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f +void handle_vehicle_attitude2(const PX4_TOPIC_T(rc_channels) &msg) { + PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp); +} + + +namespace px4 +{ +bool task_should_exit = false; +} + +// PX4_MAIN_FUNCTION(mc_att_control) { + // px4::init(argc, argv, "listener"); + + // px4::NodeHandle n; + + // PX4_SUBSCRIBE(n, rc_channels, handle_vehicle_attitude2, 1000); + + /** + * px4::spin() will enter a loop, pumping callbacks. With this version, all + * callbacks will be called from within this thread (the main one). px4::spin() + * will exit when Ctrl-C is pressed, or the node is shutdown by the master. + */ + // n.spin(); + // PX4_INFO("finished, returning"); + + // return 0; +// } + class MulticopterAttitudeControl : public MulticopterAttitudeControlBase { @@ -96,15 +132,10 @@ public: */ ~MulticopterAttitudeControl(); - /** - * Start the sensors task. - * - * @return OK on success. - */ - int start(); - void handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg); + void spin() { n.spin(); } + private: bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ @@ -123,6 +154,8 @@ private: orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ + px4::NodeHandle n; + struct { param_t roll_p; param_t roll_rate_p; @@ -184,15 +217,6 @@ private: */ void arming_status_poll(); - /** - * Shim for calling task_main from task_create. - */ - static void task_main_trampoline(int argc, char *argv[]); - - /** - * Main attitude control task. - */ - void task_main(); }; namespace mc_att_control @@ -204,7 +228,6 @@ namespace mc_att_control #endif static const int ERROR = -1; -MulticopterAttitudeControl *g_control; } MulticopterAttitudeControl::MulticopterAttitudeControl() : @@ -224,6 +247,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _att_sp_pub(-1), _v_rates_sp_pub(-1), _actuators_0_pub(-1), + n(), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) @@ -252,6 +276,25 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* fetch initial parameter values */ parameters_update(); + + /* + * do subscriptions + */ + // _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + PX4_SUBSCRIBE(n, vehicle_attitude, MulticopterAttitudeControl::handle_vehicle_attitude, this, 0); + // _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + PX4_SUBSCRIBE(n, vehicle_attitude_setpoint, 0); + // _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + PX4_SUBSCRIBE(n, vehicle_rates_setpoint, 0); + // _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + PX4_SUBSCRIBE(n, vehicle_control_mode, 0); + // _params_sub = orb_subscribe(ORB_ID(parameter_update)); + PX4_SUBSCRIBE(n, parameter_update, 0); + // _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + PX4_SUBSCRIBE(n, manual_control_setpoint, 0); + // _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + PX4_SUBSCRIBE(n, actuator_armed, 0); + } MulticopterAttitudeControl::~MulticopterAttitudeControl() @@ -275,7 +318,7 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl() } while (_control_task != -1); } - mc_att_control::g_control = nullptr; + // mc_att_control::g_control = nullptr; } int @@ -415,200 +458,140 @@ MulticopterAttitudeControl::arming_status_poll() } } -void -MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) -{ - mc_att_control::g_control->task_main(); -} +// void +// MulticopterAttitudeControl::task_main() +// { -void -MulticopterAttitudeControl::task_main() -{ - px4::NodeHandle n; - /* - * do subscriptions - */ - _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - // PX4_SUBSCRIBE(n, vehicle_attitude_setpoint, MulticopterAttitudeControl::handle_vehicle_attitude, this 0); - _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); - _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - - /* initialize parameters cache */ - parameters_update(); + // [> wakeup source: vehicle attitude <] + // struct pollfd fds[1]; - /* wakeup source: vehicle attitude */ - struct pollfd fds[1]; + // fds[0].fd = _v_att_sub; + // fds[0].events = POLLIN; - fds[0].fd = _v_att_sub; - fds[0].events = POLLIN; + // while (!_task_should_exit) { - while (!_task_should_exit) { - /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + // perf_end(_loop_perf); + // } - /* timed out - periodic check for _task_should_exit */ - if (pret == 0) { - continue; - } + // warnx("exit"); - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - warn("poll error %d, %d", pret, errno); - /* sleep a bit before next try */ - usleep(100000); - continue; - } + // _control_task = -1; + // _exit(0); +// } - perf_begin(_loop_perf); +void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) { - /* run controller on attitude changes */ - if (fds[0].revents & POLLIN) { - static uint64_t last_run = 0; - float dt = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); + perf_begin(_loop_perf); - /* guard against too small (< 2ms) and too large (> 20ms) dt's */ - if (dt < 0.002f) { - dt = 0.002f; + /* run controller on attitude changes */ + static uint64_t last_run = 0; + float dt = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); - } else if (dt > 0.02f) { - dt = 0.02f; - } + /* guard against too small (< 2ms) and too large (> 20ms) dt's */ + if (dt < 0.002f) { + dt = 0.002f; + + } else if (dt > 0.02f) { + dt = 0.02f; + } - /* copy attitude topic */ - orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); + /* copy attitude topic */ + orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); - /* check for updates in other topics */ - parameter_update_poll(); - vehicle_control_mode_poll(); - arming_status_poll(); - vehicle_manual_poll(); + /* check for updates in other topics */ + parameter_update_poll(); + vehicle_control_mode_poll(); + arming_status_poll(); + vehicle_manual_poll(); - if (_v_control_mode.flag_control_attitude_enabled) { - control_attitude(dt); + if (_v_control_mode.flag_control_attitude_enabled) { + control_attitude(dt); - /* publish the attitude setpoint if needed */ - if (_publish_att_sp) { - _v_att_sp.timestamp = hrt_absolute_time(); + /* publish the attitude setpoint if needed */ + if (_publish_att_sp) { + _v_att_sp.timestamp = hrt_absolute_time(); - if (_att_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, - &_v_att_sp); + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, + &_v_att_sp); - } else { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), - &_v_att_sp); - } - } + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), + &_v_att_sp); + } + } - /* publish attitude rates setpoint */ - _v_rates_sp.roll = _rates_sp(0); - _v_rates_sp.pitch = _rates_sp(1); - _v_rates_sp.yaw = _rates_sp(2); - _v_rates_sp.thrust = _thrust_sp; - _v_rates_sp.timestamp = hrt_absolute_time(); + /* publish attitude rates setpoint */ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = _rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); - if (_v_rates_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + if (_v_rates_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); - } else { - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); - } + } else { + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + } - } else { - /* attitude controller disabled, poll rates setpoint topic */ - if (_v_control_mode.flag_control_manual_enabled) { - /* manual rates control - ACRO mode */ - _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, - _manual_control_sp.r).emult(_params.acro_rate_max); - _thrust_sp = _manual_control_sp.z; - - /* reset yaw setpoint after ACRO */ - _reset_yaw_sp = true; - - /* publish attitude rates setpoint */ - _v_rates_sp.roll = _rates_sp(0); - _v_rates_sp.pitch = _rates_sp(1); - _v_rates_sp.yaw = _rates_sp(2); - _v_rates_sp.thrust = _thrust_sp; - _v_rates_sp.timestamp = hrt_absolute_time(); - - if (_v_rates_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); - - } else { - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); - } - - } else { - /* attitude controller disabled, poll rates setpoint topic */ - vehicle_rates_setpoint_poll(); - _rates_sp(0) = _v_rates_sp.roll; - _rates_sp(1) = _v_rates_sp.pitch; - _rates_sp(2) = _v_rates_sp.yaw; - _thrust_sp = _v_rates_sp.thrust; - } - } + } else { + /* attitude controller disabled, poll rates setpoint topic */ + if (_v_control_mode.flag_control_manual_enabled) { + /* manual rates control - ACRO mode */ + _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, + _manual_control_sp.r).emult(_params.acro_rate_max); + _thrust_sp = _manual_control_sp.z; - if (_v_control_mode.flag_control_rates_enabled) { - control_attitude_rates(dt); + /* reset yaw setpoint after ACRO */ + _reset_yaw_sp = true; - /* publish actuator controls */ - _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; - _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; - _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; - _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; - _actuators.timestamp = hrt_absolute_time(); + /* publish attitude rates setpoint */ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = _rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); - if (!_actuators_0_circuit_breaker_enabled) { - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + if (_v_rates_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); - } - } + } else { + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); } - } - perf_end(_loop_perf); + } else { + /* attitude controller disabled, poll rates setpoint topic */ + vehicle_rates_setpoint_poll(); + _rates_sp(0) = _v_rates_sp.roll; + _rates_sp(1) = _v_rates_sp.pitch; + _rates_sp(2) = _v_rates_sp.yaw; + _thrust_sp = _v_rates_sp.thrust; + } } - warnx("exit"); - - _control_task = -1; - _exit(0); -} - -void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) { - PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp); -} + if (_v_control_mode.flag_control_rates_enabled) { + control_attitude_rates(dt); -int -MulticopterAttitudeControl::start() -{ - ASSERT(_control_task == -1); + /* publish actuator controls */ + _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; + _actuators.timestamp = hrt_absolute_time(); - /* start the task */ - _control_task = task_spawn_cmd("mc_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - (main_t)&MulticopterAttitudeControl::task_main_trampoline, - nullptr); + if (!_actuators_0_circuit_breaker_enabled) { + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); - if (_control_task < 0) { - warn("task start failed"); - return -errno; + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } + } } - - return OK; } PX4_MAIN_FUNCTION(mc_att_control) @@ -621,44 +604,67 @@ PX4_MAIN_FUNCTION(mc_att_control) if (!strcmp(argv[1], "start")) { - if (mc_att_control::g_control != nullptr) { - errx(1, "already running"); + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); } - mc_att_control::g_control = new MulticopterAttitudeControl; - - if (mc_att_control::g_control == nullptr) { - errx(1, "alloc failed"); - } + task_should_exit = false; - if (OK != mc_att_control::g_control->start()) { - delete mc_att_control::g_control; - mc_att_control::g_control = nullptr; - err(1, "start failed"); - } + daemon_task = task_spawn_cmd("mc_att_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + mc_attitude_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } - if (!strcmp(argv[1], "stop")) { - if (mc_att_control::g_control == nullptr) { - errx(1, "not running"); - } + // if (!strcmp(argv[1], "stop")) { + // if (mc_att_control::g_control == nullptr) { + // errx(1, "not running"); + // } - delete mc_att_control::g_control; - mc_att_control::g_control = nullptr; - exit(0); - } + // delete mc_att_control::g_control; + // mc_att_control::g_control = nullptr; + // exit(0); + // } - if (!strcmp(argv[1], "status")) { - if (mc_att_control::g_control) { - errx(0, "running"); + // if (!strcmp(argv[1], "status")) { + // if (mc_att_control::g_control) { + // errx(0, "running"); - } else { - errx(1, "not running"); - } - } + // } else { + // errx(1, "not running"); + // } + // } warnx("unrecognized command"); return 1; } + +int mc_attitude_thread_main(int argc, char *argv[]) +{ + + warnx("starting"); + + MulticopterAttitudeControl attctl; + + thread_running = true; + + attctl.spin(); + + // while (!task_should_exit) { + // attctl.update(); + // } + + warnx("exiting."); + + thread_running = false; + + return 0; +} + + -- cgit v1.2.3