From 7634147c0f9bb1ee27464d678394f22339b5ce00 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 15:02:43 +0100 Subject: starting to port mc att ctrl multiplatform to the latest api --- .../mc_att_control.cpp | 99 +++++++++++----------- .../mc_att_control_multiplatform/mc_att_control.h | 11 ++- .../mc_att_control_base.cpp | 78 ++++++++--------- .../mc_att_control_base.h | 26 +++--- .../mc_att_control_main.cpp | 4 +- .../mc_att_control_start_nuttx.cpp | 99 ++++++++++++++++++++++ 6 files changed, 209 insertions(+), 108 deletions(-) create mode 100644 src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp (limited to 'src/modules/mc_att_control_multiplatform') diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp index 822a504b5..1e40c2b05 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp @@ -104,15 +104,15 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* * do subscriptions */ - _v_att = PX4_SUBSCRIBE(_n, vehicle_attitude, MulticopterAttitudeControl::handle_vehicle_attitude, this, 0); - _v_att_sp = PX4_SUBSCRIBE(_n, vehicle_attitude_setpoint, 0); - _v_rates_sp = PX4_SUBSCRIBE(_n, vehicle_rates_setpoint, 0); - _v_control_mode = PX4_SUBSCRIBE(_n, vehicle_control_mode, 0); - _parameter_update = PX4_SUBSCRIBE(_n, parameter_update, - MulticopterAttitudeControl::handle_parameter_update, this, 1000); - _manual_control_sp = PX4_SUBSCRIBE(_n, manual_control_setpoint, 0); - _armed = PX4_SUBSCRIBE(_n, actuator_armed, 0); - _v_status = PX4_SUBSCRIBE(_n, vehicle_status, 0); + _v_att = _n.subscribe(&MulticopterAttitudeControl::handle_vehicle_attitude, this, 0); + _v_att_sp = _n.subscribe(0); + _v_rates_sp = _n.subscribe(0); + _v_control_mode = _n.subscribe(0); + _parameter_update = _n.subscribe( + &MulticopterAttitudeControl::handle_parameter_update, this, 1000); + _manual_control_sp = _n.subscribe(0); + _armed = _n.subscribe(0); + _v_status = _n.subscribe(0); } @@ -180,12 +180,12 @@ MulticopterAttitudeControl::parameters_update() return OK; } -void MulticopterAttitudeControl::handle_parameter_update(const PX4_TOPIC_T(parameter_update) &msg) +void MulticopterAttitudeControl::handle_parameter_update(const px4_parameter_update &msg) { parameters_update(); } -void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) { +void MulticopterAttitudeControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg) { perf_begin(_loop_perf); @@ -202,95 +202,98 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi dt = 0.02f; } - if (_v_control_mode->get().flag_control_attitude_enabled) { + if (_v_control_mode->data().flag_control_attitude_enabled) { control_attitude(dt); /* publish the attitude setpoint if needed */ - if (_publish_att_sp && _v_status->get().is_rotary_wing) { - _v_att_sp_mod.timestamp = px4::get_time_micros(); + if (_publish_att_sp && _v_status->data().is_rotary_wing) { + _v_att_sp_mod.data().timestamp = px4::get_time_micros(); if (_att_sp_pub != nullptr) { _att_sp_pub->publish(_v_att_sp_mod); } else { - _att_sp_pub = PX4_ADVERTISE(_n, vehicle_attitude_setpoint); + _att_sp_pub = _n.advertise(); } } /* publish attitude rates setpoint */ - _v_rates_sp_mod.roll = _rates_sp(0); - _v_rates_sp_mod.pitch = _rates_sp(1); - _v_rates_sp_mod.yaw = _rates_sp(2); - _v_rates_sp_mod.thrust = _thrust_sp; - _v_rates_sp_mod.timestamp = px4::get_time_micros(); + _v_rates_sp_mod.data().roll = _rates_sp(0); + _v_rates_sp_mod.data().pitch = _rates_sp(1); + _v_rates_sp_mod.data().yaw = _rates_sp(2); + _v_rates_sp_mod.data().thrust = _thrust_sp; + _v_rates_sp_mod.data().timestamp = px4::get_time_micros(); if (_v_rates_sp_pub != nullptr) { _v_rates_sp_pub->publish(_v_rates_sp_mod); } else { - if (_v_status->get().is_vtol) { - _v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint); + if (_v_status->data().is_vtol) { + //XXX add a second publisher? + // _v_rates_sp_pub = _n.advertise(); } else { - _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint); + _v_rates_sp_pub = _n.advertise(); } } } else { /* attitude controller disabled, poll rates setpoint topic */ - if (_v_control_mode->get().flag_control_manual_enabled) { + if (_v_control_mode->data().flag_control_manual_enabled) { /* manual rates control - ACRO mode */ - _rates_sp = math::Vector<3>(_manual_control_sp->get().y, -_manual_control_sp->get().x, - _manual_control_sp->get().r).emult(_params.acro_rate_max); - _thrust_sp = _manual_control_sp->get().z; + _rates_sp = math::Vector<3>(_manual_control_sp->data().y, -_manual_control_sp->data().x, + _manual_control_sp->data().r).emult(_params.acro_rate_max); + _thrust_sp = _manual_control_sp->data().z; /* reset yaw setpoint after ACRO */ _reset_yaw_sp = true; /* publish attitude rates setpoint */ - _v_rates_sp_mod.roll = _rates_sp(0); - _v_rates_sp_mod.pitch = _rates_sp(1); - _v_rates_sp_mod.yaw = _rates_sp(2); - _v_rates_sp_mod.thrust = _thrust_sp; - _v_rates_sp_mod.timestamp = px4::get_time_micros(); + _v_rates_sp_mod.data().roll = _rates_sp(0); + _v_rates_sp_mod.data().pitch = _rates_sp(1); + _v_rates_sp_mod.data().yaw = _rates_sp(2); + _v_rates_sp_mod.data().thrust = _thrust_sp; + _v_rates_sp_mod.data().timestamp = px4::get_time_micros(); if (_v_rates_sp_pub != nullptr) { _v_rates_sp_pub->publish(_v_rates_sp_mod); } else { - if (_v_status->get().is_vtol) { - _v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint); + if (_v_status->data().is_vtol) { + //XXX add a second publisher? + // _v_rates_sp_pub = _n.advertise(); } else { - _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint); + _v_rates_sp_pub = _n.advertise(); } } } else { /* attitude controller disabled, poll rates setpoint topic */ - _rates_sp(0) = _v_rates_sp->get().roll; - _rates_sp(1) = _v_rates_sp->get().pitch; - _rates_sp(2) = _v_rates_sp->get().yaw; - _thrust_sp = _v_rates_sp->get().thrust; + _rates_sp(0) = _v_rates_sp->data().roll; + _rates_sp(1) = _v_rates_sp->data().pitch; + _rates_sp(2) = _v_rates_sp->data().yaw; + _thrust_sp = _v_rates_sp->data().thrust; } } - if (_v_control_mode->get().flag_control_rates_enabled) { + if (_v_control_mode->data().flag_control_rates_enabled) { control_attitude_rates(dt); /* 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 = px4::get_time_micros(); + _actuators.data().control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.data().control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.data().control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.data().control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; + _actuators.data().timestamp = px4::get_time_micros(); if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub != nullptr) { _actuators_0_pub->publish(_actuators); } else { - if (_v_status->get().is_vtol) { - _actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_virtual_mc); + if (_v_status->data().is_vtol) { + //XXX add a second publisher? + // _actuators_0_pub = _n.advertise(); } else { - _actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_0); + _actuators_0_pub = _n.advertise(); } } } diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.h b/src/modules/mc_att_control_multiplatform/mc_att_control.h index bff5289fd..95d608684 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.h @@ -59,7 +59,6 @@ #include #include #include -// #include #include #include @@ -80,8 +79,8 @@ public: ~MulticopterAttitudeControl(); /* Callbacks for topics */ - void handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg); - void handle_parameter_update(const PX4_TOPIC_T(parameter_update) &msg); + void handle_vehicle_attitude(const px4_vehicle_attitude &msg); + void handle_parameter_update(const px4_parameter_update &msg); void spin() { _n.spin(); } @@ -89,9 +88,9 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ - px4::Publisher * _att_sp_pub; /**< attitude setpoint publication */ - px4::Publisher * _v_rates_sp_pub; /**< rate setpoint publication */ - px4::Publisher * _actuators_0_pub; /**< attitude actuator controls publication */ + px4::Publisher * _att_sp_pub; /**< attitude setpoint publication */ + px4::Publisher * _v_rates_sp_pub; /**< rate setpoint publication */ + px4::Publisher * _actuators_0_pub; /**< attitude actuator controls publication */ px4::NodeHandle _n; diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp index aff59778e..0de832df9 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp @@ -92,28 +92,28 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) _publish_att_sp = false; - if (_v_control_mode->get().flag_control_manual_enabled) { + if (_v_control_mode->data().flag_control_manual_enabled) { /* manual input, set or modify attitude setpoint */ - if (_v_control_mode->get().flag_control_velocity_enabled - || _v_control_mode->get().flag_control_climb_rate_enabled) { + if (_v_control_mode->data().flag_control_velocity_enabled + || _v_control_mode->data().flag_control_climb_rate_enabled) { /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ - memcpy(&_v_att_sp_mod, _v_att_sp->get_void_ptr(), sizeof(_v_att_sp_mod)); + memcpy(&_v_att_sp_mod, &_v_att_sp->data(), sizeof(_v_att_sp_mod)); } - if (!_v_control_mode->get().flag_control_climb_rate_enabled) { + if (!_v_control_mode->data().flag_control_climb_rate_enabled) { /* pass throttle directly if not in altitude stabilized mode */ - _v_att_sp_mod.thrust = _manual_control_sp->get().z; + _v_att_sp_mod.data().thrust = _manual_control_sp->data().z; _publish_att_sp = true; } - if (!_armed->get().armed) { + if (!_armed->data().armed) { /* reset yaw setpoint when disarmed */ _reset_yaw_sp = true; } /* move yaw setpoint in all modes */ - if (_v_att_sp_mod.thrust < 0.1f) { + if (_v_att_sp_mod.data().thrust < 0.1f) { // TODO //if (_status.condition_landed) { /* reset yaw setpoint if on ground */ @@ -121,71 +121,71 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) //} } else { /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp->get().r * _params.man_yaw_max; - _v_att_sp_mod.yaw_body = _wrap_pi( - _v_att_sp_mod.yaw_body + yaw_sp_move_rate * dt); + yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max; + _v_att_sp_mod.data().yaw_body = _wrap_pi( + _v_att_sp_mod.data().yaw_body + yaw_sp_move_rate * dt); float yaw_offs_max = _params.man_yaw_max / _params.att_p(2); - float yaw_offs = _wrap_pi(_v_att_sp_mod.yaw_body - _v_att->get().yaw); + float yaw_offs = _wrap_pi(_v_att_sp_mod.data().yaw_body - _v_att->data().yaw); if (yaw_offs < -yaw_offs_max) { - _v_att_sp_mod.yaw_body = _wrap_pi(_v_att->get().yaw - yaw_offs_max); + _v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw - yaw_offs_max); } else if (yaw_offs > yaw_offs_max) { - _v_att_sp_mod.yaw_body = _wrap_pi(_v_att->get().yaw + yaw_offs_max); + _v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw + yaw_offs_max); } - _v_att_sp_mod.R_valid = false; + _v_att_sp_mod.data().R_valid = false; // _publish_att_sp = true; } /* reset yaw setpint to current position if needed */ if (_reset_yaw_sp) { _reset_yaw_sp = false; - _v_att_sp_mod.yaw_body = _v_att->get().yaw; - _v_att_sp_mod.R_valid = false; + _v_att_sp_mod.data().yaw_body = _v_att->data().yaw; + _v_att_sp_mod.data().R_valid = false; // _publish_att_sp = true; } - if (!_v_control_mode->get().flag_control_velocity_enabled) { + if (!_v_control_mode->data().flag_control_velocity_enabled) { /* update attitude setpoint if not in position control mode */ - _v_att_sp_mod.roll_body = _manual_control_sp->get().y * _params.man_roll_max; - _v_att_sp_mod.pitch_body = -_manual_control_sp->get().x + _v_att_sp_mod.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max; + _v_att_sp_mod.data().pitch_body = -_manual_control_sp->data().x * _params.man_pitch_max; - _v_att_sp_mod.R_valid = false; + _v_att_sp_mod.data().R_valid = false; // _publish_att_sp = true; } } else { /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ - memcpy(&_v_att_sp_mod, _v_att_sp->get_void_ptr(), sizeof(_v_att_sp_mod)); + memcpy(&_v_att_sp_mod, &_v_att_sp->data(), sizeof(_v_att_sp_mod)); /* reset yaw setpoint after non-manual control mode */ _reset_yaw_sp = true; } - _thrust_sp = _v_att_sp_mod.thrust; + _thrust_sp = _v_att_sp_mod.data().thrust; /* construct attitude setpoint rotation matrix */ math::Matrix<3, 3> R_sp; - if (_v_att_sp_mod.R_valid) { + if (_v_att_sp_mod.data().R_valid) { /* rotation matrix in _att_sp is valid, use it */ - R_sp.set(&_v_att_sp_mod.R_body[0]); + R_sp.set(&_v_att_sp_mod.data().R_body[0]); } else { /* rotation matrix in _att_sp is not valid, use euler angles instead */ - R_sp.from_euler(_v_att_sp_mod.roll_body, _v_att_sp_mod.pitch_body, - _v_att_sp_mod.yaw_body); + R_sp.from_euler(_v_att_sp_mod.data().roll_body, _v_att_sp_mod.data().pitch_body, + _v_att_sp_mod.data().yaw_body); /* copy rotation matrix back to setpoint struct */ - memcpy(&_v_att_sp_mod.R_body[0], &R_sp.data[0][0], - sizeof(_v_att_sp_mod.R_body)); - _v_att_sp_mod.R_valid = true; + memcpy(&_v_att_sp_mod.data().R_body[0], &R_sp.data[0][0], + sizeof(_v_att_sp_mod.data().R_body)); + _v_att_sp_mod.data().R_valid = true; } /* rotation matrix for current state */ math::Matrix<3, 3> R; - R.set(_v_att->get().R); + R.set(_v_att->data().R); /* all input data is ready, run controller itself */ @@ -266,15 +266,15 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { /* reset integral if disarmed */ - if (!_armed->get().armed || !_v_status->get().is_rotary_wing) { + if (!_armed->data().armed || !_v_status->data().is_rotary_wing) { _rates_int.zero(); } /* current body angular rates */ math::Vector < 3 > rates; - rates(0) = _v_att->get().rollspeed; - rates(1) = _v_att->get().pitchspeed; - rates(2) = _v_att->get().yawspeed; + rates(0) = _v_att->data().rollspeed; + rates(1) = _v_att->data().pitchspeed; + rates(2) = _v_att->data().yawspeed; /* angular rates error */ math::Vector < 3 > rates_err = _rates_sp - rates; @@ -302,8 +302,8 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) void MulticopterAttitudeControlBase::set_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.data().control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.data().control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.data().control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.data().control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; } diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.h b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h index cf4c726a7..124147079 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h @@ -62,6 +62,8 @@ #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f +using namespace px4; + class MulticopterAttitudeControlBase { public: @@ -86,20 +88,20 @@ public: void set_actuator_controls(); protected: - px4::PX4_SUBSCRIBER(vehicle_attitude) *_v_att; /**< vehicle attitude */ - px4::PX4_SUBSCRIBER(vehicle_attitude_setpoint) *_v_att_sp; /**< vehicle attitude setpoint */ - px4::PX4_SUBSCRIBER(vehicle_rates_setpoint) *_v_rates_sp; /**< vehicle rates setpoint */ - px4::PX4_SUBSCRIBER(vehicle_control_mode) *_v_control_mode; /**< vehicle control mode */ - px4::PX4_SUBSCRIBER(parameter_update) *_parameter_update; /**< parameter update */ - px4::PX4_SUBSCRIBER(manual_control_setpoint) *_manual_control_sp; /**< manual control setpoint */ - px4::PX4_SUBSCRIBER(actuator_armed) *_armed; /**< actuator arming status */ - px4::PX4_SUBSCRIBER(vehicle_status) *_v_status; /**< vehicle status */ - - PX4_TOPIC_T(vehicle_attitude_setpoint) _v_att_sp_mod; /**< modified vehicle attitude setpoint + px4::Subscriber *_v_att; /**< vehicle attitude */ + px4::Subscriber *_v_att_sp; /**< vehicle attitude setpoint */ + px4::Subscriber *_v_rates_sp; /**< vehicle rates setpoint */ + px4::Subscriber *_v_control_mode; /**< vehicle control mode */ + px4::Subscriber *_parameter_update; /**< parameter update */ + px4::Subscriber *_manual_control_sp; /**< manual control setpoint */ + px4::Subscriber *_armed; /**< actuator arming status */ + px4::Subscriber *_v_status; /**< vehicle status */ + + px4_vehicle_attitude_setpoint _v_att_sp_mod; /**< modified vehicle attitude setpoint that gets published eventually */ - PX4_TOPIC_T(vehicle_rates_setpoint) _v_rates_sp_mod; /**< vehicle rates setpoint + px4_vehicle_rates_setpoint _v_rates_sp_mod; /**< vehicle rates setpoint that gets published eventually*/ - PX4_TOPIC_T(actuator_controls) _actuators; /**< actuator controls */ + px4_actuator_controls_0 _actuators; /**< actuator controls */ math::Vector<3> _rates_prev; /**< angular rates on previous step */ math::Vector<3> _rates_sp; /**< angular rates setpoint */ diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp index 67ebe64cd..b356b5dc0 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp @@ -52,11 +52,9 @@ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. */ -#include -#include #include "mc_att_control.h" -static bool thread_running = false; /**< Deamon status flag */ +bool thread_running = false; /**< Deamon status flag */ static int daemon_task; /**< Handle of deamon task / thread */ namespace px4 { diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp new file mode 100644 index 000000000..e982ae354 --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_att_control_m_start_nuttx.cpp + * + * @author Thomas Gubler + */ +#include +#include +#include +#include + +extern bool thread_running; +int daemon_task; /**< Handle of deamon task / thread */ +namespace px4 +{ +bool task_should_exit = false; +} +using namespace px4; + +extern int main(int argc, char **argv); + +extern "C" __EXPORT int mc_att_control_m_main(int argc, char *argv[]); +int mc_att_control_m_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "usage: mc_att_control_m {start|stop|status}"); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); + } + + task_should_exit = false; + + daemon_task = task_spawn_cmd("mc_att_control_m", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + main, + (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + task_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); + + } else { + warnx("not started"); + } + + exit(0); + } + + warnx("unrecognized command"); + return 1; +} -- cgit v1.2.3