From 5876ff11ec90ca8dee22ee0509ffaec2b561d2fd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 Jan 2015 16:12:15 +0100 Subject: mc att control multiplatform alongside normal mc att control --- .../mc_att_control_main.cpp | 139 +++++++++++++++++++++ 1 file changed, 139 insertions(+) create mode 100644 src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp (limited to 'src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp') 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 new file mode 100644 index 000000000..69f63a5b1 --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp @@ -0,0 +1,139 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 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_main.cpp + * Multicopter attitude controller. + * + * @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * @author Thomas Gubler + * @author Julian Oes + * @author Roman Bapst + * + * The controller has two loops: P loop for angular error and PD loop for angular rate error. + * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch. + * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw, + * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis. + * These two approaches fused seamlessly with weight depending on angular error. + * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity. + * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. + * 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 */ +static int daemon_task; /**< Handle of deamon task / thread */ +namespace px4 +{ +bool task_should_exit = false; +} + +using namespace px4; + +PX4_MAIN_FUNCTION(mc_att_control_multiplatform); + +#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__)) +/** + * Multicopter attitude control app start / stop handling function + * + * @ingroup apps + */ + +extern "C" __EXPORT int mc_att_control_multiplatform_main(int argc, char *argv[]); +int mc_att_control_multiplatform_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "usage: mc_att_control {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", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 3000, + mc_att_control_multiplatform_task_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; +} +#endif + +PX4_MAIN_FUNCTION(mc_att_control_multiplatform) +{ + px4::init(argc, argv, "mc_att_control_multiplatform"); + + PX4_INFO("starting"); + MulticopterAttitudeControl attctl; + thread_running = true; + attctl.spin(); + + PX4_INFO("exiting."); + thread_running = false; + return 0; +} + + -- cgit v1.2.3 From f37fdd95af06d7b6937cb53d34ea8777342b3aef Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 Jan 2015 19:45:57 +0100 Subject: add and use PX4_ROS preprocessor define --- CMakeLists.txt | 1 + src/examples/publisher/publisher_main.cpp | 2 +- src/examples/subscriber/subscriber_main.cpp | 2 +- src/lib/mathlib/math/Matrix.hpp | 2 +- src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp | 2 +- src/platforms/px4_defines.h | 2 +- src/platforms/px4_includes.h | 2 +- src/platforms/px4_middleware.h | 4 ++-- src/platforms/px4_nodehandle.h | 4 ++-- src/platforms/px4_publisher.h | 4 ++-- src/platforms/px4_subscriber.h | 8 +++++--- 11 files changed, 18 insertions(+), 15 deletions(-) (limited to 'src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp') diff --git a/CMakeLists.txt b/CMakeLists.txt index 95a6f12fe..eca28b15e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(px4) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +add_definitions(-D__PX4_ROS) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp index 5cac42250..81439a8be 100644 --- a/src/examples/publisher/publisher_main.cpp +++ b/src/examples/publisher/publisher_main.cpp @@ -51,7 +51,7 @@ using namespace px4; PX4_MAIN_FUNCTION(publisher); -#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__)) +#if !defined(__PX4_ROS) extern "C" __EXPORT int publisher_main(int argc, char *argv[]); int publisher_main(int argc, char *argv[]) { diff --git a/src/examples/subscriber/subscriber_main.cpp b/src/examples/subscriber/subscriber_main.cpp index c824e34e3..906921e01 100644 --- a/src/examples/subscriber/subscriber_main.cpp +++ b/src/examples/subscriber/subscriber_main.cpp @@ -51,7 +51,7 @@ using namespace px4; PX4_MAIN_FUNCTION(subscriber); -#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__)) +#if !defined(__PX4_ROS) extern "C" __EXPORT int subscriber_main(int argc, char *argv[]); int subscriber_main(int argc, char *argv[]) { diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 43ba6b9d9..68f0a777d 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -122,7 +122,7 @@ public: memcpy(data, d, sizeof(data)); } -#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +#if defined(__PX4_ROS) /** * set data from boost::array */ 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 69f63a5b1..e2f2f2530 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 @@ -67,7 +67,7 @@ using namespace px4; PX4_MAIN_FUNCTION(mc_att_control_multiplatform); -#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__)) +#if !defined(__PX4_ROS) /** * Multicopter attitude control app start / stop handling function * diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index ef43c55b8..457b5abe3 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -46,7 +46,7 @@ #define PX4_PARAM_DEFINE_FLOAT(_name) PARAM_DEFINE_FLOAT(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) -#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +#if defined(__PX4_ROS) /* * Building for running within the ROS environment */ diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 4d29bc3cd..fc31162c8 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -41,7 +41,7 @@ #include -#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +#if defined(__PX4_ROS) /* * Building for running within the ROS environment */ diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index 54050de8b..cd52fd650 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -42,7 +42,7 @@ #include #include -#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +#if defined(__PX4_ROS) #define __EXPORT #endif @@ -53,7 +53,7 @@ __EXPORT void init(int argc, char *argv[], const char *process_name); __EXPORT uint64_t get_time_micros(); -#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +#if defined(__PX4_ROS) /** * Returns true if the app/task should continue to run */ diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 3c2bb2d44..624a466fd 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -43,7 +43,7 @@ #include "px4_publisher.h" #include "px4_middleware.h" -#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +#if defined(__PX4_ROS) /* includes when building for ros */ #include "ros/ros.h" #include @@ -55,7 +55,7 @@ namespace px4 { -#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +#if defined(__PX4_ROS) class NodeHandle : private ros::NodeHandle { diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index 6b6d8e39e..c6f3d6108 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -37,7 +37,7 @@ * PX4 Middleware Wrapper Node Handle */ #pragma once -#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +#if defined(__PX4_ROS) /* includes when building for ros */ #include "ros/ros.h" #else @@ -60,7 +60,7 @@ public: }; -#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +#if defined(__PX4_ROS) class Publisher : public PublisherBase { diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 4b6d5b58f..107c60189 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -36,9 +36,11 @@ * * PX4 Middleware Wrapper Subscriber */ -#include #pragma once -#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) + +#include + +#if defined(__PX4_ROS) /* includes when building for ros */ #include "ros/ros.h" #else @@ -87,7 +89,7 @@ public: virtual void * get_void_ptr() = 0; }; -#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +#if defined(__PX4_ROS) /** * Subscriber class that is templated with the ros n message type */ -- cgit v1.2.3 From c2cc247e76de53664a3826f04d251f69df6a8fab Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 9 Jan 2015 09:18:33 +0100 Subject: renamed mc_att_control_multiplatform to mc_att_control_m --- .../mc_att_control_multiplatform/mc_att_control_main.cpp | 16 ++++++++-------- src/modules/mc_att_control_multiplatform/module.mk | 2 +- 2 files changed, 9 insertions(+), 9 deletions(-) (limited to 'src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp') 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 e2f2f2530..67ebe64cd 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 @@ -65,7 +65,7 @@ bool task_should_exit = false; using namespace px4; -PX4_MAIN_FUNCTION(mc_att_control_multiplatform); +PX4_MAIN_FUNCTION(mc_att_control_m); #if !defined(__PX4_ROS) /** @@ -74,11 +74,11 @@ PX4_MAIN_FUNCTION(mc_att_control_multiplatform); * @ingroup apps */ -extern "C" __EXPORT int mc_att_control_multiplatform_main(int argc, char *argv[]); -int mc_att_control_multiplatform_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 {start|stop|status}"); + errx(1, "usage: mc_att_control_m {start|stop|status}"); } if (!strcmp(argv[1], "start")) { @@ -91,11 +91,11 @@ int mc_att_control_multiplatform_main(int argc, char *argv[]) task_should_exit = false; - daemon_task = task_spawn_cmd("mc_att_control", + daemon_task = task_spawn_cmd("mc_att_control_m", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 3000, - mc_att_control_multiplatform_task_main, + mc_att_control_m_task_main, (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); @@ -122,9 +122,9 @@ int mc_att_control_multiplatform_main(int argc, char *argv[]) } #endif -PX4_MAIN_FUNCTION(mc_att_control_multiplatform) +PX4_MAIN_FUNCTION(mc_att_control_m) { - px4::init(argc, argv, "mc_att_control_multiplatform"); + px4::init(argc, argv, "mc_att_control_m"); PX4_INFO("starting"); MulticopterAttitudeControl attctl; diff --git a/src/modules/mc_att_control_multiplatform/module.mk b/src/modules/mc_att_control_multiplatform/module.mk index 7925b8345..c61ba18b4 100644 --- a/src/modules/mc_att_control_multiplatform/module.mk +++ b/src/modules/mc_att_control_multiplatform/module.mk @@ -35,7 +35,7 @@ # Multirotor attitude controller (vector based, no Euler singularities) # -MODULE_COMMAND = mc_att_control_multiplatform +MODULE_COMMAND = mc_att_control_m SRCS = mc_att_control_main.cpp \ mc_att_control.cpp \ -- cgit v1.2.3 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/mc_att_control_main.cpp') 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 From 092a3c5129e4d26463b298d110dbfa8683fb9c26 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 18:23:46 +0100 Subject: add start wrapper for mc att ctl multip --- .../mc_att_control_main.cpp | 66 +--------------------- src/modules/mc_att_control_multiplatform/module.mk | 1 + 2 files changed, 2 insertions(+), 65 deletions(-) (limited to 'src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp') 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 b356b5dc0..5a79a8c6b 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 @@ -55,72 +55,8 @@ #include "mc_att_control.h" bool thread_running = false; /**< Deamon status flag */ -static int daemon_task; /**< Handle of deamon task / thread */ -namespace px4 -{ -bool task_should_exit = false; -} - -using namespace px4; - -PX4_MAIN_FUNCTION(mc_att_control_m); - -#if !defined(__PX4_ROS) -/** - * Multicopter attitude control app start / stop handling function - * - * @ingroup apps - */ - -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, - 3000, - mc_att_control_m_task_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; -} -#endif -PX4_MAIN_FUNCTION(mc_att_control_m) +int main(int argc, char **argv) { px4::init(argc, argv, "mc_att_control_m"); diff --git a/src/modules/mc_att_control_multiplatform/module.mk b/src/modules/mc_att_control_multiplatform/module.mk index c61ba18b4..959f6492b 100644 --- a/src/modules/mc_att_control_multiplatform/module.mk +++ b/src/modules/mc_att_control_multiplatform/module.mk @@ -38,6 +38,7 @@ MODULE_COMMAND = mc_att_control_m SRCS = mc_att_control_main.cpp \ + mc_att_control_start_nuttx.cpp \ mc_att_control.cpp \ mc_att_control_base.cpp \ mc_att_control_params.c -- cgit v1.2.3