From c4ed97f3c16311601b4983360d51f662729e81d4 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 5 Jun 2014 07:06:48 -0400 Subject: Added uORB tiny pub/sub for usage without struct on stack. --- src/drivers/roboclaw/RoboClaw.cpp | 2 +- src/modules/controllib/block/Block.cpp | 4 +- src/modules/controllib/block/Block.hpp | 12 +- src/modules/controllib/uorb/blocks.cpp | 18 +- src/modules/mc_att_control/mc_att_control_main.cpp | 207 ++++----------------- src/modules/uORB/Publication.cpp | 6 +- src/modules/uORB/Publication.hpp | 96 ++++++++-- src/modules/uORB/Subscription.cpp | 16 +- src/modules/uORB/Subscription.hpp | 110 ++++++++--- 9 files changed, 232 insertions(+), 239 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index dd5e4d3e0..280f6c831 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -65,7 +65,7 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address, _pulsesPerRev(pulsesPerRev), _uart(0), _controlPoll(), - _actuators(NULL, ORB_ID(actuator_controls_0), 20), + _actuators(ORB_ID(actuator_controls_0), 20), _motor1Position(0), _motor1Speed(0), _motor1Overflow(0), diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp index 6768bfa7e..f3cd87728 100644 --- a/src/modules/controllib/block/Block.cpp +++ b/src/modules/controllib/block/Block.cpp @@ -101,7 +101,7 @@ void Block::updateParams() void Block::updateSubscriptions() { - uORB::SubscriptionBase *sub = getSubscriptions().getHead(); + uORB::SubscriptionNode *sub = getSubscriptions().getHead(); int count = 0; while (sub != NULL) { @@ -119,7 +119,7 @@ void Block::updateSubscriptions() void Block::updatePublications() { - uORB::PublicationBase *pub = getPublications().getHead(); + uORB::PublicationNode *pub = getPublications().getHead(); int count = 0; while (pub != NULL) { diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp index 736698e21..42ca3a742 100644 --- a/src/modules/controllib/block/Block.hpp +++ b/src/modules/controllib/block/Block.hpp @@ -46,8 +46,8 @@ // forward declaration namespace uORB { - class SubscriptionBase; - class PublicationBase; + class SubscriptionNode; + class PublicationNode; } namespace control @@ -83,15 +83,15 @@ public: protected: // accessors SuperBlock *getParent() { return _parent; } - List & getSubscriptions() { return _subscriptions; } - List & getPublications() { return _publications; } + List & getSubscriptions() { return _subscriptions; } + List & getPublications() { return _publications; } List & getParams() { return _params; } // attributes const char *_name; SuperBlock *_parent; float _dt; - List _subscriptions; - List _publications; + List _subscriptions; + List _publications; List _params; }; diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index e8fecef0d..454d0db19 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -82,16 +82,16 @@ void BlockWaypointGuidance::update(vehicle_global_position_s &pos, BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) : SuperBlock(parent, name), // subscriptions - _att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20), - _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20), - _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20), - _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20), - _missionCmd(&getSubscriptions(), ORB_ID(position_setpoint_triplet), 20), - _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20), - _status(&getSubscriptions(), ORB_ID(vehicle_status), 20), - _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz + _att(ORB_ID(vehicle_attitude), 20, &getSubscriptions()), + _attCmd(ORB_ID(vehicle_attitude_setpoint), 20, &getSubscriptions()), + _ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, &getSubscriptions()), + _pos(ORB_ID(vehicle_global_position), 20, &getSubscriptions()), + _missionCmd(ORB_ID(position_setpoint_triplet), 20, &getSubscriptions()), + _manual(ORB_ID(manual_control_setpoint), 20, &getSubscriptions()), + _status(ORB_ID(vehicle_status), 20, &getSubscriptions()), + _param_update(ORB_ID(parameter_update), 1000, &getSubscriptions()), // limit to 1 Hz // publications - _actuators(&getPublications(), ORB_ID(actuator_controls_0)) + _actuators(ORB_ID(actuator_controls_0), &getPublications()) { } 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 20e016da3..b024589a6 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -60,6 +60,8 @@ #include #include #include +#include +#include #include #include #include @@ -111,17 +113,17 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ - int _v_att_sub; /**< vehicle attitude subscription */ - int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ - int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */ - int _v_control_mode_sub; /**< vehicle control mode subscription */ - int _params_sub; /**< parameter updates subscription */ - int _manual_control_sp_sub; /**< manual control setpoint subscription */ - int _armed_sub; /**< arming status subscription */ + uORB::SubscriptionTiny _v_att_sub; /**< vehicle attitude subscription */ + uORB::SubscriptionTiny _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ + uORB::SubscriptionTiny _v_rates_sp_sub; /**< vehicle rates setpoint subscription */ + uORB::SubscriptionTiny _v_control_mode_sub; /**< vehicle control mode subscription */ + uORB::SubscriptionTiny _params_sub; /**< parameter updates subscription */ + uORB::SubscriptionTiny _manual_control_sp_sub; /**< manual control setpoint subscription */ + uORB::SubscriptionTiny _armed_sub; /**< arming status subscription */ - orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ - orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ - orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ + uORB::PublicationTiny _att_sp_pub; /**< attitude setpoint publication */ + uORB::PublicationTiny _v_rates_sp_pub; /**< rate setpoint publication */ + uORB::PublicationTiny _actuators_0_pub; /**< attitude actuator controls publication */ struct vehicle_attitude_s _v_att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ @@ -186,36 +188,6 @@ private: */ int parameters_update(); - /** - * Check for parameter update and handle it. - */ - void parameter_update_poll(); - - /** - * Check for changes in vehicle control mode. - */ - void vehicle_control_mode_poll(); - - /** - * Check for changes in manual inputs. - */ - void vehicle_manual_poll(); - - /** - * Check for attitude setpoint updates. - */ - void vehicle_attitude_setpoint_poll(); - - /** - * Check for rates setpoint updates. - */ - void vehicle_rates_setpoint_poll(); - - /** - * Check for arming status updates. - */ - void arming_status_poll(); - /** * Attitude controller. */ @@ -255,17 +227,18 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _control_task(-1), /* subscriptions */ - _v_att_sub(-1), - _v_att_sp_sub(-1), - _v_control_mode_sub(-1), - _params_sub(-1), - _manual_control_sp_sub(-1), - _armed_sub(-1), + _v_att_sub(ORB_ID(vehicle_attitude)), + _v_att_sp_sub(ORB_ID(vehicle_attitude_setpoint)), + _v_rates_sp_sub(ORB_ID(vehicle_rates_setpoint)), + _v_control_mode_sub(ORB_ID(vehicle_control_mode)), + _params_sub(ORB_ID(parameter_update)), + _manual_control_sp_sub(ORB_ID(manual_control_setpoint)), + _armed_sub(ORB_ID(actuator_armed)), /* publications */ - _att_sp_pub(-1), - _v_rates_sp_pub(-1), - _actuators_0_pub(-1), + _att_sp_pub(ORB_ID(vehicle_attitude_setpoint)), + _v_rates_sp_pub(ORB_ID(vehicle_rates_setpoint)), + _actuators_0_pub(ORB_ID_VEHICLE_ATTITUDE_CONTROLS), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) @@ -405,83 +378,6 @@ MulticopterAttitudeControl::parameters_update() return OK; } -void -MulticopterAttitudeControl::parameter_update_poll() -{ - bool updated; - - /* Check HIL state if vehicle status has changed */ - orb_check(_params_sub, &updated); - - if (updated) { - struct parameter_update_s param_update; - orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); - parameters_update(); - } -} - -void -MulticopterAttitudeControl::vehicle_control_mode_poll() -{ - bool updated; - - /* Check HIL state if vehicle status has changed */ - orb_check(_v_control_mode_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); - } -} - -void -MulticopterAttitudeControl::vehicle_manual_poll() -{ - bool updated; - - /* get pilots inputs */ - orb_check(_manual_control_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); - } -} - -void -MulticopterAttitudeControl::vehicle_attitude_setpoint_poll() -{ - /* check if there is a new setpoint */ - bool updated; - orb_check(_v_att_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp); - } -} - -void -MulticopterAttitudeControl::vehicle_rates_setpoint_poll() -{ - /* check if there is a new setpoint */ - bool updated; - orb_check(_v_rates_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp); - } -} - -void -MulticopterAttitudeControl::arming_status_poll() -{ - /* check if there is a new setpoint */ - bool updated; - orb_check(_armed_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - } -} - /* * Attitude controller. * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode) @@ -498,7 +394,7 @@ MulticopterAttitudeControl::control_attitude(float dt) if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) { /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ - vehicle_attitude_setpoint_poll(); + (void) _v_att_sp_sub.update(&_v_att_sp); } if (!_v_control_mode.flag_control_climb_rate_enabled) { @@ -553,7 +449,7 @@ MulticopterAttitudeControl::control_attitude(float dt) } else { /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ - vehicle_attitude_setpoint_poll(); + (void) _v_att_sp_sub.update(&_v_att_sp); /* reset yaw setpoint after non-manual control mode */ _reset_yaw_sp = true; @@ -581,12 +477,7 @@ MulticopterAttitudeControl::control_attitude(float dt) 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); - - } else { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); - } + _att_sp_pub.update(&_v_att_sp); } /* rotation matrix for current state */ @@ -717,24 +608,13 @@ MulticopterAttitudeControl::task_main() warnx("started"); fflush(stdout); - /* - * do subscriptions - */ - _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - _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]; - fds[0].fd = _v_att_sub; + fds[0].fd = _v_att_sub.getHandle(); fds[0].events = POLLIN; while (!_task_should_exit) { @@ -771,13 +651,15 @@ MulticopterAttitudeControl::task_main() } /* copy attitude topic */ - orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); + _v_att_sub.update(&_v_att); /* check for updates in other topics */ - parameter_update_poll(); - vehicle_control_mode_poll(); - arming_status_poll(); - vehicle_manual_poll(); + if (_params_sub.updated()) { + parameters_update(); + } + (void) _v_control_mode_sub.update(&_v_control_mode); + (void) _armed_sub.update(&_armed); + (void) _manual_control_sp_sub.update(&_manual_control_sp); if (_v_control_mode.flag_control_attitude_enabled) { control_attitude(dt); @@ -789,12 +671,7 @@ MulticopterAttitudeControl::task_main() _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); - } + _v_rates_sp_pub.update(&_v_rates_sp); } else { /* attitude controller disabled, poll rates setpoint topic */ @@ -813,16 +690,11 @@ MulticopterAttitudeControl::task_main() _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); - } + _v_rates_sp_pub.update(&_v_rates_sp); } else { /* attitude controller disabled, poll rates setpoint topic */ - vehicle_rates_setpoint_poll(); + _v_rates_sp_sub.update(&_v_rates_sp); _rates_sp(0) = _v_rates_sp.roll; _rates_sp(1) = _v_rates_sp.pitch; _rates_sp(2) = _v_rates_sp.yaw; @@ -840,12 +712,7 @@ MulticopterAttitudeControl::task_main() _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); - - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); - } + _actuators_0_pub.update(&_actuators); } } diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index 5a5981617..e9398a625 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -52,10 +52,10 @@ namespace uORB { template Publication::Publication( - List * list, - const struct orb_metadata *meta) : + const struct orb_metadata *meta, + List * list) : T(), // initialize data structure to zero - PublicationBase(list, meta) { + PublicationNode(meta, list) { } template diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index 8650b3df8..1c48929e4 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -38,6 +38,8 @@ #pragma once +#include + #include #include @@ -49,55 +51,112 @@ namespace uORB * Base publication warapper class, used in list traversal * of various publications. */ -class __EXPORT PublicationBase : public ListNode +class __EXPORT PublicationBase { public: - PublicationBase( - List * list, - const struct orb_metadata *meta) : + /** + * Constructor + * + * + * @param meta The uORB metadata (usually from the ORB_ID() + * macro) for the topic. + */ + PublicationBase(const struct orb_metadata *meta) : _meta(meta), _handle(-1) { - if (list != NULL) list->add(this); } - void update() { + + /** + * Update the struct + * @param data The uORB message struct we are updating. + */ + void update(void * data) { if (_handle > 0) { - orb_publish(getMeta(), getHandle(), getDataVoidPtr()); + orb_publish(getMeta(), getHandle(), data); } else { - setHandle(orb_advertise(getMeta(), getDataVoidPtr())); + setHandle(orb_advertise(getMeta(), data)); } } - virtual void *getDataVoidPtr() = 0; + + /** + * Deconstructor + */ virtual ~PublicationBase() { orb_unsubscribe(getHandle()); } +// accessors const struct orb_metadata *getMeta() { return _meta; } int getHandle() { return _handle; } protected: +// accessors void setHandle(orb_advert_t handle) { _handle = handle; } +// attributes const struct orb_metadata *_meta; orb_advert_t _handle; }; +/** + * alias class name so it is clear that the base class + * can be used by itself if desired + */ +typedef PublicationBase PublicationTiny; + +/** + * The publication base class as a list node. + */ +class __EXPORT PublicationNode : + public PublicationBase, + public ListNode +{ +public: + /** + * Constructor + * + * + * @param meta The uORB metadata (usually from the ORB_ID() + * macro) for the topic. + * @param list A pointer to a list of subscriptions + * that this should be appended to. + */ + PublicationNode(const struct orb_metadata *meta, + List * list=nullptr) : + PublicationBase(meta) { + if (list != nullptr) list->add(this); + } + + /** + * This function is the callback for list traversal + * updates, a child class must implement it. + */ + virtual void update() = 0; +}; + /** * Publication wrapper class */ template class Publication : public T, // this must be first! - public PublicationBase + public PublicationNode { public: /** * Constructor * - * @param list A list interface for adding to list during construction - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. + * @param meta The uORB metadata (usually from + * the ORB_ID() macro) for the topic. + * @param list A list interface for adding to + * list during construction */ - Publication(List * list, - const struct orb_metadata *meta); + Publication(const struct orb_metadata *meta, + List * list=nullptr); + + /** + * Deconstructor + **/ virtual ~Publication(); + /* * XXX * This function gets the T struct, assuming @@ -106,6 +165,13 @@ public: * seem to be available */ void *getDataVoidPtr(); + + /** + * Create an update function that uses the embedded struct. + */ + void update() { + PublicationBase::update(getDataVoidPtr()); + } }; } // namespace uORB diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index c1d1a938f..1a4867c85 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -55,21 +55,13 @@ namespace uORB { -bool __EXPORT SubscriptionBase::updated() -{ - bool isUpdated = false; - orb_check(_handle, &isUpdated); - return isUpdated; -} - template Subscription::Subscription( - List * list, - const struct orb_metadata *meta, unsigned interval) : + const struct orb_metadata *meta, + unsigned interval, + List * list) : T(), // initialize data structure to zero - SubscriptionBase(list, meta) { - setHandle(orb_subscribe(getMeta())); - orb_set_interval(getHandle(), interval); + SubscriptionNode(meta, interval, list) { } template diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 34e9a83e0..bab1ef2ea 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -38,10 +38,11 @@ #pragma once +#include + #include #include - namespace uORB { @@ -49,8 +50,7 @@ namespace uORB * Base subscription warapper class, used in list traversal * of various subscriptions. */ -class __EXPORT SubscriptionBase : - public ListNode +class __EXPORT SubscriptionBase { public: // methods @@ -58,23 +58,42 @@ public: /** * Constructor * - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. + * @param meta The uORB metadata (usually from the ORB_ID() + * macro) for the topic. + * + * @param interval The minimum interval in milliseconds + * between updates */ - SubscriptionBase( - List * list, - const struct orb_metadata *meta) : + SubscriptionBase(const struct orb_metadata *meta, + unsigned interval=0) : _meta(meta), _handle() { - if (list != NULL) list->add(this); + setHandle(orb_subscribe(getMeta())); + orb_set_interval(getHandle(), interval); } - bool updated(); - void update() { + + /** + * Check if there is a new update. + * */ + bool updated() { + bool isUpdated = false; + orb_check(_handle, &isUpdated); + return isUpdated; + } + + /** + * Update the struct + * @param data The uORB message struct we are updating. + */ + void update(void * data) { if (updated()) { - orb_copy(_meta, _handle, getDataVoidPtr()); + orb_copy(_meta, _handle, data); } } - virtual void *getDataVoidPtr() = 0; + + /** + * Deconstructor + */ virtual ~SubscriptionBase() { orb_unsubscribe(_handle); } @@ -89,31 +108,80 @@ protected: int _handle; }; +/** + * alias class name so it is clear that the base class + */ +typedef SubscriptionBase SubscriptionTiny; + +/** + * The publication base class as a list node. + */ +class __EXPORT SubscriptionNode : + + public SubscriptionBase, + public ListNode +{ +public: + /** + * Constructor + * + * + * @param meta The uORB metadata (usually from the ORB_ID() + * macro) for the topic. + * @param interval The minimum interval in milliseconds + * between updates + * @param list A pointer to a list of subscriptions + * that this should be appended to. + */ + SubscriptionNode(const struct orb_metadata *meta, + unsigned interval=0, + List * list=nullptr) : + SubscriptionBase(meta, interval) { + if (list != nullptr) list->add(this); + } + + /** + * This function is the callback for list traversal + * updates, a child class must implement it. + */ + virtual void update() = 0; +}; + /** * Subscription wrapper class */ template class __EXPORT Subscription : public T, // this must be first! - public SubscriptionBase + public SubscriptionNode { public: /** * Constructor * - * @param list A list interface for adding to list during construction - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. - * @param interval The minimum interval in milliseconds between updates + * @param meta The uORB metadata (usually from + * the ORB_ID() macro) for the topic. + * @param interval The minimum interval in milliseconds + * between updates + * @param list A list interface for adding to + * list during construction */ - Subscription( - List * list, - const struct orb_metadata *meta, unsigned interval); + Subscription(const struct orb_metadata *meta, + unsigned interval=0, + List * list=nullptr); /** * Deconstructor */ virtual ~Subscription(); + + /** + * Create an update function that uses the embedded struct. + */ + void update() { + SubscriptionBase::update(getDataVoidPtr()); + } + /* * XXX * This function gets the T struct, assuming -- cgit v1.2.3 From cedfdfca6018c7c35a56e2370b84f0d4f589cf68 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Fri, 22 Aug 2014 16:00:34 +0200 Subject: some hacks for the quadshot --- ROMFS/px4fmu_common/init.d/13000_quadshot | 14 ++++++++++++++ ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf | 12 ++++++++++++ ROMFS/px4fmu_common/init.d/rc.autostart | 9 +++++++++ ROMFS/px4fmu_common/init.d/rcS | 1 + 4 files changed, 36 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/13000_quadshot create mode 100644 ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf diff --git a/ROMFS/px4fmu_common/init.d/13000_quadshot b/ROMFS/px4fmu_common/init.d/13000_quadshot new file mode 100644 index 000000000..8ee306a38 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/13000_quadshot @@ -0,0 +1,14 @@ +#!nsh +# +# Generic quadshot configuration file +# +# Roman Bapst +# + +sh /etc/init.d/rc.mc_defaults + +set MIXER FMU_quadshot + +set PWM_OUTPUTS 1234 +set PWM_MIN 1070 +set PWM_MAX 2000 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf b/ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf new file mode 100644 index 000000000..d26e4a372 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf @@ -0,0 +1,12 @@ +#!nsh +# Configure stream for Mavlink instance on TELEM2 because it is annoying always removing the SDcard +# +#usleep 100000 +#mavlink stream -d /dev/ttyS2 -s ATTITUDE_CONTROLS -r 50 +#usleep 100000 +#mavlink stream -d /dev/ttyS2 -s RC_CHANNELS_RAW -r 50 +#usleep 100000 +#mavlink stream -d /dev/ttyS2 -s VFR_HUD -r 50 +usleep 100000 +mavlink stream -d /dev/ttyS2 -s MANUAL_CONTROL -r 50 +echo "Added additional streams on TELEM2" \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 9de7d9ecd..4f0ec0278 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -16,6 +16,7 @@ # 10000 .. 10999 Wide arm / H frame # 11000 .. 11999 Hexa Cox # 12000 .. 12999 Octo Cox +# 13000 .. 13999 Vtol # # Simulation setups @@ -232,3 +233,11 @@ if param compare SYS_AUTOSTART 12001 then sh /etc/init.d/12001_octo_cox fi + +# +# Quadshot +# + if param compare SYS_AUTOSTART 13000 + then + sh /etc/init.d/13000_quadshot + fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c9e6a27ca..cd1aa14b0 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -586,6 +586,7 @@ then then echo "[init] Starting addons script: $EXTRAS_FILE" sh $EXTRAS_FILE + sh /etc/init.d/Roman_mavlink_stream_conf else echo "[init] No addons script: $EXTRAS_FILE" fi -- cgit v1.2.3 From cce45865d53eab4d298e8a22671f6eb6fb32801c Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 28 Aug 2014 10:57:59 +0200 Subject: NuttX update --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 41fffa0df..088146b90 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 41fffa0df192e4a26d2325c7ac5d9b5d7ba0211c +Subproject commit 088146b90eee5b614ea6386a64dae343a49a5172 -- cgit v1.2.3 From 87259cf89de207b3cb21ab9f44578fd2c21ce119 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 28 Aug 2014 10:58:46 +0200 Subject: added updates --- .catkin_workspace | 1 + build/CATKIN_IGNORE | 0 build/CMakeCache.txt | 450 +++++++++++++++++++++ build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake | 56 +++ build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake | 57 +++ .../2.8.12.2/CMakeDetermineCompilerABI_C.bin | Bin 0 -> 8547 bytes .../2.8.12.2/CMakeDetermineCompilerABI_CXX.bin | Bin 0 -> 8560 bytes build/CMakeFiles/2.8.12.2/CMakeSystem.cmake | 15 + .../2.8.12.2/CompilerIdC/CMakeCCompilerId.c | 389 ++++++++++++++++++ build/CMakeFiles/2.8.12.2/CompilerIdC/a.out | Bin 0 -> 8643 bytes .../2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp | 377 +++++++++++++++++ build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out | Bin 0 -> 8652 bytes build/CMakeFiles/CMakeDirectoryInformation.cmake | 16 + build/CMakeFiles/CMakeError.log | 53 +++ build/CMakeFiles/CMakeOutput.log | 293 ++++++++++++++ build/CMakeFiles/CMakeRuleHashes.txt | 5 + build/CMakeFiles/Makefile.cmake | 150 +++++++ build/CMakeFiles/Makefile2 | 266 ++++++++++++ build/CMakeFiles/TargetDirectories.txt | 6 + .../clean_test_results.dir/DependInfo.cmake | 20 + build/CMakeFiles/clean_test_results.dir/build.make | 66 +++ .../clean_test_results.dir/cmake_clean.cmake | 8 + .../clean_test_results.dir/progress.make | 1 + build/CMakeFiles/cmake.check_cache | 1 + build/CMakeFiles/doxygen.dir/DependInfo.cmake | 20 + build/CMakeFiles/doxygen.dir/build.make | 65 +++ build/CMakeFiles/doxygen.dir/cmake_clean.cmake | 8 + build/CMakeFiles/doxygen.dir/progress.make | 1 + build/CMakeFiles/progress.marks | 1 + build/CMakeFiles/run_tests.dir/DependInfo.cmake | 20 + build/CMakeFiles/run_tests.dir/build.make | 65 +++ build/CMakeFiles/run_tests.dir/cmake_clean.cmake | 8 + build/CMakeFiles/run_tests.dir/progress.make | 1 + build/CMakeFiles/tests.dir/DependInfo.cmake | 20 + build/CMakeFiles/tests.dir/build.make | 65 +++ build/CMakeFiles/tests.dir/cmake_clean.cmake | 8 + build/CMakeFiles/tests.dir/progress.make | 1 + build/CTestTestfile.cmake | 7 + build/Makefile | 262 ++++++++++++ .../catkin/catkin_generated/version/package.cmake | 9 + build/catkin_generated/env_cached.sh | 16 + build/catkin_generated/generate_cached_setup.py | 29 ++ build/catkin_generated/installspace/.rosinstall | 2 + build/catkin_generated/installspace/_setup_util.py | 287 +++++++++++++ build/catkin_generated/installspace/env.sh | 16 + build/catkin_generated/installspace/setup.bash | 8 + build/catkin_generated/installspace/setup.sh | 87 ++++ build/catkin_generated/installspace/setup.zsh | 8 + build/catkin_generated/order_packages.cmake | 10 + build/catkin_generated/order_packages.py | 5 + build/catkin_generated/setup_cached.sh | 20 + .../Project/interrogate_setup_dot_py.py.stamp | 250 ++++++++++++ .../stamps/Project/order_packages.cmake.em.stamp | 56 +++ .../stamps/Project/package.xml.stamp | 37 ++ build/catkin_make.cache | 1 + build/cmake_install.cmake | 140 +++++++ .../CMakeFiles/CMakeDirectoryInformation.cmake | 16 + build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake | 27 ++ build/gtest/CMakeFiles/gtest.dir/build.make | 102 +++++ build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake | 10 + build/gtest/CMakeFiles/gtest.dir/depend.make | 2 + build/gtest/CMakeFiles/gtest.dir/flags.make | 8 + build/gtest/CMakeFiles/gtest.dir/link.txt | 1 + build/gtest/CMakeFiles/gtest.dir/progress.make | 2 + .../CMakeFiles/gtest_main.dir/DependInfo.cmake | 28 ++ build/gtest/CMakeFiles/gtest_main.dir/build.make | 103 +++++ .../CMakeFiles/gtest_main.dir/cmake_clean.cmake | 10 + build/gtest/CMakeFiles/gtest_main.dir/depend.make | 2 + build/gtest/CMakeFiles/gtest_main.dir/flags.make | 8 + build/gtest/CMakeFiles/gtest_main.dir/link.txt | 1 + .../gtest/CMakeFiles/gtest_main.dir/progress.make | 2 + build/gtest/CMakeFiles/progress.marks | 1 + build/gtest/CTestTestfile.cmake | 6 + build/gtest/Makefile | 262 ++++++++++++ build/gtest/cmake_install.cmake | 34 ++ devel/.catkin | 1 + devel/.rosinstall | 2 + devel/_setup_util.py | 287 +++++++++++++ devel/env.sh | 16 + devel/setup.bash | 8 + devel/setup.sh | 87 ++++ devel/setup.zsh | 8 + mavlink/include/mavlink/v1.0 | 2 +- uavcan | 2 +- 84 files changed, 4799 insertions(+), 2 deletions(-) create mode 100644 .catkin_workspace create mode 100644 build/CATKIN_IGNORE create mode 100644 build/CMakeCache.txt create mode 100644 build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake create mode 100644 build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake create mode 100755 build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin create mode 100755 build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin create mode 100644 build/CMakeFiles/2.8.12.2/CMakeSystem.cmake create mode 100644 build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c create mode 100755 build/CMakeFiles/2.8.12.2/CompilerIdC/a.out create mode 100644 build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp create mode 100755 build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out create mode 100644 build/CMakeFiles/CMakeDirectoryInformation.cmake create mode 100644 build/CMakeFiles/CMakeError.log create mode 100644 build/CMakeFiles/CMakeOutput.log create mode 100644 build/CMakeFiles/CMakeRuleHashes.txt create mode 100644 build/CMakeFiles/Makefile.cmake create mode 100644 build/CMakeFiles/Makefile2 create mode 100644 build/CMakeFiles/TargetDirectories.txt create mode 100644 build/CMakeFiles/clean_test_results.dir/DependInfo.cmake create mode 100644 build/CMakeFiles/clean_test_results.dir/build.make create mode 100644 build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake create mode 100644 build/CMakeFiles/clean_test_results.dir/progress.make create mode 100644 build/CMakeFiles/cmake.check_cache create mode 100644 build/CMakeFiles/doxygen.dir/DependInfo.cmake create mode 100644 build/CMakeFiles/doxygen.dir/build.make create mode 100644 build/CMakeFiles/doxygen.dir/cmake_clean.cmake create mode 100644 build/CMakeFiles/doxygen.dir/progress.make create mode 100644 build/CMakeFiles/progress.marks create mode 100644 build/CMakeFiles/run_tests.dir/DependInfo.cmake create mode 100644 build/CMakeFiles/run_tests.dir/build.make create mode 100644 build/CMakeFiles/run_tests.dir/cmake_clean.cmake create mode 100644 build/CMakeFiles/run_tests.dir/progress.make create mode 100644 build/CMakeFiles/tests.dir/DependInfo.cmake create mode 100644 build/CMakeFiles/tests.dir/build.make create mode 100644 build/CMakeFiles/tests.dir/cmake_clean.cmake create mode 100644 build/CMakeFiles/tests.dir/progress.make create mode 100644 build/CTestTestfile.cmake create mode 100644 build/Makefile create mode 100644 build/catkin/catkin_generated/version/package.cmake create mode 100755 build/catkin_generated/env_cached.sh create mode 100644 build/catkin_generated/generate_cached_setup.py create mode 100644 build/catkin_generated/installspace/.rosinstall create mode 100755 build/catkin_generated/installspace/_setup_util.py create mode 100755 build/catkin_generated/installspace/env.sh create mode 100644 build/catkin_generated/installspace/setup.bash create mode 100644 build/catkin_generated/installspace/setup.sh create mode 100644 build/catkin_generated/installspace/setup.zsh create mode 100644 build/catkin_generated/order_packages.cmake create mode 100644 build/catkin_generated/order_packages.py create mode 100755 build/catkin_generated/setup_cached.sh create mode 100644 build/catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp create mode 100644 build/catkin_generated/stamps/Project/order_packages.cmake.em.stamp create mode 100644 build/catkin_generated/stamps/Project/package.xml.stamp create mode 100644 build/catkin_make.cache create mode 100644 build/cmake_install.cmake create mode 100644 build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake create mode 100644 build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake create mode 100644 build/gtest/CMakeFiles/gtest.dir/build.make create mode 100644 build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake create mode 100644 build/gtest/CMakeFiles/gtest.dir/depend.make create mode 100644 build/gtest/CMakeFiles/gtest.dir/flags.make create mode 100644 build/gtest/CMakeFiles/gtest.dir/link.txt create mode 100644 build/gtest/CMakeFiles/gtest.dir/progress.make create mode 100644 build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake create mode 100644 build/gtest/CMakeFiles/gtest_main.dir/build.make create mode 100644 build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake create mode 100644 build/gtest/CMakeFiles/gtest_main.dir/depend.make create mode 100644 build/gtest/CMakeFiles/gtest_main.dir/flags.make create mode 100644 build/gtest/CMakeFiles/gtest_main.dir/link.txt create mode 100644 build/gtest/CMakeFiles/gtest_main.dir/progress.make create mode 100644 build/gtest/CMakeFiles/progress.marks create mode 100644 build/gtest/CTestTestfile.cmake create mode 100644 build/gtest/Makefile create mode 100644 build/gtest/cmake_install.cmake create mode 100644 devel/.catkin create mode 100644 devel/.rosinstall create mode 100755 devel/_setup_util.py create mode 100755 devel/env.sh create mode 100644 devel/setup.bash create mode 100644 devel/setup.sh create mode 100644 devel/setup.zsh diff --git a/.catkin_workspace b/.catkin_workspace new file mode 100644 index 000000000..52fd97e7e --- /dev/null +++ b/.catkin_workspace @@ -0,0 +1 @@ +# This file currently only serves to mark the location of a catkin workspace for tool integration diff --git a/build/CATKIN_IGNORE b/build/CATKIN_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/build/CMakeCache.txt b/build/CMakeCache.txt new file mode 100644 index 000000000..8fc5c4b4a --- /dev/null +++ b/build/CMakeCache.txt @@ -0,0 +1,450 @@ +# This is the CMakeCache file. +# For build in directory: /home/roman/src/Firmware/build +# It was generated by CMake: /usr/bin/cmake +# You can edit this file to change values found and used by cmake. +# If you do not want to change any of the values, simply exit the editor. +# If you do want to change a value, simply edit, save, and exit the editor. +# The syntax for the file is as follows: +# KEY:TYPE=VALUE +# KEY is the name of a variable in the cache. +# TYPE is a hint to GUIs for the type of VALUE, DO NOT EDIT TYPE!. +# VALUE is the current value for the KEY. + +######################## +# EXTERNAL cache entries +######################## + +//Build shared libraries (DLLs). +BUILD_SHARED_LIBS:BOOL=ON + +//List of ';' separated packages to exclude +CATKIN_BLACKLIST_PACKAGES:STRING= + +//catkin devel space +CATKIN_DEVEL_PREFIX:PATH=/home/roman/src/Firmware/devel + +//Catkin enable testing +CATKIN_ENABLE_TESTING:BOOL=ON + +//Catkin skip testing +CATKIN_SKIP_TESTING:BOOL=OFF + +//List of ';' separated packages to build +CATKIN_WHITELIST_PACKAGES:STRING= + +//Path to a program. +CMAKE_AR:FILEPATH=/usr/bin/ar + +//Choose the type of build, options are: None(CMAKE_CXX_FLAGS or +// CMAKE_C_FLAGS used) Debug Release RelWithDebInfo MinSizeRel. +CMAKE_BUILD_TYPE:STRING= + +//Enable/Disable color output during build. +CMAKE_COLOR_MAKEFILE:BOOL=ON + +//CXX compiler. +CMAKE_CXX_COMPILER:FILEPATH=/usr/bin/c++ + +//Flags used by the compiler during all build types. +CMAKE_CXX_FLAGS:STRING= + +//Flags used by the compiler during debug builds. +CMAKE_CXX_FLAGS_DEBUG:STRING=-g + +//Flags used by the compiler during release minsize builds. +CMAKE_CXX_FLAGS_MINSIZEREL:STRING=-Os -DNDEBUG + +//Flags used by the compiler during release builds (/MD /Ob1 /Oi +// /Ot /Oy /Gs will produce slightly less optimized but smaller +// files). +CMAKE_CXX_FLAGS_RELEASE:STRING=-O3 -DNDEBUG + +//Flags used by the compiler during Release with Debug Info builds. +CMAKE_CXX_FLAGS_RELWITHDEBINFO:STRING=-O2 -g -DNDEBUG + +//C compiler. +CMAKE_C_COMPILER:FILEPATH=/usr/bin/cc + +//Flags used by the compiler during all build types. +CMAKE_C_FLAGS:STRING= + +//Flags used by the compiler during debug builds. +CMAKE_C_FLAGS_DEBUG:STRING=-g + +//Flags used by the compiler during release minsize builds. +CMAKE_C_FLAGS_MINSIZEREL:STRING=-Os -DNDEBUG + +//Flags used by the compiler during release builds (/MD /Ob1 /Oi +// /Ot /Oy /Gs will produce slightly less optimized but smaller +// files). +CMAKE_C_FLAGS_RELEASE:STRING=-O3 -DNDEBUG + +//Flags used by the compiler during Release with Debug Info builds. +CMAKE_C_FLAGS_RELWITHDEBINFO:STRING=-O2 -g -DNDEBUG + +//Flags used by the linker. +CMAKE_EXE_LINKER_FLAGS:STRING=' ' + +//Flags used by the linker during debug builds. +CMAKE_EXE_LINKER_FLAGS_DEBUG:STRING= + +//Flags used by the linker during release minsize builds. +CMAKE_EXE_LINKER_FLAGS_MINSIZEREL:STRING= + +//Flags used by the linker during release builds. +CMAKE_EXE_LINKER_FLAGS_RELEASE:STRING= + +//Flags used by the linker during Release with Debug Info builds. +CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO:STRING= + +//Enable/Disable output of compile commands during generation. +CMAKE_EXPORT_COMPILE_COMMANDS:BOOL=OFF + +//Install path prefix, prepended onto install directories. +CMAKE_INSTALL_PREFIX:PATH=/home/roman/src/Firmware/install + +//Path to a program. +CMAKE_LINKER:FILEPATH=/usr/bin/ld + +//Path to a program. +CMAKE_MAKE_PROGRAM:FILEPATH=/usr/bin/make + +//Flags used by the linker during the creation of modules. +CMAKE_MODULE_LINKER_FLAGS:STRING=' ' + +//Flags used by the linker during debug builds. +CMAKE_MODULE_LINKER_FLAGS_DEBUG:STRING= + +//Flags used by the linker during release minsize builds. +CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL:STRING= + +//Flags used by the linker during release builds. +CMAKE_MODULE_LINKER_FLAGS_RELEASE:STRING= + +//Flags used by the linker during Release with Debug Info builds. +CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO:STRING= + +//Path to a program. +CMAKE_NM:FILEPATH=/usr/bin/nm + +//Path to a program. +CMAKE_OBJCOPY:FILEPATH=/usr/bin/objcopy + +//Path to a program. +CMAKE_OBJDUMP:FILEPATH=/usr/bin/objdump + +//Value Computed by CMake +CMAKE_PROJECT_NAME:STATIC=Project + +//Path to a program. +CMAKE_RANLIB:FILEPATH=/usr/bin/ranlib + +//Flags used by the linker during the creation of dll's. +CMAKE_SHARED_LINKER_FLAGS:STRING=' ' + +//Flags used by the linker during debug builds. +CMAKE_SHARED_LINKER_FLAGS_DEBUG:STRING= + +//Flags used by the linker during release minsize builds. +CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL:STRING= + +//Flags used by the linker during release builds. +CMAKE_SHARED_LINKER_FLAGS_RELEASE:STRING= + +//Flags used by the linker during Release with Debug Info builds. +CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO:STRING= + +//If set, runtime paths are not added when installing shared libraries, +// but are added when building. +CMAKE_SKIP_INSTALL_RPATH:BOOL=NO + +//If set, runtime paths are not added when using shared libraries. +CMAKE_SKIP_RPATH:BOOL=NO + +//Flags used by the linker during the creation of static libraries. +CMAKE_STATIC_LINKER_FLAGS:STRING= + +//Flags used by the linker during debug builds. +CMAKE_STATIC_LINKER_FLAGS_DEBUG:STRING= + +//Flags used by the linker during release minsize builds. +CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL:STRING= + +//Flags used by the linker during release builds. +CMAKE_STATIC_LINKER_FLAGS_RELEASE:STRING= + +//Flags used by the linker during Release with Debug Info builds. +CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO:STRING= + +//Path to a program. +CMAKE_STRIP:FILEPATH=/usr/bin/strip + +//If true, cmake will use relative paths in makefiles and projects. +CMAKE_USE_RELATIVE_PATHS:BOOL=OFF + +//If this value is on, makefiles will be generated without the +// .SILENT directive, and all commands will be echoed to the console +// during the make. This is useful for debugging only. With Visual +// Studio IDE projects all commands are done without /nologo. +CMAKE_VERBOSE_MAKEFILE:BOOL=FALSE + +//Path to a program. +DOXYGEN_EXECUTABLE:FILEPATH=DOXYGEN_EXECUTABLE-NOTFOUND + +//Path to a program. +EMPY_EXECUTABLE:FILEPATH=/usr/bin/empy + +//Empy script +EMPY_SCRIPT:STRING=/usr/bin/empy + +//Path to a file. +GTEST_INCLUDE_DIR:PATH=/usr/include + +//Path to a library. +GTEST_LIBRARY:FILEPATH=GTEST_LIBRARY-NOTFOUND + +//Path to a library. +GTEST_LIBRARY_DEBUG:FILEPATH=GTEST_LIBRARY_DEBUG-NOTFOUND + +//Path to a library. +GTEST_MAIN_LIBRARY:FILEPATH=GTEST_MAIN_LIBRARY-NOTFOUND + +//Path to a library. +GTEST_MAIN_LIBRARY_DEBUG:FILEPATH=GTEST_MAIN_LIBRARY_DEBUG-NOTFOUND + +//lsb_release executable was found +LSB_FOUND:BOOL=TRUE + +//Path to a program. +LSB_RELEASE_EXECUTABLE:FILEPATH=/usr/bin/lsb_release + +//Path to a program. +NOSETESTS:FILEPATH=/usr/bin/nosetests-2.7 + +//Path to a program. +PYTHON_EXECUTABLE:FILEPATH=/usr/bin/python + +//Specify specific Python version to use ('major.minor' or 'major') +PYTHON_VERSION:STRING= + +//Value Computed by CMake +Project_BINARY_DIR:STATIC=/home/roman/src/Firmware/build + +//Value Computed by CMake +Project_SOURCE_DIR:STATIC=/home/roman/src/Firmware/src + +//Path to a library. +RT_LIBRARY:FILEPATH=/usr/lib/x86_64-linux-gnu/librt.so + +//Enable debian style python package layout +SETUPTOOLS_DEB_LAYOUT:BOOL=ON + +//LSB Distrib tag +UBUNTU:BOOL=TRUE + +//LSB Distrib - codename tag +UBUNTU_TRUSTY:BOOL=TRUE + +//Path to a file. +_CATKIN_GTEST_INCLUDE:FILEPATH=/usr/include/gtest/gtest.h + +//Path to a file. +_CATKIN_GTEST_SRC:FILEPATH=/usr/src/gtest/src/gtest.cc + +//The directory containing a CMake configuration file for catkin. +catkin_DIR:PATH=/opt/ros/indigo/share/catkin/cmake + +//Value Computed by CMake +gtest_BINARY_DIR:STATIC=/home/roman/src/Firmware/build/gtest + +//Dependencies for the target +gtest_LIB_DEPENDS:STATIC=general;-lpthread; + +//Value Computed by CMake +gtest_SOURCE_DIR:STATIC=/usr/src/gtest + +//Build gtest's sample programs. +gtest_build_samples:BOOL=OFF + +//Build all of gtest's own tests. +gtest_build_tests:BOOL=OFF + +//Disable uses of pthreads in gtest. +gtest_disable_pthreads:BOOL=OFF + +//Use shared (DLL) run-time lib even when Google Test is built +// as static lib. +gtest_force_shared_crt:BOOL=OFF + +//Dependencies for the target +gtest_main_LIB_DEPENDS:STATIC=general;-lpthread;general;gtest; + + +######################## +# INTERNAL cache entries +######################## + +//catkin environment +CATKIN_ENV:INTERNAL=/home/roman/src/Firmware/build/catkin_generated/env_cached.sh +CATKIN_TEST_RESULTS_DIR:INTERNAL=/home/roman/src/Firmware/build/test_results +//ADVANCED property for variable: CMAKE_AR +CMAKE_AR-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_BUILD_TOOL +CMAKE_BUILD_TOOL-ADVANCED:INTERNAL=1 +//What is the target build tool cmake is generating for. +CMAKE_BUILD_TOOL:INTERNAL=/usr/bin/make +//This is the directory where this CMakeCache.txt was created +CMAKE_CACHEFILE_DIR:INTERNAL=/home/roman/src/Firmware/build +//Major version of cmake used to create the current loaded cache +CMAKE_CACHE_MAJOR_VERSION:INTERNAL=2 +//Minor version of cmake used to create the current loaded cache +CMAKE_CACHE_MINOR_VERSION:INTERNAL=8 +//Patch version of cmake used to create the current loaded cache +CMAKE_CACHE_PATCH_VERSION:INTERNAL=12 +//ADVANCED property for variable: CMAKE_COLOR_MAKEFILE +CMAKE_COLOR_MAKEFILE-ADVANCED:INTERNAL=1 +//Path to CMake executable. +CMAKE_COMMAND:INTERNAL=/usr/bin/cmake +//Path to cpack program executable. +CMAKE_CPACK_COMMAND:INTERNAL=/usr/bin/cpack +//Path to ctest program executable. +CMAKE_CTEST_COMMAND:INTERNAL=/usr/bin/ctest +//ADVANCED property for variable: CMAKE_CXX_COMPILER +CMAKE_CXX_COMPILER-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS +CMAKE_CXX_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS_DEBUG +CMAKE_CXX_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS_MINSIZEREL +CMAKE_CXX_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS_RELEASE +CMAKE_CXX_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS_RELWITHDEBINFO +CMAKE_CXX_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_COMPILER +CMAKE_C_COMPILER-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS +CMAKE_C_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS_DEBUG +CMAKE_C_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS_MINSIZEREL +CMAKE_C_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS_RELEASE +CMAKE_C_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS_RELWITHDEBINFO +CMAKE_C_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//Executable file format +CMAKE_EXECUTABLE_FORMAT:INTERNAL=ELF +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS +CMAKE_EXE_LINKER_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_DEBUG +CMAKE_EXE_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_MINSIZEREL +CMAKE_EXE_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_RELEASE +CMAKE_EXE_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO +CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_EXPORT_COMPILE_COMMANDS +CMAKE_EXPORT_COMPILE_COMMANDS-ADVANCED:INTERNAL=1 +//Name of generator. +CMAKE_GENERATOR:INTERNAL=Unix Makefiles +//Name of generator toolset. +CMAKE_GENERATOR_TOOLSET:INTERNAL= +//Have symbol pthread_create +CMAKE_HAVE_LIBC_CREATE:INTERNAL= +//Have library pthreads +CMAKE_HAVE_PTHREADS_CREATE:INTERNAL= +//Have library pthread +CMAKE_HAVE_PTHREAD_CREATE:INTERNAL=1 +//Have include pthread.h +CMAKE_HAVE_PTHREAD_H:INTERNAL=1 +//Start directory with the top level CMakeLists.txt file for this +// project +CMAKE_HOME_DIRECTORY:INTERNAL=/home/roman/src/Firmware/src +//Install .so files without execute permission. +CMAKE_INSTALL_SO_NO_EXE:INTERNAL=1 +//ADVANCED property for variable: CMAKE_LINKER +CMAKE_LINKER-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MAKE_PROGRAM +CMAKE_MAKE_PROGRAM-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS +CMAKE_MODULE_LINKER_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_DEBUG +CMAKE_MODULE_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL +CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_RELEASE +CMAKE_MODULE_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO +CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_NM +CMAKE_NM-ADVANCED:INTERNAL=1 +//number of local generators +CMAKE_NUMBER_OF_LOCAL_GENERATORS:INTERNAL=2 +//ADVANCED property for variable: CMAKE_OBJCOPY +CMAKE_OBJCOPY-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_OBJDUMP +CMAKE_OBJDUMP-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_RANLIB +CMAKE_RANLIB-ADVANCED:INTERNAL=1 +//Path to CMake installation. +CMAKE_ROOT:INTERNAL=/usr/share/cmake-2.8 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS +CMAKE_SHARED_LINKER_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_DEBUG +CMAKE_SHARED_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL +CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_RELEASE +CMAKE_SHARED_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO +CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SKIP_INSTALL_RPATH +CMAKE_SKIP_INSTALL_RPATH-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SKIP_RPATH +CMAKE_SKIP_RPATH-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS +CMAKE_STATIC_LINKER_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_DEBUG +CMAKE_STATIC_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL +CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_RELEASE +CMAKE_STATIC_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO +CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STRIP +CMAKE_STRIP-ADVANCED:INTERNAL=1 +//uname command +CMAKE_UNAME:INTERNAL=/bin/uname +//ADVANCED property for variable: CMAKE_USE_RELATIVE_PATHS +CMAKE_USE_RELATIVE_PATHS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_VERBOSE_MAKEFILE +CMAKE_VERBOSE_MAKEFILE-ADVANCED:INTERNAL=1 +//Details about finding PythonInterp +FIND_PACKAGE_MESSAGE_DETAILS_PythonInterp:INTERNAL=[/usr/bin/python][v2.7.6()] +//Details about finding Threads +FIND_PACKAGE_MESSAGE_DETAILS_Threads:INTERNAL=[TRUE][v()] +GTEST_FROM_SOURCE_FOUND:INTERNAL=TRUE +GTEST_FROM_SOURCE_INCLUDE_DIRS:INTERNAL=/usr/include +GTEST_FROM_SOURCE_LIBRARIES:INTERNAL=gtest +GTEST_FROM_SOURCE_LIBRARY_DIRS:INTERNAL=/home/roman/src/Firmware/build/gtest +GTEST_FROM_SOURCE_MAIN_LIBRARIES:INTERNAL=gtest_main +//ADVANCED property for variable: GTEST_INCLUDE_DIR +GTEST_INCLUDE_DIR-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: GTEST_LIBRARY +GTEST_LIBRARY-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: GTEST_LIBRARY_DEBUG +GTEST_LIBRARY_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: GTEST_MAIN_LIBRARY +GTEST_MAIN_LIBRARY-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: GTEST_MAIN_LIBRARY_DEBUG +GTEST_MAIN_LIBRARY_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: PYTHON_EXECUTABLE +PYTHON_EXECUTABLE-ADVANCED:INTERNAL=1 +//This needs to be in PYTHONPATH when 'setup.py install' is called. +// And it needs to match. But setuptools won't tell us where +// it will install things. +PYTHON_INSTALL_DIR:INTERNAL=lib/python2.7/dist-packages + diff --git a/build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake b/build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake new file mode 100644 index 000000000..83254ce49 --- /dev/null +++ b/build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake @@ -0,0 +1,56 @@ +set(CMAKE_C_COMPILER "/usr/bin/cc") +set(CMAKE_C_COMPILER_ARG1 "") +set(CMAKE_C_COMPILER_ID "GNU") +set(CMAKE_C_COMPILER_VERSION "4.8.2") +set(CMAKE_C_PLATFORM_ID "Linux") + +set(CMAKE_AR "/usr/bin/ar") +set(CMAKE_RANLIB "/usr/bin/ranlib") +set(CMAKE_LINKER "/usr/bin/ld") +set(CMAKE_COMPILER_IS_GNUCC 1) +set(CMAKE_C_COMPILER_LOADED 1) +set(CMAKE_C_COMPILER_WORKS TRUE) +set(CMAKE_C_ABI_COMPILED TRUE) +set(CMAKE_COMPILER_IS_MINGW ) +set(CMAKE_COMPILER_IS_CYGWIN ) +if(CMAKE_COMPILER_IS_CYGWIN) + set(CYGWIN 1) + set(UNIX 1) +endif() + +set(CMAKE_C_COMPILER_ENV_VAR "CC") + +if(CMAKE_COMPILER_IS_MINGW) + set(MINGW 1) +endif() +set(CMAKE_C_COMPILER_ID_RUN 1) +set(CMAKE_C_SOURCE_FILE_EXTENSIONS c) +set(CMAKE_C_IGNORE_EXTENSIONS h;H;o;O;obj;OBJ;def;DEF;rc;RC) +set(CMAKE_C_LINKER_PREFERENCE 10) + +# Save compiler ABI information. +set(CMAKE_C_SIZEOF_DATA_PTR "8") +set(CMAKE_C_COMPILER_ABI "ELF") +set(CMAKE_C_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") + +if(CMAKE_C_SIZEOF_DATA_PTR) + set(CMAKE_SIZEOF_VOID_P "${CMAKE_C_SIZEOF_DATA_PTR}") +endif() + +if(CMAKE_C_COMPILER_ABI) + set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_C_COMPILER_ABI}") +endif() + +if(CMAKE_C_LIBRARY_ARCHITECTURE) + set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") +endif() + + + + +set(CMAKE_C_IMPLICIT_LINK_LIBRARIES "c") +set(CMAKE_C_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib") +set(CMAKE_C_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "") + + + diff --git a/build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake b/build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake new file mode 100644 index 000000000..c4373d570 --- /dev/null +++ b/build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake @@ -0,0 +1,57 @@ +set(CMAKE_CXX_COMPILER "/usr/bin/c++") +set(CMAKE_CXX_COMPILER_ARG1 "") +set(CMAKE_CXX_COMPILER_ID "GNU") +set(CMAKE_CXX_COMPILER_VERSION "4.8.2") +set(CMAKE_CXX_PLATFORM_ID "Linux") + +set(CMAKE_AR "/usr/bin/ar") +set(CMAKE_RANLIB "/usr/bin/ranlib") +set(CMAKE_LINKER "/usr/bin/ld") +set(CMAKE_COMPILER_IS_GNUCXX 1) +set(CMAKE_CXX_COMPILER_LOADED 1) +set(CMAKE_CXX_COMPILER_WORKS TRUE) +set(CMAKE_CXX_ABI_COMPILED TRUE) +set(CMAKE_COMPILER_IS_MINGW ) +set(CMAKE_COMPILER_IS_CYGWIN ) +if(CMAKE_COMPILER_IS_CYGWIN) + set(CYGWIN 1) + set(UNIX 1) +endif() + +set(CMAKE_CXX_COMPILER_ENV_VAR "CXX") + +if(CMAKE_COMPILER_IS_MINGW) + set(MINGW 1) +endif() +set(CMAKE_CXX_COMPILER_ID_RUN 1) +set(CMAKE_CXX_IGNORE_EXTENSIONS inl;h;hpp;HPP;H;o;O;obj;OBJ;def;DEF;rc;RC) +set(CMAKE_CXX_SOURCE_FILE_EXTENSIONS C;M;c++;cc;cpp;cxx;m;mm;CPP) +set(CMAKE_CXX_LINKER_PREFERENCE 30) +set(CMAKE_CXX_LINKER_PREFERENCE_PROPAGATES 1) + +# Save compiler ABI information. +set(CMAKE_CXX_SIZEOF_DATA_PTR "8") +set(CMAKE_CXX_COMPILER_ABI "ELF") +set(CMAKE_CXX_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") + +if(CMAKE_CXX_SIZEOF_DATA_PTR) + set(CMAKE_SIZEOF_VOID_P "${CMAKE_CXX_SIZEOF_DATA_PTR}") +endif() + +if(CMAKE_CXX_COMPILER_ABI) + set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_CXX_COMPILER_ABI}") +endif() + +if(CMAKE_CXX_LIBRARY_ARCHITECTURE) + set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") +endif() + + + + +set(CMAKE_CXX_IMPLICIT_LINK_LIBRARIES "stdc++;m;c") +set(CMAKE_CXX_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib") +set(CMAKE_CXX_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "") + + + diff --git a/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin b/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin new file mode 100755 index 000000000..2f2ebe478 Binary files /dev/null and b/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin differ diff --git a/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin b/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin new file mode 100755 index 000000000..16c737f26 Binary files /dev/null and b/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin differ diff --git a/build/CMakeFiles/2.8.12.2/CMakeSystem.cmake b/build/CMakeFiles/2.8.12.2/CMakeSystem.cmake new file mode 100644 index 000000000..0616b7c1e --- /dev/null +++ b/build/CMakeFiles/2.8.12.2/CMakeSystem.cmake @@ -0,0 +1,15 @@ +set(CMAKE_HOST_SYSTEM "Linux-3.13.0-24-generic") +set(CMAKE_HOST_SYSTEM_NAME "Linux") +set(CMAKE_HOST_SYSTEM_VERSION "3.13.0-24-generic") +set(CMAKE_HOST_SYSTEM_PROCESSOR "x86_64") + + + +set(CMAKE_SYSTEM "Linux-3.13.0-24-generic") +set(CMAKE_SYSTEM_NAME "Linux") +set(CMAKE_SYSTEM_VERSION "3.13.0-24-generic") +set(CMAKE_SYSTEM_PROCESSOR "x86_64") + +set(CMAKE_CROSSCOMPILING "FALSE") + +set(CMAKE_SYSTEM_LOADED 1) diff --git a/build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c b/build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c new file mode 100644 index 000000000..cba81d4a6 --- /dev/null +++ b/build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c @@ -0,0 +1,389 @@ +#ifdef __cplusplus +# error "A C++ compiler has been selected for C." +#endif + +/* Version number components: V=Version, R=Revision, P=Patch + Version date components: YYYY=Year, MM=Month, DD=Day */ + +#if defined(__18CXX) +# define ID_VOID_MAIN +#endif + +#if defined(__INTEL_COMPILER) || defined(__ICC) +# define COMPILER_ID "Intel" + /* __INTEL_COMPILER = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__INTEL_COMPILER/100) +# define COMPILER_VERSION_MINOR DEC(__INTEL_COMPILER/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER % 10) +# if defined(__INTEL_COMPILER_BUILD_DATE) + /* __INTEL_COMPILER_BUILD_DATE = YYYYMMDD */ +# define COMPILER_VERSION_TWEAK DEC(__INTEL_COMPILER_BUILD_DATE) +# endif + +#elif defined(__PATHCC__) +# define COMPILER_ID "PathScale" +# define COMPILER_VERSION_MAJOR DEC(__PATHCC__) +# define COMPILER_VERSION_MINOR DEC(__PATHCC_MINOR__) +# if defined(__PATHCC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__PATHCC_PATCHLEVEL__) +# endif + +#elif defined(__clang__) +# define COMPILER_ID "Clang" +# define COMPILER_VERSION_MAJOR DEC(__clang_major__) +# define COMPILER_VERSION_MINOR DEC(__clang_minor__) +# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__) + +#elif defined(__BORLANDC__) && defined(__CODEGEARC_VERSION__) +# define COMPILER_ID "Embarcadero" +# define COMPILER_VERSION_MAJOR HEX(__CODEGEARC_VERSION__>>24 & 0x00FF) +# define COMPILER_VERSION_MINOR HEX(__CODEGEARC_VERSION__>>16 & 0x00FF) +# define COMPILER_VERSION_PATCH HEX(__CODEGEARC_VERSION__ & 0xFFFF) + +#elif defined(__BORLANDC__) +# define COMPILER_ID "Borland" + /* __BORLANDC__ = 0xVRR */ +# define COMPILER_VERSION_MAJOR HEX(__BORLANDC__>>8) +# define COMPILER_VERSION_MINOR HEX(__BORLANDC__ & 0xFF) + +#elif defined(__WATCOMC__) +# define COMPILER_ID "Watcom" + /* __WATCOMC__ = VVRR */ +# define COMPILER_VERSION_MAJOR DEC(__WATCOMC__ / 100) +# define COMPILER_VERSION_MINOR DEC(__WATCOMC__ % 100) + +#elif defined(__SUNPRO_C) +# define COMPILER_ID "SunPro" +# if __SUNPRO_C >= 0x5100 + /* __SUNPRO_C = 0xVRRP */ +# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_C>>12) +# define COMPILER_VERSION_MINOR HEX(__SUNPRO_C>>4 & 0xFF) +# define COMPILER_VERSION_PATCH HEX(__SUNPRO_C & 0xF) +# else + /* __SUNPRO_C = 0xVRP */ +# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_C>>8) +# define COMPILER_VERSION_MINOR HEX(__SUNPRO_C>>4 & 0xF) +# define COMPILER_VERSION_PATCH HEX(__SUNPRO_C & 0xF) +# endif + +#elif defined(__HP_cc) +# define COMPILER_ID "HP" + /* __HP_cc = VVRRPP */ +# define COMPILER_VERSION_MAJOR DEC(__HP_cc/10000) +# define COMPILER_VERSION_MINOR DEC(__HP_cc/100 % 100) +# define COMPILER_VERSION_PATCH DEC(__HP_cc % 100) + +#elif defined(__DECC) +# define COMPILER_ID "Compaq" + /* __DECC_VER = VVRRTPPPP */ +# define COMPILER_VERSION_MAJOR DEC(__DECC_VER/10000000) +# define COMPILER_VERSION_MINOR DEC(__DECC_VER/100000 % 100) +# define COMPILER_VERSION_PATCH DEC(__DECC_VER % 10000) + +#elif defined(__IBMC__) +# if defined(__COMPILER_VER__) +# define COMPILER_ID "zOS" +# else +# if __IBMC__ >= 800 +# define COMPILER_ID "XL" +# else +# define COMPILER_ID "VisualAge" +# endif + /* __IBMC__ = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__IBMC__/100) +# define COMPILER_VERSION_MINOR DEC(__IBMC__/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__IBMC__ % 10) +# endif + +#elif defined(__PGI) +# define COMPILER_ID "PGI" +# define COMPILER_VERSION_MAJOR DEC(__PGIC__) +# define COMPILER_VERSION_MINOR DEC(__PGIC_MINOR__) +# if defined(__PGIC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__PGIC_PATCHLEVEL__) +# endif + +#elif defined(_CRAYC) +# define COMPILER_ID "Cray" +# define COMPILER_VERSION_MAJOR DEC(_RELEASE) +# define COMPILER_VERSION_MINOR DEC(_RELEASE_MINOR) + +#elif defined(__TI_COMPILER_VERSION__) +# define COMPILER_ID "TI" + /* __TI_COMPILER_VERSION__ = VVVRRRPPP */ +# define COMPILER_VERSION_MAJOR DEC(__TI_COMPILER_VERSION__/1000000) +# define COMPILER_VERSION_MINOR DEC(__TI_COMPILER_VERSION__/1000 % 1000) +# define COMPILER_VERSION_PATCH DEC(__TI_COMPILER_VERSION__ % 1000) + +#elif defined(__TINYC__) +# define COMPILER_ID "TinyCC" + +#elif defined(__SCO_VERSION__) +# define COMPILER_ID "SCO" + +#elif defined(__GNUC__) +# define COMPILER_ID "GNU" +# define COMPILER_VERSION_MAJOR DEC(__GNUC__) +# define COMPILER_VERSION_MINOR DEC(__GNUC_MINOR__) +# if defined(__GNUC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) +# endif + +#elif defined(_MSC_VER) +# define COMPILER_ID "MSVC" + /* _MSC_VER = VVRR */ +# define COMPILER_VERSION_MAJOR DEC(_MSC_VER / 100) +# define COMPILER_VERSION_MINOR DEC(_MSC_VER % 100) +# if defined(_MSC_FULL_VER) +# if _MSC_VER >= 1400 + /* _MSC_FULL_VER = VVRRPPPPP */ +# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 100000) +# else + /* _MSC_FULL_VER = VVRRPPPP */ +# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 10000) +# endif +# endif +# if defined(_MSC_BUILD) +# define COMPILER_VERSION_TWEAK DEC(_MSC_BUILD) +# endif + +/* Analog VisualDSP++ >= 4.5.6 */ +#elif defined(__VISUALDSPVERSION__) +# define COMPILER_ID "ADSP" + /* __VISUALDSPVERSION__ = 0xVVRRPP00 */ +# define COMPILER_VERSION_MAJOR HEX(__VISUALDSPVERSION__>>24) +# define COMPILER_VERSION_MINOR HEX(__VISUALDSPVERSION__>>16 & 0xFF) +# define COMPILER_VERSION_PATCH HEX(__VISUALDSPVERSION__>>8 & 0xFF) + +/* Analog VisualDSP++ < 4.5.6 */ +#elif defined(__ADSPBLACKFIN__) || defined(__ADSPTS__) || defined(__ADSP21000__) +# define COMPILER_ID "ADSP" + +/* IAR Systems compiler for embedded systems. + http://www.iar.com */ +#elif defined(__IAR_SYSTEMS_ICC__ ) || defined(__IAR_SYSTEMS_ICC) +# define COMPILER_ID "IAR" + +/* sdcc, the small devices C compiler for embedded systems, + http://sdcc.sourceforge.net */ +#elif defined(SDCC) +# define COMPILER_ID "SDCC" + /* SDCC = VRP */ +# define COMPILER_VERSION_MAJOR DEC(SDCC/100) +# define COMPILER_VERSION_MINOR DEC(SDCC/10 % 10) +# define COMPILER_VERSION_PATCH DEC(SDCC % 10) + +#elif defined(_SGI_COMPILER_VERSION) || defined(_COMPILER_VERSION) +# define COMPILER_ID "MIPSpro" +# if defined(_SGI_COMPILER_VERSION) + /* _SGI_COMPILER_VERSION = VRP */ +# define COMPILER_VERSION_MAJOR DEC(_SGI_COMPILER_VERSION/100) +# define COMPILER_VERSION_MINOR DEC(_SGI_COMPILER_VERSION/10 % 10) +# define COMPILER_VERSION_PATCH DEC(_SGI_COMPILER_VERSION % 10) +# else + /* _COMPILER_VERSION = VRP */ +# define COMPILER_VERSION_MAJOR DEC(_COMPILER_VERSION/100) +# define COMPILER_VERSION_MINOR DEC(_COMPILER_VERSION/10 % 10) +# define COMPILER_VERSION_PATCH DEC(_COMPILER_VERSION % 10) +# endif + +/* This compiler is either not known or is too old to define an + identification macro. Try to identify the platform and guess that + it is the native compiler. */ +#elif defined(__sgi) +# define COMPILER_ID "MIPSpro" + +#elif defined(__hpux) || defined(__hpua) +# define COMPILER_ID "HP" + +#else /* unknown compiler */ +# define COMPILER_ID "" + +#endif + +/* Construct the string literal in pieces to prevent the source from + getting matched. Store it in a pointer rather than an array + because some compilers will just produce instructions to fill the + array rather than assigning a pointer to a static array. */ +char const* info_compiler = "INFO" ":" "compiler[" COMPILER_ID "]"; + +/* Identify known platforms by name. */ +#if defined(__linux) || defined(__linux__) || defined(linux) +# define PLATFORM_ID "Linux" + +#elif defined(__CYGWIN__) +# define PLATFORM_ID "Cygwin" + +#elif defined(__MINGW32__) +# define PLATFORM_ID "MinGW" + +#elif defined(__APPLE__) +# define PLATFORM_ID "Darwin" + +#elif defined(_WIN32) || defined(__WIN32__) || defined(WIN32) +# define PLATFORM_ID "Windows" + +#elif defined(__FreeBSD__) || defined(__FreeBSD) +# define PLATFORM_ID "FreeBSD" + +#elif defined(__NetBSD__) || defined(__NetBSD) +# define PLATFORM_ID "NetBSD" + +#elif defined(__OpenBSD__) || defined(__OPENBSD) +# define PLATFORM_ID "OpenBSD" + +#elif defined(__sun) || defined(sun) +# define PLATFORM_ID "SunOS" + +#elif defined(_AIX) || defined(__AIX) || defined(__AIX__) || defined(__aix) || defined(__aix__) +# define PLATFORM_ID "AIX" + +#elif defined(__sgi) || defined(__sgi__) || defined(_SGI) +# define PLATFORM_ID "IRIX" + +#elif defined(__hpux) || defined(__hpux__) +# define PLATFORM_ID "HP-UX" + +#elif defined(__HAIKU__) +# define PLATFORM_ID "Haiku" + +#elif defined(__BeOS) || defined(__BEOS__) || defined(_BEOS) +# define PLATFORM_ID "BeOS" + +#elif defined(__QNX__) || defined(__QNXNTO__) +# define PLATFORM_ID "QNX" + +#elif defined(__tru64) || defined(_tru64) || defined(__TRU64__) +# define PLATFORM_ID "Tru64" + +#elif defined(__riscos) || defined(__riscos__) +# define PLATFORM_ID "RISCos" + +#elif defined(__sinix) || defined(__sinix__) || defined(__SINIX__) +# define PLATFORM_ID "SINIX" + +#elif defined(__UNIX_SV__) +# define PLATFORM_ID "UNIX_SV" + +#elif defined(__bsdos__) +# define PLATFORM_ID "BSDOS" + +#elif defined(_MPRAS) || defined(MPRAS) +# define PLATFORM_ID "MP-RAS" + +#elif defined(__osf) || defined(__osf__) +# define PLATFORM_ID "OSF1" + +#elif defined(_SCO_SV) || defined(SCO_SV) || defined(sco_sv) +# define PLATFORM_ID "SCO_SV" + +#elif defined(__ultrix) || defined(__ultrix__) || defined(_ULTRIX) +# define PLATFORM_ID "ULTRIX" + +#elif defined(__XENIX__) || defined(_XENIX) || defined(XENIX) +# define PLATFORM_ID "Xenix" + +#else /* unknown platform */ +# define PLATFORM_ID "" + +#endif + +/* For windows compilers MSVC and Intel we can determine + the architecture of the compiler being used. This is because + the compilers do not have flags that can change the architecture, + but rather depend on which compiler is being used +*/ +#if defined(_WIN32) && defined(_MSC_VER) +# if defined(_M_IA64) +# define ARCHITECTURE_ID "IA64" + +# elif defined(_M_X64) || defined(_M_AMD64) +# define ARCHITECTURE_ID "x64" + +# elif defined(_M_IX86) +# define ARCHITECTURE_ID "X86" + +# elif defined(_M_ARM) +# define ARCHITECTURE_ID "ARM" + +# elif defined(_M_MIPS) +# define ARCHITECTURE_ID "MIPS" + +# elif defined(_M_SH) +# define ARCHITECTURE_ID "SHx" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#else +# define ARCHITECTURE_ID "" +#endif + +/* Convert integer to decimal digit literals. */ +#define DEC(n) \ + ('0' + (((n) / 10000000)%10)), \ + ('0' + (((n) / 1000000)%10)), \ + ('0' + (((n) / 100000)%10)), \ + ('0' + (((n) / 10000)%10)), \ + ('0' + (((n) / 1000)%10)), \ + ('0' + (((n) / 100)%10)), \ + ('0' + (((n) / 10)%10)), \ + ('0' + ((n) % 10)) + +/* Convert integer to hex digit literals. */ +#define HEX(n) \ + ('0' + ((n)>>28 & 0xF)), \ + ('0' + ((n)>>24 & 0xF)), \ + ('0' + ((n)>>20 & 0xF)), \ + ('0' + ((n)>>16 & 0xF)), \ + ('0' + ((n)>>12 & 0xF)), \ + ('0' + ((n)>>8 & 0xF)), \ + ('0' + ((n)>>4 & 0xF)), \ + ('0' + ((n) & 0xF)) + +/* Construct a string literal encoding the version number components. */ +#ifdef COMPILER_VERSION_MAJOR +char const info_version[] = { + 'I', 'N', 'F', 'O', ':', + 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','[', + COMPILER_VERSION_MAJOR, +# ifdef COMPILER_VERSION_MINOR + '.', COMPILER_VERSION_MINOR, +# ifdef COMPILER_VERSION_PATCH + '.', COMPILER_VERSION_PATCH, +# ifdef COMPILER_VERSION_TWEAK + '.', COMPILER_VERSION_TWEAK, +# endif +# endif +# endif + ']','\0'}; +#endif + +/* Construct the string literal in pieces to prevent the source from + getting matched. Store it in a pointer rather than an array + because some compilers will just produce instructions to fill the + array rather than assigning a pointer to a static array. */ +char const* info_platform = "INFO" ":" "platform[" PLATFORM_ID "]"; +char const* info_arch = "INFO" ":" "arch[" ARCHITECTURE_ID "]"; + + + +/*--------------------------------------------------------------------------*/ + +#ifdef ID_VOID_MAIN +void main() {} +#else +int main(int argc, char* argv[]) +{ + int require = 0; + require += info_compiler[argc]; + require += info_platform[argc]; + require += info_arch[argc]; +#ifdef COMPILER_VERSION_MAJOR + require += info_version[argc]; +#endif + (void)argv; + return require; +} +#endif diff --git a/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out b/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out new file mode 100755 index 000000000..33a3d2b58 Binary files /dev/null and b/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out differ diff --git a/build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp b/build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp new file mode 100644 index 000000000..e8220b26e --- /dev/null +++ b/build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp @@ -0,0 +1,377 @@ +/* This source file must have a .cpp extension so that all C++ compilers + recognize the extension without flags. Borland does not know .cxx for + example. */ +#ifndef __cplusplus +# error "A C compiler has been selected for C++." +#endif + +/* Version number components: V=Version, R=Revision, P=Patch + Version date components: YYYY=Year, MM=Month, DD=Day */ + +#if defined(__COMO__) +# define COMPILER_ID "Comeau" + /* __COMO_VERSION__ = VRR */ +# define COMPILER_VERSION_MAJOR DEC(__COMO_VERSION__ / 100) +# define COMPILER_VERSION_MINOR DEC(__COMO_VERSION__ % 100) + +#elif defined(__INTEL_COMPILER) || defined(__ICC) +# define COMPILER_ID "Intel" + /* __INTEL_COMPILER = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__INTEL_COMPILER/100) +# define COMPILER_VERSION_MINOR DEC(__INTEL_COMPILER/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER % 10) +# if defined(__INTEL_COMPILER_BUILD_DATE) + /* __INTEL_COMPILER_BUILD_DATE = YYYYMMDD */ +# define COMPILER_VERSION_TWEAK DEC(__INTEL_COMPILER_BUILD_DATE) +# endif + +#elif defined(__PATHCC__) +# define COMPILER_ID "PathScale" +# define COMPILER_VERSION_MAJOR DEC(__PATHCC__) +# define COMPILER_VERSION_MINOR DEC(__PATHCC_MINOR__) +# if defined(__PATHCC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__PATHCC_PATCHLEVEL__) +# endif + +#elif defined(__clang__) +# define COMPILER_ID "Clang" +# define COMPILER_VERSION_MAJOR DEC(__clang_major__) +# define COMPILER_VERSION_MINOR DEC(__clang_minor__) +# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__) + +#elif defined(__BORLANDC__) && defined(__CODEGEARC_VERSION__) +# define COMPILER_ID "Embarcadero" +# define COMPILER_VERSION_MAJOR HEX(__CODEGEARC_VERSION__>>24 & 0x00FF) +# define COMPILER_VERSION_MINOR HEX(__CODEGEARC_VERSION__>>16 & 0x00FF) +# define COMPILER_VERSION_PATCH HEX(__CODEGEARC_VERSION__ & 0xFFFF) + +#elif defined(__BORLANDC__) +# define COMPILER_ID "Borland" + /* __BORLANDC__ = 0xVRR */ +# define COMPILER_VERSION_MAJOR HEX(__BORLANDC__>>8) +# define COMPILER_VERSION_MINOR HEX(__BORLANDC__ & 0xFF) + +#elif defined(__WATCOMC__) +# define COMPILER_ID "Watcom" + /* __WATCOMC__ = VVRR */ +# define COMPILER_VERSION_MAJOR DEC(__WATCOMC__ / 100) +# define COMPILER_VERSION_MINOR DEC(__WATCOMC__ % 100) + +#elif defined(__SUNPRO_CC) +# define COMPILER_ID "SunPro" +# if __SUNPRO_CC >= 0x5100 + /* __SUNPRO_CC = 0xVRRP */ +# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_CC>>12) +# define COMPILER_VERSION_MINOR HEX(__SUNPRO_CC>>4 & 0xFF) +# define COMPILER_VERSION_PATCH HEX(__SUNPRO_CC & 0xF) +# else + /* __SUNPRO_CC = 0xVRP */ +# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_CC>>8) +# define COMPILER_VERSION_MINOR HEX(__SUNPRO_CC>>4 & 0xF) +# define COMPILER_VERSION_PATCH HEX(__SUNPRO_CC & 0xF) +# endif + +#elif defined(__HP_aCC) +# define COMPILER_ID "HP" + /* __HP_aCC = VVRRPP */ +# define COMPILER_VERSION_MAJOR DEC(__HP_aCC/10000) +# define COMPILER_VERSION_MINOR DEC(__HP_aCC/100 % 100) +# define COMPILER_VERSION_PATCH DEC(__HP_aCC % 100) + +#elif defined(__DECCXX) +# define COMPILER_ID "Compaq" + /* __DECCXX_VER = VVRRTPPPP */ +# define COMPILER_VERSION_MAJOR DEC(__DECCXX_VER/10000000) +# define COMPILER_VERSION_MINOR DEC(__DECCXX_VER/100000 % 100) +# define COMPILER_VERSION_PATCH DEC(__DECCXX_VER % 10000) + +#elif defined(__IBMCPP__) +# if defined(__COMPILER_VER__) +# define COMPILER_ID "zOS" +# else +# if __IBMCPP__ >= 800 +# define COMPILER_ID "XL" +# else +# define COMPILER_ID "VisualAge" +# endif + /* __IBMCPP__ = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__IBMCPP__/100) +# define COMPILER_VERSION_MINOR DEC(__IBMCPP__/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__IBMCPP__ % 10) +# endif + +#elif defined(__PGI) +# define COMPILER_ID "PGI" +# define COMPILER_VERSION_MAJOR DEC(__PGIC__) +# define COMPILER_VERSION_MINOR DEC(__PGIC_MINOR__) +# if defined(__PGIC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__PGIC_PATCHLEVEL__) +# endif + +#elif defined(_CRAYC) +# define COMPILER_ID "Cray" +# define COMPILER_VERSION_MAJOR DEC(_RELEASE) +# define COMPILER_VERSION_MINOR DEC(_RELEASE_MINOR) + +#elif defined(__TI_COMPILER_VERSION__) +# define COMPILER_ID "TI" + /* __TI_COMPILER_VERSION__ = VVVRRRPPP */ +# define COMPILER_VERSION_MAJOR DEC(__TI_COMPILER_VERSION__/1000000) +# define COMPILER_VERSION_MINOR DEC(__TI_COMPILER_VERSION__/1000 % 1000) +# define COMPILER_VERSION_PATCH DEC(__TI_COMPILER_VERSION__ % 1000) + +#elif defined(__SCO_VERSION__) +# define COMPILER_ID "SCO" + +#elif defined(__GNUC__) +# define COMPILER_ID "GNU" +# define COMPILER_VERSION_MAJOR DEC(__GNUC__) +# define COMPILER_VERSION_MINOR DEC(__GNUC_MINOR__) +# if defined(__GNUC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) +# endif + +#elif defined(_MSC_VER) +# define COMPILER_ID "MSVC" + /* _MSC_VER = VVRR */ +# define COMPILER_VERSION_MAJOR DEC(_MSC_VER / 100) +# define COMPILER_VERSION_MINOR DEC(_MSC_VER % 100) +# if defined(_MSC_FULL_VER) +# if _MSC_VER >= 1400 + /* _MSC_FULL_VER = VVRRPPPPP */ +# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 100000) +# else + /* _MSC_FULL_VER = VVRRPPPP */ +# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 10000) +# endif +# endif +# if defined(_MSC_BUILD) +# define COMPILER_VERSION_TWEAK DEC(_MSC_BUILD) +# endif + +/* Analog VisualDSP++ >= 4.5.6 */ +#elif defined(__VISUALDSPVERSION__) +# define COMPILER_ID "ADSP" + /* __VISUALDSPVERSION__ = 0xVVRRPP00 */ +# define COMPILER_VERSION_MAJOR HEX(__VISUALDSPVERSION__>>24) +# define COMPILER_VERSION_MINOR HEX(__VISUALDSPVERSION__>>16 & 0xFF) +# define COMPILER_VERSION_PATCH HEX(__VISUALDSPVERSION__>>8 & 0xFF) + +/* Analog VisualDSP++ < 4.5.6 */ +#elif defined(__ADSPBLACKFIN__) || defined(__ADSPTS__) || defined(__ADSP21000__) +# define COMPILER_ID "ADSP" + +/* IAR Systems compiler for embedded systems. + http://www.iar.com */ +#elif defined(__IAR_SYSTEMS_ICC__ ) || defined(__IAR_SYSTEMS_ICC) +# define COMPILER_ID "IAR" + +#elif defined(_SGI_COMPILER_VERSION) || defined(_COMPILER_VERSION) +# define COMPILER_ID "MIPSpro" +# if defined(_SGI_COMPILER_VERSION) + /* _SGI_COMPILER_VERSION = VRP */ +# define COMPILER_VERSION_MAJOR DEC(_SGI_COMPILER_VERSION/100) +# define COMPILER_VERSION_MINOR DEC(_SGI_COMPILER_VERSION/10 % 10) +# define COMPILER_VERSION_PATCH DEC(_SGI_COMPILER_VERSION % 10) +# else + /* _COMPILER_VERSION = VRP */ +# define COMPILER_VERSION_MAJOR DEC(_COMPILER_VERSION/100) +# define COMPILER_VERSION_MINOR DEC(_COMPILER_VERSION/10 % 10) +# define COMPILER_VERSION_PATCH DEC(_COMPILER_VERSION % 10) +# endif + +/* This compiler is either not known or is too old to define an + identification macro. Try to identify the platform and guess that + it is the native compiler. */ +#elif defined(__sgi) +# define COMPILER_ID "MIPSpro" + +#elif defined(__hpux) || defined(__hpua) +# define COMPILER_ID "HP" + +#else /* unknown compiler */ +# define COMPILER_ID "" + +#endif + +/* Construct the string literal in pieces to prevent the source from + getting matched. Store it in a pointer rather than an array + because some compilers will just produce instructions to fill the + array rather than assigning a pointer to a static array. */ +char const* info_compiler = "INFO" ":" "compiler[" COMPILER_ID "]"; + +/* Identify known platforms by name. */ +#if defined(__linux) || defined(__linux__) || defined(linux) +# define PLATFORM_ID "Linux" + +#elif defined(__CYGWIN__) +# define PLATFORM_ID "Cygwin" + +#elif defined(__MINGW32__) +# define PLATFORM_ID "MinGW" + +#elif defined(__APPLE__) +# define PLATFORM_ID "Darwin" + +#elif defined(_WIN32) || defined(__WIN32__) || defined(WIN32) +# define PLATFORM_ID "Windows" + +#elif defined(__FreeBSD__) || defined(__FreeBSD) +# define PLATFORM_ID "FreeBSD" + +#elif defined(__NetBSD__) || defined(__NetBSD) +# define PLATFORM_ID "NetBSD" + +#elif defined(__OpenBSD__) || defined(__OPENBSD) +# define PLATFORM_ID "OpenBSD" + +#elif defined(__sun) || defined(sun) +# define PLATFORM_ID "SunOS" + +#elif defined(_AIX) || defined(__AIX) || defined(__AIX__) || defined(__aix) || defined(__aix__) +# define PLATFORM_ID "AIX" + +#elif defined(__sgi) || defined(__sgi__) || defined(_SGI) +# define PLATFORM_ID "IRIX" + +#elif defined(__hpux) || defined(__hpux__) +# define PLATFORM_ID "HP-UX" + +#elif defined(__HAIKU__) +# define PLATFORM_ID "Haiku" + +#elif defined(__BeOS) || defined(__BEOS__) || defined(_BEOS) +# define PLATFORM_ID "BeOS" + +#elif defined(__QNX__) || defined(__QNXNTO__) +# define PLATFORM_ID "QNX" + +#elif defined(__tru64) || defined(_tru64) || defined(__TRU64__) +# define PLATFORM_ID "Tru64" + +#elif defined(__riscos) || defined(__riscos__) +# define PLATFORM_ID "RISCos" + +#elif defined(__sinix) || defined(__sinix__) || defined(__SINIX__) +# define PLATFORM_ID "SINIX" + +#elif defined(__UNIX_SV__) +# define PLATFORM_ID "UNIX_SV" + +#elif defined(__bsdos__) +# define PLATFORM_ID "BSDOS" + +#elif defined(_MPRAS) || defined(MPRAS) +# define PLATFORM_ID "MP-RAS" + +#elif defined(__osf) || defined(__osf__) +# define PLATFORM_ID "OSF1" + +#elif defined(_SCO_SV) || defined(SCO_SV) || defined(sco_sv) +# define PLATFORM_ID "SCO_SV" + +#elif defined(__ultrix) || defined(__ultrix__) || defined(_ULTRIX) +# define PLATFORM_ID "ULTRIX" + +#elif defined(__XENIX__) || defined(_XENIX) || defined(XENIX) +# define PLATFORM_ID "Xenix" + +#else /* unknown platform */ +# define PLATFORM_ID "" + +#endif + +/* For windows compilers MSVC and Intel we can determine + the architecture of the compiler being used. This is because + the compilers do not have flags that can change the architecture, + but rather depend on which compiler is being used +*/ +#if defined(_WIN32) && defined(_MSC_VER) +# if defined(_M_IA64) +# define ARCHITECTURE_ID "IA64" + +# elif defined(_M_X64) || defined(_M_AMD64) +# define ARCHITECTURE_ID "x64" + +# elif defined(_M_IX86) +# define ARCHITECTURE_ID "X86" + +# elif defined(_M_ARM) +# define ARCHITECTURE_ID "ARM" + +# elif defined(_M_MIPS) +# define ARCHITECTURE_ID "MIPS" + +# elif defined(_M_SH) +# define ARCHITECTURE_ID "SHx" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#else +# define ARCHITECTURE_ID "" +#endif + +/* Convert integer to decimal digit literals. */ +#define DEC(n) \ + ('0' + (((n) / 10000000)%10)), \ + ('0' + (((n) / 1000000)%10)), \ + ('0' + (((n) / 100000)%10)), \ + ('0' + (((n) / 10000)%10)), \ + ('0' + (((n) / 1000)%10)), \ + ('0' + (((n) / 100)%10)), \ + ('0' + (((n) / 10)%10)), \ + ('0' + ((n) % 10)) + +/* Convert integer to hex digit literals. */ +#define HEX(n) \ + ('0' + ((n)>>28 & 0xF)), \ + ('0' + ((n)>>24 & 0xF)), \ + ('0' + ((n)>>20 & 0xF)), \ + ('0' + ((n)>>16 & 0xF)), \ + ('0' + ((n)>>12 & 0xF)), \ + ('0' + ((n)>>8 & 0xF)), \ + ('0' + ((n)>>4 & 0xF)), \ + ('0' + ((n) & 0xF)) + +/* Construct a string literal encoding the version number components. */ +#ifdef COMPILER_VERSION_MAJOR +char const info_version[] = { + 'I', 'N', 'F', 'O', ':', + 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','[', + COMPILER_VERSION_MAJOR, +# ifdef COMPILER_VERSION_MINOR + '.', COMPILER_VERSION_MINOR, +# ifdef COMPILER_VERSION_PATCH + '.', COMPILER_VERSION_PATCH, +# ifdef COMPILER_VERSION_TWEAK + '.', COMPILER_VERSION_TWEAK, +# endif +# endif +# endif + ']','\0'}; +#endif + +/* Construct the string literal in pieces to prevent the source from + getting matched. Store it in a pointer rather than an array + because some compilers will just produce instructions to fill the + array rather than assigning a pointer to a static array. */ +char const* info_platform = "INFO" ":" "platform[" PLATFORM_ID "]"; +char const* info_arch = "INFO" ":" "arch[" ARCHITECTURE_ID "]"; + + + +/*--------------------------------------------------------------------------*/ + +int main(int argc, char* argv[]) +{ + int require = 0; + require += info_compiler[argc]; + require += info_platform[argc]; +#ifdef COMPILER_VERSION_MAJOR + require += info_version[argc]; +#endif + (void)argv; + return require; +} diff --git a/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out b/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out new file mode 100755 index 000000000..db35dbde0 Binary files /dev/null and b/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out differ diff --git a/build/CMakeFiles/CMakeDirectoryInformation.cmake b/build/CMakeFiles/CMakeDirectoryInformation.cmake new file mode 100644 index 000000000..b2cc9b7e7 --- /dev/null +++ b/build/CMakeFiles/CMakeDirectoryInformation.cmake @@ -0,0 +1,16 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +# Relative path conversion top directories. +SET(CMAKE_RELATIVE_PATH_TOP_SOURCE "/home/roman/src/Firmware/src") +SET(CMAKE_RELATIVE_PATH_TOP_BINARY "/home/roman/src/Firmware/build") + +# Force unix paths in dependencies. +SET(CMAKE_FORCE_UNIX_PATHS 1) + + +# The C and CXX include file regular expressions for this directory. +SET(CMAKE_C_INCLUDE_REGEX_SCAN "^.*$") +SET(CMAKE_C_INCLUDE_REGEX_COMPLAIN "^$") +SET(CMAKE_CXX_INCLUDE_REGEX_SCAN ${CMAKE_C_INCLUDE_REGEX_SCAN}) +SET(CMAKE_CXX_INCLUDE_REGEX_COMPLAIN ${CMAKE_C_INCLUDE_REGEX_COMPLAIN}) diff --git a/build/CMakeFiles/CMakeError.log b/build/CMakeFiles/CMakeError.log new file mode 100644 index 000000000..9ab6f1e9a --- /dev/null +++ b/build/CMakeFiles/CMakeError.log @@ -0,0 +1,53 @@ +Determining if the pthread_create exist failed with the following output: +Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp + +Run Build Command:/usr/bin/make "cmTryCompileExec1424100880/fast" +/usr/bin/make -f CMakeFiles/cmTryCompileExec1424100880.dir/build.make CMakeFiles/cmTryCompileExec1424100880.dir/build +make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' +/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 +Building C object CMakeFiles/cmTryCompileExec1424100880.dir/CheckSymbolExists.c.o +/usr/bin/cc -o CMakeFiles/cmTryCompileExec1424100880.dir/CheckSymbolExists.c.o -c /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CheckSymbolExists.c +Linking C executable cmTryCompileExec1424100880 +/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec1424100880.dir/link.txt --verbose=1 +/usr/bin/cc CMakeFiles/cmTryCompileExec1424100880.dir/CheckSymbolExists.c.o -o cmTryCompileExec1424100880 -rdynamic +CMakeFiles/cmTryCompileExec1424100880.dir/CheckSymbolExists.c.o: In function `main': +CheckSymbolExists.c:(.text+0x16): undefined reference to `pthread_create' +collect2: error: ld returned 1 exit status +make[1]: *** [cmTryCompileExec1424100880] Error 1 +make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' +make: *** [cmTryCompileExec1424100880/fast] Error 2 + +File /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CheckSymbolExists.c: +/* */ +#include + +int main(int argc, char** argv) +{ + (void)argv; +#ifndef pthread_create + return ((int*)(&pthread_create))[argc]; +#else + (void)argc; + return 0; +#endif +} + +Determining if the function pthread_create exists in the pthreads failed with the following output: +Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp + +Run Build Command:/usr/bin/make "cmTryCompileExec26988121/fast" +/usr/bin/make -f CMakeFiles/cmTryCompileExec26988121.dir/build.make CMakeFiles/cmTryCompileExec26988121.dir/build +make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' +/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 +Building C object CMakeFiles/cmTryCompileExec26988121.dir/CheckFunctionExists.c.o +/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create -o CMakeFiles/cmTryCompileExec26988121.dir/CheckFunctionExists.c.o -c /usr/share/cmake-2.8/Modules/CheckFunctionExists.c +Linking C executable cmTryCompileExec26988121 +/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec26988121.dir/link.txt --verbose=1 +/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create CMakeFiles/cmTryCompileExec26988121.dir/CheckFunctionExists.c.o -o cmTryCompileExec26988121 -rdynamic -lpthreads +/usr/bin/ld: cannot find -lpthreads +collect2: error: ld returned 1 exit status +make[1]: *** [cmTryCompileExec26988121] Error 1 +make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' +make: *** [cmTryCompileExec26988121/fast] Error 2 + + diff --git a/build/CMakeFiles/CMakeOutput.log b/build/CMakeFiles/CMakeOutput.log new file mode 100644 index 000000000..a3757fc34 --- /dev/null +++ b/build/CMakeFiles/CMakeOutput.log @@ -0,0 +1,293 @@ +The system is: Linux - 3.13.0-24-generic - x86_64 +Compiling the C compiler identification source file "CMakeCCompilerId.c" succeeded. +Compiler: /usr/bin/cc +Build flags: +Id flags: + +The output was: +0 + + +Compilation of the C compiler identification source "CMakeCCompilerId.c" produced "a.out" + +The C compiler identification is GNU, found in "/home/roman/src/Firmware/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out" + +Compiling the CXX compiler identification source file "CMakeCXXCompilerId.cpp" succeeded. +Compiler: /usr/bin/c++ +Build flags: +Id flags: + +The output was: +0 + + +Compilation of the CXX compiler identification source "CMakeCXXCompilerId.cpp" produced "a.out" + +The CXX compiler identification is GNU, found in "/home/roman/src/Firmware/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out" + +Determining if the C compiler works passed with the following output: +Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp + +Run Build Command:/usr/bin/make "cmTryCompileExec4189733644/fast" +/usr/bin/make -f CMakeFiles/cmTryCompileExec4189733644.dir/build.make CMakeFiles/cmTryCompileExec4189733644.dir/build +make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' +/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 +Building C object CMakeFiles/cmTryCompileExec4189733644.dir/testCCompiler.c.o +/usr/bin/cc -o CMakeFiles/cmTryCompileExec4189733644.dir/testCCompiler.c.o -c /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/testCCompiler.c +Linking C executable cmTryCompileExec4189733644 +/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec4189733644.dir/link.txt --verbose=1 +/usr/bin/cc CMakeFiles/cmTryCompileExec4189733644.dir/testCCompiler.c.o -o cmTryCompileExec4189733644 -rdynamic +make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' + + +Detecting C compiler ABI info compiled with the following output: +Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp + +Run Build Command:/usr/bin/make "cmTryCompileExec2944435992/fast" +/usr/bin/make -f CMakeFiles/cmTryCompileExec2944435992.dir/build.make CMakeFiles/cmTryCompileExec2944435992.dir/build +make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' +/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 +Building C object CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o +/usr/bin/cc -o CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -c /usr/share/cmake-2.8/Modules/CMakeCCompilerABI.c +Linking C executable cmTryCompileExec2944435992 +/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec2944435992.dir/link.txt --verbose=1 +/usr/bin/cc -v CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -o cmTryCompileExec2944435992 -rdynamic +Using built-in specs. +COLLECT_GCC=/usr/bin/cc +COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/4.8/lto-wrapper +Target: x86_64-linux-gnu +Configured with: ../src/configure -v --with-pkgversion='Ubuntu 4.8.2-19ubuntu1' --with-bugurl=file:///usr/share/doc/gcc-4.8/README.Bugs --enable-languages=c,c++,java,go,d,fortran,objc,obj-c++ --prefix=/usr --program-suffix=-4.8 --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --with-gxx-include-dir=/usr/include/c++/4.8 --libdir=/usr/lib --enable-nls --with-sysroot=/ --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --enable-gnu-unique-object --disable-libmudflap --enable-plugin --with-system-zlib --disable-browser-plugin --enable-java-awt=gtk --enable-gtk-cairo --with-java-home=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64/jre --enable-java-home --with-jvm-root-dir=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64 --with-jvm-jar-dir=/usr/lib/jvm-exports/java-1.5.0-gcj-4.8-amd64 --with-arch-directory=amd64 --with-ecj-jar=/usr/share/java/eclipse-ecj.jar --enable-objc-gc --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --with-tune=generic --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu +Thread model: posix +gcc version 4.8.2 (Ubuntu 4.8.2-19ubuntu1) +COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/ +LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../:/lib/:/usr/lib/ +COLLECT_GCC_OPTIONS='-v' '-o' 'cmTryCompileExec2944435992' '-rdynamic' '-mtune=generic' '-march=x86-64' + /usr/lib/gcc/x86_64-linux-gnu/4.8/collect2 --sysroot=/ --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -z relro -o cmTryCompileExec2944435992 /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o -L/usr/lib/gcc/x86_64-linux-gnu/4.8 -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../.. CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -lgcc --as-needed -lgcc_s --no-as-needed -lc -lgcc --as-needed -lgcc_s --no-as-needed /usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o +make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' + + +Parsed C implicit link information from above output: + link line regex: [^( *|.*[/\])(ld|([^/\]+-)?ld|collect2)[^/\]*( |$)] + ignore line: [Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp] + ignore line: [] + ignore line: [Run Build Command:/usr/bin/make "cmTryCompileExec2944435992/fast"] + ignore line: [/usr/bin/make -f CMakeFiles/cmTryCompileExec2944435992.dir/build.make CMakeFiles/cmTryCompileExec2944435992.dir/build] + ignore line: [make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'] + ignore line: [/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1] + ignore line: [Building C object CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o] + ignore line: [/usr/bin/cc -o CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -c /usr/share/cmake-2.8/Modules/CMakeCCompilerABI.c] + ignore line: [Linking C executable cmTryCompileExec2944435992] + ignore line: [/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec2944435992.dir/link.txt --verbose=1] + ignore line: [/usr/bin/cc -v CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -o cmTryCompileExec2944435992 -rdynamic ] + ignore line: [Using built-in specs.] + ignore line: [COLLECT_GCC=/usr/bin/cc] + ignore line: [COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/4.8/lto-wrapper] + ignore line: [Target: x86_64-linux-gnu] + ignore line: [Configured with: ../src/configure -v --with-pkgversion='Ubuntu 4.8.2-19ubuntu1' --with-bugurl=file:///usr/share/doc/gcc-4.8/README.Bugs --enable-languages=c,c++,java,go,d,fortran,objc,obj-c++ --prefix=/usr --program-suffix=-4.8 --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --with-gxx-include-dir=/usr/include/c++/4.8 --libdir=/usr/lib --enable-nls --with-sysroot=/ --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --enable-gnu-unique-object --disable-libmudflap --enable-plugin --with-system-zlib --disable-browser-plugin --enable-java-awt=gtk --enable-gtk-cairo --with-java-home=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64/jre --enable-java-home --with-jvm-root-dir=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64 --with-jvm-jar-dir=/usr/lib/jvm-exports/java-1.5.0-gcj-4.8-amd64 --with-arch-directory=amd64 --with-ecj-jar=/usr/share/java/eclipse-ecj.jar --enable-objc-gc --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --with-tune=generic --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu] + ignore line: [Thread model: posix] + ignore line: [gcc version 4.8.2 (Ubuntu 4.8.2-19ubuntu1) ] + ignore line: [COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/] + ignore line: [LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../:/lib/:/usr/lib/] + ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'cmTryCompileExec2944435992' '-rdynamic' '-mtune=generic' '-march=x86-64'] + link line: [ /usr/lib/gcc/x86_64-linux-gnu/4.8/collect2 --sysroot=/ --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -z relro -o cmTryCompileExec2944435992 /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o -L/usr/lib/gcc/x86_64-linux-gnu/4.8 -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../.. CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -lgcc --as-needed -lgcc_s --no-as-needed -lc -lgcc --as-needed -lgcc_s --no-as-needed /usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o] + arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/collect2] ==> ignore + arg [--sysroot=/] ==> ignore + arg [--build-id] ==> ignore + arg [--eh-frame-hdr] ==> ignore + arg [-m] ==> ignore + arg [elf_x86_64] ==> ignore + arg [--hash-style=gnu] ==> ignore + arg [--as-needed] ==> ignore + arg [-export-dynamic] ==> ignore + arg [-dynamic-linker] ==> ignore + arg [/lib64/ld-linux-x86-64.so.2] ==> ignore + arg [-zrelro] ==> ignore + arg [-o] ==> ignore + arg [cmTryCompileExec2944435992] ==> ignore + arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o] ==> ignore + arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o] ==> ignore + arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o] ==> ignore + arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8] + arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] + arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] + arg [-L/lib/x86_64-linux-gnu] ==> dir [/lib/x86_64-linux-gnu] + arg [-L/lib/../lib] ==> dir [/lib/../lib] + arg [-L/usr/lib/x86_64-linux-gnu] ==> dir [/usr/lib/x86_64-linux-gnu] + arg [-L/usr/lib/../lib] ==> dir [/usr/lib/../lib] + arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] + arg [CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o] ==> ignore + arg [-lgcc] ==> lib [gcc] + arg [--as-needed] ==> ignore + arg [-lgcc_s] ==> lib [gcc_s] + arg [--no-as-needed] ==> ignore + arg [-lc] ==> lib [c] + arg [-lgcc] ==> lib [gcc] + arg [--as-needed] ==> ignore + arg [-lgcc_s] ==> lib [gcc_s] + arg [--no-as-needed] ==> ignore + arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o] ==> ignore + arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o] ==> ignore + remove lib [gcc] + remove lib [gcc_s] + remove lib [gcc] + remove lib [gcc_s] + collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8] ==> [/usr/lib/gcc/x86_64-linux-gnu/4.8] + collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu] + collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] ==> [/usr/lib] + collapse library dir [/lib/x86_64-linux-gnu] ==> [/lib/x86_64-linux-gnu] + collapse library dir [/lib/../lib] ==> [/lib] + collapse library dir [/usr/lib/x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu] + collapse library dir [/usr/lib/../lib] ==> [/usr/lib] + collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] ==> [/usr/lib] + implicit libs: [c] + implicit dirs: [/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib] + implicit fwks: [] + + +Determining if the CXX compiler works passed with the following output: +Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp + +Run Build Command:/usr/bin/make "cmTryCompileExec191173331/fast" +/usr/bin/make -f CMakeFiles/cmTryCompileExec191173331.dir/build.make CMakeFiles/cmTryCompileExec191173331.dir/build +make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' +/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 +Building CXX object CMakeFiles/cmTryCompileExec191173331.dir/testCXXCompiler.cxx.o +/usr/bin/c++ -o CMakeFiles/cmTryCompileExec191173331.dir/testCXXCompiler.cxx.o -c /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/testCXXCompiler.cxx +Linking CXX executable cmTryCompileExec191173331 +/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec191173331.dir/link.txt --verbose=1 +/usr/bin/c++ CMakeFiles/cmTryCompileExec191173331.dir/testCXXCompiler.cxx.o -o cmTryCompileExec191173331 -rdynamic +make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' + + +Detecting CXX compiler ABI info compiled with the following output: +Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp + +Run Build Command:/usr/bin/make "cmTryCompileExec843003076/fast" +/usr/bin/make -f CMakeFiles/cmTryCompileExec843003076.dir/build.make CMakeFiles/cmTryCompileExec843003076.dir/build +make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' +/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 +Building CXX object CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o +/usr/bin/c++ -o CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -c /usr/share/cmake-2.8/Modules/CMakeCXXCompilerABI.cpp +Linking CXX executable cmTryCompileExec843003076 +/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec843003076.dir/link.txt --verbose=1 +/usr/bin/c++ -v CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -o cmTryCompileExec843003076 -rdynamic +Using built-in specs. +COLLECT_GCC=/usr/bin/c++ +COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/4.8/lto-wrapper +Target: x86_64-linux-gnu +Configured with: ../src/configure -v --with-pkgversion='Ubuntu 4.8.2-19ubuntu1' --with-bugurl=file:///usr/share/doc/gcc-4.8/README.Bugs --enable-languages=c,c++,java,go,d,fortran,objc,obj-c++ --prefix=/usr --program-suffix=-4.8 --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --with-gxx-include-dir=/usr/include/c++/4.8 --libdir=/usr/lib --enable-nls --with-sysroot=/ --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --enable-gnu-unique-object --disable-libmudflap --enable-plugin --with-system-zlib --disable-browser-plugin --enable-java-awt=gtk --enable-gtk-cairo --with-java-home=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64/jre --enable-java-home --with-jvm-root-dir=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64 --with-jvm-jar-dir=/usr/lib/jvm-exports/java-1.5.0-gcj-4.8-amd64 --with-arch-directory=amd64 --with-ecj-jar=/usr/share/java/eclipse-ecj.jar --enable-objc-gc --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --with-tune=generic --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu +Thread model: posix +gcc version 4.8.2 (Ubuntu 4.8.2-19ubuntu1) +COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/ +LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../:/lib/:/usr/lib/ +COLLECT_GCC_OPTIONS='-v' '-o' 'cmTryCompileExec843003076' '-rdynamic' '-shared-libgcc' '-mtune=generic' '-march=x86-64' + /usr/lib/gcc/x86_64-linux-gnu/4.8/collect2 --sysroot=/ --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -z relro -o cmTryCompileExec843003076 /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o -L/usr/lib/gcc/x86_64-linux-gnu/4.8 -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../.. CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -lstdc++ -lm -lgcc_s -lgcc -lc -lgcc_s -lgcc /usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o +make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' + + +Parsed CXX implicit link information from above output: + link line regex: [^( *|.*[/\])(ld|([^/\]+-)?ld|collect2)[^/\]*( |$)] + ignore line: [Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp] + ignore line: [] + ignore line: [Run Build Command:/usr/bin/make "cmTryCompileExec843003076/fast"] + ignore line: [/usr/bin/make -f CMakeFiles/cmTryCompileExec843003076.dir/build.make CMakeFiles/cmTryCompileExec843003076.dir/build] + ignore line: [make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'] + ignore line: [/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1] + ignore line: [Building CXX object CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o] + ignore line: [/usr/bin/c++ -o CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -c /usr/share/cmake-2.8/Modules/CMakeCXXCompilerABI.cpp] + ignore line: [Linking CXX executable cmTryCompileExec843003076] + ignore line: [/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec843003076.dir/link.txt --verbose=1] + ignore line: [/usr/bin/c++ -v CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -o cmTryCompileExec843003076 -rdynamic ] + ignore line: [Using built-in specs.] + ignore line: [COLLECT_GCC=/usr/bin/c++] + ignore line: [COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/4.8/lto-wrapper] + ignore line: [Target: x86_64-linux-gnu] + ignore line: [Configured with: ../src/configure -v --with-pkgversion='Ubuntu 4.8.2-19ubuntu1' --with-bugurl=file:///usr/share/doc/gcc-4.8/README.Bugs --enable-languages=c,c++,java,go,d,fortran,objc,obj-c++ --prefix=/usr --program-suffix=-4.8 --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --with-gxx-include-dir=/usr/include/c++/4.8 --libdir=/usr/lib --enable-nls --with-sysroot=/ --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --enable-gnu-unique-object --disable-libmudflap --enable-plugin --with-system-zlib --disable-browser-plugin --enable-java-awt=gtk --enable-gtk-cairo --with-java-home=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64/jre --enable-java-home --with-jvm-root-dir=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64 --with-jvm-jar-dir=/usr/lib/jvm-exports/java-1.5.0-gcj-4.8-amd64 --with-arch-directory=amd64 --with-ecj-jar=/usr/share/java/eclipse-ecj.jar --enable-objc-gc --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --with-tune=generic --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu] + ignore line: [Thread model: posix] + ignore line: [gcc version 4.8.2 (Ubuntu 4.8.2-19ubuntu1) ] + ignore line: [COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/] + ignore line: [LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../:/lib/:/usr/lib/] + ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'cmTryCompileExec843003076' '-rdynamic' '-shared-libgcc' '-mtune=generic' '-march=x86-64'] + link line: [ /usr/lib/gcc/x86_64-linux-gnu/4.8/collect2 --sysroot=/ --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -z relro -o cmTryCompileExec843003076 /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o -L/usr/lib/gcc/x86_64-linux-gnu/4.8 -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../.. CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -lstdc++ -lm -lgcc_s -lgcc -lc -lgcc_s -lgcc /usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o] + arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/collect2] ==> ignore + arg [--sysroot=/] ==> ignore + arg [--build-id] ==> ignore + arg [--eh-frame-hdr] ==> ignore + arg [-m] ==> ignore + arg [elf_x86_64] ==> ignore + arg [--hash-style=gnu] ==> ignore + arg [--as-needed] ==> ignore + arg [-export-dynamic] ==> ignore + arg [-dynamic-linker] ==> ignore + arg [/lib64/ld-linux-x86-64.so.2] ==> ignore + arg [-zrelro] ==> ignore + arg [-o] ==> ignore + arg [cmTryCompileExec843003076] ==> ignore + arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o] ==> ignore + arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o] ==> ignore + arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o] ==> ignore + arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8] + arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] + arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] + arg [-L/lib/x86_64-linux-gnu] ==> dir [/lib/x86_64-linux-gnu] + arg [-L/lib/../lib] ==> dir [/lib/../lib] + arg [-L/usr/lib/x86_64-linux-gnu] ==> dir [/usr/lib/x86_64-linux-gnu] + arg [-L/usr/lib/../lib] ==> dir [/usr/lib/../lib] + arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] + arg [CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o] ==> ignore + arg [-lstdc++] ==> lib [stdc++] + arg [-lm] ==> lib [m] + arg [-lgcc_s] ==> lib [gcc_s] + arg [-lgcc] ==> lib [gcc] + arg [-lc] ==> lib [c] + arg [-lgcc_s] ==> lib [gcc_s] + arg [-lgcc] ==> lib [gcc] + arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o] ==> ignore + arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o] ==> ignore + remove lib [gcc_s] + remove lib [gcc] + remove lib [gcc_s] + remove lib [gcc] + collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8] ==> [/usr/lib/gcc/x86_64-linux-gnu/4.8] + collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu] + collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] ==> [/usr/lib] + collapse library dir [/lib/x86_64-linux-gnu] ==> [/lib/x86_64-linux-gnu] + collapse library dir [/lib/../lib] ==> [/lib] + collapse library dir [/usr/lib/x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu] + collapse library dir [/usr/lib/../lib] ==> [/usr/lib] + collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] ==> [/usr/lib] + implicit libs: [stdc++;m;c] + implicit dirs: [/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib] + implicit fwks: [] + + +Determining if files pthread.h exist passed with the following output: +Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp + +Run Build Command:/usr/bin/make "cmTryCompileExec3897003384/fast" +/usr/bin/make -f CMakeFiles/cmTryCompileExec3897003384.dir/build.make CMakeFiles/cmTryCompileExec3897003384.dir/build +make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' +/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 +Building C object CMakeFiles/cmTryCompileExec3897003384.dir/CheckIncludeFiles.c.o +/usr/bin/cc -o CMakeFiles/cmTryCompileExec3897003384.dir/CheckIncludeFiles.c.o -c /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CheckIncludeFiles.c +Linking C executable cmTryCompileExec3897003384 +/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec3897003384.dir/link.txt --verbose=1 +/usr/bin/cc CMakeFiles/cmTryCompileExec3897003384.dir/CheckIncludeFiles.c.o -o cmTryCompileExec3897003384 -rdynamic +make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' + + +Determining if the function pthread_create exists in the pthread passed with the following output: +Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp + +Run Build Command:/usr/bin/make "cmTryCompileExec4077710905/fast" +/usr/bin/make -f CMakeFiles/cmTryCompileExec4077710905.dir/build.make CMakeFiles/cmTryCompileExec4077710905.dir/build +make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' +/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 +Building C object CMakeFiles/cmTryCompileExec4077710905.dir/CheckFunctionExists.c.o +/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create -o CMakeFiles/cmTryCompileExec4077710905.dir/CheckFunctionExists.c.o -c /usr/share/cmake-2.8/Modules/CheckFunctionExists.c +Linking C executable cmTryCompileExec4077710905 +/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec4077710905.dir/link.txt --verbose=1 +/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create CMakeFiles/cmTryCompileExec4077710905.dir/CheckFunctionExists.c.o -o cmTryCompileExec4077710905 -rdynamic -lpthread +make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' + + diff --git a/build/CMakeFiles/CMakeRuleHashes.txt b/build/CMakeFiles/CMakeRuleHashes.txt new file mode 100644 index 000000000..b1bfe30c0 --- /dev/null +++ b/build/CMakeFiles/CMakeRuleHashes.txt @@ -0,0 +1,5 @@ +# Hashes of file build rules. +353bd50eb4c4b757d9a3d734d52d4f76 CMakeFiles/clean_test_results +d83f452b18a5909d95cdb786c10abffb CMakeFiles/doxygen +d83f452b18a5909d95cdb786c10abffb CMakeFiles/run_tests +d83f452b18a5909d95cdb786c10abffb CMakeFiles/tests diff --git a/build/CMakeFiles/Makefile.cmake b/build/CMakeFiles/Makefile.cmake new file mode 100644 index 000000000..d232bacff --- /dev/null +++ b/build/CMakeFiles/Makefile.cmake @@ -0,0 +1,150 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +# The generator used is: +SET(CMAKE_DEPENDS_GENERATOR "Unix Makefiles") + +# The top level Makefile was generated from the following files: +SET(CMAKE_MAKEFILE_DEPENDS + "CMakeCache.txt" + "CMakeFiles/2.8.12.2/CMakeCCompiler.cmake" + "CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake" + "CMakeFiles/2.8.12.2/CMakeSystem.cmake" + "catkin/catkin_generated/version/package.cmake" + "catkin_generated/order_packages.cmake" + "/home/roman/src/Firmware/src/CMakeLists.txt" + "/opt/ros/indigo/share/catkin/cmake/../package.xml" + "/opt/ros/indigo/share/catkin/cmake/all.cmake" + "/opt/ros/indigo/share/catkin/cmake/assert.cmake" + "/opt/ros/indigo/share/catkin/cmake/atomic_configure_file.cmake" + "/opt/ros/indigo/share/catkin/cmake/catkinConfig-version.cmake" + "/opt/ros/indigo/share/catkin/cmake/catkinConfig.cmake" + "/opt/ros/indigo/share/catkin/cmake/catkin_add_env_hooks.cmake" + "/opt/ros/indigo/share/catkin/cmake/catkin_destinations.cmake" + "/opt/ros/indigo/share/catkin/cmake/catkin_generate_environment.cmake" + "/opt/ros/indigo/share/catkin/cmake/catkin_install_python.cmake" + "/opt/ros/indigo/share/catkin/cmake/catkin_libraries.cmake" + "/opt/ros/indigo/share/catkin/cmake/catkin_metapackage.cmake" + "/opt/ros/indigo/share/catkin/cmake/catkin_package.cmake" + "/opt/ros/indigo/share/catkin/cmake/catkin_package_xml.cmake" + "/opt/ros/indigo/share/catkin/cmake/catkin_python_setup.cmake" + "/opt/ros/indigo/share/catkin/cmake/catkin_workspace.cmake" + "/opt/ros/indigo/share/catkin/cmake/debug_message.cmake" + "/opt/ros/indigo/share/catkin/cmake/em/order_packages.cmake.em" + "/opt/ros/indigo/share/catkin/cmake/em_expand.cmake" + "/opt/ros/indigo/share/catkin/cmake/empy.cmake" + "/opt/ros/indigo/share/catkin/cmake/env-hooks/05.catkin-test-results.sh.develspace.in" + "/opt/ros/indigo/share/catkin/cmake/find_program_required.cmake" + "/opt/ros/indigo/share/catkin/cmake/interrogate_setup_dot_py.py" + "/opt/ros/indigo/share/catkin/cmake/legacy.cmake" + "/opt/ros/indigo/share/catkin/cmake/list_append_deduplicate.cmake" + "/opt/ros/indigo/share/catkin/cmake/list_append_unique.cmake" + "/opt/ros/indigo/share/catkin/cmake/list_insert_in_workspace_order.cmake" + "/opt/ros/indigo/share/catkin/cmake/platform/lsb.cmake" + "/opt/ros/indigo/share/catkin/cmake/platform/ubuntu.cmake" + "/opt/ros/indigo/share/catkin/cmake/platform/windows.cmake" + "/opt/ros/indigo/share/catkin/cmake/python.cmake" + "/opt/ros/indigo/share/catkin/cmake/safe_execute_process.cmake" + "/opt/ros/indigo/share/catkin/cmake/stamp.cmake" + "/opt/ros/indigo/share/catkin/cmake/string_starts_with.cmake" + "/opt/ros/indigo/share/catkin/cmake/templates/_setup_util.py.in" + "/opt/ros/indigo/share/catkin/cmake/templates/env.sh.in" + "/opt/ros/indigo/share/catkin/cmake/templates/generate_cached_setup.py.in" + "/opt/ros/indigo/share/catkin/cmake/templates/order_packages.context.py.in" + "/opt/ros/indigo/share/catkin/cmake/templates/rosinstall.in" + "/opt/ros/indigo/share/catkin/cmake/templates/setup.bash.in" + "/opt/ros/indigo/share/catkin/cmake/templates/setup.sh.in" + "/opt/ros/indigo/share/catkin/cmake/templates/setup.zsh.in" + "/opt/ros/indigo/share/catkin/cmake/test/catkin_download_test_data.cmake" + "/opt/ros/indigo/share/catkin/cmake/test/gtest.cmake" + "/opt/ros/indigo/share/catkin/cmake/test/nosetests.cmake" + "/opt/ros/indigo/share/catkin/cmake/test/tests.cmake" + "/opt/ros/indigo/share/catkin/cmake/tools/doxygen.cmake" + "/opt/ros/indigo/share/catkin/cmake/tools/libraries.cmake" + "/opt/ros/indigo/share/catkin/cmake/tools/rt.cmake" + "/usr/share/cmake-2.8/Modules/CMakeCCompiler.cmake.in" + "/usr/share/cmake-2.8/Modules/CMakeCCompilerABI.c" + "/usr/share/cmake-2.8/Modules/CMakeCInformation.cmake" + "/usr/share/cmake-2.8/Modules/CMakeCXXCompiler.cmake.in" + "/usr/share/cmake-2.8/Modules/CMakeCXXCompilerABI.cpp" + "/usr/share/cmake-2.8/Modules/CMakeCXXInformation.cmake" + "/usr/share/cmake-2.8/Modules/CMakeClDeps.cmake" + "/usr/share/cmake-2.8/Modules/CMakeCommonLanguageInclude.cmake" + "/usr/share/cmake-2.8/Modules/CMakeConfigurableFile.in" + "/usr/share/cmake-2.8/Modules/CMakeDetermineCCompiler.cmake" + "/usr/share/cmake-2.8/Modules/CMakeDetermineCXXCompiler.cmake" + "/usr/share/cmake-2.8/Modules/CMakeDetermineCompiler.cmake" + "/usr/share/cmake-2.8/Modules/CMakeDetermineCompilerABI.cmake" + "/usr/share/cmake-2.8/Modules/CMakeDetermineCompilerId.cmake" + "/usr/share/cmake-2.8/Modules/CMakeDetermineSystem.cmake" + "/usr/share/cmake-2.8/Modules/CMakeFindBinUtils.cmake" + "/usr/share/cmake-2.8/Modules/CMakeGenericSystem.cmake" + "/usr/share/cmake-2.8/Modules/CMakeParseArguments.cmake" + "/usr/share/cmake-2.8/Modules/CMakeParseImplicitLinkInfo.cmake" + "/usr/share/cmake-2.8/Modules/CMakeSystem.cmake.in" + "/usr/share/cmake-2.8/Modules/CMakeSystemSpecificInformation.cmake" + "/usr/share/cmake-2.8/Modules/CMakeTestCCompiler.cmake" + "/usr/share/cmake-2.8/Modules/CMakeTestCXXCompiler.cmake" + "/usr/share/cmake-2.8/Modules/CMakeTestCompilerCommon.cmake" + "/usr/share/cmake-2.8/Modules/CMakeUnixFindMake.cmake" + "/usr/share/cmake-2.8/Modules/CheckFunctionExists.c" + "/usr/share/cmake-2.8/Modules/CheckIncludeFiles.cmake" + "/usr/share/cmake-2.8/Modules/CheckLibraryExists.cmake" + "/usr/share/cmake-2.8/Modules/CheckSymbolExists.cmake" + "/usr/share/cmake-2.8/Modules/Compiler/GNU-C.cmake" + "/usr/share/cmake-2.8/Modules/Compiler/GNU-CXX.cmake" + "/usr/share/cmake-2.8/Modules/Compiler/GNU.cmake" + "/usr/share/cmake-2.8/Modules/FindGTest.cmake" + "/usr/share/cmake-2.8/Modules/FindPackageHandleStandardArgs.cmake" + "/usr/share/cmake-2.8/Modules/FindPackageMessage.cmake" + "/usr/share/cmake-2.8/Modules/FindPythonInterp.cmake" + "/usr/share/cmake-2.8/Modules/FindThreads.cmake" + "/usr/share/cmake-2.8/Modules/MultiArchCross.cmake" + "/usr/share/cmake-2.8/Modules/Platform/Linux-CXX.cmake" + "/usr/share/cmake-2.8/Modules/Platform/Linux-GNU-C.cmake" + "/usr/share/cmake-2.8/Modules/Platform/Linux-GNU-CXX.cmake" + "/usr/share/cmake-2.8/Modules/Platform/Linux-GNU.cmake" + "/usr/share/cmake-2.8/Modules/Platform/Linux.cmake" + "/usr/share/cmake-2.8/Modules/Platform/UnixPaths.cmake" + "/usr/src/gtest/CMakeLists.txt" + "/usr/src/gtest/cmake/internal_utils.cmake" + ) + +# The corresponding makefile is: +SET(CMAKE_MAKEFILE_OUTPUTS + "Makefile" + "CMakeFiles/cmake.check_cache" + ) + +# Byproducts of CMake generate step: +SET(CMAKE_MAKEFILE_PRODUCTS + "CMakeFiles/2.8.12.2/CMakeSystem.cmake" + "CMakeFiles/2.8.12.2/CMakeCCompiler.cmake" + "CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake" + "CMakeFiles/2.8.12.2/CMakeCCompiler.cmake" + "CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake" + "catkin_generated/stamps/Project/package.xml.stamp" + "catkin_generated/installspace/_setup_util.py" + "catkin_generated/installspace/env.sh" + "catkin_generated/installspace/setup.bash" + "catkin_generated/installspace/setup.sh" + "catkin_generated/installspace/setup.zsh" + "catkin_generated/installspace/.rosinstall" + "catkin_generated/generate_cached_setup.py" + "catkin_generated/env_cached.sh" + "catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp" + "catkin_generated/order_packages.py" + "catkin_generated/stamps/Project/order_packages.cmake.em.stamp" + "CMakeFiles/CMakeDirectoryInformation.cmake" + "gtest/CMakeFiles/CMakeDirectoryInformation.cmake" + ) + +# Dependency information for all targets: +SET(CMAKE_DEPEND_INFO_FILES + "CMakeFiles/clean_test_results.dir/DependInfo.cmake" + "CMakeFiles/doxygen.dir/DependInfo.cmake" + "CMakeFiles/run_tests.dir/DependInfo.cmake" + "CMakeFiles/tests.dir/DependInfo.cmake" + "gtest/CMakeFiles/gtest.dir/DependInfo.cmake" + "gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake" + ) diff --git a/build/CMakeFiles/Makefile2 b/build/CMakeFiles/Makefile2 new file mode 100644 index 000000000..2a11fe8ef --- /dev/null +++ b/build/CMakeFiles/Makefile2 @@ -0,0 +1,266 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +# Default target executed when no arguments are given to make. +default_target: all +.PHONY : default_target + +# The main recursive all target +all: +.PHONY : all + +# The main recursive preinstall target +preinstall: +.PHONY : preinstall + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E remove -f + +# Escaping for special characters. +EQUALS = = + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /home/roman/src/Firmware/build + +#============================================================================= +# Target rules for target CMakeFiles/clean_test_results.dir + +# All Build rule for target. +CMakeFiles/clean_test_results.dir/all: + $(MAKE) -f CMakeFiles/clean_test_results.dir/build.make CMakeFiles/clean_test_results.dir/depend + $(MAKE) -f CMakeFiles/clean_test_results.dir/build.make CMakeFiles/clean_test_results.dir/build + $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles + @echo "Built target clean_test_results" +.PHONY : CMakeFiles/clean_test_results.dir/all + +# Build rule for subdir invocation for target. +CMakeFiles/clean_test_results.dir/rule: cmake_check_build_system + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 + $(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/clean_test_results.dir/all + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 +.PHONY : CMakeFiles/clean_test_results.dir/rule + +# Convenience name for target. +clean_test_results: CMakeFiles/clean_test_results.dir/rule +.PHONY : clean_test_results + +# clean rule for target. +CMakeFiles/clean_test_results.dir/clean: + $(MAKE) -f CMakeFiles/clean_test_results.dir/build.make CMakeFiles/clean_test_results.dir/clean +.PHONY : CMakeFiles/clean_test_results.dir/clean + +# clean rule for target. +clean: CMakeFiles/clean_test_results.dir/clean +.PHONY : clean + +#============================================================================= +# Target rules for target CMakeFiles/doxygen.dir + +# All Build rule for target. +CMakeFiles/doxygen.dir/all: + $(MAKE) -f CMakeFiles/doxygen.dir/build.make CMakeFiles/doxygen.dir/depend + $(MAKE) -f CMakeFiles/doxygen.dir/build.make CMakeFiles/doxygen.dir/build + $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles + @echo "Built target doxygen" +.PHONY : CMakeFiles/doxygen.dir/all + +# Build rule for subdir invocation for target. +CMakeFiles/doxygen.dir/rule: cmake_check_build_system + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 + $(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/doxygen.dir/all + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 +.PHONY : CMakeFiles/doxygen.dir/rule + +# Convenience name for target. +doxygen: CMakeFiles/doxygen.dir/rule +.PHONY : doxygen + +# clean rule for target. +CMakeFiles/doxygen.dir/clean: + $(MAKE) -f CMakeFiles/doxygen.dir/build.make CMakeFiles/doxygen.dir/clean +.PHONY : CMakeFiles/doxygen.dir/clean + +# clean rule for target. +clean: CMakeFiles/doxygen.dir/clean +.PHONY : clean + +#============================================================================= +# Target rules for target CMakeFiles/run_tests.dir + +# All Build rule for target. +CMakeFiles/run_tests.dir/all: + $(MAKE) -f CMakeFiles/run_tests.dir/build.make CMakeFiles/run_tests.dir/depend + $(MAKE) -f CMakeFiles/run_tests.dir/build.make CMakeFiles/run_tests.dir/build + $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles + @echo "Built target run_tests" +.PHONY : CMakeFiles/run_tests.dir/all + +# Build rule for subdir invocation for target. +CMakeFiles/run_tests.dir/rule: cmake_check_build_system + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 + $(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/run_tests.dir/all + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 +.PHONY : CMakeFiles/run_tests.dir/rule + +# Convenience name for target. +run_tests: CMakeFiles/run_tests.dir/rule +.PHONY : run_tests + +# clean rule for target. +CMakeFiles/run_tests.dir/clean: + $(MAKE) -f CMakeFiles/run_tests.dir/build.make CMakeFiles/run_tests.dir/clean +.PHONY : CMakeFiles/run_tests.dir/clean + +# clean rule for target. +clean: CMakeFiles/run_tests.dir/clean +.PHONY : clean + +#============================================================================= +# Target rules for target CMakeFiles/tests.dir + +# All Build rule for target. +CMakeFiles/tests.dir/all: + $(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/depend + $(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/build + $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles + @echo "Built target tests" +.PHONY : CMakeFiles/tests.dir/all + +# Build rule for subdir invocation for target. +CMakeFiles/tests.dir/rule: cmake_check_build_system + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 + $(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/tests.dir/all + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 +.PHONY : CMakeFiles/tests.dir/rule + +# Convenience name for target. +tests: CMakeFiles/tests.dir/rule +.PHONY : tests + +# clean rule for target. +CMakeFiles/tests.dir/clean: + $(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/clean +.PHONY : CMakeFiles/tests.dir/clean + +# clean rule for target. +clean: CMakeFiles/tests.dir/clean +.PHONY : clean + +#============================================================================= +# Directory level rules for directory gtest + +# Convenience name for "all" pass in the directory. +gtest/all: +.PHONY : gtest/all + +# Convenience name for "clean" pass in the directory. +gtest/clean: gtest/CMakeFiles/gtest.dir/clean +gtest/clean: gtest/CMakeFiles/gtest_main.dir/clean +.PHONY : gtest/clean + +# Convenience name for "preinstall" pass in the directory. +gtest/preinstall: +.PHONY : gtest/preinstall + +#============================================================================= +# Target rules for target gtest/CMakeFiles/gtest.dir + +# All Build rule for target. +gtest/CMakeFiles/gtest.dir/all: + $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/depend + $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/build + $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles 1 + @echo "Built target gtest" +.PHONY : gtest/CMakeFiles/gtest.dir/all + +# Build rule for subdir invocation for target. +gtest/CMakeFiles/gtest.dir/rule: cmake_check_build_system + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 1 + $(MAKE) -f CMakeFiles/Makefile2 gtest/CMakeFiles/gtest.dir/all + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 +.PHONY : gtest/CMakeFiles/gtest.dir/rule + +# Convenience name for target. +gtest: gtest/CMakeFiles/gtest.dir/rule +.PHONY : gtest + +# clean rule for target. +gtest/CMakeFiles/gtest.dir/clean: + $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/clean +.PHONY : gtest/CMakeFiles/gtest.dir/clean + +# clean rule for target. +clean: gtest/CMakeFiles/gtest.dir/clean +.PHONY : clean + +#============================================================================= +# Target rules for target gtest/CMakeFiles/gtest_main.dir + +# All Build rule for target. +gtest/CMakeFiles/gtest_main.dir/all: gtest/CMakeFiles/gtest.dir/all + $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/depend + $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/build + $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles 2 + @echo "Built target gtest_main" +.PHONY : gtest/CMakeFiles/gtest_main.dir/all + +# Build rule for subdir invocation for target. +gtest/CMakeFiles/gtest_main.dir/rule: cmake_check_build_system + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 2 + $(MAKE) -f CMakeFiles/Makefile2 gtest/CMakeFiles/gtest_main.dir/all + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 +.PHONY : gtest/CMakeFiles/gtest_main.dir/rule + +# Convenience name for target. +gtest_main: gtest/CMakeFiles/gtest_main.dir/rule +.PHONY : gtest_main + +# clean rule for target. +gtest/CMakeFiles/gtest_main.dir/clean: + $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/clean +.PHONY : gtest/CMakeFiles/gtest_main.dir/clean + +# clean rule for target. +clean: gtest/CMakeFiles/gtest_main.dir/clean +.PHONY : clean + +#============================================================================= +# Special targets to cleanup operation of make. + +# Special rule to run CMake to check the build system integrity. +# No rule that depends on this can have commands that come from listfiles +# because they might be regenerated. +cmake_check_build_system: + $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 +.PHONY : cmake_check_build_system + diff --git a/build/CMakeFiles/TargetDirectories.txt b/build/CMakeFiles/TargetDirectories.txt new file mode 100644 index 000000000..552e0c540 --- /dev/null +++ b/build/CMakeFiles/TargetDirectories.txt @@ -0,0 +1,6 @@ +/home/roman/src/Firmware/build/CMakeFiles/clean_test_results.dir +/home/roman/src/Firmware/build/CMakeFiles/doxygen.dir +/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest.dir +/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest_main.dir +/home/roman/src/Firmware/build/CMakeFiles/run_tests.dir +/home/roman/src/Firmware/build/CMakeFiles/tests.dir diff --git a/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake b/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake new file mode 100644 index 000000000..7aff3a53c --- /dev/null +++ b/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake @@ -0,0 +1,20 @@ +# The set of languages for which implicit dependencies are needed: +SET(CMAKE_DEPENDS_LANGUAGES + ) +# The set of files for implicit dependencies of each language: + +# Preprocessor definitions for this target. +SET(CMAKE_TARGET_DEFINITIONS + "ROS_BUILD_SHARED_LIBS=1" + ) + +# Targets to which this target links. +SET(CMAKE_TARGET_LINKED_INFO_FILES + ) + +# The include file search paths: +SET(CMAKE_C_TARGET_INCLUDE_PATH + ) +SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) +SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) +SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) diff --git a/build/CMakeFiles/clean_test_results.dir/build.make b/build/CMakeFiles/clean_test_results.dir/build.make new file mode 100644 index 000000000..b3776d0df --- /dev/null +++ b/build/CMakeFiles/clean_test_results.dir/build.make @@ -0,0 +1,66 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E remove -f + +# Escaping for special characters. +EQUALS = = + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /home/roman/src/Firmware/build + +# Utility rule file for clean_test_results. + +# Include the progress variables for this target. +include CMakeFiles/clean_test_results.dir/progress.make + +CMakeFiles/clean_test_results: + /usr/bin/cmake -E remove_directory /home/roman/src/Firmware/build/test_results + +clean_test_results: CMakeFiles/clean_test_results +clean_test_results: CMakeFiles/clean_test_results.dir/build.make +.PHONY : clean_test_results + +# Rule to build all files generated by this target. +CMakeFiles/clean_test_results.dir/build: clean_test_results +.PHONY : CMakeFiles/clean_test_results.dir/build + +CMakeFiles/clean_test_results.dir/clean: + $(CMAKE_COMMAND) -P CMakeFiles/clean_test_results.dir/cmake_clean.cmake +.PHONY : CMakeFiles/clean_test_results.dir/clean + +CMakeFiles/clean_test_results.dir/depend: + cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /home/roman/src/Firmware/src /home/roman/src/Firmware/build /home/roman/src/Firmware/build /home/roman/src/Firmware/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake --color=$(COLOR) +.PHONY : CMakeFiles/clean_test_results.dir/depend + diff --git a/build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake b/build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake new file mode 100644 index 000000000..46c1cb338 --- /dev/null +++ b/build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake @@ -0,0 +1,8 @@ +FILE(REMOVE_RECURSE + "CMakeFiles/clean_test_results" +) + +# Per-language clean rules from dependency scanning. +FOREACH(lang) + INCLUDE(CMakeFiles/clean_test_results.dir/cmake_clean_${lang}.cmake OPTIONAL) +ENDFOREACH(lang) diff --git a/build/CMakeFiles/clean_test_results.dir/progress.make b/build/CMakeFiles/clean_test_results.dir/progress.make new file mode 100644 index 000000000..8b1378917 --- /dev/null +++ b/build/CMakeFiles/clean_test_results.dir/progress.make @@ -0,0 +1 @@ + diff --git a/build/CMakeFiles/cmake.check_cache b/build/CMakeFiles/cmake.check_cache new file mode 100644 index 000000000..3dccd7317 --- /dev/null +++ b/build/CMakeFiles/cmake.check_cache @@ -0,0 +1 @@ +# This file is generated by cmake for dependency checking of the CMakeCache.txt file diff --git a/build/CMakeFiles/doxygen.dir/DependInfo.cmake b/build/CMakeFiles/doxygen.dir/DependInfo.cmake new file mode 100644 index 000000000..7aff3a53c --- /dev/null +++ b/build/CMakeFiles/doxygen.dir/DependInfo.cmake @@ -0,0 +1,20 @@ +# The set of languages for which implicit dependencies are needed: +SET(CMAKE_DEPENDS_LANGUAGES + ) +# The set of files for implicit dependencies of each language: + +# Preprocessor definitions for this target. +SET(CMAKE_TARGET_DEFINITIONS + "ROS_BUILD_SHARED_LIBS=1" + ) + +# Targets to which this target links. +SET(CMAKE_TARGET_LINKED_INFO_FILES + ) + +# The include file search paths: +SET(CMAKE_C_TARGET_INCLUDE_PATH + ) +SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) +SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) +SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) diff --git a/build/CMakeFiles/doxygen.dir/build.make b/build/CMakeFiles/doxygen.dir/build.make new file mode 100644 index 000000000..f5cc02bb3 --- /dev/null +++ b/build/CMakeFiles/doxygen.dir/build.make @@ -0,0 +1,65 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E remove -f + +# Escaping for special characters. +EQUALS = = + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /home/roman/src/Firmware/build + +# Utility rule file for doxygen. + +# Include the progress variables for this target. +include CMakeFiles/doxygen.dir/progress.make + +CMakeFiles/doxygen: + +doxygen: CMakeFiles/doxygen +doxygen: CMakeFiles/doxygen.dir/build.make +.PHONY : doxygen + +# Rule to build all files generated by this target. +CMakeFiles/doxygen.dir/build: doxygen +.PHONY : CMakeFiles/doxygen.dir/build + +CMakeFiles/doxygen.dir/clean: + $(CMAKE_COMMAND) -P CMakeFiles/doxygen.dir/cmake_clean.cmake +.PHONY : CMakeFiles/doxygen.dir/clean + +CMakeFiles/doxygen.dir/depend: + cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /home/roman/src/Firmware/src /home/roman/src/Firmware/build /home/roman/src/Firmware/build /home/roman/src/Firmware/build/CMakeFiles/doxygen.dir/DependInfo.cmake --color=$(COLOR) +.PHONY : CMakeFiles/doxygen.dir/depend + diff --git a/build/CMakeFiles/doxygen.dir/cmake_clean.cmake b/build/CMakeFiles/doxygen.dir/cmake_clean.cmake new file mode 100644 index 000000000..3cf72d90f --- /dev/null +++ b/build/CMakeFiles/doxygen.dir/cmake_clean.cmake @@ -0,0 +1,8 @@ +FILE(REMOVE_RECURSE + "CMakeFiles/doxygen" +) + +# Per-language clean rules from dependency scanning. +FOREACH(lang) + INCLUDE(CMakeFiles/doxygen.dir/cmake_clean_${lang}.cmake OPTIONAL) +ENDFOREACH(lang) diff --git a/build/CMakeFiles/doxygen.dir/progress.make b/build/CMakeFiles/doxygen.dir/progress.make new file mode 100644 index 000000000..8b1378917 --- /dev/null +++ b/build/CMakeFiles/doxygen.dir/progress.make @@ -0,0 +1 @@ + diff --git a/build/CMakeFiles/progress.marks b/build/CMakeFiles/progress.marks new file mode 100644 index 000000000..573541ac9 --- /dev/null +++ b/build/CMakeFiles/progress.marks @@ -0,0 +1 @@ +0 diff --git a/build/CMakeFiles/run_tests.dir/DependInfo.cmake b/build/CMakeFiles/run_tests.dir/DependInfo.cmake new file mode 100644 index 000000000..7aff3a53c --- /dev/null +++ b/build/CMakeFiles/run_tests.dir/DependInfo.cmake @@ -0,0 +1,20 @@ +# The set of languages for which implicit dependencies are needed: +SET(CMAKE_DEPENDS_LANGUAGES + ) +# The set of files for implicit dependencies of each language: + +# Preprocessor definitions for this target. +SET(CMAKE_TARGET_DEFINITIONS + "ROS_BUILD_SHARED_LIBS=1" + ) + +# Targets to which this target links. +SET(CMAKE_TARGET_LINKED_INFO_FILES + ) + +# The include file search paths: +SET(CMAKE_C_TARGET_INCLUDE_PATH + ) +SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) +SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) +SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) diff --git a/build/CMakeFiles/run_tests.dir/build.make b/build/CMakeFiles/run_tests.dir/build.make new file mode 100644 index 000000000..3907c7573 --- /dev/null +++ b/build/CMakeFiles/run_tests.dir/build.make @@ -0,0 +1,65 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E remove -f + +# Escaping for special characters. +EQUALS = = + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /home/roman/src/Firmware/build + +# Utility rule file for run_tests. + +# Include the progress variables for this target. +include CMakeFiles/run_tests.dir/progress.make + +CMakeFiles/run_tests: + +run_tests: CMakeFiles/run_tests +run_tests: CMakeFiles/run_tests.dir/build.make +.PHONY : run_tests + +# Rule to build all files generated by this target. +CMakeFiles/run_tests.dir/build: run_tests +.PHONY : CMakeFiles/run_tests.dir/build + +CMakeFiles/run_tests.dir/clean: + $(CMAKE_COMMAND) -P CMakeFiles/run_tests.dir/cmake_clean.cmake +.PHONY : CMakeFiles/run_tests.dir/clean + +CMakeFiles/run_tests.dir/depend: + cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /home/roman/src/Firmware/src /home/roman/src/Firmware/build /home/roman/src/Firmware/build /home/roman/src/Firmware/build/CMakeFiles/run_tests.dir/DependInfo.cmake --color=$(COLOR) +.PHONY : CMakeFiles/run_tests.dir/depend + diff --git a/build/CMakeFiles/run_tests.dir/cmake_clean.cmake b/build/CMakeFiles/run_tests.dir/cmake_clean.cmake new file mode 100644 index 000000000..45a3e057b --- /dev/null +++ b/build/CMakeFiles/run_tests.dir/cmake_clean.cmake @@ -0,0 +1,8 @@ +FILE(REMOVE_RECURSE + "CMakeFiles/run_tests" +) + +# Per-language clean rules from dependency scanning. +FOREACH(lang) + INCLUDE(CMakeFiles/run_tests.dir/cmake_clean_${lang}.cmake OPTIONAL) +ENDFOREACH(lang) diff --git a/build/CMakeFiles/run_tests.dir/progress.make b/build/CMakeFiles/run_tests.dir/progress.make new file mode 100644 index 000000000..8b1378917 --- /dev/null +++ b/build/CMakeFiles/run_tests.dir/progress.make @@ -0,0 +1 @@ + diff --git a/build/CMakeFiles/tests.dir/DependInfo.cmake b/build/CMakeFiles/tests.dir/DependInfo.cmake new file mode 100644 index 000000000..7aff3a53c --- /dev/null +++ b/build/CMakeFiles/tests.dir/DependInfo.cmake @@ -0,0 +1,20 @@ +# The set of languages for which implicit dependencies are needed: +SET(CMAKE_DEPENDS_LANGUAGES + ) +# The set of files for implicit dependencies of each language: + +# Preprocessor definitions for this target. +SET(CMAKE_TARGET_DEFINITIONS + "ROS_BUILD_SHARED_LIBS=1" + ) + +# Targets to which this target links. +SET(CMAKE_TARGET_LINKED_INFO_FILES + ) + +# The include file search paths: +SET(CMAKE_C_TARGET_INCLUDE_PATH + ) +SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) +SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) +SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) diff --git a/build/CMakeFiles/tests.dir/build.make b/build/CMakeFiles/tests.dir/build.make new file mode 100644 index 000000000..720d53000 --- /dev/null +++ b/build/CMakeFiles/tests.dir/build.make @@ -0,0 +1,65 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E remove -f + +# Escaping for special characters. +EQUALS = = + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /home/roman/src/Firmware/build + +# Utility rule file for tests. + +# Include the progress variables for this target. +include CMakeFiles/tests.dir/progress.make + +CMakeFiles/tests: + +tests: CMakeFiles/tests +tests: CMakeFiles/tests.dir/build.make +.PHONY : tests + +# Rule to build all files generated by this target. +CMakeFiles/tests.dir/build: tests +.PHONY : CMakeFiles/tests.dir/build + +CMakeFiles/tests.dir/clean: + $(CMAKE_COMMAND) -P CMakeFiles/tests.dir/cmake_clean.cmake +.PHONY : CMakeFiles/tests.dir/clean + +CMakeFiles/tests.dir/depend: + cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /home/roman/src/Firmware/src /home/roman/src/Firmware/build /home/roman/src/Firmware/build /home/roman/src/Firmware/build/CMakeFiles/tests.dir/DependInfo.cmake --color=$(COLOR) +.PHONY : CMakeFiles/tests.dir/depend + diff --git a/build/CMakeFiles/tests.dir/cmake_clean.cmake b/build/CMakeFiles/tests.dir/cmake_clean.cmake new file mode 100644 index 000000000..a0424cfc7 --- /dev/null +++ b/build/CMakeFiles/tests.dir/cmake_clean.cmake @@ -0,0 +1,8 @@ +FILE(REMOVE_RECURSE + "CMakeFiles/tests" +) + +# Per-language clean rules from dependency scanning. +FOREACH(lang) + INCLUDE(CMakeFiles/tests.dir/cmake_clean_${lang}.cmake OPTIONAL) +ENDFOREACH(lang) diff --git a/build/CMakeFiles/tests.dir/progress.make b/build/CMakeFiles/tests.dir/progress.make new file mode 100644 index 000000000..8b1378917 --- /dev/null +++ b/build/CMakeFiles/tests.dir/progress.make @@ -0,0 +1 @@ + diff --git a/build/CTestTestfile.cmake b/build/CTestTestfile.cmake new file mode 100644 index 000000000..7b5534824 --- /dev/null +++ b/build/CTestTestfile.cmake @@ -0,0 +1,7 @@ +# CMake generated Testfile for +# Source directory: /home/roman/src/Firmware/src +# Build directory: /home/roman/src/Firmware/build +# +# This file includes the relevant testing commands required for +# testing this directory and lists subdirectories to be tested as well. +SUBDIRS(gtest) diff --git a/build/Makefile b/build/Makefile new file mode 100644 index 000000000..c7d796cc4 --- /dev/null +++ b/build/Makefile @@ -0,0 +1,262 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +# Default target executed when no arguments are given to make. +default_target: all +.PHONY : default_target + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E remove -f + +# Escaping for special characters. +EQUALS = = + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /home/roman/src/Firmware/build + +#============================================================================= +# Targets provided globally by CMake. + +# Special rule for the target edit_cache +edit_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running interactive CMake command-line interface..." + /usr/bin/cmake -i . +.PHONY : edit_cache + +# Special rule for the target edit_cache +edit_cache/fast: edit_cache +.PHONY : edit_cache/fast + +# Special rule for the target install +install: preinstall + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." + /usr/bin/cmake -P cmake_install.cmake +.PHONY : install + +# Special rule for the target install +install/fast: preinstall/fast + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." + /usr/bin/cmake -P cmake_install.cmake +.PHONY : install/fast + +# Special rule for the target install/local +install/local: preinstall + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..." + /usr/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake +.PHONY : install/local + +# Special rule for the target install/local +install/local/fast: install/local +.PHONY : install/local/fast + +# Special rule for the target install/strip +install/strip: preinstall + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..." + /usr/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake +.PHONY : install/strip + +# Special rule for the target install/strip +install/strip/fast: install/strip +.PHONY : install/strip/fast + +# Special rule for the target list_install_components +list_install_components: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Available install components are: \"Unspecified\"" +.PHONY : list_install_components + +# Special rule for the target list_install_components +list_install_components/fast: list_install_components +.PHONY : list_install_components/fast + +# Special rule for the target rebuild_cache +rebuild_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..." + /usr/bin/cmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) +.PHONY : rebuild_cache + +# Special rule for the target rebuild_cache +rebuild_cache/fast: rebuild_cache +.PHONY : rebuild_cache/fast + +# Special rule for the target test +test: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running tests..." + /usr/bin/ctest --force-new-ctest-process $(ARGS) +.PHONY : test + +# Special rule for the target test +test/fast: test +.PHONY : test/fast + +# The main all target +all: cmake_check_build_system + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles /home/roman/src/Firmware/build/CMakeFiles/progress.marks + $(MAKE) -f CMakeFiles/Makefile2 all + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 +.PHONY : all + +# The main clean target +clean: + $(MAKE) -f CMakeFiles/Makefile2 clean +.PHONY : clean + +# The main clean target +clean/fast: clean +.PHONY : clean/fast + +# Prepare targets for installation. +preinstall: all + $(MAKE) -f CMakeFiles/Makefile2 preinstall +.PHONY : preinstall + +# Prepare targets for installation. +preinstall/fast: + $(MAKE) -f CMakeFiles/Makefile2 preinstall +.PHONY : preinstall/fast + +# clear depends +depend: + $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1 +.PHONY : depend + +#============================================================================= +# Target rules for targets named clean_test_results + +# Build rule for target. +clean_test_results: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 clean_test_results +.PHONY : clean_test_results + +# fast build rule for target. +clean_test_results/fast: + $(MAKE) -f CMakeFiles/clean_test_results.dir/build.make CMakeFiles/clean_test_results.dir/build +.PHONY : clean_test_results/fast + +#============================================================================= +# Target rules for targets named doxygen + +# Build rule for target. +doxygen: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 doxygen +.PHONY : doxygen + +# fast build rule for target. +doxygen/fast: + $(MAKE) -f CMakeFiles/doxygen.dir/build.make CMakeFiles/doxygen.dir/build +.PHONY : doxygen/fast + +#============================================================================= +# Target rules for targets named run_tests + +# Build rule for target. +run_tests: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 run_tests +.PHONY : run_tests + +# fast build rule for target. +run_tests/fast: + $(MAKE) -f CMakeFiles/run_tests.dir/build.make CMakeFiles/run_tests.dir/build +.PHONY : run_tests/fast + +#============================================================================= +# Target rules for targets named tests + +# Build rule for target. +tests: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 tests +.PHONY : tests + +# fast build rule for target. +tests/fast: + $(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/build +.PHONY : tests/fast + +#============================================================================= +# Target rules for targets named gtest + +# Build rule for target. +gtest: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 gtest +.PHONY : gtest + +# fast build rule for target. +gtest/fast: + $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/build +.PHONY : gtest/fast + +#============================================================================= +# Target rules for targets named gtest_main + +# Build rule for target. +gtest_main: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 gtest_main +.PHONY : gtest_main + +# fast build rule for target. +gtest_main/fast: + $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/build +.PHONY : gtest_main/fast + +# Help Target +help: + @echo "The following are some of the valid targets for this Makefile:" + @echo "... all (the default if no target is provided)" + @echo "... clean" + @echo "... depend" + @echo "... clean_test_results" + @echo "... doxygen" + @echo "... edit_cache" + @echo "... install" + @echo "... install/local" + @echo "... install/strip" + @echo "... list_install_components" + @echo "... rebuild_cache" + @echo "... run_tests" + @echo "... test" + @echo "... tests" + @echo "... gtest" + @echo "... gtest_main" +.PHONY : help + + + +#============================================================================= +# Special targets to cleanup operation of make. + +# Special rule to run CMake to check the build system integrity. +# No rule that depends on this can have commands that come from listfiles +# because they might be regenerated. +cmake_check_build_system: + $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 +.PHONY : cmake_check_build_system + diff --git a/build/catkin/catkin_generated/version/package.cmake b/build/catkin/catkin_generated/version/package.cmake new file mode 100644 index 000000000..1cbfc826b --- /dev/null +++ b/build/catkin/catkin_generated/version/package.cmake @@ -0,0 +1,9 @@ +set(_CATKIN_CURRENT_PACKAGE "catkin") +set(catkin_VERSION "0.6.9") +set(catkin_BUILD_DEPENDS_python-catkin-pkg_VERSION_GTE "0.2.2") +set(catkin_BUILD_DEPENDS "python-empy" "python-argparse" "python-catkin-pkg") +set(catkin_DEPRECATED "") +set(catkin_RUN_DEPENDS "python-argparse" "python-catkin-pkg" "gtest" "python-empy" "python-nose") +set(catkin_MAINTAINER "Dirk Thomas ") +set(catkin_BUILDTOOL_DEPENDS "cmake") +set(catkin_RUN_DEPENDS_python-catkin-pkg_VERSION_GTE "0.2.2") \ No newline at end of file diff --git a/build/catkin_generated/env_cached.sh b/build/catkin_generated/env_cached.sh new file mode 100755 index 000000000..d6be91db5 --- /dev/null +++ b/build/catkin_generated/env_cached.sh @@ -0,0 +1,16 @@ +#!/usr/bin/env sh +# generated from catkin/cmake/templates/env.sh.in + +if [ $# -eq 0 ] ; then + /bin/echo "Usage: env.sh COMMANDS" + /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually." + exit 1 +fi + +# ensure to not use different shell type which was set before +CATKIN_SHELL=sh + +# source setup_cached.sh from same directory as this file +_CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd) +. "$_CATKIN_SETUP_DIR/setup_cached.sh" +exec "$@" diff --git a/build/catkin_generated/generate_cached_setup.py b/build/catkin_generated/generate_cached_setup.py new file mode 100644 index 000000000..7a56cdb7f --- /dev/null +++ b/build/catkin_generated/generate_cached_setup.py @@ -0,0 +1,29 @@ +from __future__ import print_function +import argparse +import os +import stat +import sys + +# find the import for catkin's python package - either from source space or from an installed underlay +if os.path.exists(os.path.join('/opt/ros/indigo/share/catkin/cmake', 'catkinConfig.cmake.in')): + sys.path.insert(0, os.path.join('/opt/ros/indigo/share/catkin/cmake', '..', 'python')) +try: + from catkin.environment_cache import generate_environment_script +except ImportError: + # search for catkin package in all workspaces and prepend to path + for workspace in "/home/roman/catkin_ws/devel;/opt/ros/indigo".split(';'): + python_path = os.path.join(workspace, 'lib/python2.7/dist-packages') + if os.path.isdir(os.path.join(python_path, 'catkin')): + sys.path.insert(0, python_path) + break + from catkin.environment_cache import generate_environment_script + +code = generate_environment_script('/home/roman/src/Firmware/devel/env.sh') + +output_filename = '/home/roman/src/Firmware/build/catkin_generated/setup_cached.sh' +with open(output_filename, 'w') as f: + #print('Generate script for cached setup "%s"' % output_filename) + f.write('\n'.join(code)) + +mode = os.stat(output_filename).st_mode +os.chmod(output_filename, mode | stat.S_IXUSR) diff --git a/build/catkin_generated/installspace/.rosinstall b/build/catkin_generated/installspace/.rosinstall new file mode 100644 index 000000000..a959c71ef --- /dev/null +++ b/build/catkin_generated/installspace/.rosinstall @@ -0,0 +1,2 @@ +- setup-file: + local-name: /home/roman/src/Firmware/install/setup.sh diff --git a/build/catkin_generated/installspace/_setup_util.py b/build/catkin_generated/installspace/_setup_util.py new file mode 100755 index 000000000..8db644140 --- /dev/null +++ b/build/catkin_generated/installspace/_setup_util.py @@ -0,0 +1,287 @@ +#!/usr/bin/python + +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * 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. +# * Neither the name of Willow Garage, Inc. 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. + +'''This file generates shell code for the setup.SHELL scripts to set environment variables''' + +from __future__ import print_function +import argparse +import copy +import errno +import os +import platform +import sys + +CATKIN_MARKER_FILE = '.catkin' + +system = platform.system() +IS_DARWIN = (system == 'Darwin') +IS_WINDOWS = (system == 'Windows') + +# subfolder of workspace prepended to CMAKE_PREFIX_PATH +ENV_VAR_SUBFOLDERS = { + 'CMAKE_PREFIX_PATH': '', + 'CPATH': 'include', + 'LD_LIBRARY_PATH' if not IS_DARWIN else 'DYLD_LIBRARY_PATH': ['lib', os.path.join('lib', 'x86_64-linux-gnu')], + 'PATH': 'bin', + 'PKG_CONFIG_PATH': [os.path.join('lib', 'pkgconfig'), os.path.join('lib', 'x86_64-linux-gnu', 'pkgconfig')], + 'PYTHONPATH': 'lib/python2.7/dist-packages', +} + + +def rollback_env_variables(environ, env_var_subfolders): + ''' + Generate shell code to reset environment variables + by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH. + This does not cover modifications performed by environment hooks. + ''' + lines = [] + unmodified_environ = copy.copy(environ) + for key in sorted(env_var_subfolders.keys()): + subfolders = env_var_subfolders[key] + if not isinstance(subfolders, list): + subfolders = [subfolders] + for subfolder in subfolders: + value = _rollback_env_variable(unmodified_environ, key, subfolder) + if value is not None: + environ[key] = value + lines.append(assignment(key, value)) + if lines: + lines.insert(0, comment('reset environment variables by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH')) + return lines + + +def _rollback_env_variable(environ, name, subfolder): + ''' + For each catkin workspace in CMAKE_PREFIX_PATH remove the first entry from env[NAME] matching workspace + subfolder. + + :param subfolder: str '' or subfoldername that may start with '/' + :returns: the updated value of the environment variable. + ''' + value = environ[name] if name in environ else '' + env_paths = [path for path in value.split(os.pathsep) if path] + value_modified = False + if subfolder: + if subfolder.startswith(os.path.sep) or (os.path.altsep and subfolder.startswith(os.path.altsep)): + subfolder = subfolder[1:] + if subfolder.endswith(os.path.sep) or (os.path.altsep and subfolder.endswith(os.path.altsep)): + subfolder = subfolder[:-1] + for ws_path in _get_workspaces(environ, include_fuerte=True, include_non_existing=True): + path_to_find = os.path.join(ws_path, subfolder) if subfolder else ws_path + path_to_remove = None + for env_path in env_paths: + env_path_clean = env_path[:-1] if env_path and env_path[-1] in [os.path.sep, os.path.altsep] else env_path + if env_path_clean == path_to_find: + path_to_remove = env_path + break + if path_to_remove: + env_paths.remove(path_to_remove) + value_modified = True + new_value = os.pathsep.join(env_paths) + return new_value if value_modified else None + + +def _get_workspaces(environ, include_fuerte=False, include_non_existing=False): + ''' + Based on CMAKE_PREFIX_PATH return all catkin workspaces. + + :param include_fuerte: The flag if paths starting with '/opt/ros/fuerte' should be considered workspaces, ``bool`` + ''' + # get all cmake prefix paths + env_name = 'CMAKE_PREFIX_PATH' + value = environ[env_name] if env_name in environ else '' + paths = [path for path in value.split(os.pathsep) if path] + # remove non-workspace paths + workspaces = [path for path in paths if os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE)) or (include_fuerte and path.startswith('/opt/ros/fuerte')) or (include_non_existing and not os.path.exists(path))] + return workspaces + + +def prepend_env_variables(environ, env_var_subfolders, workspaces): + ''' + Generate shell code to prepend environment variables + for the all workspaces. + ''' + lines = [] + lines.append(comment('prepend folders of workspaces to environment variables')) + + paths = [path for path in workspaces.split(os.pathsep) if path] + + prefix = _prefix_env_variable(environ, 'CMAKE_PREFIX_PATH', paths, '') + lines.append(prepend(environ, 'CMAKE_PREFIX_PATH', prefix)) + + for key in sorted([key for key in env_var_subfolders.keys() if key != 'CMAKE_PREFIX_PATH']): + subfolder = env_var_subfolders[key] + prefix = _prefix_env_variable(environ, key, paths, subfolder) + lines.append(prepend(environ, key, prefix)) + return lines + + +def _prefix_env_variable(environ, name, paths, subfolders): + ''' + Return the prefix to prepend to the environment variable NAME, adding any path in NEW_PATHS_STR without creating duplicate or empty items. + ''' + value = environ[name] if name in environ else '' + environ_paths = [path for path in value.split(os.pathsep) if path] + checked_paths = [] + for path in paths: + if not isinstance(subfolders, list): + subfolders = [subfolders] + for subfolder in subfolders: + path_tmp = path + if subfolder: + path_tmp = os.path.join(path_tmp, subfolder) + # exclude any path already in env and any path we already added + if path_tmp not in environ_paths and path_tmp not in checked_paths: + checked_paths.append(path_tmp) + prefix_str = os.pathsep.join(checked_paths) + if prefix_str != '' and environ_paths: + prefix_str += os.pathsep + return prefix_str + + +def assignment(key, value): + if not IS_WINDOWS: + return 'export %s="%s"' % (key, value) + else: + return 'set %s=%s' % (key, value) + + +def comment(msg): + if not IS_WINDOWS: + return '# %s' % msg + else: + return 'REM %s' % msg + + +def prepend(environ, key, prefix): + if key not in environ or not environ[key]: + return assignment(key, prefix) + if not IS_WINDOWS: + return 'export %s="%s$%s"' % (key, prefix, key) + else: + return 'set %s=%s%%%s%%' % (key, prefix, key) + + +def find_env_hooks(environ, cmake_prefix_path): + ''' + Generate shell code with found environment hooks + for the all workspaces. + ''' + lines = [] + lines.append(comment('found environment hooks in workspaces')) + + generic_env_hooks = [] + generic_env_hooks_workspace = [] + specific_env_hooks = [] + specific_env_hooks_workspace = [] + generic_env_hooks_by_filename = {} + specific_env_hooks_by_filename = {} + generic_env_hook_ext = 'bat' if IS_WINDOWS else 'sh' + specific_env_hook_ext = environ['CATKIN_SHELL'] if not IS_WINDOWS and 'CATKIN_SHELL' in environ and environ['CATKIN_SHELL'] else None + # remove non-workspace paths + workspaces = [path for path in cmake_prefix_path.split(os.pathsep) if path and os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE))] + for workspace in reversed(workspaces): + env_hook_dir = os.path.join(workspace, 'etc', 'catkin', 'profile.d') + if os.path.isdir(env_hook_dir): + for filename in sorted(os.listdir(env_hook_dir)): + if filename.endswith('.%s' % generic_env_hook_ext): + # remove previous env hook with same name if present + if filename in generic_env_hooks_by_filename: + i = generic_env_hooks.index(generic_env_hooks_by_filename[filename]) + generic_env_hooks.pop(i) + generic_env_hooks_workspace.pop(i) + # append env hook + generic_env_hooks.append(os.path.join(env_hook_dir, filename)) + generic_env_hooks_workspace.append(workspace) + generic_env_hooks_by_filename[filename] = generic_env_hooks[-1] + elif specific_env_hook_ext is not None and filename.endswith('.%s' % specific_env_hook_ext): + # remove previous env hook with same name if present + if filename in specific_env_hooks_by_filename: + i = specific_env_hooks.index(specific_env_hooks_by_filename[filename]) + specific_env_hooks.pop(i) + specific_env_hooks_workspace.pop(i) + # append env hook + specific_env_hooks.append(os.path.join(env_hook_dir, filename)) + specific_env_hooks_workspace.append(workspace) + specific_env_hooks_by_filename[filename] = specific_env_hooks[-1] + env_hooks = generic_env_hooks + specific_env_hooks + env_hooks_workspace = generic_env_hooks_workspace + specific_env_hooks_workspace + count = len(env_hooks) + lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_COUNT', count)) + for i in range(count): + lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d' % i, env_hooks[i])) + lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d_WORKSPACE' % i, env_hooks_workspace[i])) + return lines + + +def _parse_arguments(args=None): + parser = argparse.ArgumentParser(description='Generates code blocks for the setup.SHELL script.') + parser.add_argument('--extend', action='store_true', help='Skip unsetting previous environment variables to extend context') + return parser.parse_known_args(args=args)[0] + + +if __name__ == '__main__': + try: + try: + args = _parse_arguments() + except Exception as e: + print(e, file=sys.stderr) + sys.exit(1) + + # environment at generation time + CMAKE_PREFIX_PATH = '/home/roman/catkin_ws/devel;/opt/ros/indigo'.split(';') + # prepend current workspace if not already part of CPP + base_path = os.path.dirname(__file__) + if base_path not in CMAKE_PREFIX_PATH: + CMAKE_PREFIX_PATH.insert(0, base_path) + CMAKE_PREFIX_PATH = os.pathsep.join(CMAKE_PREFIX_PATH) + + environ = dict(os.environ) + lines = [] + if not args.extend: + lines += rollback_env_variables(environ, ENV_VAR_SUBFOLDERS) + lines += prepend_env_variables(environ, ENV_VAR_SUBFOLDERS, CMAKE_PREFIX_PATH) + lines += find_env_hooks(environ, CMAKE_PREFIX_PATH) + print('\n'.join(lines)) + + # need to explicitly flush the output + sys.stdout.flush() + except IOError as e: + # and catch potantial "broken pipe" if stdout is not writable + # which can happen when piping the output to a file but the disk is full + if e.errno == errno.EPIPE: + print(e, file=sys.stderr) + sys.exit(2) + raise + + sys.exit(0) diff --git a/build/catkin_generated/installspace/env.sh b/build/catkin_generated/installspace/env.sh new file mode 100755 index 000000000..8aa9d244a --- /dev/null +++ b/build/catkin_generated/installspace/env.sh @@ -0,0 +1,16 @@ +#!/usr/bin/env sh +# generated from catkin/cmake/templates/env.sh.in + +if [ $# -eq 0 ] ; then + /bin/echo "Usage: env.sh COMMANDS" + /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually." + exit 1 +fi + +# ensure to not use different shell type which was set before +CATKIN_SHELL=sh + +# source setup.sh from same directory as this file +_CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd) +. "$_CATKIN_SETUP_DIR/setup.sh" +exec "$@" diff --git a/build/catkin_generated/installspace/setup.bash b/build/catkin_generated/installspace/setup.bash new file mode 100644 index 000000000..ff47af8f3 --- /dev/null +++ b/build/catkin_generated/installspace/setup.bash @@ -0,0 +1,8 @@ +#!/usr/bin/env bash +# generated from catkin/cmake/templates/setup.bash.in + +CATKIN_SHELL=bash + +# source setup.sh from same directory as this file +_CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd) +. "$_CATKIN_SETUP_DIR/setup.sh" diff --git a/build/catkin_generated/installspace/setup.sh b/build/catkin_generated/installspace/setup.sh new file mode 100644 index 000000000..95e1571be --- /dev/null +++ b/build/catkin_generated/installspace/setup.sh @@ -0,0 +1,87 @@ +#!/usr/bin/env sh +# generated from catkin/cmake/template/setup.sh.in + +# Sets various environment variables and sources additional environment hooks. +# It tries it's best to undo changes from a previously sourced setup file before. +# Supported command line options: +# --extend: skips the undoing of changes from a previously sourced setup file + +# since this file is sourced either use the provided _CATKIN_SETUP_DIR +# or fall back to the destination set at configure time +: ${_CATKIN_SETUP_DIR:=/home/roman/src/Firmware/install} +_SETUP_UTIL="$_CATKIN_SETUP_DIR/_setup_util.py" +unset _CATKIN_SETUP_DIR + +if [ ! -f "$_SETUP_UTIL" ]; then + echo "Missing Python script: $_SETUP_UTIL" + return 22 +fi + +# detect if running on Darwin platform +_UNAME=`uname -s` +_IS_DARWIN=0 +if [ "$_UNAME" = "Darwin" ]; then + _IS_DARWIN=1 +fi +unset _UNAME + +# make sure to export all environment variables +export CMAKE_PREFIX_PATH +export CPATH +if [ $_IS_DARWIN -eq 0 ]; then + export LD_LIBRARY_PATH +else + export DYLD_LIBRARY_PATH +fi +unset _IS_DARWIN +export PATH +export PKG_CONFIG_PATH +export PYTHONPATH + +# remember type of shell if not already set +if [ -z "$CATKIN_SHELL" ]; then + CATKIN_SHELL=sh +fi + +# invoke Python script to generate necessary exports of environment variables +_SETUP_TMP=`mktemp /tmp/setup.sh.XXXXXXXXXX` +if [ $? -ne 0 -o ! -f "$_SETUP_TMP" ]; then + echo "Could not create temporary file: $_SETUP_TMP" + return 1 +fi +CATKIN_SHELL=$CATKIN_SHELL "$_SETUP_UTIL" $@ > $_SETUP_TMP +_RC=$? +if [ $_RC -ne 0 ]; then + if [ $_RC -eq 2 ]; then + echo "Could not write the output of '$_SETUP_UTIL' to temporary file '$_SETUP_TMP': may be the disk if full?" + else + echo "Failed to run '\"$_SETUP_UTIL\" $@': return code $_RC" + fi + unset _RC + unset _SETUP_UTIL + rm -f $_SETUP_TMP + unset _SETUP_TMP + return 1 +fi +unset _RC +unset _SETUP_UTIL +. $_SETUP_TMP +rm -f $_SETUP_TMP +unset _SETUP_TMP + +# source all environment hooks +_i=0 +while [ $_i -lt $_CATKIN_ENVIRONMENT_HOOKS_COUNT ]; do + eval _envfile=\$_CATKIN_ENVIRONMENT_HOOKS_$_i + unset _CATKIN_ENVIRONMENT_HOOKS_$_i + eval _envfile_workspace=\$_CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE + unset _CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE + # set workspace for environment hook + CATKIN_ENV_HOOK_WORKSPACE=$_envfile_workspace + . "$_envfile" + unset CATKIN_ENV_HOOK_WORKSPACE + _i=$((_i + 1)) +done +unset _i + +unset _CATKIN_ENVIRONMENT_HOOKS_COUNT diff --git a/build/catkin_generated/installspace/setup.zsh b/build/catkin_generated/installspace/setup.zsh new file mode 100644 index 000000000..b66071766 --- /dev/null +++ b/build/catkin_generated/installspace/setup.zsh @@ -0,0 +1,8 @@ +#!/usr/bin/env zsh +# generated from catkin/cmake/templates/setup.zsh.in + +CATKIN_SHELL=zsh +_CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd) +emulate sh # emulate POSIX +. "$_CATKIN_SETUP_DIR/setup.sh" +emulate zsh # back to zsh mode diff --git a/build/catkin_generated/order_packages.cmake b/build/catkin_generated/order_packages.cmake new file mode 100644 index 000000000..6b0e2dff9 --- /dev/null +++ b/build/catkin_generated/order_packages.cmake @@ -0,0 +1,10 @@ +# generated from catkin/cmake/em/order_packages.cmake.em + +set(CATKIN_ORDERED_PACKAGES "") +set(CATKIN_ORDERED_PACKAGE_PATHS "") +set(CATKIN_ORDERED_PACKAGES_IS_META "") +set(CATKIN_ORDERED_PACKAGES_BUILD_TYPE "") + +set(CATKIN_MESSAGE_GENERATORS ) + +set(CATKIN_METAPACKAGE_CMAKE_TEMPLATE "/usr/lib/pymodules/python2.7/catkin_pkg/templates/metapackage.cmake.in") diff --git a/build/catkin_generated/order_packages.py b/build/catkin_generated/order_packages.py new file mode 100644 index 000000000..5cb176580 --- /dev/null +++ b/build/catkin_generated/order_packages.py @@ -0,0 +1,5 @@ +# generated from catkin/cmake/template/order_packages.context.py.in +source_root_dir = "/home/roman/src/Firmware/src" +whitelisted_packages = "".split(';') if "" != "" else [] +blacklisted_packages = "".split(';') if "" != "" else [] +underlay_workspaces = "/home/roman/catkin_ws/devel;/opt/ros/indigo".split(';') if "/home/roman/catkin_ws/devel;/opt/ros/indigo" != "" else [] diff --git a/build/catkin_generated/setup_cached.sh b/build/catkin_generated/setup_cached.sh new file mode 100755 index 000000000..e03bce4ba --- /dev/null +++ b/build/catkin_generated/setup_cached.sh @@ -0,0 +1,20 @@ +#!/usr/bin/env sh +# generated from catkin/python/catkin/environment_cache.py + +# based on a snapshot of the environment before and after calling the setup script +# it emulates the modifications of the setup script without recurring computations + +# new environment variables + +# modified environment variables +export CATKIN_TEST_RESULTS_DIR="/home/roman/src/Firmware/build/test_results" +export CMAKE_PREFIX_PATH="/home/roman/src/Firmware/devel:$CMAKE_PREFIX_PATH" +export CPATH="/home/roman/src/Firmware/devel/include:$CPATH" +export LD_LIBRARY_PATH="/home/roman/src/Firmware/devel/lib:/home/roman/src/Firmware/devel/lib/x86_64-linux-gnu:/home/roman/catkin_ws/devel/lib/x86_64-linux-gnu:/opt/ros/indigo/lib/x86_64-linux-gnu:/home/roman/catkin_ws/devel/lib:/opt/ros/indigo/lib" +export PATH="/home/roman/src/Firmware/devel/bin:$PATH" +export PKG_CONFIG_PATH="/home/roman/src/Firmware/devel/lib/pkgconfig:/home/roman/src/Firmware/devel/lib/x86_64-linux-gnu/pkgconfig:$PKG_CONFIG_PATH" +export PWD="/home/roman/src/Firmware/build" +export PYTHONPATH="/home/roman/src/Firmware/devel/lib/python2.7/dist-packages:$PYTHONPATH" +export ROSLISP_PACKAGE_DIRECTORIES="/home/roman/src/Firmware/devel/share/common-lisp:$ROSLISP_PACKAGE_DIRECTORIES" +export ROS_PACKAGE_PATH="/home/roman/src/Firmware/src:$ROS_PACKAGE_PATH" +export ROS_TEST_RESULTS_DIR="/home/roman/src/Firmware/build/test_results" \ No newline at end of file diff --git a/build/catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp b/build/catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp new file mode 100644 index 000000000..26882f02a --- /dev/null +++ b/build/catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp @@ -0,0 +1,250 @@ +#!/usr/bin/env python + +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * 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. +# * Neither the name of Willow Garage, Inc. 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. + +from __future__ import print_function +import os +import sys + +import distutils.core +try: + import setuptools +except ImportError: + pass + +from argparse import ArgumentParser + + +def _get_locations(pkgs, package_dir): + """ + based on setuptools logic and the package_dir dict, builds a dict + of location roots for each pkg in pkgs. + See http://docs.python.org/distutils/setupscript.html + + :returns: a dict {pkgname: root} for each pkgname in pkgs (and each of their parents) + """ + # package_dir contains a dict {package_name: relativepath} + # Example {'': 'src', 'foo': 'lib', 'bar': 'lib2'} + # + # '' means where to look for any package unless a parent package + # is listed so package bar.pot is expected at lib2/bar/pot, + # whereas package sup.dee is expected at src/sup/dee + # + # if package_dir does not state anything about a package, + # setuptool expects the package folder to be in the root of the + # project + locations = {} + allprefix = package_dir.get('', '') + for pkg in pkgs: + parent_location = None + splits = pkg.split('.') + # we iterate over compound name from parent to child + # so once we found parent, children just append to their parent + for key_len in range(len(splits)): + key = '.'.join(splits[:key_len + 1]) + if key not in locations: + if key in package_dir: + locations[key] = package_dir[key] + elif parent_location is not None: + locations[key] = parent_location + else: + locations[key] = allprefix + parent_location = locations[key] + return locations + + +def generate_cmake_file(package_name, version, scripts, package_dir, pkgs, modules): + """ + Generates lines to add to a cmake file which will set variables + + :param version: str, format 'int.int.int' + :param scripts: [list of str]: relative paths to scripts + :param package_dir: {modulename: path} + :pkgs: [list of str] python_packages declared in catkin package + :modules: [list of str] python modules + """ + prefix = '%s_SETUP_PY' % package_name + result = [] + result.append(r'set(%s_VERSION "%s")' % (prefix, version)) + result.append(r'set(%s_SCRIPTS "%s")' % (prefix, ';'.join(scripts))) + + # Remove packages with '.' separators. + # + # setuptools allows specifying submodules in other folders than + # their parent + # + # The symlink approach of catkin does not work with such submodules. + # In the common case, this does not matter as the submodule is + # within the containing module. We verify this assumption, and if + # it passes, we remove submodule packages. + locations = _get_locations(pkgs, package_dir) + for pkgname, location in locations.items(): + if not '.' in pkgname: + continue + splits = pkgname.split('.') + # hack: ignore write-combining setup.py files for msg and srv files + if splits[1] in ['msg', 'srv']: + continue + # check every child has the same root folder as its parent + parent_name = '.'.join(splits[:1]) + if location != locations[parent_name]: + raise RuntimeError( + "catkin_export_python does not support setup.py files that combine across multiple directories: %s in %s, %s in %s" % (pkgname, location, parent_name, locations[parent_name])) + + # If checks pass, remove all submodules + pkgs = [p for p in pkgs if '.' not in p] + + resolved_pkgs = [] + for pkg in pkgs: + resolved_pkgs += [os.path.join(locations[pkg], pkg)] + + result.append(r'set(%s_PACKAGES "%s")' % (prefix, ';'.join(pkgs))) + result.append(r'set(%s_PACKAGE_DIRS "%s")' % (prefix, ';'.join(resolved_pkgs).replace("\\", "/"))) + + # skip modules which collide with package names + filtered_modules = [] + for modname in modules: + splits = modname.split('.') + # check all parents too + equals_package = [('.'.join(splits[:-i]) in locations) for i in range(len(splits))] + if any(equals_package): + continue + filtered_modules.append(modname) + module_locations = _get_locations(filtered_modules, package_dir) + + result.append(r'set(%s_MODULES "%s")' % (prefix, ';'.join(['%s.py' % m.replace('.', '/') for m in filtered_modules]))) + result.append(r'set(%s_MODULE_DIRS "%s")' % (prefix, ';'.join([module_locations[m] for m in filtered_modules]).replace("\\", "/"))) + + return result + + +def _create_mock_setup_function(package_name, outfile): + """ + Creates a function to call instead of distutils.core.setup or + setuptools.setup, which just captures some args and writes them + into a file that can be used from cmake + + :param package_name: name of the package + :param outfile: filename that cmake will use afterwards + :returns: a function to replace disutils.core.setup and setuptools.setup + """ + + def setup(*args, **kwargs): + ''' + Checks kwargs and writes a scriptfile + ''' + if 'version' not in kwargs: + sys.stderr.write("\n*** Unable to find 'version' in setup.py of %s\n" % package_name) + raise RuntimeError("version not found in setup.py") + version = kwargs['version'] + package_dir = kwargs.get('package_dir', {}) + + pkgs = kwargs.get('packages', []) + scripts = kwargs.get('scripts', []) + modules = kwargs.get('py_modules', []) + + unsupported_args = [ + 'entry_points', + 'exclude_package_data', + 'ext_modules ', + 'ext_package', + 'include_package_data', + 'namespace_packages', + 'setup_requires', + 'use_2to3', + 'zip_safe'] + used_unsupported_args = [arg for arg in unsupported_args if arg in kwargs] + if used_unsupported_args: + sys.stderr.write("*** Arguments %s to setup() not supported in catkin devel space in setup.py of %s\n" % (used_unsupported_args, package_name)) + + result = generate_cmake_file(package_name=package_name, + version=version, + scripts=scripts, + package_dir=package_dir, + pkgs=pkgs, + modules=modules) + with open(outfile, 'w') as out: + out.write('\n'.join(result)) + + return setup + + +def main(): + """ + Script main, parses arguments and invokes Dummy.setup indirectly. + """ + parser = ArgumentParser(description='Utility to read setup.py values from cmake macros. Creates a file with CMake set commands setting variables.') + parser.add_argument('package_name', help='Name of catkin package') + parser.add_argument('setupfile_path', help='Full path to setup.py') + parser.add_argument('outfile', help='Where to write result to') + + args = parser.parse_args() + + # print("%s" % sys.argv) + # PACKAGE_NAME = sys.argv[1] + # OUTFILE = sys.argv[3] + # print("Interrogating setup.py for package %s into %s " % (PACKAGE_NAME, OUTFILE), + # file=sys.stderr) + + # print("executing %s" % args.setupfile_path) + + # be sure you're in the directory containing + # setup.py so the sys.path manipulation works, + # so the import of __version__ works + os.chdir(os.path.dirname(os.path.abspath(args.setupfile_path))) + + # patch setup() function of distutils and setuptools for the + # context of evaluating setup.py + try: + fake_setup = _create_mock_setup_function(package_name=args.package_name, + outfile=args.outfile) + + distutils_backup = distutils.core.setup + distutils.core.setup = fake_setup + try: + setuptools_backup = setuptools.setup + setuptools.setup = fake_setup + except NameError: + pass + + with open(args.setupfile_path, 'r') as fh: + exec(fh.read()) + finally: + distutils.core.setup = distutils_backup + try: + setuptools.setup = setuptools_backup + except NameError: + pass + +if __name__ == '__main__': + main() diff --git a/build/catkin_generated/stamps/Project/order_packages.cmake.em.stamp b/build/catkin_generated/stamps/Project/order_packages.cmake.em.stamp new file mode 100644 index 000000000..087d4d802 --- /dev/null +++ b/build/catkin_generated/stamps/Project/order_packages.cmake.em.stamp @@ -0,0 +1,56 @@ +# generated from catkin/cmake/em/order_packages.cmake.em +@{ +import os +try: + from catkin_pkg.cmake import get_metapackage_cmake_template_path +except ImportError as e: + raise RuntimeError('ImportError: "from catkin_pkg.cmake import get_metapackage_cmake_template_path" failed: %s\nMake sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.' % e) +try: + from catkin_pkg.topological_order import topological_order +except ImportError as e: + raise RuntimeError('ImportError: "from catkin_pkg.topological_order import topological_order" failed: %s\nMake sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.' % e) +try: + from catkin_pkg.package import InvalidPackage +except ImportError as e: + raise RuntimeError('ImportError: "from catkin_pkg.package import InvalidPackage" failed: %s\nMake sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.' % e) +# vars defined in order_packages.context.py.in +try: + ordered_packages = topological_order(os.path.normpath(source_root_dir), whitelisted=whitelisted_packages, blacklisted=blacklisted_packages, underlay_workspaces=underlay_workspaces) +except InvalidPackage as e: + print('message(FATAL_ERROR "%s")' % ('%s' % e).replace('"', '\\"')) + ordered_packages = [] +fatal_error = False +}@ + +set(CATKIN_ORDERED_PACKAGES "") +set(CATKIN_ORDERED_PACKAGE_PATHS "") +set(CATKIN_ORDERED_PACKAGES_IS_META "") +set(CATKIN_ORDERED_PACKAGES_BUILD_TYPE "") +@[for path, package in ordered_packages]@ +@[if path is None]@ +message(FATAL_ERROR "Circular dependency in subset of packages:\n@package") +@{ +fatal_error = True +}@ +@[elif package.name != 'catkin']@ +list(APPEND CATKIN_ORDERED_PACKAGES "@(package.name)") +list(APPEND CATKIN_ORDERED_PACKAGE_PATHS "@(path.replace('\\','/'))") +list(APPEND CATKIN_ORDERED_PACKAGES_IS_META "@(str('metapackage' in [e.tagname for e in package.exports]))") +list(APPEND CATKIN_ORDERED_PACKAGES_BUILD_TYPE "@(str([e.content for e in package.exports if e.tagname == 'build_type'][0]) if 'build_type' in [e.tagname for e in package.exports] else 'catkin')") +@{ +deprecated = [e for e in package.exports if e.tagname == 'deprecated'] +}@ +@[if deprecated]@ +message("WARNING: Package '@(package.name)' is deprecated@(' (%s)' % deprecated[0].content if deprecated[0].content else '')") +@[end if]@ +@[end if]@ +@[end for]@ + +@[if not fatal_error]@ +@{ +message_generators = [package.name for (_, package) in ordered_packages if 'message_generator' in [e.tagname for e in package.exports]] +}@ +set(CATKIN_MESSAGE_GENERATORS @(' '.join(message_generators))) +@[end if]@ + +set(CATKIN_METAPACKAGE_CMAKE_TEMPLATE "@(get_metapackage_cmake_template_path().replace('\\','/'))") diff --git a/build/catkin_generated/stamps/Project/package.xml.stamp b/build/catkin_generated/stamps/Project/package.xml.stamp new file mode 100644 index 000000000..7c2708ac3 --- /dev/null +++ b/build/catkin_generated/stamps/Project/package.xml.stamp @@ -0,0 +1,37 @@ + + + catkin + 0.6.9 + Low-level build system macros and infrastructure for ROS. + Dirk Thomas + BSD + + http://www.ros.org/wiki/catkin + https://github.com/ros/catkin/issues + https://github.com/ros/catkin + + Troy Straszheim + Morten Kjaergaard + Brian Gerkey + Dirk Thomas + + cmake + cmake + + python-argparse + python-catkin-pkg + + python-empy + + gtest + python-empy + python-nose + + python-mock + python-nose + + + + + + diff --git a/build/catkin_make.cache b/build/catkin_make.cache new file mode 100644 index 000000000..8b1378917 --- /dev/null +++ b/build/catkin_make.cache @@ -0,0 +1 @@ + diff --git a/build/cmake_install.cmake b/build/cmake_install.cmake new file mode 100644 index 000000000..982fd705c --- /dev/null +++ b/build/cmake_install.cmake @@ -0,0 +1,140 @@ +# Install script for directory: /home/roman/src/Firmware/src + +# Set the install prefix +IF(NOT DEFINED CMAKE_INSTALL_PREFIX) + SET(CMAKE_INSTALL_PREFIX "/home/roman/src/Firmware/install") +ENDIF(NOT DEFINED CMAKE_INSTALL_PREFIX) +STRING(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}") + +# Set the install configuration name. +IF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) + IF(BUILD_TYPE) + STRING(REGEX REPLACE "^[^A-Za-z0-9_]+" "" + CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}") + ELSE(BUILD_TYPE) + SET(CMAKE_INSTALL_CONFIG_NAME "") + ENDIF(BUILD_TYPE) + MESSAGE(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"") +ENDIF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) + +# Set the component getting installed. +IF(NOT CMAKE_INSTALL_COMPONENT) + IF(COMPONENT) + MESSAGE(STATUS "Install component: \"${COMPONENT}\"") + SET(CMAKE_INSTALL_COMPONENT "${COMPONENT}") + ELSE(COMPONENT) + SET(CMAKE_INSTALL_COMPONENT) + ENDIF(COMPONENT) +ENDIF(NOT CMAKE_INSTALL_COMPONENT) + +# Install shared libraries without execute permission? +IF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) + SET(CMAKE_INSTALL_SO_NO_EXE "1") +ENDIF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) + +IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + + if (NOT EXISTS "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}") + file(MAKE_DIRECTORY "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}") + endif() + if (NOT EXISTS "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/.catkin") + file(WRITE "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/.catkin" "") + endif() +ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + +IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES + "/home/roman/src/Firmware/install/_setup_util.py") + IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) + message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") + ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) + IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) + message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") + ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) +FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE PROGRAM FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/_setup_util.py") +ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + +IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES + "/home/roman/src/Firmware/install/env.sh") + IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) + message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") + ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) + IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) + message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") + ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) +FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE PROGRAM FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/env.sh") +ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + +IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES + "/home/roman/src/Firmware/install/setup.bash") + IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) + message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") + ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) + IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) + message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") + ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) +FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE FILE FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/setup.bash") +ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + +IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES + "/home/roman/src/Firmware/install/setup.sh") + IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) + message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") + ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) + IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) + message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") + ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) +FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE FILE FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/setup.sh") +ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + +IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES + "/home/roman/src/Firmware/install/setup.zsh") + IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) + message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") + ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) + IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) + message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") + ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) +FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE FILE FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/setup.zsh") +ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + +IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES + "/home/roman/src/Firmware/install/.rosinstall") + IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) + message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") + ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) + IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) + message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") + ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) +FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE FILE FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/.rosinstall") +ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + +IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + FILE(INSTALL DESTINATION "${CMAKE_INSTALL_PREFIX}/etc/catkin/profile.d" TYPE FILE FILES "/opt/ros/indigo/share/catkin/cmake/env-hooks/05.catkin_make.bash") +ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + +IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + FILE(INSTALL DESTINATION "${CMAKE_INSTALL_PREFIX}/etc/catkin/profile.d" TYPE FILE FILES "/opt/ros/indigo/share/catkin/cmake/env-hooks/05.catkin_make_isolated.bash") +ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") + +IF(NOT CMAKE_INSTALL_LOCAL_ONLY) + # Include the install script for each subdirectory. + INCLUDE("/home/roman/src/Firmware/build/gtest/cmake_install.cmake") + +ENDIF(NOT CMAKE_INSTALL_LOCAL_ONLY) + +IF(CMAKE_INSTALL_COMPONENT) + SET(CMAKE_INSTALL_MANIFEST "install_manifest_${CMAKE_INSTALL_COMPONENT}.txt") +ELSE(CMAKE_INSTALL_COMPONENT) + SET(CMAKE_INSTALL_MANIFEST "install_manifest.txt") +ENDIF(CMAKE_INSTALL_COMPONENT) + +FILE(WRITE "/home/roman/src/Firmware/build/${CMAKE_INSTALL_MANIFEST}" "") +FOREACH(file ${CMAKE_INSTALL_MANIFEST_FILES}) + FILE(APPEND "/home/roman/src/Firmware/build/${CMAKE_INSTALL_MANIFEST}" "${file}\n") +ENDFOREACH(file) diff --git a/build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake b/build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake new file mode 100644 index 000000000..06b7c63c1 --- /dev/null +++ b/build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake @@ -0,0 +1,16 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +# Relative path conversion top directories. +SET(CMAKE_RELATIVE_PATH_TOP_SOURCE "/usr/src/gtest") +SET(CMAKE_RELATIVE_PATH_TOP_BINARY "/home/roman/src/Firmware/build") + +# Force unix paths in dependencies. +SET(CMAKE_FORCE_UNIX_PATHS 1) + + +# The C and CXX include file regular expressions for this directory. +SET(CMAKE_C_INCLUDE_REGEX_SCAN "^.*$") +SET(CMAKE_C_INCLUDE_REGEX_COMPLAIN "^$") +SET(CMAKE_CXX_INCLUDE_REGEX_SCAN ${CMAKE_C_INCLUDE_REGEX_SCAN}) +SET(CMAKE_CXX_INCLUDE_REGEX_COMPLAIN ${CMAKE_C_INCLUDE_REGEX_COMPLAIN}) diff --git a/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake b/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake new file mode 100644 index 000000000..40234804c --- /dev/null +++ b/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake @@ -0,0 +1,27 @@ +# The set of languages for which implicit dependencies are needed: +SET(CMAKE_DEPENDS_LANGUAGES + "CXX" + ) +# The set of files for implicit dependencies of each language: +SET(CMAKE_DEPENDS_CHECK_CXX + "/usr/src/gtest/src/gtest-all.cc" "/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o" + ) +SET(CMAKE_CXX_COMPILER_ID "GNU") + +# Preprocessor definitions for this target. +SET(CMAKE_TARGET_DEFINITIONS + "GTEST_CREATE_SHARED_LIBRARY=1" + ) + +# Targets to which this target links. +SET(CMAKE_TARGET_LINKED_INFO_FILES + ) + +# The include file search paths: +SET(CMAKE_C_TARGET_INCLUDE_PATH + "/usr/src/gtest/include" + "/usr/src/gtest" + ) +SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) +SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) +SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) diff --git a/build/gtest/CMakeFiles/gtest.dir/build.make b/build/gtest/CMakeFiles/gtest.dir/build.make new file mode 100644 index 000000000..744feec6d --- /dev/null +++ b/build/gtest/CMakeFiles/gtest.dir/build.make @@ -0,0 +1,102 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E remove -f + +# Escaping for special characters. +EQUALS = = + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /home/roman/src/Firmware/build + +# Include any dependencies generated for this target. +include gtest/CMakeFiles/gtest.dir/depend.make + +# Include the progress variables for this target. +include gtest/CMakeFiles/gtest.dir/progress.make + +# Include the compile flags for this target's objects. +include gtest/CMakeFiles/gtest.dir/flags.make + +gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o: gtest/CMakeFiles/gtest.dir/flags.make +gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o: /usr/src/gtest/src/gtest-all.cc + $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles $(CMAKE_PROGRESS_1) + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Building CXX object gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o" + cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++ $(CXX_DEFINES) $(CXX_FLAGS) -o CMakeFiles/gtest.dir/src/gtest-all.cc.o -c /usr/src/gtest/src/gtest-all.cc + +gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.i: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/gtest.dir/src/gtest-all.cc.i" + cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++ $(CXX_DEFINES) $(CXX_FLAGS) -E /usr/src/gtest/src/gtest-all.cc > CMakeFiles/gtest.dir/src/gtest-all.cc.i + +gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.s: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/gtest.dir/src/gtest-all.cc.s" + cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++ $(CXX_DEFINES) $(CXX_FLAGS) -S /usr/src/gtest/src/gtest-all.cc -o CMakeFiles/gtest.dir/src/gtest-all.cc.s + +gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.requires: +.PHONY : gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.requires + +gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.provides: gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.requires + $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.provides.build +.PHONY : gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.provides + +gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.provides.build: gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o + +# Object files for target gtest +gtest_OBJECTS = \ +"CMakeFiles/gtest.dir/src/gtest-all.cc.o" + +# External object files for target gtest +gtest_EXTERNAL_OBJECTS = + +gtest/libgtest.so: gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o +gtest/libgtest.so: gtest/CMakeFiles/gtest.dir/build.make +gtest/libgtest.so: gtest/CMakeFiles/gtest.dir/link.txt + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --red --bold "Linking CXX shared library libgtest.so" + cd /home/roman/src/Firmware/build/gtest && $(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/gtest.dir/link.txt --verbose=$(VERBOSE) + +# Rule to build all files generated by this target. +gtest/CMakeFiles/gtest.dir/build: gtest/libgtest.so +.PHONY : gtest/CMakeFiles/gtest.dir/build + +gtest/CMakeFiles/gtest.dir/requires: gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.requires +.PHONY : gtest/CMakeFiles/gtest.dir/requires + +gtest/CMakeFiles/gtest.dir/clean: + cd /home/roman/src/Firmware/build/gtest && $(CMAKE_COMMAND) -P CMakeFiles/gtest.dir/cmake_clean.cmake +.PHONY : gtest/CMakeFiles/gtest.dir/clean + +gtest/CMakeFiles/gtest.dir/depend: + cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /usr/src/gtest /home/roman/src/Firmware/build /home/roman/src/Firmware/build/gtest /home/roman/src/Firmware/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake --color=$(COLOR) +.PHONY : gtest/CMakeFiles/gtest.dir/depend + diff --git a/build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake b/build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake new file mode 100644 index 000000000..015a1ee75 --- /dev/null +++ b/build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake @@ -0,0 +1,10 @@ +FILE(REMOVE_RECURSE + "CMakeFiles/gtest.dir/src/gtest-all.cc.o" + "libgtest.pdb" + "libgtest.so" +) + +# Per-language clean rules from dependency scanning. +FOREACH(lang CXX) + INCLUDE(CMakeFiles/gtest.dir/cmake_clean_${lang}.cmake OPTIONAL) +ENDFOREACH(lang) diff --git a/build/gtest/CMakeFiles/gtest.dir/depend.make b/build/gtest/CMakeFiles/gtest.dir/depend.make new file mode 100644 index 000000000..37ac348db --- /dev/null +++ b/build/gtest/CMakeFiles/gtest.dir/depend.make @@ -0,0 +1,2 @@ +# Empty dependencies file for gtest. +# This may be replaced when dependencies are built. diff --git a/build/gtest/CMakeFiles/gtest.dir/flags.make b/build/gtest/CMakeFiles/gtest.dir/flags.make new file mode 100644 index 000000000..fa55300ac --- /dev/null +++ b/build/gtest/CMakeFiles/gtest.dir/flags.make @@ -0,0 +1,8 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +# compile CXX with /usr/bin/c++ +CXX_FLAGS = -fPIC -I/usr/src/gtest/include -I/usr/src/gtest -Wall -Wshadow -DGTEST_HAS_PTHREAD=1 -fexceptions -Wextra + +CXX_DEFINES = -DGTEST_CREATE_SHARED_LIBRARY=1 -Dgtest_EXPORTS + diff --git a/build/gtest/CMakeFiles/gtest.dir/link.txt b/build/gtest/CMakeFiles/gtest.dir/link.txt new file mode 100644 index 000000000..8745cedd2 --- /dev/null +++ b/build/gtest/CMakeFiles/gtest.dir/link.txt @@ -0,0 +1 @@ +/usr/bin/c++ -fPIC -shared -Wl,-soname,libgtest.so -o libgtest.so CMakeFiles/gtest.dir/src/gtest-all.cc.o -L/home/roman/src/Firmware/build/gtest/src -lpthread -Wl,-rpath,/home/roman/src/Firmware/build/gtest/src diff --git a/build/gtest/CMakeFiles/gtest.dir/progress.make b/build/gtest/CMakeFiles/gtest.dir/progress.make new file mode 100644 index 000000000..781c7de27 --- /dev/null +++ b/build/gtest/CMakeFiles/gtest.dir/progress.make @@ -0,0 +1,2 @@ +CMAKE_PROGRESS_1 = 1 + diff --git a/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake b/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake new file mode 100644 index 000000000..9f3beee6e --- /dev/null +++ b/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake @@ -0,0 +1,28 @@ +# The set of languages for which implicit dependencies are needed: +SET(CMAKE_DEPENDS_LANGUAGES + "CXX" + ) +# The set of files for implicit dependencies of each language: +SET(CMAKE_DEPENDS_CHECK_CXX + "/usr/src/gtest/src/gtest_main.cc" "/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o" + ) +SET(CMAKE_CXX_COMPILER_ID "GNU") + +# Preprocessor definitions for this target. +SET(CMAKE_TARGET_DEFINITIONS + "GTEST_CREATE_SHARED_LIBRARY=1" + ) + +# Targets to which this target links. +SET(CMAKE_TARGET_LINKED_INFO_FILES + "/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake" + ) + +# The include file search paths: +SET(CMAKE_C_TARGET_INCLUDE_PATH + "/usr/src/gtest/include" + "/usr/src/gtest" + ) +SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) +SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) +SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) diff --git a/build/gtest/CMakeFiles/gtest_main.dir/build.make b/build/gtest/CMakeFiles/gtest_main.dir/build.make new file mode 100644 index 000000000..67a7ec018 --- /dev/null +++ b/build/gtest/CMakeFiles/gtest_main.dir/build.make @@ -0,0 +1,103 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E remove -f + +# Escaping for special characters. +EQUALS = = + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /home/roman/src/Firmware/build + +# Include any dependencies generated for this target. +include gtest/CMakeFiles/gtest_main.dir/depend.make + +# Include the progress variables for this target. +include gtest/CMakeFiles/gtest_main.dir/progress.make + +# Include the compile flags for this target's objects. +include gtest/CMakeFiles/gtest_main.dir/flags.make + +gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o: gtest/CMakeFiles/gtest_main.dir/flags.make +gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o: /usr/src/gtest/src/gtest_main.cc + $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles $(CMAKE_PROGRESS_1) + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Building CXX object gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o" + cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++ $(CXX_DEFINES) $(CXX_FLAGS) -o CMakeFiles/gtest_main.dir/src/gtest_main.cc.o -c /usr/src/gtest/src/gtest_main.cc + +gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.i: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/gtest_main.dir/src/gtest_main.cc.i" + cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++ $(CXX_DEFINES) $(CXX_FLAGS) -E /usr/src/gtest/src/gtest_main.cc > CMakeFiles/gtest_main.dir/src/gtest_main.cc.i + +gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.s: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/gtest_main.dir/src/gtest_main.cc.s" + cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++ $(CXX_DEFINES) $(CXX_FLAGS) -S /usr/src/gtest/src/gtest_main.cc -o CMakeFiles/gtest_main.dir/src/gtest_main.cc.s + +gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.requires: +.PHONY : gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.requires + +gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.provides: gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.requires + $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.provides.build +.PHONY : gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.provides + +gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.provides.build: gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o + +# Object files for target gtest_main +gtest_main_OBJECTS = \ +"CMakeFiles/gtest_main.dir/src/gtest_main.cc.o" + +# External object files for target gtest_main +gtest_main_EXTERNAL_OBJECTS = + +gtest/libgtest_main.so: gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o +gtest/libgtest_main.so: gtest/CMakeFiles/gtest_main.dir/build.make +gtest/libgtest_main.so: gtest/libgtest.so +gtest/libgtest_main.so: gtest/CMakeFiles/gtest_main.dir/link.txt + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --red --bold "Linking CXX shared library libgtest_main.so" + cd /home/roman/src/Firmware/build/gtest && $(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/gtest_main.dir/link.txt --verbose=$(VERBOSE) + +# Rule to build all files generated by this target. +gtest/CMakeFiles/gtest_main.dir/build: gtest/libgtest_main.so +.PHONY : gtest/CMakeFiles/gtest_main.dir/build + +gtest/CMakeFiles/gtest_main.dir/requires: gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.requires +.PHONY : gtest/CMakeFiles/gtest_main.dir/requires + +gtest/CMakeFiles/gtest_main.dir/clean: + cd /home/roman/src/Firmware/build/gtest && $(CMAKE_COMMAND) -P CMakeFiles/gtest_main.dir/cmake_clean.cmake +.PHONY : gtest/CMakeFiles/gtest_main.dir/clean + +gtest/CMakeFiles/gtest_main.dir/depend: + cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /usr/src/gtest /home/roman/src/Firmware/build /home/roman/src/Firmware/build/gtest /home/roman/src/Firmware/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake --color=$(COLOR) +.PHONY : gtest/CMakeFiles/gtest_main.dir/depend + diff --git a/build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake b/build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake new file mode 100644 index 000000000..c8fe83819 --- /dev/null +++ b/build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake @@ -0,0 +1,10 @@ +FILE(REMOVE_RECURSE + "CMakeFiles/gtest_main.dir/src/gtest_main.cc.o" + "libgtest_main.pdb" + "libgtest_main.so" +) + +# Per-language clean rules from dependency scanning. +FOREACH(lang CXX) + INCLUDE(CMakeFiles/gtest_main.dir/cmake_clean_${lang}.cmake OPTIONAL) +ENDFOREACH(lang) diff --git a/build/gtest/CMakeFiles/gtest_main.dir/depend.make b/build/gtest/CMakeFiles/gtest_main.dir/depend.make new file mode 100644 index 000000000..1d67c1ab5 --- /dev/null +++ b/build/gtest/CMakeFiles/gtest_main.dir/depend.make @@ -0,0 +1,2 @@ +# Empty dependencies file for gtest_main. +# This may be replaced when dependencies are built. diff --git a/build/gtest/CMakeFiles/gtest_main.dir/flags.make b/build/gtest/CMakeFiles/gtest_main.dir/flags.make new file mode 100644 index 000000000..93ab70f12 --- /dev/null +++ b/build/gtest/CMakeFiles/gtest_main.dir/flags.make @@ -0,0 +1,8 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +# compile CXX with /usr/bin/c++ +CXX_FLAGS = -fPIC -I/usr/src/gtest/include -I/usr/src/gtest -Wall -Wshadow -DGTEST_HAS_PTHREAD=1 -fexceptions -Wextra + +CXX_DEFINES = -DGTEST_CREATE_SHARED_LIBRARY=1 -Dgtest_main_EXPORTS + diff --git a/build/gtest/CMakeFiles/gtest_main.dir/link.txt b/build/gtest/CMakeFiles/gtest_main.dir/link.txt new file mode 100644 index 000000000..d35b7f61a --- /dev/null +++ b/build/gtest/CMakeFiles/gtest_main.dir/link.txt @@ -0,0 +1 @@ +/usr/bin/c++ -fPIC -shared -Wl,-soname,libgtest_main.so -o libgtest_main.so CMakeFiles/gtest_main.dir/src/gtest_main.cc.o -L/home/roman/src/Firmware/build/gtest/src -lpthread libgtest.so -lpthread -Wl,-rpath,/home/roman/src/Firmware/build/gtest/src:/home/roman/src/Firmware/build/gtest diff --git a/build/gtest/CMakeFiles/gtest_main.dir/progress.make b/build/gtest/CMakeFiles/gtest_main.dir/progress.make new file mode 100644 index 000000000..164e1d26c --- /dev/null +++ b/build/gtest/CMakeFiles/gtest_main.dir/progress.make @@ -0,0 +1,2 @@ +CMAKE_PROGRESS_1 = 2 + diff --git a/build/gtest/CMakeFiles/progress.marks b/build/gtest/CMakeFiles/progress.marks new file mode 100644 index 000000000..573541ac9 --- /dev/null +++ b/build/gtest/CMakeFiles/progress.marks @@ -0,0 +1 @@ +0 diff --git a/build/gtest/CTestTestfile.cmake b/build/gtest/CTestTestfile.cmake new file mode 100644 index 000000000..bd4b57a8b --- /dev/null +++ b/build/gtest/CTestTestfile.cmake @@ -0,0 +1,6 @@ +# CMake generated Testfile for +# Source directory: /usr/src/gtest +# Build directory: /home/roman/src/Firmware/build/gtest +# +# This file includes the relevant testing commands required for +# testing this directory and lists subdirectories to be tested as well. diff --git a/build/gtest/Makefile b/build/gtest/Makefile new file mode 100644 index 000000000..bc8c23a00 --- /dev/null +++ b/build/gtest/Makefile @@ -0,0 +1,262 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +# Default target executed when no arguments are given to make. +default_target: all +.PHONY : default_target + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E remove -f + +# Escaping for special characters. +EQUALS = = + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /home/roman/src/Firmware/build + +#============================================================================= +# Targets provided globally by CMake. + +# Special rule for the target edit_cache +edit_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running interactive CMake command-line interface..." + /usr/bin/cmake -i . +.PHONY : edit_cache + +# Special rule for the target edit_cache +edit_cache/fast: edit_cache +.PHONY : edit_cache/fast + +# Special rule for the target install +install: preinstall + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." + /usr/bin/cmake -P cmake_install.cmake +.PHONY : install + +# Special rule for the target install +install/fast: preinstall/fast + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." + /usr/bin/cmake -P cmake_install.cmake +.PHONY : install/fast + +# Special rule for the target install/local +install/local: preinstall + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..." + /usr/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake +.PHONY : install/local + +# Special rule for the target install/local +install/local/fast: install/local +.PHONY : install/local/fast + +# Special rule for the target install/strip +install/strip: preinstall + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..." + /usr/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake +.PHONY : install/strip + +# Special rule for the target install/strip +install/strip/fast: install/strip +.PHONY : install/strip/fast + +# Special rule for the target list_install_components +list_install_components: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Available install components are: \"Unspecified\"" +.PHONY : list_install_components + +# Special rule for the target list_install_components +list_install_components/fast: list_install_components +.PHONY : list_install_components/fast + +# Special rule for the target rebuild_cache +rebuild_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..." + /usr/bin/cmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) +.PHONY : rebuild_cache + +# Special rule for the target rebuild_cache +rebuild_cache/fast: rebuild_cache +.PHONY : rebuild_cache/fast + +# Special rule for the target test +test: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running tests..." + /usr/bin/ctest --force-new-ctest-process $(ARGS) +.PHONY : test + +# Special rule for the target test +test/fast: test +.PHONY : test/fast + +# The main all target +all: cmake_check_build_system + cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles /home/roman/src/Firmware/build/gtest/CMakeFiles/progress.marks + cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/all + $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 +.PHONY : all + +# The main clean target +clean: + cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/clean +.PHONY : clean + +# The main clean target +clean/fast: clean +.PHONY : clean/fast + +# Prepare targets for installation. +preinstall: all + cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/preinstall +.PHONY : preinstall + +# Prepare targets for installation. +preinstall/fast: + cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/preinstall +.PHONY : preinstall/fast + +# clear depends +depend: + cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1 +.PHONY : depend + +# Convenience name for target. +gtest/CMakeFiles/gtest.dir/rule: + cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/CMakeFiles/gtest.dir/rule +.PHONY : gtest/CMakeFiles/gtest.dir/rule + +# Convenience name for target. +gtest: gtest/CMakeFiles/gtest.dir/rule +.PHONY : gtest + +# fast build rule for target. +gtest/fast: + cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/build +.PHONY : gtest/fast + +# Convenience name for target. +gtest/CMakeFiles/gtest_main.dir/rule: + cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/CMakeFiles/gtest_main.dir/rule +.PHONY : gtest/CMakeFiles/gtest_main.dir/rule + +# Convenience name for target. +gtest_main: gtest/CMakeFiles/gtest_main.dir/rule +.PHONY : gtest_main + +# fast build rule for target. +gtest_main/fast: + cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/build +.PHONY : gtest_main/fast + +src/gtest-all.o: src/gtest-all.cc.o +.PHONY : src/gtest-all.o + +# target to build an object file +src/gtest-all.cc.o: + cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o +.PHONY : src/gtest-all.cc.o + +src/gtest-all.i: src/gtest-all.cc.i +.PHONY : src/gtest-all.i + +# target to preprocess a source file +src/gtest-all.cc.i: + cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.i +.PHONY : src/gtest-all.cc.i + +src/gtest-all.s: src/gtest-all.cc.s +.PHONY : src/gtest-all.s + +# target to generate assembly for a file +src/gtest-all.cc.s: + cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.s +.PHONY : src/gtest-all.cc.s + +src/gtest_main.o: src/gtest_main.cc.o +.PHONY : src/gtest_main.o + +# target to build an object file +src/gtest_main.cc.o: + cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o +.PHONY : src/gtest_main.cc.o + +src/gtest_main.i: src/gtest_main.cc.i +.PHONY : src/gtest_main.i + +# target to preprocess a source file +src/gtest_main.cc.i: + cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.i +.PHONY : src/gtest_main.cc.i + +src/gtest_main.s: src/gtest_main.cc.s +.PHONY : src/gtest_main.s + +# target to generate assembly for a file +src/gtest_main.cc.s: + cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.s +.PHONY : src/gtest_main.cc.s + +# Help Target +help: + @echo "The following are some of the valid targets for this Makefile:" + @echo "... all (the default if no target is provided)" + @echo "... clean" + @echo "... depend" + @echo "... edit_cache" + @echo "... gtest" + @echo "... gtest_main" + @echo "... install" + @echo "... install/local" + @echo "... install/strip" + @echo "... list_install_components" + @echo "... rebuild_cache" + @echo "... test" + @echo "... src/gtest-all.o" + @echo "... src/gtest-all.i" + @echo "... src/gtest-all.s" + @echo "... src/gtest_main.o" + @echo "... src/gtest_main.i" + @echo "... src/gtest_main.s" +.PHONY : help + + + +#============================================================================= +# Special targets to cleanup operation of make. + +# Special rule to run CMake to check the build system integrity. +# No rule that depends on this can have commands that come from listfiles +# because they might be regenerated. +cmake_check_build_system: + cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 +.PHONY : cmake_check_build_system + diff --git a/build/gtest/cmake_install.cmake b/build/gtest/cmake_install.cmake new file mode 100644 index 000000000..f51b65651 --- /dev/null +++ b/build/gtest/cmake_install.cmake @@ -0,0 +1,34 @@ +# Install script for directory: /usr/src/gtest + +# Set the install prefix +IF(NOT DEFINED CMAKE_INSTALL_PREFIX) + SET(CMAKE_INSTALL_PREFIX "/home/roman/src/Firmware/install") +ENDIF(NOT DEFINED CMAKE_INSTALL_PREFIX) +STRING(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}") + +# Set the install configuration name. +IF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) + IF(BUILD_TYPE) + STRING(REGEX REPLACE "^[^A-Za-z0-9_]+" "" + CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}") + ELSE(BUILD_TYPE) + SET(CMAKE_INSTALL_CONFIG_NAME "") + ENDIF(BUILD_TYPE) + MESSAGE(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"") +ENDIF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) + +# Set the component getting installed. +IF(NOT CMAKE_INSTALL_COMPONENT) + IF(COMPONENT) + MESSAGE(STATUS "Install component: \"${COMPONENT}\"") + SET(CMAKE_INSTALL_COMPONENT "${COMPONENT}") + ELSE(COMPONENT) + SET(CMAKE_INSTALL_COMPONENT) + ENDIF(COMPONENT) +ENDIF(NOT CMAKE_INSTALL_COMPONENT) + +# Install shared libraries without execute permission? +IF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) + SET(CMAKE_INSTALL_SO_NO_EXE "1") +ENDIF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) + diff --git a/devel/.catkin b/devel/.catkin new file mode 100644 index 000000000..50d43da7e --- /dev/null +++ b/devel/.catkin @@ -0,0 +1 @@ +/home/roman/src/Firmware/src \ No newline at end of file diff --git a/devel/.rosinstall b/devel/.rosinstall new file mode 100644 index 000000000..fda964e6c --- /dev/null +++ b/devel/.rosinstall @@ -0,0 +1,2 @@ +- setup-file: + local-name: /home/roman/src/Firmware/devel/setup.sh diff --git a/devel/_setup_util.py b/devel/_setup_util.py new file mode 100755 index 000000000..8db644140 --- /dev/null +++ b/devel/_setup_util.py @@ -0,0 +1,287 @@ +#!/usr/bin/python + +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * 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. +# * Neither the name of Willow Garage, Inc. 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. + +'''This file generates shell code for the setup.SHELL scripts to set environment variables''' + +from __future__ import print_function +import argparse +import copy +import errno +import os +import platform +import sys + +CATKIN_MARKER_FILE = '.catkin' + +system = platform.system() +IS_DARWIN = (system == 'Darwin') +IS_WINDOWS = (system == 'Windows') + +# subfolder of workspace prepended to CMAKE_PREFIX_PATH +ENV_VAR_SUBFOLDERS = { + 'CMAKE_PREFIX_PATH': '', + 'CPATH': 'include', + 'LD_LIBRARY_PATH' if not IS_DARWIN else 'DYLD_LIBRARY_PATH': ['lib', os.path.join('lib', 'x86_64-linux-gnu')], + 'PATH': 'bin', + 'PKG_CONFIG_PATH': [os.path.join('lib', 'pkgconfig'), os.path.join('lib', 'x86_64-linux-gnu', 'pkgconfig')], + 'PYTHONPATH': 'lib/python2.7/dist-packages', +} + + +def rollback_env_variables(environ, env_var_subfolders): + ''' + Generate shell code to reset environment variables + by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH. + This does not cover modifications performed by environment hooks. + ''' + lines = [] + unmodified_environ = copy.copy(environ) + for key in sorted(env_var_subfolders.keys()): + subfolders = env_var_subfolders[key] + if not isinstance(subfolders, list): + subfolders = [subfolders] + for subfolder in subfolders: + value = _rollback_env_variable(unmodified_environ, key, subfolder) + if value is not None: + environ[key] = value + lines.append(assignment(key, value)) + if lines: + lines.insert(0, comment('reset environment variables by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH')) + return lines + + +def _rollback_env_variable(environ, name, subfolder): + ''' + For each catkin workspace in CMAKE_PREFIX_PATH remove the first entry from env[NAME] matching workspace + subfolder. + + :param subfolder: str '' or subfoldername that may start with '/' + :returns: the updated value of the environment variable. + ''' + value = environ[name] if name in environ else '' + env_paths = [path for path in value.split(os.pathsep) if path] + value_modified = False + if subfolder: + if subfolder.startswith(os.path.sep) or (os.path.altsep and subfolder.startswith(os.path.altsep)): + subfolder = subfolder[1:] + if subfolder.endswith(os.path.sep) or (os.path.altsep and subfolder.endswith(os.path.altsep)): + subfolder = subfolder[:-1] + for ws_path in _get_workspaces(environ, include_fuerte=True, include_non_existing=True): + path_to_find = os.path.join(ws_path, subfolder) if subfolder else ws_path + path_to_remove = None + for env_path in env_paths: + env_path_clean = env_path[:-1] if env_path and env_path[-1] in [os.path.sep, os.path.altsep] else env_path + if env_path_clean == path_to_find: + path_to_remove = env_path + break + if path_to_remove: + env_paths.remove(path_to_remove) + value_modified = True + new_value = os.pathsep.join(env_paths) + return new_value if value_modified else None + + +def _get_workspaces(environ, include_fuerte=False, include_non_existing=False): + ''' + Based on CMAKE_PREFIX_PATH return all catkin workspaces. + + :param include_fuerte: The flag if paths starting with '/opt/ros/fuerte' should be considered workspaces, ``bool`` + ''' + # get all cmake prefix paths + env_name = 'CMAKE_PREFIX_PATH' + value = environ[env_name] if env_name in environ else '' + paths = [path for path in value.split(os.pathsep) if path] + # remove non-workspace paths + workspaces = [path for path in paths if os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE)) or (include_fuerte and path.startswith('/opt/ros/fuerte')) or (include_non_existing and not os.path.exists(path))] + return workspaces + + +def prepend_env_variables(environ, env_var_subfolders, workspaces): + ''' + Generate shell code to prepend environment variables + for the all workspaces. + ''' + lines = [] + lines.append(comment('prepend folders of workspaces to environment variables')) + + paths = [path for path in workspaces.split(os.pathsep) if path] + + prefix = _prefix_env_variable(environ, 'CMAKE_PREFIX_PATH', paths, '') + lines.append(prepend(environ, 'CMAKE_PREFIX_PATH', prefix)) + + for key in sorted([key for key in env_var_subfolders.keys() if key != 'CMAKE_PREFIX_PATH']): + subfolder = env_var_subfolders[key] + prefix = _prefix_env_variable(environ, key, paths, subfolder) + lines.append(prepend(environ, key, prefix)) + return lines + + +def _prefix_env_variable(environ, name, paths, subfolders): + ''' + Return the prefix to prepend to the environment variable NAME, adding any path in NEW_PATHS_STR without creating duplicate or empty items. + ''' + value = environ[name] if name in environ else '' + environ_paths = [path for path in value.split(os.pathsep) if path] + checked_paths = [] + for path in paths: + if not isinstance(subfolders, list): + subfolders = [subfolders] + for subfolder in subfolders: + path_tmp = path + if subfolder: + path_tmp = os.path.join(path_tmp, subfolder) + # exclude any path already in env and any path we already added + if path_tmp not in environ_paths and path_tmp not in checked_paths: + checked_paths.append(path_tmp) + prefix_str = os.pathsep.join(checked_paths) + if prefix_str != '' and environ_paths: + prefix_str += os.pathsep + return prefix_str + + +def assignment(key, value): + if not IS_WINDOWS: + return 'export %s="%s"' % (key, value) + else: + return 'set %s=%s' % (key, value) + + +def comment(msg): + if not IS_WINDOWS: + return '# %s' % msg + else: + return 'REM %s' % msg + + +def prepend(environ, key, prefix): + if key not in environ or not environ[key]: + return assignment(key, prefix) + if not IS_WINDOWS: + return 'export %s="%s$%s"' % (key, prefix, key) + else: + return 'set %s=%s%%%s%%' % (key, prefix, key) + + +def find_env_hooks(environ, cmake_prefix_path): + ''' + Generate shell code with found environment hooks + for the all workspaces. + ''' + lines = [] + lines.append(comment('found environment hooks in workspaces')) + + generic_env_hooks = [] + generic_env_hooks_workspace = [] + specific_env_hooks = [] + specific_env_hooks_workspace = [] + generic_env_hooks_by_filename = {} + specific_env_hooks_by_filename = {} + generic_env_hook_ext = 'bat' if IS_WINDOWS else 'sh' + specific_env_hook_ext = environ['CATKIN_SHELL'] if not IS_WINDOWS and 'CATKIN_SHELL' in environ and environ['CATKIN_SHELL'] else None + # remove non-workspace paths + workspaces = [path for path in cmake_prefix_path.split(os.pathsep) if path and os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE))] + for workspace in reversed(workspaces): + env_hook_dir = os.path.join(workspace, 'etc', 'catkin', 'profile.d') + if os.path.isdir(env_hook_dir): + for filename in sorted(os.listdir(env_hook_dir)): + if filename.endswith('.%s' % generic_env_hook_ext): + # remove previous env hook with same name if present + if filename in generic_env_hooks_by_filename: + i = generic_env_hooks.index(generic_env_hooks_by_filename[filename]) + generic_env_hooks.pop(i) + generic_env_hooks_workspace.pop(i) + # append env hook + generic_env_hooks.append(os.path.join(env_hook_dir, filename)) + generic_env_hooks_workspace.append(workspace) + generic_env_hooks_by_filename[filename] = generic_env_hooks[-1] + elif specific_env_hook_ext is not None and filename.endswith('.%s' % specific_env_hook_ext): + # remove previous env hook with same name if present + if filename in specific_env_hooks_by_filename: + i = specific_env_hooks.index(specific_env_hooks_by_filename[filename]) + specific_env_hooks.pop(i) + specific_env_hooks_workspace.pop(i) + # append env hook + specific_env_hooks.append(os.path.join(env_hook_dir, filename)) + specific_env_hooks_workspace.append(workspace) + specific_env_hooks_by_filename[filename] = specific_env_hooks[-1] + env_hooks = generic_env_hooks + specific_env_hooks + env_hooks_workspace = generic_env_hooks_workspace + specific_env_hooks_workspace + count = len(env_hooks) + lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_COUNT', count)) + for i in range(count): + lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d' % i, env_hooks[i])) + lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d_WORKSPACE' % i, env_hooks_workspace[i])) + return lines + + +def _parse_arguments(args=None): + parser = argparse.ArgumentParser(description='Generates code blocks for the setup.SHELL script.') + parser.add_argument('--extend', action='store_true', help='Skip unsetting previous environment variables to extend context') + return parser.parse_known_args(args=args)[0] + + +if __name__ == '__main__': + try: + try: + args = _parse_arguments() + except Exception as e: + print(e, file=sys.stderr) + sys.exit(1) + + # environment at generation time + CMAKE_PREFIX_PATH = '/home/roman/catkin_ws/devel;/opt/ros/indigo'.split(';') + # prepend current workspace if not already part of CPP + base_path = os.path.dirname(__file__) + if base_path not in CMAKE_PREFIX_PATH: + CMAKE_PREFIX_PATH.insert(0, base_path) + CMAKE_PREFIX_PATH = os.pathsep.join(CMAKE_PREFIX_PATH) + + environ = dict(os.environ) + lines = [] + if not args.extend: + lines += rollback_env_variables(environ, ENV_VAR_SUBFOLDERS) + lines += prepend_env_variables(environ, ENV_VAR_SUBFOLDERS, CMAKE_PREFIX_PATH) + lines += find_env_hooks(environ, CMAKE_PREFIX_PATH) + print('\n'.join(lines)) + + # need to explicitly flush the output + sys.stdout.flush() + except IOError as e: + # and catch potantial "broken pipe" if stdout is not writable + # which can happen when piping the output to a file but the disk is full + if e.errno == errno.EPIPE: + print(e, file=sys.stderr) + sys.exit(2) + raise + + sys.exit(0) diff --git a/devel/env.sh b/devel/env.sh new file mode 100755 index 000000000..8aa9d244a --- /dev/null +++ b/devel/env.sh @@ -0,0 +1,16 @@ +#!/usr/bin/env sh +# generated from catkin/cmake/templates/env.sh.in + +if [ $# -eq 0 ] ; then + /bin/echo "Usage: env.sh COMMANDS" + /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually." + exit 1 +fi + +# ensure to not use different shell type which was set before +CATKIN_SHELL=sh + +# source setup.sh from same directory as this file +_CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd) +. "$_CATKIN_SETUP_DIR/setup.sh" +exec "$@" diff --git a/devel/setup.bash b/devel/setup.bash new file mode 100644 index 000000000..ff47af8f3 --- /dev/null +++ b/devel/setup.bash @@ -0,0 +1,8 @@ +#!/usr/bin/env bash +# generated from catkin/cmake/templates/setup.bash.in + +CATKIN_SHELL=bash + +# source setup.sh from same directory as this file +_CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd) +. "$_CATKIN_SETUP_DIR/setup.sh" diff --git a/devel/setup.sh b/devel/setup.sh new file mode 100644 index 000000000..7d80d638a --- /dev/null +++ b/devel/setup.sh @@ -0,0 +1,87 @@ +#!/usr/bin/env sh +# generated from catkin/cmake/template/setup.sh.in + +# Sets various environment variables and sources additional environment hooks. +# It tries it's best to undo changes from a previously sourced setup file before. +# Supported command line options: +# --extend: skips the undoing of changes from a previously sourced setup file + +# since this file is sourced either use the provided _CATKIN_SETUP_DIR +# or fall back to the destination set at configure time +: ${_CATKIN_SETUP_DIR:=/home/roman/src/Firmware/devel} +_SETUP_UTIL="$_CATKIN_SETUP_DIR/_setup_util.py" +unset _CATKIN_SETUP_DIR + +if [ ! -f "$_SETUP_UTIL" ]; then + echo "Missing Python script: $_SETUP_UTIL" + return 22 +fi + +# detect if running on Darwin platform +_UNAME=`uname -s` +_IS_DARWIN=0 +if [ "$_UNAME" = "Darwin" ]; then + _IS_DARWIN=1 +fi +unset _UNAME + +# make sure to export all environment variables +export CMAKE_PREFIX_PATH +export CPATH +if [ $_IS_DARWIN -eq 0 ]; then + export LD_LIBRARY_PATH +else + export DYLD_LIBRARY_PATH +fi +unset _IS_DARWIN +export PATH +export PKG_CONFIG_PATH +export PYTHONPATH + +# remember type of shell if not already set +if [ -z "$CATKIN_SHELL" ]; then + CATKIN_SHELL=sh +fi + +# invoke Python script to generate necessary exports of environment variables +_SETUP_TMP=`mktemp /tmp/setup.sh.XXXXXXXXXX` +if [ $? -ne 0 -o ! -f "$_SETUP_TMP" ]; then + echo "Could not create temporary file: $_SETUP_TMP" + return 1 +fi +CATKIN_SHELL=$CATKIN_SHELL "$_SETUP_UTIL" $@ > $_SETUP_TMP +_RC=$? +if [ $_RC -ne 0 ]; then + if [ $_RC -eq 2 ]; then + echo "Could not write the output of '$_SETUP_UTIL' to temporary file '$_SETUP_TMP': may be the disk if full?" + else + echo "Failed to run '\"$_SETUP_UTIL\" $@': return code $_RC" + fi + unset _RC + unset _SETUP_UTIL + rm -f $_SETUP_TMP + unset _SETUP_TMP + return 1 +fi +unset _RC +unset _SETUP_UTIL +. $_SETUP_TMP +rm -f $_SETUP_TMP +unset _SETUP_TMP + +# source all environment hooks +_i=0 +while [ $_i -lt $_CATKIN_ENVIRONMENT_HOOKS_COUNT ]; do + eval _envfile=\$_CATKIN_ENVIRONMENT_HOOKS_$_i + unset _CATKIN_ENVIRONMENT_HOOKS_$_i + eval _envfile_workspace=\$_CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE + unset _CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE + # set workspace for environment hook + CATKIN_ENV_HOOK_WORKSPACE=$_envfile_workspace + . "$_envfile" + unset CATKIN_ENV_HOOK_WORKSPACE + _i=$((_i + 1)) +done +unset _i + +unset _CATKIN_ENVIRONMENT_HOOKS_COUNT diff --git a/devel/setup.zsh b/devel/setup.zsh new file mode 100644 index 000000000..b66071766 --- /dev/null +++ b/devel/setup.zsh @@ -0,0 +1,8 @@ +#!/usr/bin/env zsh +# generated from catkin/cmake/templates/setup.zsh.in + +CATKIN_SHELL=zsh +_CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd) +emulate sh # emulate POSIX +. "$_CATKIN_SETUP_DIR/setup.sh" +emulate zsh # back to zsh mode diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 2423e47b4..4d7487c2b 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 2423e47b4f9169e77f7194b36fa2118e018c94e2 +Subproject commit 4d7487c2bc5f5ccf87bca82970fb2c08d6d8bd50 diff --git a/uavcan b/uavcan index c7872def1..6980ee824 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit c7872def16e82a8b318d571829fe9622e2d35ff0 +Subproject commit 6980ee824074aa2f7a62221bf6040ee411119520 -- cgit v1.2.3 From 5ecb2262d1578d4af4e8e5da74df85ab9c9fc19d Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 28 Aug 2014 11:05:26 +0200 Subject: removed devel --- devel/.catkin | 1 - devel/.rosinstall | 2 - devel/_setup_util.py | 287 --------------------------------------------------- devel/env.sh | 16 --- devel/setup.bash | 8 -- devel/setup.sh | 87 ---------------- devel/setup.zsh | 8 -- 7 files changed, 409 deletions(-) delete mode 100644 devel/.catkin delete mode 100644 devel/.rosinstall delete mode 100755 devel/_setup_util.py delete mode 100755 devel/env.sh delete mode 100644 devel/setup.bash delete mode 100644 devel/setup.sh delete mode 100644 devel/setup.zsh diff --git a/devel/.catkin b/devel/.catkin deleted file mode 100644 index 50d43da7e..000000000 --- a/devel/.catkin +++ /dev/null @@ -1 +0,0 @@ -/home/roman/src/Firmware/src \ No newline at end of file diff --git a/devel/.rosinstall b/devel/.rosinstall deleted file mode 100644 index fda964e6c..000000000 --- a/devel/.rosinstall +++ /dev/null @@ -1,2 +0,0 @@ -- setup-file: - local-name: /home/roman/src/Firmware/devel/setup.sh diff --git a/devel/_setup_util.py b/devel/_setup_util.py deleted file mode 100755 index 8db644140..000000000 --- a/devel/_setup_util.py +++ /dev/null @@ -1,287 +0,0 @@ -#!/usr/bin/python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * 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. -# * Neither the name of Willow Garage, Inc. 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. - -'''This file generates shell code for the setup.SHELL scripts to set environment variables''' - -from __future__ import print_function -import argparse -import copy -import errno -import os -import platform -import sys - -CATKIN_MARKER_FILE = '.catkin' - -system = platform.system() -IS_DARWIN = (system == 'Darwin') -IS_WINDOWS = (system == 'Windows') - -# subfolder of workspace prepended to CMAKE_PREFIX_PATH -ENV_VAR_SUBFOLDERS = { - 'CMAKE_PREFIX_PATH': '', - 'CPATH': 'include', - 'LD_LIBRARY_PATH' if not IS_DARWIN else 'DYLD_LIBRARY_PATH': ['lib', os.path.join('lib', 'x86_64-linux-gnu')], - 'PATH': 'bin', - 'PKG_CONFIG_PATH': [os.path.join('lib', 'pkgconfig'), os.path.join('lib', 'x86_64-linux-gnu', 'pkgconfig')], - 'PYTHONPATH': 'lib/python2.7/dist-packages', -} - - -def rollback_env_variables(environ, env_var_subfolders): - ''' - Generate shell code to reset environment variables - by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH. - This does not cover modifications performed by environment hooks. - ''' - lines = [] - unmodified_environ = copy.copy(environ) - for key in sorted(env_var_subfolders.keys()): - subfolders = env_var_subfolders[key] - if not isinstance(subfolders, list): - subfolders = [subfolders] - for subfolder in subfolders: - value = _rollback_env_variable(unmodified_environ, key, subfolder) - if value is not None: - environ[key] = value - lines.append(assignment(key, value)) - if lines: - lines.insert(0, comment('reset environment variables by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH')) - return lines - - -def _rollback_env_variable(environ, name, subfolder): - ''' - For each catkin workspace in CMAKE_PREFIX_PATH remove the first entry from env[NAME] matching workspace + subfolder. - - :param subfolder: str '' or subfoldername that may start with '/' - :returns: the updated value of the environment variable. - ''' - value = environ[name] if name in environ else '' - env_paths = [path for path in value.split(os.pathsep) if path] - value_modified = False - if subfolder: - if subfolder.startswith(os.path.sep) or (os.path.altsep and subfolder.startswith(os.path.altsep)): - subfolder = subfolder[1:] - if subfolder.endswith(os.path.sep) or (os.path.altsep and subfolder.endswith(os.path.altsep)): - subfolder = subfolder[:-1] - for ws_path in _get_workspaces(environ, include_fuerte=True, include_non_existing=True): - path_to_find = os.path.join(ws_path, subfolder) if subfolder else ws_path - path_to_remove = None - for env_path in env_paths: - env_path_clean = env_path[:-1] if env_path and env_path[-1] in [os.path.sep, os.path.altsep] else env_path - if env_path_clean == path_to_find: - path_to_remove = env_path - break - if path_to_remove: - env_paths.remove(path_to_remove) - value_modified = True - new_value = os.pathsep.join(env_paths) - return new_value if value_modified else None - - -def _get_workspaces(environ, include_fuerte=False, include_non_existing=False): - ''' - Based on CMAKE_PREFIX_PATH return all catkin workspaces. - - :param include_fuerte: The flag if paths starting with '/opt/ros/fuerte' should be considered workspaces, ``bool`` - ''' - # get all cmake prefix paths - env_name = 'CMAKE_PREFIX_PATH' - value = environ[env_name] if env_name in environ else '' - paths = [path for path in value.split(os.pathsep) if path] - # remove non-workspace paths - workspaces = [path for path in paths if os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE)) or (include_fuerte and path.startswith('/opt/ros/fuerte')) or (include_non_existing and not os.path.exists(path))] - return workspaces - - -def prepend_env_variables(environ, env_var_subfolders, workspaces): - ''' - Generate shell code to prepend environment variables - for the all workspaces. - ''' - lines = [] - lines.append(comment('prepend folders of workspaces to environment variables')) - - paths = [path for path in workspaces.split(os.pathsep) if path] - - prefix = _prefix_env_variable(environ, 'CMAKE_PREFIX_PATH', paths, '') - lines.append(prepend(environ, 'CMAKE_PREFIX_PATH', prefix)) - - for key in sorted([key for key in env_var_subfolders.keys() if key != 'CMAKE_PREFIX_PATH']): - subfolder = env_var_subfolders[key] - prefix = _prefix_env_variable(environ, key, paths, subfolder) - lines.append(prepend(environ, key, prefix)) - return lines - - -def _prefix_env_variable(environ, name, paths, subfolders): - ''' - Return the prefix to prepend to the environment variable NAME, adding any path in NEW_PATHS_STR without creating duplicate or empty items. - ''' - value = environ[name] if name in environ else '' - environ_paths = [path for path in value.split(os.pathsep) if path] - checked_paths = [] - for path in paths: - if not isinstance(subfolders, list): - subfolders = [subfolders] - for subfolder in subfolders: - path_tmp = path - if subfolder: - path_tmp = os.path.join(path_tmp, subfolder) - # exclude any path already in env and any path we already added - if path_tmp not in environ_paths and path_tmp not in checked_paths: - checked_paths.append(path_tmp) - prefix_str = os.pathsep.join(checked_paths) - if prefix_str != '' and environ_paths: - prefix_str += os.pathsep - return prefix_str - - -def assignment(key, value): - if not IS_WINDOWS: - return 'export %s="%s"' % (key, value) - else: - return 'set %s=%s' % (key, value) - - -def comment(msg): - if not IS_WINDOWS: - return '# %s' % msg - else: - return 'REM %s' % msg - - -def prepend(environ, key, prefix): - if key not in environ or not environ[key]: - return assignment(key, prefix) - if not IS_WINDOWS: - return 'export %s="%s$%s"' % (key, prefix, key) - else: - return 'set %s=%s%%%s%%' % (key, prefix, key) - - -def find_env_hooks(environ, cmake_prefix_path): - ''' - Generate shell code with found environment hooks - for the all workspaces. - ''' - lines = [] - lines.append(comment('found environment hooks in workspaces')) - - generic_env_hooks = [] - generic_env_hooks_workspace = [] - specific_env_hooks = [] - specific_env_hooks_workspace = [] - generic_env_hooks_by_filename = {} - specific_env_hooks_by_filename = {} - generic_env_hook_ext = 'bat' if IS_WINDOWS else 'sh' - specific_env_hook_ext = environ['CATKIN_SHELL'] if not IS_WINDOWS and 'CATKIN_SHELL' in environ and environ['CATKIN_SHELL'] else None - # remove non-workspace paths - workspaces = [path for path in cmake_prefix_path.split(os.pathsep) if path and os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE))] - for workspace in reversed(workspaces): - env_hook_dir = os.path.join(workspace, 'etc', 'catkin', 'profile.d') - if os.path.isdir(env_hook_dir): - for filename in sorted(os.listdir(env_hook_dir)): - if filename.endswith('.%s' % generic_env_hook_ext): - # remove previous env hook with same name if present - if filename in generic_env_hooks_by_filename: - i = generic_env_hooks.index(generic_env_hooks_by_filename[filename]) - generic_env_hooks.pop(i) - generic_env_hooks_workspace.pop(i) - # append env hook - generic_env_hooks.append(os.path.join(env_hook_dir, filename)) - generic_env_hooks_workspace.append(workspace) - generic_env_hooks_by_filename[filename] = generic_env_hooks[-1] - elif specific_env_hook_ext is not None and filename.endswith('.%s' % specific_env_hook_ext): - # remove previous env hook with same name if present - if filename in specific_env_hooks_by_filename: - i = specific_env_hooks.index(specific_env_hooks_by_filename[filename]) - specific_env_hooks.pop(i) - specific_env_hooks_workspace.pop(i) - # append env hook - specific_env_hooks.append(os.path.join(env_hook_dir, filename)) - specific_env_hooks_workspace.append(workspace) - specific_env_hooks_by_filename[filename] = specific_env_hooks[-1] - env_hooks = generic_env_hooks + specific_env_hooks - env_hooks_workspace = generic_env_hooks_workspace + specific_env_hooks_workspace - count = len(env_hooks) - lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_COUNT', count)) - for i in range(count): - lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d' % i, env_hooks[i])) - lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d_WORKSPACE' % i, env_hooks_workspace[i])) - return lines - - -def _parse_arguments(args=None): - parser = argparse.ArgumentParser(description='Generates code blocks for the setup.SHELL script.') - parser.add_argument('--extend', action='store_true', help='Skip unsetting previous environment variables to extend context') - return parser.parse_known_args(args=args)[0] - - -if __name__ == '__main__': - try: - try: - args = _parse_arguments() - except Exception as e: - print(e, file=sys.stderr) - sys.exit(1) - - # environment at generation time - CMAKE_PREFIX_PATH = '/home/roman/catkin_ws/devel;/opt/ros/indigo'.split(';') - # prepend current workspace if not already part of CPP - base_path = os.path.dirname(__file__) - if base_path not in CMAKE_PREFIX_PATH: - CMAKE_PREFIX_PATH.insert(0, base_path) - CMAKE_PREFIX_PATH = os.pathsep.join(CMAKE_PREFIX_PATH) - - environ = dict(os.environ) - lines = [] - if not args.extend: - lines += rollback_env_variables(environ, ENV_VAR_SUBFOLDERS) - lines += prepend_env_variables(environ, ENV_VAR_SUBFOLDERS, CMAKE_PREFIX_PATH) - lines += find_env_hooks(environ, CMAKE_PREFIX_PATH) - print('\n'.join(lines)) - - # need to explicitly flush the output - sys.stdout.flush() - except IOError as e: - # and catch potantial "broken pipe" if stdout is not writable - # which can happen when piping the output to a file but the disk is full - if e.errno == errno.EPIPE: - print(e, file=sys.stderr) - sys.exit(2) - raise - - sys.exit(0) diff --git a/devel/env.sh b/devel/env.sh deleted file mode 100755 index 8aa9d244a..000000000 --- a/devel/env.sh +++ /dev/null @@ -1,16 +0,0 @@ -#!/usr/bin/env sh -# generated from catkin/cmake/templates/env.sh.in - -if [ $# -eq 0 ] ; then - /bin/echo "Usage: env.sh COMMANDS" - /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually." - exit 1 -fi - -# ensure to not use different shell type which was set before -CATKIN_SHELL=sh - -# source setup.sh from same directory as this file -_CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd) -. "$_CATKIN_SETUP_DIR/setup.sh" -exec "$@" diff --git a/devel/setup.bash b/devel/setup.bash deleted file mode 100644 index ff47af8f3..000000000 --- a/devel/setup.bash +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env bash -# generated from catkin/cmake/templates/setup.bash.in - -CATKIN_SHELL=bash - -# source setup.sh from same directory as this file -_CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd) -. "$_CATKIN_SETUP_DIR/setup.sh" diff --git a/devel/setup.sh b/devel/setup.sh deleted file mode 100644 index 7d80d638a..000000000 --- a/devel/setup.sh +++ /dev/null @@ -1,87 +0,0 @@ -#!/usr/bin/env sh -# generated from catkin/cmake/template/setup.sh.in - -# Sets various environment variables and sources additional environment hooks. -# It tries it's best to undo changes from a previously sourced setup file before. -# Supported command line options: -# --extend: skips the undoing of changes from a previously sourced setup file - -# since this file is sourced either use the provided _CATKIN_SETUP_DIR -# or fall back to the destination set at configure time -: ${_CATKIN_SETUP_DIR:=/home/roman/src/Firmware/devel} -_SETUP_UTIL="$_CATKIN_SETUP_DIR/_setup_util.py" -unset _CATKIN_SETUP_DIR - -if [ ! -f "$_SETUP_UTIL" ]; then - echo "Missing Python script: $_SETUP_UTIL" - return 22 -fi - -# detect if running on Darwin platform -_UNAME=`uname -s` -_IS_DARWIN=0 -if [ "$_UNAME" = "Darwin" ]; then - _IS_DARWIN=1 -fi -unset _UNAME - -# make sure to export all environment variables -export CMAKE_PREFIX_PATH -export CPATH -if [ $_IS_DARWIN -eq 0 ]; then - export LD_LIBRARY_PATH -else - export DYLD_LIBRARY_PATH -fi -unset _IS_DARWIN -export PATH -export PKG_CONFIG_PATH -export PYTHONPATH - -# remember type of shell if not already set -if [ -z "$CATKIN_SHELL" ]; then - CATKIN_SHELL=sh -fi - -# invoke Python script to generate necessary exports of environment variables -_SETUP_TMP=`mktemp /tmp/setup.sh.XXXXXXXXXX` -if [ $? -ne 0 -o ! -f "$_SETUP_TMP" ]; then - echo "Could not create temporary file: $_SETUP_TMP" - return 1 -fi -CATKIN_SHELL=$CATKIN_SHELL "$_SETUP_UTIL" $@ > $_SETUP_TMP -_RC=$? -if [ $_RC -ne 0 ]; then - if [ $_RC -eq 2 ]; then - echo "Could not write the output of '$_SETUP_UTIL' to temporary file '$_SETUP_TMP': may be the disk if full?" - else - echo "Failed to run '\"$_SETUP_UTIL\" $@': return code $_RC" - fi - unset _RC - unset _SETUP_UTIL - rm -f $_SETUP_TMP - unset _SETUP_TMP - return 1 -fi -unset _RC -unset _SETUP_UTIL -. $_SETUP_TMP -rm -f $_SETUP_TMP -unset _SETUP_TMP - -# source all environment hooks -_i=0 -while [ $_i -lt $_CATKIN_ENVIRONMENT_HOOKS_COUNT ]; do - eval _envfile=\$_CATKIN_ENVIRONMENT_HOOKS_$_i - unset _CATKIN_ENVIRONMENT_HOOKS_$_i - eval _envfile_workspace=\$_CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE - unset _CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE - # set workspace for environment hook - CATKIN_ENV_HOOK_WORKSPACE=$_envfile_workspace - . "$_envfile" - unset CATKIN_ENV_HOOK_WORKSPACE - _i=$((_i + 1)) -done -unset _i - -unset _CATKIN_ENVIRONMENT_HOOKS_COUNT diff --git a/devel/setup.zsh b/devel/setup.zsh deleted file mode 100644 index b66071766..000000000 --- a/devel/setup.zsh +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env zsh -# generated from catkin/cmake/templates/setup.zsh.in - -CATKIN_SHELL=zsh -_CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd) -emulate sh # emulate POSIX -. "$_CATKIN_SETUP_DIR/setup.sh" -emulate zsh # back to zsh mode -- cgit v1.2.3 From 475f9a594b1088e8cb34b2d7c6f550de5a0193d7 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 23 Sep 2014 16:55:19 +0200 Subject: Adapted math library for use of PX4 and ROS as shared library. First version, it works but some things might still be ugly --- src/lib/mathlib/math/Limits.hpp | 5 ++++- src/lib/mathlib/math/Matrix.hpp | 20 ++++++++++++++++++++ src/lib/mathlib/math/Quaternion.hpp | 4 ++++ src/lib/mathlib/math/Vector.hpp | 11 +++++++++++ 4 files changed, 39 insertions(+), 1 deletion(-) diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp index fb778dd66..713cb51b5 100644 --- a/src/lib/mathlib/math/Limits.hpp +++ b/src/lib/mathlib/math/Limits.hpp @@ -39,7 +39,10 @@ #pragma once +#ifdef CONFIG_ARCH_ARM #include +#endif + #include namespace math { @@ -84,4 +87,4 @@ float __EXPORT degrees(float radians); double __EXPORT degrees(double radians); -} \ No newline at end of file +} diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index ca931e2da..8fb3caa0d 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -44,7 +44,15 @@ #define MATRIX_HPP #include +#include + +#ifdef CONFIG_ARCH_ARM #include "../CMSIS/Include/arm_math.h" +#else +#include +#include +#define M_PI_2_F 1.570769 +#endif namespace math { @@ -65,7 +73,11 @@ public: /** * struct for using arm_math functions */ + #ifdef CONFIG_ARCH_ARM arm_matrix_instance_f32 arm_mat; + #else + eigen_matrix_instance arm_mat; + #endif /** * trivial ctor @@ -352,8 +364,16 @@ public: * multiplication by a vector */ Vector operator *(const Vector &v) const { + #ifdef CONFIG_ARCH_ARM Vector res; arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); + #else + //probably nicer if this could go into a function like "eigen_mat_mult" or so + Eigen::Matrix Me = Eigen::Map >(this->arm_mat.pData); + Eigen::VectorXf Vec = Eigen::Map(v.arm_col.pData,N); + Eigen::VectorXf Product = Me * Vec; + Vector res(Product.data()); + #endif return res; } }; diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 21d05c7ef..d8acc4443 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -44,7 +44,11 @@ #define QUATERNION_HPP #include + +#ifdef CONFIG_ARCH_ARM #include "../CMSIS/Include/arm_math.h" +#endif + #include "Vector.hpp" #include "Matrix.hpp" diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 0ddf77615..51a84d27e 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -45,7 +45,13 @@ #include #include + +#ifdef CONFIG_ARCH_ARM #include "../CMSIS/Include/arm_math.h" +#else +#include +#include +#endif namespace math { @@ -65,7 +71,12 @@ public: /** * struct for using arm_math functions, represents column vector */ + #ifdef CONFIG_ARCH_ARM arm_matrix_instance_f32 arm_col; + #else + eigen_matrix_instance arm_col; + #endif + /** * trivial ctor -- cgit v1.2.3 From 0553771f4fd6fbdba43669a8f17185ed61f96a51 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 24 Sep 2014 08:52:28 +0200 Subject: Adapted for sharded library use with ROS. Problems to solve: error library from PX4 does not work yet. math functions such as isfinite need to be shared as well. performance library needs to be shared as well (commented for now) --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 926a8db2a..0019ef94d 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -45,7 +45,14 @@ #include #include #include + +#ifdef CONFIG_ARCH_ARM #include +#else +#include +#include +#define isfinite std::isfinite +#endif ECL_PitchController::ECL_PitchController() : _last_run(0), @@ -61,8 +68,8 @@ ECL_PitchController::ECL_PitchController() : _integrator(0.0f), _rate_error(0.0f), _rate_setpoint(0.0f), - _bodyrate_setpoint(0.0f), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input")) + _bodyrate_setpoint(0.0f) + //_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input")) { } @@ -75,7 +82,7 @@ float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, fl { /* Do not calculate control signal with bad inputs */ if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) { - perf_count(_nonfinite_input_perf); + //perf_count(_nonfinite_input_perf); warnx("not controlling pitch"); return _rate_setpoint; } @@ -138,7 +145,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) && isfinite(airspeed_max) && isfinite(scaler))) { - perf_count(_nonfinite_input_perf); + //perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } -- cgit v1.2.3 From cfe14d78c5a9d2f80ebc0282e4bc400dcba6a795 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 24 Sep 2014 08:53:23 +0200 Subject: Adapted for sharded library use with ROS. Problems to solve: error library from PX4 does not work yet. math functions such as isfinite need to be shared as well. performance library needs to be shared as well (commented for now) --- src/lib/ecl/attitude_fw/ecl_pitch_controller.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 39b9f9d03..973e15d98 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -51,8 +51,9 @@ #include #include +#ifdef CONFIG_ARM_ARCH #include - +#endif class __EXPORT ECL_PitchController //XXX: create controller superclass { public: @@ -129,7 +130,7 @@ private: float _rate_error; float _rate_setpoint; float _bodyrate_setpoint; - perf_counter_t _nonfinite_input_perf; + //perf_counter_t _nonfinite_input_perf; }; #endif // ECL_PITCH_CONTROLLER_H -- cgit v1.2.3 From 9f9fab400efbf8ad9cd44a0ff82cb9a97706134b Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 24 Sep 2014 08:57:20 +0200 Subject: Adapted for shared library use with ROS --- src/lib/ecl/ecl.h | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h index aa3c5000a..d6098dbfe 100644 --- a/src/lib/ecl/ecl.h +++ b/src/lib/ecl/ecl.h @@ -36,8 +36,11 @@ * Adapter / shim layer for system calls needed by ECL * */ - +#ifdef CONFIG_ARCH_ARM #include - #define ecl_absolute_time hrt_absolute_time #define ecl_elapsed_time hrt_elapsed_time + +#else +#include +#endif -- cgit v1.2.3 From 5aefe11975c0fa04ec0a921ffb54d7cf8da5c39a Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 24 Sep 2014 09:01:30 +0200 Subject: Had to add definition of PI is used with ROS because cannot share math library yet-needs to be solved --- src/lib/mathlib/math/Limits.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp index d4c892d8a..c593936ce 100644 --- a/src/lib/mathlib/math/Limits.cpp +++ b/src/lib/mathlib/math/Limits.cpp @@ -45,7 +45,7 @@ namespace math { - +#define M_PI_F 3.14159265358979323846 float __EXPORT min(float val1, float val2) { @@ -143,4 +143,4 @@ double __EXPORT degrees(double radians) return (radians / M_PI) * 180.0; } -} \ No newline at end of file +} -- cgit v1.2.3 From 77c823d3cd0f49014a33632ec9ef3efdd7d3dfa5 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 24 Sep 2014 09:11:44 +0200 Subject: Adapted for sharded library use with ROS. Problems to solve: error library from PX4 does not work yet. math functions such as isfinite need to be shared as well. performance library needs to be shared as well (commented for now) --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 2 +- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 15 +++++++++++---- src/lib/ecl/attitude_fw/ecl_roll_controller.h | 5 ++++- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 15 +++++++++++---- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 5 ++++- 5 files changed, 31 insertions(+), 11 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 0019ef94d..b840206d5 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -75,7 +75,7 @@ ECL_PitchController::ECL_PitchController() : ECL_PitchController::~ECL_PitchController() { - perf_free(_nonfinite_input_perf); + //perf_free(_nonfinite_input_perf); } float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed) diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 94bd26f03..1b9925f63 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -45,7 +45,14 @@ #include #include #include + +#ifdef CONFIG_ARCH_ARM #include +#else +#include +#include +#define isfinite std::isfinite +#endif ECL_RollController::ECL_RollController() : _last_run(0), @@ -60,20 +67,20 @@ ECL_RollController::ECL_RollController() : _rate_error(0.0f), _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input")) + //_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input")) { } ECL_RollController::~ECL_RollController() { - perf_free(_nonfinite_input_perf); + //perf_free(_nonfinite_input_perf); } float ECL_RollController::control_attitude(float roll_setpoint, float roll) { /* Do not calculate control signal with bad inputs */ if (!(isfinite(roll_setpoint) && isfinite(roll))) { - perf_count(_nonfinite_input_perf); + //perf_count(_nonfinite_input_perf); return _rate_setpoint; } @@ -101,7 +108,7 @@ float ECL_RollController::control_bodyrate(float pitch, if (!(isfinite(pitch) && isfinite(roll_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) && isfinite(airspeed_max) && isfinite(scaler))) { - perf_count(_nonfinite_input_perf); + //perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index 0799dbe03..84e6e9fe4 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -51,7 +51,10 @@ #include #include + +#ifdef CONIG_ARCH_ARM #include +#endif class __EXPORT ECL_RollController //XXX: create controller superclass { @@ -120,7 +123,7 @@ private: float _rate_error; float _rate_setpoint; float _bodyrate_setpoint; - perf_counter_t _nonfinite_input_perf; + //perf_counter_t _nonfinite_input_perf; }; #endif // ECL_ROLL_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index fe03b8065..5b023fa8f 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -44,7 +44,14 @@ #include #include #include + +#ifdef CONFIG_ARCH_ARM #include +#else +#include +#include +#define isfinite std::isfinite +#endif ECL_YawController::ECL_YawController() : _last_run(0), @@ -59,13 +66,13 @@ ECL_YawController::ECL_YawController() : _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), _coordinated_min_speed(1.0f), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input")) + //_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input")) { } ECL_YawController::~ECL_YawController() { - perf_free(_nonfinite_input_perf); + //perf_free(_nonfinite_input_perf); } float ECL_YawController::control_attitude(float roll, float pitch, @@ -76,7 +83,7 @@ float ECL_YawController::control_attitude(float roll, float pitch, if (!(isfinite(roll) && isfinite(pitch) && isfinite(speed_body_u) && isfinite(speed_body_v) && isfinite(speed_body_w) && isfinite(roll_rate_setpoint) && isfinite(pitch_rate_setpoint))) { - perf_count(_nonfinite_input_perf); + //perf_count(_nonfinite_input_perf); return _rate_setpoint; } // static int counter = 0; @@ -120,7 +127,7 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) && isfinite(pitch_rate_setpoint) && isfinite(airspeed_min) && isfinite(airspeed_max) && isfinite(scaler))) { - perf_count(_nonfinite_input_perf); + //perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } /* get the usual dt estimate */ diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index a360c14b8..61657e95b 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -50,7 +50,10 @@ #include #include + +#ifdef CONFIG_ARCH_ARM #include +#endif class __EXPORT ECL_YawController //XXX: create controller superclass { @@ -121,7 +124,7 @@ private: float _rate_setpoint; float _bodyrate_setpoint; float _coordinated_min_speed; - perf_counter_t _nonfinite_input_perf; + //perf_counter_t _nonfinite_input_perf; }; -- cgit v1.2.3 From 2b8a9b632555708731d93f4aa7945d19e83d3134 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 24 Sep 2014 10:29:28 +0200 Subject: Restored performance counter functionality, ROS package used own source file for function definitions but per_counter.h stays the same --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 6 +++--- src/lib/ecl/attitude_fw/ecl_pitch_controller.h | 5 ++--- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 8 ++++---- src/lib/ecl/attitude_fw/ecl_roll_controller.h | 6 ++---- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 8 ++++---- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 6 ++---- 6 files changed, 17 insertions(+), 22 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index b840206d5..8ee8b9c68 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -69,13 +69,13 @@ ECL_PitchController::ECL_PitchController() : _rate_error(0.0f), _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f) - //_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input")) + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input")) { } ECL_PitchController::~ECL_PitchController() { - //perf_free(_nonfinite_input_perf); + perf_free(_nonfinite_input_perf); } float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed) @@ -145,7 +145,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) && isfinite(airspeed_max) && isfinite(scaler))) { - //perf_count(_nonfinite_input_perf); + perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 973e15d98..39b9f9d03 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -51,9 +51,8 @@ #include #include -#ifdef CONFIG_ARM_ARCH #include -#endif + class __EXPORT ECL_PitchController //XXX: create controller superclass { public: @@ -130,7 +129,7 @@ private: float _rate_error; float _rate_setpoint; float _bodyrate_setpoint; - //perf_counter_t _nonfinite_input_perf; + perf_counter_t _nonfinite_input_perf; }; #endif // ECL_PITCH_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 1b9925f63..6707a11ba 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -67,20 +67,20 @@ ECL_RollController::ECL_RollController() : _rate_error(0.0f), _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), - //_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input")) + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input")), { } ECL_RollController::~ECL_RollController() { - //perf_free(_nonfinite_input_perf); + perf_free(_nonfinite_input_perf); } float ECL_RollController::control_attitude(float roll_setpoint, float roll) { /* Do not calculate control signal with bad inputs */ if (!(isfinite(roll_setpoint) && isfinite(roll))) { - //perf_count(_nonfinite_input_perf); + perf_count(_nonfinite_input_perf); return _rate_setpoint; } @@ -108,7 +108,7 @@ float ECL_RollController::control_bodyrate(float pitch, if (!(isfinite(pitch) && isfinite(roll_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) && isfinite(airspeed_max) && isfinite(scaler))) { - //perf_count(_nonfinite_input_perf); + perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index 84e6e9fe4..dbcabd847 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -51,10 +51,8 @@ #include #include - -#ifdef CONIG_ARCH_ARM #include -#endif + class __EXPORT ECL_RollController //XXX: create controller superclass { @@ -123,7 +121,7 @@ private: float _rate_error; float _rate_setpoint; float _bodyrate_setpoint; - //perf_counter_t _nonfinite_input_perf; + perf_counter_t _nonfinite_input_perf; }; #endif // ECL_ROLL_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 5b023fa8f..7ff864175 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -66,13 +66,13 @@ ECL_YawController::ECL_YawController() : _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), _coordinated_min_speed(1.0f), - //_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input")) + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input")) { } ECL_YawController::~ECL_YawController() { - //perf_free(_nonfinite_input_perf); + perf_free(_nonfinite_input_perf); } float ECL_YawController::control_attitude(float roll, float pitch, @@ -83,7 +83,7 @@ float ECL_YawController::control_attitude(float roll, float pitch, if (!(isfinite(roll) && isfinite(pitch) && isfinite(speed_body_u) && isfinite(speed_body_v) && isfinite(speed_body_w) && isfinite(roll_rate_setpoint) && isfinite(pitch_rate_setpoint))) { - //perf_count(_nonfinite_input_perf); + perf_count(_nonfinite_input_perf); return _rate_setpoint; } // static int counter = 0; @@ -127,7 +127,7 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) && isfinite(pitch_rate_setpoint) && isfinite(airspeed_min) && isfinite(airspeed_max) && isfinite(scaler))) { - //perf_count(_nonfinite_input_perf); + perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } /* get the usual dt estimate */ diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 61657e95b..c9e80930f 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -50,10 +50,8 @@ #include #include - -#ifdef CONFIG_ARCH_ARM #include -#endif + class __EXPORT ECL_YawController //XXX: create controller superclass { @@ -124,7 +122,7 @@ private: float _rate_setpoint; float _bodyrate_setpoint; float _coordinated_min_speed; - //perf_counter_t _nonfinite_input_perf; + perf_counter_t _nonfinite_input_perf; }; -- cgit v1.2.3 From d7cf6c4319cd9522286f0a7abb73e24a3050a6f7 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 24 Sep 2014 13:45:38 +0200 Subject: Use namespace std so that we can use the isfinite function, which should be available from math.h but the compiler gives an error that the function is undeclared --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 5 +++-- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 4 ++-- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 2 +- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 8ee8b9c68..fa0f98e16 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -51,7 +51,8 @@ #else #include #include -#define isfinite std::isfinite +//#define isfinite std::isfinite +using namespace std; #endif ECL_PitchController::ECL_PitchController() : @@ -68,7 +69,7 @@ ECL_PitchController::ECL_PitchController() : _integrator(0.0f), _rate_error(0.0f), _rate_setpoint(0.0f), - _bodyrate_setpoint(0.0f) + _bodyrate_setpoint(0.0f), _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input")) { } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 6707a11ba..640940c99 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -51,7 +51,7 @@ #else #include #include -#define isfinite std::isfinite +using namespace std; #endif ECL_RollController::ECL_RollController() : @@ -67,7 +67,7 @@ ECL_RollController::ECL_RollController() : _rate_error(0.0f), _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input")), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input")) { } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 7ff864175..5f01cecf9 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -50,7 +50,7 @@ #else #include #include -#define isfinite std::isfinite +using namespace std; #endif ECL_YawController::ECL_YawController() : -- cgit v1.2.3 From f347e87391d35d43bdf4bdf51323ea0c53e9ead7 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 24 Sep 2014 15:54:34 +0200 Subject: Added base class for fixed wing attitude controller -> still working on it --- src/modules/fw_att_control/fw_att_control_base.cpp | 50 +++++++++ src/modules/fw_att_control/fw_att_control_base.h | 124 +++++++++++++++++++++ 2 files changed, 174 insertions(+) create mode 100644 src/modules/fw_att_control/fw_att_control_base.cpp create mode 100644 src/modules/fw_att_control/fw_att_control_base.h diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp new file mode 100644 index 000000000..4f61f02ef --- /dev/null +++ b/src/modules/fw_att_control/fw_att_control_base.cpp @@ -0,0 +1,50 @@ +/* + * fw_att_control_base.cpp + * + * Created on: Sep 24, 2014 + * Author: roman + */ + +#include "fw_att_control_base.h" + + +FixedwingAttitudeControlBase::FixedwingAttitudeControlBase() : + + _task_should_exit(false), + _task_running(false), + _control_task(-1), + + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")), + _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), +/* states */ + _setpoint_valid(false), + _debug(false) +{ + /* safely initialize structs */ + _att = {}; + _accel = {}; + _att_sp = {}; + _manual = {}; + _airspeed = {}; + _vcontrol_mode = {}; + _actuators = {}; + _actuators_airframe = {}; + _global_pos = {}; + _vehicle_status = {}; + +} + + + +FixedwingAttitudeControlBase::~FixedwingAttitudeControlBase() +{ + +} + +void FixedwingAttitudeControlBase::control_attitude() +{ + +} diff --git a/src/modules/fw_att_control/fw_att_control_base.h b/src/modules/fw_att_control/fw_att_control_base.h new file mode 100644 index 000000000..9a2d9b195 --- /dev/null +++ b/src/modules/fw_att_control/fw_att_control_base.h @@ -0,0 +1,124 @@ +/* + * fw_att_control_base.h + * + * Created on: Sep 24, 2014 + * Author: roman + */ + +#ifndef FW_ATT_CONTROL_BASE_H_ +#define FW_ATT_CONTROL_BASE_H_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +class FixedwingAttitudeControlBase +{ +public: + /** + * Constructor + */ + FixedwingAttitudeControlBase(); + + /** + * Destructor, also kills the sensors task. + */ + ~FixedwingAttitudeControlBase(); + + +protected: + + bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_running; /**< if true, task is running in its mainloop */ + int _control_task; /**< task handle for sensor task */ + + + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct accel_report _accel; /**< body frame accelerations */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ + struct vehicle_global_position_s _global_pos; /**< global position */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ + perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ + + bool _setpoint_valid; /**< flag if the position control setpoint is valid */ + bool _debug; /**< if set to true, print debug output */ + + struct { + float tconst; + float p_p; + float p_d; + float p_i; + float p_ff; + float p_rmax_pos; + float p_rmax_neg; + float p_integrator_max; + float p_roll_feedforward; + float r_p; + float r_d; + float r_i; + float r_ff; + float r_integrator_max; + float r_rmax; + float y_p; + float y_i; + float y_d; + float y_ff; + float y_roll_feedforward; + float y_integrator_max; + float y_coordinated_min_speed; + float y_rmax; + + float airspeed_min; + float airspeed_trim; + float airspeed_max; + + float trim_roll; + float trim_pitch; + float trim_yaw; + float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ + float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ + float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ + float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ + float man_roll_max; /**< Max Roll in rad */ + float man_pitch_max; /**< Max Pitch in rad */ + + } _parameters; /**< local copies of interesting parameters */ + + + + + ECL_RollController _roll_ctrl; + ECL_PitchController _pitch_ctrl; + ECL_YawController _yaw_ctrl; + + + + +}; + + + +#endif /* FW_ATT_CONTROL_BASE_H_ */ -- cgit v1.2.3 From 81a5aeb6f5cb022e6987a1651658d9b62359cccb Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 25 Sep 2014 09:11:54 +0200 Subject: Restored to original PX4 version since the hrt_time functions are now also implemented in ROS --- src/lib/ecl/ecl.h | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h index d6098dbfe..662e3a39f 100644 --- a/src/lib/ecl/ecl.h +++ b/src/lib/ecl/ecl.h @@ -36,11 +36,8 @@ * Adapter / shim layer for system calls needed by ECL * */ -#ifdef CONFIG_ARCH_ARM + #include #define ecl_absolute_time hrt_absolute_time #define ecl_elapsed_time hrt_elapsed_time -#else -#include -#endif -- cgit v1.2.3 From 96b22f1ba8980ea8e1cbc405a808ae920102f064 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 25 Sep 2014 10:20:15 +0200 Subject: Adapted uORB topic files to work with ROS (data stuctures are used but not the uORB functionality) --- src/modules/uORB/topics/actuator_controls.h | 4 ++++ src/modules/uORB/topics/airspeed.h | 4 ++++ src/modules/uORB/topics/manual_control_setpoint.h | 6 ++++-- src/modules/uORB/topics/parameter_update.h | 6 +++++- src/modules/uORB/topics/vehicle_attitude.h | 4 ++++ src/modules/uORB/topics/vehicle_attitude_setpoint.h | 4 ++++ src/modules/uORB/topics/vehicle_control_mode.h | 4 ++++ src/modules/uORB/topics/vehicle_global_position.h | 5 ++++- src/modules/uORB/topics/vehicle_rates_setpoint.h | 4 ++++ src/modules/uORB/topics/vehicle_status.h | 4 ++++ 10 files changed, 41 insertions(+), 4 deletions(-) diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index e768ab2f6..6c641dbce 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -47,7 +47,9 @@ #define TOPIC_ACTUATOR_CONTROLS_H #include +#ifdef CONFIG_ARCH_ARM #include "../uORB.h" +#endif #define NUM_ACTUATOR_CONTROLS 8 #define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ @@ -70,9 +72,11 @@ struct actuator_controls_s { */ /* actuator control sets; this list can be expanded as more controllers emerge */ +#ifdef CONFIG_ARCH_ARM ORB_DECLARE(actuator_controls_0); ORB_DECLARE(actuator_controls_1); ORB_DECLARE(actuator_controls_2); ORB_DECLARE(actuator_controls_3); +#endif #endif diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h index d2ee754cd..4c115a811 100644 --- a/src/modules/uORB/topics/airspeed.h +++ b/src/modules/uORB/topics/airspeed.h @@ -40,7 +40,9 @@ #ifndef TOPIC_AIRSPEED_H_ #define TOPIC_AIRSPEED_H_ +#ifdef CONFIG_ARCH_ARM #include "../uORB.h" +#endif #include /** @@ -63,6 +65,8 @@ struct airspeed_s { */ /* register this as object request broker structure */ +#ifdef CONFIG_ARCH_ARM ORB_DECLARE(airspeed); +#endif #endif diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index dde237adc..af5df6979 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -41,8 +41,9 @@ #define TOPIC_MANUAL_CONTROL_SETPOINT_H_ #include +#ifdef CONFIG_ARCH_ARM #include "../uORB.h" - +#endif /** * Switch position */ @@ -106,6 +107,7 @@ struct manual_control_setpoint_s { */ /* register this as object request broker structure */ +#ifdef CONFIG_ARCH_ARM ORB_DECLARE(manual_control_setpoint); - +#endif #endif diff --git a/src/modules/uORB/topics/parameter_update.h b/src/modules/uORB/topics/parameter_update.h index 68964deb0..7afb78d49 100644 --- a/src/modules/uORB/topics/parameter_update.h +++ b/src/modules/uORB/topics/parameter_update.h @@ -40,7 +40,9 @@ #define TOPIC_PARAMETER_UPDATE_H #include +#ifdef CONFIG_ARCH_ARM #include "../uORB.h" +#endif /** * @addtogroup topics @@ -56,6 +58,8 @@ struct parameter_update_s { * @} */ +#ifdef CONFIG_ARCH_ARM ORB_DECLARE(parameter_update); +#endif -#endif \ No newline at end of file +#endif diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h index 40328af14..7780988c8 100755 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ b/src/modules/uORB/topics/vehicle_attitude.h @@ -44,7 +44,9 @@ #include #include +#ifdef CONFIG_ARCH_ARM #include "../uORB.h" +#endif /** * @addtogroup topics @@ -87,6 +89,8 @@ struct vehicle_attitude_s { */ /* register this as object request broker structure */ +#ifdef CONFIG_ARCH_ARM ORB_DECLARE(vehicle_attitude); +#endif #endif diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index 8446e9c6e..8b5a76143 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -42,7 +42,9 @@ #include #include +#ifdef CONFIG_ARCH_ARM #include "../uORB.h" +#endif /** * @addtogroup topics @@ -82,6 +84,8 @@ struct vehicle_attitude_setpoint_s { */ /* register this as object request broker structure */ +#ifdef CONFIG_ARCH_ARM ORB_DECLARE(vehicle_attitude_setpoint); +#endif #endif /* TOPIC_ARDRONE_CONTROL_H_ */ diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 49e2ba4b5..b071e0fa3 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -48,7 +48,9 @@ #include #include +#ifdef CONFIG_ARCH_ARM #include "../uORB.h" +#endif #include "vehicle_status.h" /** @@ -89,6 +91,8 @@ struct vehicle_control_mode_s { */ /* register this as object request broker structure */ +#ifdef CONFIG_ARCH_ARM ORB_DECLARE(vehicle_control_mode); +#endif #endif diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index c3bb3b893..e8f010924 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -45,7 +45,9 @@ #include #include +#ifdef CONFIG_ARCH_ARM #include "../uORB.h" +#endif /** * @addtogroup topics @@ -81,6 +83,7 @@ struct vehicle_global_position_s { */ /* register this as object request broker structure */ +#ifdef CONFIG_ARCH_ARM ORB_DECLARE(vehicle_global_position); - +#endif #endif diff --git a/src/modules/uORB/topics/vehicle_rates_setpoint.h b/src/modules/uORB/topics/vehicle_rates_setpoint.h index 9f8b412a7..cbfab89d6 100644 --- a/src/modules/uORB/topics/vehicle_rates_setpoint.h +++ b/src/modules/uORB/topics/vehicle_rates_setpoint.h @@ -41,7 +41,9 @@ #define TOPIC_VEHICLE_RATES_SETPOINT_H_ #include +#ifdef CONFIG_ARCH_ARM #include "../uORB.h" +#endif /** * @addtogroup topics @@ -62,6 +64,8 @@ struct vehicle_rates_setpoint_s { */ /* register this as object request broker structure */ +#ifdef CONFIG_ARCH_ARM ORB_DECLARE(vehicle_rates_setpoint); +#endif #endif diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index b683bf98a..973987f5c 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -53,7 +53,9 @@ #include #include +#ifdef CONFIG_ARCH_ARM #include "../uORB.h" +#endif /** * @addtogroup topics @{ @@ -234,6 +236,8 @@ struct vehicle_status_s { */ /* register this as object request broker structure */ +#ifdef CONFIG_ARCH_ARM ORB_DECLARE(vehicle_status); +#endif #endif -- cgit v1.2.3 From af290826d9957a75ff62dbd1451988228c603a45 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 25 Sep 2014 10:24:04 +0200 Subject: Fixed so that when working with ROS the same err.h is included --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 4 +--- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 3 +-- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 3 +-- 3 files changed, 3 insertions(+), 7 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index fa0f98e16..d1f79f0ea 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -45,13 +45,11 @@ #include #include #include +#include #ifdef CONFIG_ARCH_ARM -#include #else -#include #include -//#define isfinite std::isfinite using namespace std; #endif diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 640940c99..30176f92f 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -45,11 +45,10 @@ #include #include #include +#include #ifdef CONFIG_ARCH_ARM -#include #else -#include #include using namespace std; #endif diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 5f01cecf9..1b4d8486c 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -44,11 +44,10 @@ #include #include #include +#include #ifdef CONFIG_ARCH_ARM -#include #else -#include #include using namespace std; #endif -- cgit v1.2.3 From a69ae3493d99250bd5a46ba1ac5f11fcf2d05069 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 25 Sep 2014 10:24:39 +0200 Subject: Adapted so that can be used with ROS --- src/drivers/drv_accel.h | 64 ++++++++++++++++++++++++++----------------------- 1 file changed, 34 insertions(+), 30 deletions(-) diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 1f98d966b..49b104d38 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -42,12 +42,13 @@ #include #include - +#ifdef CONFIG_ARCH_ARM #include "drv_sensor.h" #include "drv_orb_dev.h" #define ACCEL_DEVICE_PATH "/dev/accel" +#endif /** * accel report structure. Reads from the device must be in multiples of this * structure. @@ -81,46 +82,49 @@ struct accel_scale { /* * ObjDev tag for raw accelerometer data. */ -ORB_DECLARE(sensor_accel0); -ORB_DECLARE(sensor_accel1); -ORB_DECLARE(sensor_accel2); +#ifdef CONFIG_ARCH_ARM + ORB_DECLARE(sensor_accel0); + ORB_DECLARE(sensor_accel1); + ORB_DECLARE(sensor_accel2); -/* - * ioctl() definitions - * - * Accelerometer drivers also implement the generic sensor driver - * interfaces from drv_sensor.h - */ + /* + * ioctl() definitions + * + * Accelerometer drivers also implement the generic sensor driver + * interfaces from drv_sensor.h + */ + + #define _ACCELIOCBASE (0x2100) + #define _ACCELIOC(_n) (_IOC(_ACCELIOCBASE, _n)) -#define _ACCELIOCBASE (0x2100) -#define _ACCELIOC(_n) (_IOC(_ACCELIOCBASE, _n)) + /** set the accel internal sample rate to at least (arg) Hz */ + #define ACCELIOCSSAMPLERATE _ACCELIOC(0) -/** set the accel internal sample rate to at least (arg) Hz */ -#define ACCELIOCSSAMPLERATE _ACCELIOC(0) + /** return the accel internal sample rate in Hz */ + #define ACCELIOCGSAMPLERATE _ACCELIOC(1) -/** return the accel internal sample rate in Hz */ -#define ACCELIOCGSAMPLERATE _ACCELIOC(1) + /** set the accel internal lowpass filter to no lower than (arg) Hz */ + #define ACCELIOCSLOWPASS _ACCELIOC(2) -/** set the accel internal lowpass filter to no lower than (arg) Hz */ -#define ACCELIOCSLOWPASS _ACCELIOC(2) + /** return the accel internal lowpass filter in Hz */ + #define ACCELIOCGLOWPASS _ACCELIOC(3) -/** return the accel internal lowpass filter in Hz */ -#define ACCELIOCGLOWPASS _ACCELIOC(3) + /** set the accel scaling constants to the structure pointed to by (arg) */ + #define ACCELIOCSSCALE _ACCELIOC(5) -/** set the accel scaling constants to the structure pointed to by (arg) */ -#define ACCELIOCSSCALE _ACCELIOC(5) + /** get the accel scaling constants into the structure pointed to by (arg) */ + #define ACCELIOCGSCALE _ACCELIOC(6) -/** get the accel scaling constants into the structure pointed to by (arg) */ -#define ACCELIOCGSCALE _ACCELIOC(6) + /** set the accel measurement range to handle at least (arg) g */ + #define ACCELIOCSRANGE _ACCELIOC(7) -/** set the accel measurement range to handle at least (arg) g */ -#define ACCELIOCSRANGE _ACCELIOC(7) + /** get the current accel measurement range in g */ + #define ACCELIOCGRANGE _ACCELIOC(8) -/** get the current accel measurement range in g */ -#define ACCELIOCGRANGE _ACCELIOC(8) + /** get the result of a sensor self-test */ + #define ACCELIOCSELFTEST _ACCELIOC(9) -/** get the result of a sensor self-test */ -#define ACCELIOCSELFTEST _ACCELIOC(9) +#endif #endif /* _DRV_ACCEL_H */ -- cgit v1.2.3 From 6329ca1a70b220b99ea793f416aedc8ab6fbccff Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 25 Sep 2014 10:25:57 +0200 Subject: Adapted so that this header can also be used in a ROS environment --- src/modules/systemlib/err.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/systemlib/err.h b/src/modules/systemlib/err.h index ca13d6265..2a201ee80 100644 --- a/src/modules/systemlib/err.h +++ b/src/modules/systemlib/err.h @@ -67,6 +67,7 @@ #include +#ifdef CONFIG_ARCH_ARM __BEGIN_DECLS __EXPORT const char *getprogname(void); @@ -86,4 +87,8 @@ __EXPORT void vwarnx(const char *fmt, va_list) __attribute__((format(printf, 1, __END_DECLS +#else //we are using ROS (should make a variable!!!) +#include +#define warnx ROS_WARN +#endif #endif -- cgit v1.2.3 From 43d9ebc231501fe2d3fe6541f1079d2d019a2698 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 25 Sep 2014 10:28:49 +0200 Subject: Added control_attitude function and cleaned up --- src/modules/fw_att_control/fw_att_control_base.cpp | 223 +++++++++++++++++++-- 1 file changed, 206 insertions(+), 17 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp index 4f61f02ef..7218e4e9b 100644 --- a/src/modules/fw_att_control/fw_att_control_base.cpp +++ b/src/modules/fw_att_control/fw_att_control_base.cpp @@ -6,23 +6,23 @@ */ #include "fw_att_control_base.h" +#include +#include +#include +#include +using namespace std; FixedwingAttitudeControlBase::FixedwingAttitudeControlBase() : - _task_should_exit(false), - _task_running(false), - _control_task(-1), + _task_should_exit(false), _task_running(false), _control_task(-1), - -/* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")), - _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), -/* states */ - _setpoint_valid(false), - _debug(false) -{ + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), _nonfinite_input_perf( + perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf( + perf_alloc(PC_COUNT, "fw att control nonfinite output")), + /* states */ + _setpoint_valid(false), _debug(false) { /* safely initialize structs */ _att = {}; _accel = {}; @@ -37,14 +37,203 @@ FixedwingAttitudeControlBase::FixedwingAttitudeControlBase() : } +FixedwingAttitudeControlBase::~FixedwingAttitudeControlBase() { +} -FixedwingAttitudeControlBase::~FixedwingAttitudeControlBase() -{ +void FixedwingAttitudeControlBase::control_attitude() { + bool lock_integrator = false; + static int loop_counter = 0; + /* scale around tuning airspeed */ -} + float airspeed; + + /* if airspeed is not updating, we assume the normal average speed */ + if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) + || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { + airspeed = _parameters.airspeed_trim; + if (nonfinite) { + perf_count(_nonfinite_input_perf); + } + } else { + /* prevent numerical drama by requiring 0.5 m/s minimal speed */ + airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s); + } + + /* + * For scaling our actuators using anything less than the min (close to stall) + * speed doesn't make any sense - its the strongest reasonable deflection we + * want to do in flight and its the baseline a human pilot would choose. + * + * Forcing the scaling to this value allows reasonable handheld tests. + */ + + float airspeed_scaling = _parameters.airspeed_trim + / ((airspeed < _parameters.airspeed_min) ? + _parameters.airspeed_min : airspeed); + + float roll_sp = _parameters.rollsp_offset_rad; + float pitch_sp = _parameters.pitchsp_offset_rad; + float throttle_sp = 0.0f; + + if (_vcontrol_mode.flag_control_velocity_enabled + || _vcontrol_mode.flag_control_position_enabled) { + /* read in attitude setpoint from attitude setpoint uorb topic */ + roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; + pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; + throttle_sp = _att_sp.thrust; + + /* reset integrals where needed */ + if (_att_sp.roll_reset_integral) { + _roll_ctrl.reset_integrator(); + } + if (_att_sp.pitch_reset_integral) { + _pitch_ctrl.reset_integrator(); + } + if (_att_sp.yaw_reset_integral) { + _yaw_ctrl.reset_integrator(); + } + } else { + /* + * Scale down roll and pitch as the setpoints are radians + * and a typical remote can only do around 45 degrees, the mapping is + * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch) + * + * With this mapping the stick angle is a 1:1 representation of + * the commanded attitude. + * + * The trim gets subtracted here from the manual setpoint to get + * the intended attitude setpoint. Later, after the rate control step the + * trim is added again to the control signal. + */ + roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) + + _parameters.rollsp_offset_rad; + pitch_sp = -(_manual.x * _parameters.man_pitch_max + - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; + throttle_sp = _manual.z; + _actuators.control[4] = _manual.flaps; + + /* + * in manual mode no external source should / does emit attitude setpoints. + * emit the manual setpoint here to allow attitude controller tuning + * in attitude control mode. + */ + struct vehicle_attitude_setpoint_s att_sp; + att_sp.timestamp = hrt_absolute_time(); + att_sp.roll_body = roll_sp; + att_sp.pitch_body = pitch_sp; + att_sp.yaw_body = 0.0f - _parameters.trim_yaw; + att_sp.thrust = throttle_sp; + + } + + /* If the aircraft is on ground reset the integrators */ + if (_vehicle_status.condition_landed) { + _roll_ctrl.reset_integrator(); + _pitch_ctrl.reset_integrator(); + _yaw_ctrl.reset_integrator(); + } + + /* Prepare speed_body_u and speed_body_w */ + float speed_body_u = 0.0f; + float speed_body_v = 0.0f; + float speed_body_w = 0.0f; + if (_att.R_valid) { + speed_body_u = _att.R[0][0] * _global_pos.vel_n + + _att.R[1][0] * _global_pos.vel_e + + _att.R[2][0] * _global_pos.vel_d; + speed_body_v = _att.R[0][1] * _global_pos.vel_n + + _att.R[1][1] * _global_pos.vel_e + + _att.R[2][1] * _global_pos.vel_d; + speed_body_w = _att.R[0][2] * _global_pos.vel_n + + _att.R[1][2] * _global_pos.vel_e + + _att.R[2][2] * _global_pos.vel_d; + } else { + if (_debug && loop_counter % 10 == 0) { + warnx("Did not get a valid R\n"); + } + } + + /* Run attitude controllers */ + if (isfinite(roll_sp) && isfinite(pitch_sp)) { + _roll_ctrl.control_attitude(roll_sp, _att.roll); + _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed); + _yaw_ctrl.control_attitude(_att.roll, _att.pitch, speed_body_u, + speed_body_v, speed_body_w, _roll_ctrl.get_desired_rate(), + _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude + + /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ + float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, _att.rollspeed, + _att.yawspeed, _yaw_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, + airspeed_scaling, lock_integrator); + _actuators.control[0] = + (isfinite(roll_u)) ? + roll_u + _parameters.trim_roll : _parameters.trim_roll; + if (!isfinite(roll_u)) { + _roll_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); + + if (_debug && loop_counter % 10 == 0) { + warnx("roll_u %.4f", (double) roll_u); + } + } + + float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, + airspeed_scaling, lock_integrator); + _actuators.control[1] = + (isfinite(pitch_u)) ? + pitch_u + _parameters.trim_pitch : + _parameters.trim_pitch; + if (!isfinite(pitch_u)) { + _pitch_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); + if (_debug && loop_counter % 10 == 0) { + warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," + " airspeed %.4f, airspeed_scaling %.4f," + " roll_sp %.4f, pitch_sp %.4f," + " _roll_ctrl.get_desired_rate() %.4f," + " _pitch_ctrl.get_desired_rate() %.4f" + " att_sp.roll_body %.4f", (double) pitch_u, + (double) _yaw_ctrl.get_desired_rate(), + (double) airspeed, (double) airspeed_scaling, + (double) roll_sp, (double) pitch_sp, + (double) _roll_ctrl.get_desired_rate(), + (double) _pitch_ctrl.get_desired_rate(), + (double) _att_sp.roll_body); + } + } + + float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, _pitch_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, + airspeed_scaling, lock_integrator); + _actuators.control[2] = + (isfinite(yaw_u)) ? + yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; + if (!isfinite(yaw_u)) { + _yaw_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); + if (_debug && loop_counter % 10 == 0) { + warnx("yaw_u %.4f", (double) yaw_u); + } + } -void FixedwingAttitudeControlBase::control_attitude() -{ + /* throttle passed through */ + _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + if (!isfinite(throttle_sp)) { + if (_debug && loop_counter % 10 == 0) { + warnx("throttle_sp %.4f", (double) throttle_sp); + } + } + } else { + perf_count(_nonfinite_input_perf); + if (_debug && loop_counter % 10 == 0) { + warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", + (double) roll_sp, (double) pitch_sp); + } + } } -- cgit v1.2.3 From 43056258839c67590dec811e923553fad66a0f4b Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 25 Sep 2014 10:29:30 +0200 Subject: Added control_attitude function --- src/modules/fw_att_control/fw_att_control_base.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/fw_att_control/fw_att_control_base.h b/src/modules/fw_att_control/fw_att_control_base.h index 9a2d9b195..6e071fe20 100644 --- a/src/modules/fw_att_control/fw_att_control_base.h +++ b/src/modules/fw_att_control/fw_att_control_base.h @@ -114,6 +114,8 @@ protected: ECL_PitchController _pitch_ctrl; ECL_YawController _yaw_ctrl; + void control_attitude(); + -- cgit v1.2.3 From 020a8292155cd2821cb66a9709a8c7c18524bdac Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Sat, 27 Sep 2014 12:00:15 +0200 Subject: Adapted for shared library use with ROS --- src/lib/mathlib/math/Matrix.hpp | 114 ++++++++++++++++++++++++---------------- 1 file changed, 68 insertions(+), 46 deletions(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 8fb3caa0d..34fc4604a 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -54,10 +54,9 @@ #define M_PI_2_F 1.570769 #endif -namespace math -{ +namespace math { -template +template class __EXPORT Matrix; // MxN matrix with float elements @@ -73,19 +72,19 @@ public: /** * struct for using arm_math functions */ - #ifdef CONFIG_ARCH_ARM +#ifdef CONFIG_ARCH_ARM arm_matrix_instance_f32 arm_mat; - #else +#else eigen_matrix_instance arm_mat; - #endif +#endif /** * trivial ctor * Initializes the elements to zero. */ - MatrixBase() : - data{}, - arm_mat{M, N, &data[0][0]} + MatrixBase() : + data {}, + arm_mat {M, N, &data[0][0]} { } @@ -95,19 +94,19 @@ public: * copyt ctor */ MatrixBase(const MatrixBase &m) : - arm_mat{M, N, &data[0][0]} + arm_mat {M, N, &data[0][0]} { memcpy(data, m.data, sizeof(data)); } MatrixBase(const float *d) : - arm_mat{M, N, &data[0][0]} + arm_mat {M, N, &data[0][0]} { memcpy(data, d, sizeof(data)); } - MatrixBase(const float d[M][N]) : - arm_mat{M, N, &data[0][0]} + MatrixBase(const float d[M][N]) : + arm_mat {M, N, &data[0][0]} { memcpy(data, d, sizeof(data)); } @@ -159,9 +158,9 @@ public: */ bool operator ==(const Matrix &m) const { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m.data[i][j]) - return false; + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m.data[i][j]) + return false; return true; } @@ -171,9 +170,9 @@ public: */ bool operator !=(const Matrix &m) const { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m.data[i][j]) - return true; + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m.data[i][j]) + return true; return false; } @@ -193,8 +192,8 @@ public: Matrix res; for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - res.data[i][j] = -data[i][j]; + for (unsigned int j = 0; j < M; j++) + res.data[i][j] = -data[i][j]; return res; } @@ -206,16 +205,16 @@ public: Matrix res; for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - res.data[i][j] = data[i][j] + m.data[i][j]; + for (unsigned int j = 0; j < M; j++) + res.data[i][j] = data[i][j] + m.data[i][j]; return res; } Matrix &operator +=(const Matrix &m) { for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - data[i][j] += m.data[i][j]; + for (unsigned int j = 0; j < M; j++) + data[i][j] += m.data[i][j]; return *static_cast*>(this); } @@ -227,16 +226,16 @@ public: Matrix res; for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - res.data[i][j] = data[i][j] - m.data[i][j]; + for (unsigned int j = 0; j < N; j++) + res.data[i][j] = data[i][j] - m.data[i][j]; return res; } Matrix &operator -=(const Matrix &m) { for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - data[i][j] -= m.data[i][j]; + for (unsigned int j = 0; j < M; j++) + data[i][j] -= m.data[i][j]; return *static_cast*>(this); } @@ -248,16 +247,17 @@ public: Matrix res; for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - res.data[i][j] = data[i][j] * num; + for (unsigned int j = 0; j < N; j++) + res.data[i][j] = data[i][j] * num; return res; + } Matrix &operator *=(const float num) { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - data[i][j] *= num; + for (unsigned int j = 0; j < N; j++) + data[i][j] *= num; return *static_cast*>(this); } @@ -266,16 +266,16 @@ public: Matrix res; for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - res[i][j] = data[i][j] / num; + for (unsigned int j = 0; j < N; j++) + res[i][j] = data[i][j] / num; return res; } Matrix &operator /=(const float num) { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - data[i][j] /= num; + for (unsigned int j = 0; j < N; j++) + data[i][j] /= num; return *static_cast*>(this); } @@ -285,27 +285,49 @@ public: */ template Matrix operator *(const Matrix &m) const { +#ifdef CONFIG_ARCH_ARM Matrix res; arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); return res; +#else + Eigen::Matrix Me = Eigen::Map >(this->arm_mat.pData); + Eigen::Matrix Him = Eigen::Map >(m.arm_mat.pData); + Eigen::Matrix Product = Me * Him; + Matrix res(Product.data()); + return res; +#endif } /** * transpose the matrix */ Matrix transposed(void) const { +#ifdef CONFIG_ARCH_ARM Matrix res; arm_mat_trans_f32(&this->arm_mat, &res.arm_mat); return res; +#else + Eigen::Matrix Me = Eigen::Map >(this->arm_mat.pData); + Me.transposeInPlace(); + Matrix res(Me.data()); + return res; +#endif } /** * invert the matrix */ Matrix inversed(void) const { +#ifdef CONFIG_ARCH_ARM Matrix res; arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); return res; +#else + Eigen::Matrix Me = Eigen::Map >(this->arm_mat.pData); + Eigen::Matrix MyInverse = Me.inverse();//not sure if A = A.inverse() is a good idea + Matrix res(MyInverse.data()); + return res; +#endif } /** @@ -323,7 +345,7 @@ public: unsigned int n = (M < N) ? M : N; for (unsigned int i = 0; i < n; i++) - data[i][i] = 1; + data[i][i] = 1; } void print(void) { @@ -331,7 +353,7 @@ public: printf("[ "); for (unsigned int j = 0; j < N; j++) - printf("%.3f\t", data[i][j]); + printf("%.3f\t", data[i][j]); printf(" ]\n"); } @@ -364,16 +386,16 @@ public: * multiplication by a vector */ Vector operator *(const Vector &v) const { - #ifdef CONFIG_ARCH_ARM +#ifdef CONFIG_ARCH_ARM Vector res; arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); - #else +#else //probably nicer if this could go into a function like "eigen_mat_mult" or so - Eigen::Matrix Me = Eigen::Map >(this->arm_mat.pData); - Eigen::VectorXf Vec = Eigen::Map(v.arm_col.pData,N); + Eigen::Matrix Me = Eigen::Map >(this->arm_mat.pData); + Eigen::VectorXf Vec = Eigen::Map(v.arm_col.pData,N); Eigen::VectorXf Product = Me * Vec; Vector res(Product.data()); - #endif +#endif return res; } }; @@ -405,8 +427,8 @@ public: */ Vector<3> operator *(const Vector<3> &v) const { Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2], - data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2], - data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]); + data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2], + data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]); return res; } -- cgit v1.2.3 From c8b1c5b119e6552e8e5420ca8fecd24b2a3ad4a2 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Sat, 27 Sep 2014 12:01:11 +0200 Subject: Adapted for shared library use with ROS --- src/lib/geo/geo.h | 6 +- src/lib/geo_lookup/geo_mag_declination.h | 4 + src/modules/fw_att_control/fw_att_control_main.cpp | 419 +++++++++++---------- src/modules/uORB/topics/actuator_armed.h | 6 +- 4 files changed, 227 insertions(+), 208 deletions(-) diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 2311e0a7c..ff2d92389 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -45,14 +45,16 @@ #pragma once +#ifdef CONFIG_ARCH_ARM #include "uORB/topics/fence.h" #include "uORB/topics/vehicle_global_position.h" __BEGIN_DECLS - +#endif #include "geo_lookup/geo_mag_declination.h" #include +#include #define CONSTANTS_ONE_G 9.80665f /* m/s^2 */ #define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f /* kg/m^3 */ @@ -276,4 +278,6 @@ __EXPORT float _wrap_360(float bearing); __EXPORT float _wrap_pi(float bearing); __EXPORT float _wrap_2pi(float bearing); +#ifdef CONFIG_ARCH_ARM __END_DECLS +#endif diff --git a/src/lib/geo_lookup/geo_mag_declination.h b/src/lib/geo_lookup/geo_mag_declination.h index 0ac062d6d..d79b78412 100644 --- a/src/lib/geo_lookup/geo_mag_declination.h +++ b/src/lib/geo_lookup/geo_mag_declination.h @@ -40,8 +40,12 @@ #pragma once +#ifdef CONFIG_ARCH_ARM __BEGIN_DECLS __EXPORT float get_mag_declination(float lat, float lon); __END_DECLS +#else +float get_mag_declination(float lat, float lon); +#endif diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index c60d8d348..5cff390e2 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -83,8 +83,7 @@ */ extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]); -class FixedwingAttitudeControl -{ +class FixedwingAttitudeControl { public: /** * Constructor @@ -101,54 +100,56 @@ public: * * @return OK on success. */ - int start(); + int start(); /** * Task status * * @return true if the mainloop is running */ - bool task_running() { return _task_running; } + bool task_running() { + return _task_running; + } private: - bool _task_should_exit; /**< if true, sensor task should exit */ - bool _task_running; /**< if true, task is running in its mainloop */ - int _control_task; /**< task handle for sensor task */ - - int _att_sub; /**< vehicle attitude subscription */ - int _accel_sub; /**< accelerometer subscription */ - int _att_sp_sub; /**< vehicle attitude setpoint */ - int _attitude_sub; /**< raw rc channels data subscription */ - int _airspeed_sub; /**< airspeed subscription */ - int _vcontrol_mode_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_sub; /**< notification of manual control updates */ - int _global_pos_sub; /**< global position subscription */ - int _vehicle_status_sub; /**< vehicle status subscription */ - - orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ - orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ - orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ - orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */ - - struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct accel_report _accel; /**< body frame accelerations */ - struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ - struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ - struct actuator_controls_s _actuators; /**< actuator control inputs */ - struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ - struct vehicle_global_position_s _global_pos; /**< global position */ - struct vehicle_status_s _vehicle_status; /**< vehicle status */ - - perf_counter_t _loop_perf; /**< loop performance counter */ - perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ - perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ - - bool _setpoint_valid; /**< flag if the position control setpoint is valid */ - bool _debug; /**< if set to true, print debug output */ + bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_running; /**< if true, task is running in its mainloop */ + int _control_task; /**< task handle for sensor task */ + + int _att_sub; /**< vehicle attitude subscription */ + int _accel_sub; /**< accelerometer subscription */ + int _att_sp_sub; /**< vehicle attitude setpoint */ + int _attitude_sub; /**< raw rc channels data subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _vcontrol_mode_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ + int _global_pos_sub; /**< global position subscription */ + int _vehicle_status_sub; /**< vehicle status subscription */ + + orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ + orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ + orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ + orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct accel_report _accel; /**< body frame accelerations */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ + struct vehicle_global_position_s _global_pos; /**< global position */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ + perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ + + bool _setpoint_valid; /**< flag if the position control setpoint is valid */ + bool _debug; /**< if set to true, print debug output */ struct { float tconst; @@ -182,14 +183,14 @@ private: float trim_roll; float trim_pitch; float trim_yaw; - float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ - float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ - float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ - float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ - float man_roll_max; /**< Max Roll in rad */ - float man_pitch_max; /**< Max Pitch in rad */ + float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ + float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ + float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ + float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ + float man_roll_max; /**< Max Roll in rad */ + float man_pitch_max; /**< Max Pitch in rad */ - } _parameters; /**< local copies of interesting parameters */ + } _parameters; /**< local copies of interesting parameters */ struct { @@ -228,75 +229,71 @@ private: param_t pitchsp_offset_deg; param_t man_roll_max; param_t man_pitch_max; - } _parameter_handles; /**< handles for interesting parameters */ - - - ECL_RollController _roll_ctrl; - ECL_PitchController _pitch_ctrl; - ECL_YawController _yaw_ctrl; + } _parameter_handles; /**< handles for interesting parameters */ + ECL_RollController _roll_ctrl; + ECL_PitchController _pitch_ctrl; + ECL_YawController _yaw_ctrl; /** * Update our local parameter cache. */ - int parameters_update(); + int parameters_update(); /** * Update control outputs * */ - void control_update(); + void control_update(); /** * Check for changes in vehicle control mode. */ - void vehicle_control_mode_poll(); + void vehicle_control_mode_poll(); /** * Check for changes in manual inputs. */ - void vehicle_manual_poll(); - + void vehicle_manual_poll(); /** * Check for airspeed updates. */ - void vehicle_airspeed_poll(); + void vehicle_airspeed_poll(); /** * Check for accel updates. */ - void vehicle_accel_poll(); + void vehicle_accel_poll(); /** * Check for set triplet updates. */ - void vehicle_setpoint_poll(); + void vehicle_setpoint_poll(); /** * Check for global position updates. */ - void global_pos_poll(); + void global_pos_poll(); /** * Check for vehicle status updates. */ - void vehicle_status_poll(); + void vehicle_status_poll(); /** * Shim for calling task_main from task_create. */ - static void task_main_trampoline(int argc, char *argv[]); + static void task_main_trampoline(int argc, char *argv[]); /** * Main sensor collection task. */ - void task_main(); + void task_main(); }; -namespace att_control -{ +namespace att_control { /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -304,39 +301,28 @@ namespace att_control #endif static const int ERROR = -1; -FixedwingAttitudeControl *g_control = nullptr; +FixedwingAttitudeControl *g_control = nullptr; } FixedwingAttitudeControl::FixedwingAttitudeControl() : - _task_should_exit(false), - _task_running(false), - _control_task(-1), - -/* subscriptions */ - _att_sub(-1), - _accel_sub(-1), - _airspeed_sub(-1), - _vcontrol_mode_sub(-1), - _params_sub(-1), - _manual_sub(-1), - _global_pos_sub(-1), - _vehicle_status_sub(-1), - -/* publications */ - _rate_sp_pub(-1), - _attitude_sp_pub(-1), - _actuators_0_pub(-1), - _actuators_1_pub(-1), - -/* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")), - _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), -/* states */ - _setpoint_valid(false), - _debug(false) -{ + _task_should_exit(false), _task_running(false), _control_task(-1), + + /* subscriptions */ + _att_sub(-1), _accel_sub(-1), _airspeed_sub(-1), _vcontrol_mode_sub(-1), _params_sub( + -1), _manual_sub(-1), _global_pos_sub(-1), _vehicle_status_sub( + -1), + + /* publications */ + _rate_sp_pub(-1), _attitude_sp_pub(-1), _actuators_0_pub(-1), _actuators_1_pub( + -1), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), _nonfinite_input_perf( + perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf( + perf_alloc(PC_COUNT, "fw att control nonfinite output")), + /* states */ + _setpoint_valid(false), _debug(false) { /* safely initialize structs */ _att = {}; _accel = {}; @@ -349,7 +335,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _global_pos = {}; _vehicle_status = {}; - _parameter_handles.tconst = param_find("FW_ATT_TC"); _parameter_handles.p_p = param_find("FW_PR_P"); _parameter_handles.p_i = param_find("FW_PR_I"); @@ -390,8 +375,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : parameters_update(); } -FixedwingAttitudeControl::~FixedwingAttitudeControl() -{ +FixedwingAttitudeControl::~FixedwingAttitudeControl() { if (_control_task != -1) { /* task wakes up every 100ms or so at the longest */ @@ -419,9 +403,7 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl() att_control::g_control = nullptr; } -int -FixedwingAttitudeControl::parameters_update() -{ +int FixedwingAttitudeControl::parameters_update() { param_get(_parameter_handles.tconst, &(_parameters.tconst)); param_get(_parameter_handles.p_p, &(_parameters.p_p)); @@ -429,21 +411,26 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.p_ff, &(_parameters.p_ff)); param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos)); param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg)); - param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max)); - param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward)); + param_get(_parameter_handles.p_integrator_max, + &(_parameters.p_integrator_max)); + param_get(_parameter_handles.p_roll_feedforward, + &(_parameters.p_roll_feedforward)); param_get(_parameter_handles.r_p, &(_parameters.r_p)); param_get(_parameter_handles.r_i, &(_parameters.r_i)); param_get(_parameter_handles.r_ff, &(_parameters.r_ff)); - param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max)); + param_get(_parameter_handles.r_integrator_max, + &(_parameters.r_integrator_max)); param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax)); param_get(_parameter_handles.y_p, &(_parameters.y_p)); param_get(_parameter_handles.y_i, &(_parameters.y_i)); param_get(_parameter_handles.y_ff, &(_parameters.y_ff)); - param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max)); - param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed)); + param_get(_parameter_handles.y_integrator_max, + &(_parameters.y_integrator_max)); + param_get(_parameter_handles.y_coordinated_min_speed, + &(_parameters.y_coordinated_min_speed)); param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax)); param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); @@ -453,16 +440,19 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll)); param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch)); param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw)); - param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg)); - param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg)); - _parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg); - _parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg); + param_get(_parameter_handles.rollsp_offset_deg, + &(_parameters.rollsp_offset_deg)); + param_get(_parameter_handles.pitchsp_offset_deg, + &(_parameters.pitchsp_offset_deg)); + _parameters.rollsp_offset_rad = math::radians( + _parameters.rollsp_offset_deg); + _parameters.pitchsp_offset_rad = math::radians( + _parameters.pitchsp_offset_deg); param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max)); param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max)); _parameters.man_roll_max = math::radians(_parameters.man_roll_max); _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); - /* pitch control parameters */ _pitch_ctrl.set_time_constant(_parameters.tconst); _pitch_ctrl.set_k_p(_parameters.p_p); @@ -492,9 +482,7 @@ FixedwingAttitudeControl::parameters_update() return OK; } -void -FixedwingAttitudeControl::vehicle_control_mode_poll() -{ +void FixedwingAttitudeControl::vehicle_control_mode_poll() { bool vcontrol_mode_updated; /* Check HIL state if vehicle status has changed */ @@ -502,13 +490,12 @@ FixedwingAttitudeControl::vehicle_control_mode_poll() if (vcontrol_mode_updated) { - orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode); + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, + &_vcontrol_mode); } } -void -FixedwingAttitudeControl::vehicle_manual_poll() -{ +void FixedwingAttitudeControl::vehicle_manual_poll() { bool manual_updated; /* get pilots inputs */ @@ -520,9 +507,7 @@ FixedwingAttitudeControl::vehicle_manual_poll() } } -void -FixedwingAttitudeControl::vehicle_airspeed_poll() -{ +void FixedwingAttitudeControl::vehicle_airspeed_poll() { /* check if there is a new position */ bool airspeed_updated; orb_check(_airspeed_sub, &airspeed_updated); @@ -533,9 +518,7 @@ FixedwingAttitudeControl::vehicle_airspeed_poll() } } -void -FixedwingAttitudeControl::vehicle_accel_poll() -{ +void FixedwingAttitudeControl::vehicle_accel_poll() { /* check if there is a new position */ bool accel_updated; orb_check(_accel_sub, &accel_updated); @@ -545,9 +528,7 @@ FixedwingAttitudeControl::vehicle_accel_poll() } } -void -FixedwingAttitudeControl::vehicle_setpoint_poll() -{ +void FixedwingAttitudeControl::vehicle_setpoint_poll() { /* check if there is a new setpoint */ bool att_sp_updated; orb_check(_att_sp_sub, &att_sp_updated); @@ -558,21 +539,18 @@ FixedwingAttitudeControl::vehicle_setpoint_poll() } } -void -FixedwingAttitudeControl::global_pos_poll() -{ +void FixedwingAttitudeControl::global_pos_poll() { /* check if there is a new global position */ bool global_pos_updated; orb_check(_global_pos_sub, &global_pos_updated); if (global_pos_updated) { - orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, + &_global_pos); } } -void -FixedwingAttitudeControl::vehicle_status_poll() -{ +void FixedwingAttitudeControl::vehicle_status_poll() { /* check if there is new status information */ bool vehicle_status_updated; orb_check(_vehicle_status_sub, &vehicle_status_updated); @@ -582,15 +560,11 @@ FixedwingAttitudeControl::vehicle_status_poll() } } -void -FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) -{ +void FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) { att_control::g_control->task_main(); } -void -FixedwingAttitudeControl::task_main() -{ +void FixedwingAttitudeControl::task_main() { /* inform about start */ warnx("Initializing.."); @@ -667,7 +641,6 @@ FixedwingAttitudeControl::task_main() /* only run controller if attitude changed */ if (fds[1].revents & POLLIN) { - static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); @@ -721,8 +694,8 @@ FixedwingAttitudeControl::task_main() float airspeed; /* if airspeed is not updating, we assume the normal average speed */ - if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) || - hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { + if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) + || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; if (nonfinite) { perf_count(_nonfinite_input_perf); @@ -740,16 +713,20 @@ FixedwingAttitudeControl::task_main() * Forcing the scaling to this value allows reasonable handheld tests. */ - float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed); + float airspeed_scaling = _parameters.airspeed_trim + / ((airspeed < _parameters.airspeed_min) ? + _parameters.airspeed_min : airspeed); float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; float throttle_sp = 0.0f; - if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) { + if (_vcontrol_mode.flag_control_velocity_enabled + || _vcontrol_mode.flag_control_position_enabled) { /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; - pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; + pitch_sp = _att_sp.pitch_body + + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ @@ -775,10 +752,12 @@ FixedwingAttitudeControl::task_main() * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ - roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) - + _parameters.rollsp_offset_rad; - pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch) - + _parameters.pitchsp_offset_rad; + roll_sp = (_manual.y * _parameters.man_roll_max + - _parameters.trim_roll) + + _parameters.rollsp_offset_rad; + pitch_sp = -(_manual.x * _parameters.man_pitch_max + - _parameters.trim_pitch) + + _parameters.pitchsp_offset_rad; throttle_sp = _manual.z; _actuators.control[4] = _manual.flaps; @@ -797,11 +776,13 @@ FixedwingAttitudeControl::task_main() /* lazily publish the setpoint only once available */ if (_attitude_sp_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); + orb_publish(ORB_ID(vehicle_attitude_setpoint), + _attitude_sp_pub, &att_sp); } else { /* advertise and publish */ - _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + _attitude_sp_pub = orb_advertise( + ORB_ID(vehicle_attitude_setpoint), &att_sp); } } @@ -816,11 +797,17 @@ FixedwingAttitudeControl::task_main() float speed_body_u = 0.0f; float speed_body_v = 0.0f; float speed_body_w = 0.0f; - if(_att.R_valid) { - speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d; - speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; - speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; - } else { + if (_att.R_valid) { + speed_body_u = _att.R[0][0] * _global_pos.vel_n + + _att.R[1][0] * _global_pos.vel_e + + _att.R[2][0] * _global_pos.vel_d; + speed_body_v = _att.R[0][1] * _global_pos.vel_n + + _att.R[1][1] * _global_pos.vel_e + + _att.R[2][1] * _global_pos.vel_d; + speed_body_w = _att.R[0][2] * _global_pos.vel_n + + _att.R[1][2] * _global_pos.vel_e + + _att.R[2][2] * _global_pos.vel_d; + } else { if (_debug && loop_counter % 10 == 0) { warnx("Did not get a valid R\n"); } @@ -829,74 +816,94 @@ FixedwingAttitudeControl::task_main() /* Run attitude controllers */ if (isfinite(roll_sp) && isfinite(pitch_sp)) { _roll_ctrl.control_attitude(roll_sp, _att.roll); - _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed); + _pitch_ctrl.control_attitude(pitch_sp, _att.roll, + _att.pitch, airspeed); _yaw_ctrl.control_attitude(_att.roll, _att.pitch, speed_body_u, speed_body_v, speed_body_w, - _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude + _roll_ctrl.get_desired_rate(), + _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, _att.rollspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; + _parameters.airspeed_min, _parameters.airspeed_max, + airspeed, airspeed_scaling, lock_integrator); + _actuators.control[0] = + (isfinite(roll_u)) ? + roll_u + _parameters.trim_roll : + _parameters.trim_roll; if (!isfinite(roll_u)) { _roll_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { - warnx("roll_u %.4f", (double)roll_u); + warnx("roll_u %.4f", (double) roll_u); } } - float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, - _att.pitchspeed, _att.yawspeed, + float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, + _att.pitch, _att.pitchspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; + _parameters.airspeed_min, _parameters.airspeed_max, + airspeed, airspeed_scaling, lock_integrator); + _actuators.control[1] = + (isfinite(pitch_u)) ? + pitch_u + _parameters.trim_pitch : + _parameters.trim_pitch; if (!isfinite(pitch_u)) { _pitch_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { - warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," - " airspeed %.4f, airspeed_scaling %.4f," - " roll_sp %.4f, pitch_sp %.4f," - " _roll_ctrl.get_desired_rate() %.4f," - " _pitch_ctrl.get_desired_rate() %.4f" - " att_sp.roll_body %.4f", - (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), - (double)airspeed, (double)airspeed_scaling, - (double)roll_sp, (double)pitch_sp, - (double)_roll_ctrl.get_desired_rate(), - (double)_pitch_ctrl.get_desired_rate(), - (double)_att_sp.roll_body); + warnx( + "pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," + " airspeed %.4f, airspeed_scaling %.4f," + " roll_sp %.4f, pitch_sp %.4f," + " _roll_ctrl.get_desired_rate() %.4f," + " _pitch_ctrl.get_desired_rate() %.4f" + " att_sp.roll_body %.4f", + (double) pitch_u, + (double) _yaw_ctrl.get_desired_rate(), + (double) airspeed, + (double) airspeed_scaling, (double) roll_sp, + (double) pitch_sp, + (double) _roll_ctrl.get_desired_rate(), + (double) _pitch_ctrl.get_desired_rate(), + (double) _att_sp.roll_body); } } - float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, - _att.pitchspeed, _att.yawspeed, + float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, + _att.pitch, _att.pitchspeed, _att.yawspeed, _pitch_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; + _parameters.airspeed_min, _parameters.airspeed_max, + airspeed, airspeed_scaling, lock_integrator); + _actuators.control[2] = + (isfinite(yaw_u)) ? + yaw_u + _parameters.trim_yaw : + _parameters.trim_yaw; if (!isfinite(yaw_u)) { _yaw_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { - warnx("yaw_u %.4f", (double)yaw_u); + warnx("yaw_u %.4f", (double) yaw_u); } } /* throttle passed through */ - _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + _actuators.control[3] = + (isfinite(throttle_sp)) ? throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { if (_debug && loop_counter % 10 == 0) { - warnx("throttle_sp %.4f", (double)throttle_sp); + warnx("throttle_sp %.4f", (double) throttle_sp); } } } else { perf_count(_nonfinite_input_perf); if (_debug && loop_counter % 10 == 0) { - warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); + warnx( + "Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", + (double) roll_sp, (double) pitch_sp); } } @@ -913,11 +920,13 @@ FixedwingAttitudeControl::task_main() if (_rate_sp_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp); + orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, + &rates_sp); } else { /* advertise and publish */ - _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), + &rates_sp); } } else { @@ -939,16 +948,19 @@ FixedwingAttitudeControl::task_main() if (_actuators_0_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, + &_actuators); } else { /* advertise and publish */ - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), + &_actuators); } if (_actuators_1_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe); + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, + &_actuators_airframe); // warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f", // (double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2], // (double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5], @@ -956,7 +968,8 @@ FixedwingAttitudeControl::task_main() } else { /* advertise and publish */ - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe); + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), + &_actuators_airframe); } } @@ -972,18 +985,14 @@ FixedwingAttitudeControl::task_main() _exit(0); } -int -FixedwingAttitudeControl::start() -{ +int FixedwingAttitudeControl::start() { ASSERT(_control_task == -1); /* start the task */ _control_task = task_spawn_cmd("fw_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2048, - (main_t)&FixedwingAttitudeControl::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, 2048, + (main_t) &FixedwingAttitudeControl::task_main_trampoline, nullptr); if (_control_task < 0) { warn("task start failed"); @@ -993,8 +1002,7 @@ FixedwingAttitudeControl::start() return OK; } -int fw_att_control_main(int argc, char *argv[]) -{ +int fw_att_control_main(int argc, char *argv[]) { if (argc < 1) errx(1, "usage: fw_att_control {start|stop|status}"); @@ -1015,7 +1023,8 @@ int fw_att_control_main(int argc, char *argv[]) } /* avoid memory fragmentation by not exiting start handler until the task has fully started */ - while (att_control::g_control == nullptr || !att_control::g_control->task_running()) { + while (att_control::g_control == nullptr + || !att_control::g_control->task_running()) { usleep(50000); printf("."); fflush(stdout); diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h index 0f6c9aca1..9ec9d10ab 100644 --- a/src/modules/uORB/topics/actuator_armed.h +++ b/src/modules/uORB/topics/actuator_armed.h @@ -42,8 +42,9 @@ #define TOPIC_ACTUATOR_ARMED_H #include +#ifdef CONFIG_ARCH_ARM #include "../uORB.h" - +#endif /** * @addtogroup topics * @{ @@ -64,6 +65,7 @@ struct actuator_armed_s { */ /* register this as object request broker structure */ +#ifdef CONFIG_ARCH_ARM ORB_DECLARE(actuator_armed); - +#endif #endif -- cgit v1.2.3 From ac0f01e92e647944b34f7dcf3adbacd283d685cb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 3 Nov 2014 07:41:05 +0100 Subject: First stab at ROS integration --- CMakeLists.txt | 163 ++++++++++++++++++++++++++++++++++++++++++++++++++++ msg/rc_channels.msg | 8 +++ package.xml | 59 +++++++++++++++++++ 3 files changed, 230 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 msg/rc_channels.msg create mode 100644 package.xml diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 000000000..834a8f0c1 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,163 @@ +cmake_minimum_required(VERSION 2.8.3) +project(px4) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependencies might have been +## pulled in transitively but can be declared for certainty nonetheless: +## * add a build_depend tag for "message_generation" +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs +) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES px4 +# CATKIN_DEPENDS roscpp rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + ${catkin_INCLUDE_DIRS} +) + +## Declare a cpp library +# add_library(px4 +# src/${PROJECT_NAME}/px4test.cpp # src/platform/ros/ros.cpp +# ) + +## Declare a cpp executable +add_executable(rostest_node src/platform/ros/ros.cpp) + +## Add cmake target dependencies of the executable/library +## as an example, message headers may need to be generated before nodes +add_dependencies(rostest_node px4_generate_messages_cpp) + +## Specify libraries to link a library or executable target against +target_link_libraries(rostest_node + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS px4 mc_attitude_control +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_px4test.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg new file mode 100644 index 000000000..9b3c70835 --- /dev/null +++ b/msg/rc_channels.msg @@ -0,0 +1,8 @@ +Header header +int32 RC_CHANNELS_FUNCTION_MAX=18 +uint64 timestamp_last_valid # Timestamp of last valid RC signal +float32 channels[RC_CHANNELS_FUNCTION_MAX] # Scaled to -1..1 (throttle: 0..1) +uint8 channel_count # Number of valid channels +int8 function[RC_CHANNELS_FUNCTION_MAX] # Functions mapping +uint8 rssi # Receive signal strength index +bool signal_lost # Control signal lost, should be checked together with topic timeout \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 000000000..3575119be --- /dev/null +++ b/package.xml @@ -0,0 +1,59 @@ + + + px4 + 1.0.0 + The PX4 Flight Stack package + + + + + Lorenz Meier + + + + + + BSD + + + + + + http://px4.io/ros + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + + + + + + + + + + + \ No newline at end of file -- cgit v1.2.3 From 3b2b280a41f093020eab1a8a8945603fb3ffacfb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Oct 2014 00:49:25 -0700 Subject: Get ROS examples to compile, add simple RC channels message --- CMakeLists.txt | 32 +++++--- msg/rc_channels.msg | 6 +- src/examples/publisher/publisher.cpp | 137 +++++++++++++++++++++++++++++++++ src/examples/subscriber/subscriber.cpp | 93 ++++++++++++++++++++++ 4 files changed, 255 insertions(+), 13 deletions(-) create mode 100644 src/examples/publisher/publisher.cpp create mode 100644 src/examples/subscriber/subscriber.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 834a8f0c1..d370c5a3f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs + message_generation ) ## System dependencies are found with CMake's conventions @@ -44,11 +45,10 @@ find_package(catkin REQUIRED COMPONENTS ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) +add_message_files( + FILES + rc_channels.msg +) ## Generate services in the 'srv' folder # add_service_files( @@ -99,18 +99,30 @@ include_directories( ## Declare a cpp library # add_library(px4 -# src/${PROJECT_NAME}/px4test.cpp # src/platform/ros/ros.cpp +# src/${PROJECT_NAME}/px4test.cpp # src/platforms/ros/ros.cpp # ) -## Declare a cpp executable -add_executable(rostest_node src/platform/ros/ros.cpp) +## Declare a test publisher +add_executable(publisher src/examples/publisher/publisher.cpp) + +## Add cmake target dependencies of the executable/library +## as an example, message headers may need to be generated before nodes +add_dependencies(publisher px4_generate_messages_cpp) + +## Specify libraries to link a library or executable target against +target_link_libraries(publisher + ${catkin_LIBRARIES} +) + +## Declare a test subscriber +add_executable(subscriber src/examples/subscriber/subscriber.cpp) ## Add cmake target dependencies of the executable/library ## as an example, message headers may need to be generated before nodes -add_dependencies(rostest_node px4_generate_messages_cpp) +add_dependencies(subscriber px4_generate_messages_cpp) ## Specify libraries to link a library or executable target against -target_link_libraries(rostest_node +target_link_libraries(subscriber ${catkin_LIBRARIES} ) diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg index 9b3c70835..11dc57c42 100644 --- a/msg/rc_channels.msg +++ b/msg/rc_channels.msg @@ -1,8 +1,8 @@ Header header int32 RC_CHANNELS_FUNCTION_MAX=18 uint64 timestamp_last_valid # Timestamp of last valid RC signal -float32 channels[RC_CHANNELS_FUNCTION_MAX] # Scaled to -1..1 (throttle: 0..1) +float32[18] channels # Scaled to -1..1 (throttle: 0..1) uint8 channel_count # Number of valid channels -int8 function[RC_CHANNELS_FUNCTION_MAX] # Functions mapping +int8[18] function # Functions mapping uint8 rssi # Receive signal strength index -bool signal_lost # Control signal lost, should be checked together with topic timeout \ No newline at end of file +bool signal_lost # Control signal lost, should be checked together with topic timeout diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp new file mode 100644 index 000000000..2c0f84476 --- /dev/null +++ b/src/examples/publisher/publisher.cpp @@ -0,0 +1,137 @@ +/* + * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * 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. + * * Neither the names of Stanford University or Willow Garage, Inc. 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. + */ +// %Tag(FULLTEXT)% +// %Tag(ROS_HEADER)% +#include "ros/ros.h" +// %EndTag(ROS_HEADER)% +// %Tag(MSG_HEADER)% +#include "std_msgs/String.h" +// %EndTag(MSG_HEADER)% + +#include + +/** + * This tutorial demonstrates simple sending of messages over the ROS system. + */ +int main(int argc, char **argv) +{ + /** + * The ros::init() function needs to see argc and argv so that it can perform + * any ROS arguments and name remapping that were provided at the command line. For programmatic + * remappings you can use a different version of init() which takes remappings + * directly, but for most command-line programs, passing argc and argv is the easiest + * way to do it. The third argument to init() is the name of the node. + * + * You must call one of the versions of ros::init() before using any other + * part of the ROS system. + */ +// %Tag(INIT)% + ros::init(argc, argv, "talker"); +// %EndTag(INIT)% + + /** + * NodeHandle is the main access point to communications with the ROS system. + * The first NodeHandle constructed will fully initialize this node, and the last + * NodeHandle destructed will close down the node. + */ +// %Tag(NODEHANDLE)% + ros::NodeHandle n; +// %EndTag(NODEHANDLE)% + + /** + * The advertise() function is how you tell ROS that you want to + * publish on a given topic name. This invokes a call to the ROS + * master node, which keeps a registry of who is publishing and who + * is subscribing. After this advertise() call is made, the master + * node will notify anyone who is trying to subscribe to this topic name, + * and they will in turn negotiate a peer-to-peer connection with this + * node. advertise() returns a Publisher object which allows you to + * publish messages on that topic through a call to publish(). Once + * all copies of the returned Publisher object are destroyed, the topic + * will be automatically unadvertised. + * + * The second parameter to advertise() is the size of the message queue + * used for publishing messages. If messages are published more quickly + * than we can send them, the number here specifies how many messages to + * buffer up before throwing some away. + */ +// %Tag(PUBLISHER)% + ros::Publisher chatter_pub = n.advertise("chatter", 1000); +// %EndTag(PUBLISHER)% + +// %Tag(LOOP_RATE)% + ros::Rate loop_rate(10); +// %EndTag(LOOP_RATE)% + + /** + * A count of how many messages we have sent. This is used to create + * a unique string for each message. + */ +// %Tag(ROS_OK)% + int count = 0; + while (ros::ok()) + { +// %EndTag(ROS_OK)% + /** + * This is a message object. You stuff it with data, and then publish it. + */ +// %Tag(FILL_MESSAGE)% + std_msgs::String msg; + + std::stringstream ss; + ss << "hello world " << count; + msg.data = ss.str(); +// %EndTag(FILL_MESSAGE)% + +// %Tag(ROSCONSOLE)% + ROS_INFO("%s", msg.data.c_str()); +// %EndTag(ROSCONSOLE)% + + /** + * The publish() function is how you send messages. The parameter + * is the message object. The type of this object must agree with the type + * given as a template parameter to the advertise<>() call, as was done + * in the constructor above. + */ +// %Tag(PUBLISH)% + chatter_pub.publish(msg); +// %EndTag(PUBLISH)% + +// %Tag(SPINONCE)% + ros::spinOnce(); +// %EndTag(SPINONCE)% + +// %Tag(RATE_SLEEP)% + loop_rate.sleep(); +// %EndTag(RATE_SLEEP)% + ++count; + } + + + return 0; +} +// %EndTag(FULLTEXT)% \ No newline at end of file diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp new file mode 100644 index 000000000..e46055306 --- /dev/null +++ b/src/examples/subscriber/subscriber.cpp @@ -0,0 +1,93 @@ +/* + * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * 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. + * * Neither the names of Stanford University or Willow Garage, Inc. 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. + */ + +// %Tag(FULLTEXT)% +#include "ros/ros.h" +#include "std_msgs/String.h" + +/** + * This tutorial demonstrates simple receipt of messages over the ROS system. + */ +// %Tag(CALLBACK)% +void chatterCallback(const std_msgs::String::ConstPtr& msg) +{ + ROS_INFO("I heard: [%s]", msg->data.c_str()); +} +// %EndTag(CALLBACK)% + +int main(int argc, char **argv) +{ + /** + * The ros::init() function needs to see argc and argv so that it can perform + * any ROS arguments and name remapping that were provided at the command line. For programmatic + * remappings you can use a different version of init() which takes remappings + * directly, but for most command-line programs, passing argc and argv is the easiest + * way to do it. The third argument to init() is the name of the node. + * + * You must call one of the versions of ros::init() before using any other + * part of the ROS system. + */ + ros::init(argc, argv, "listener"); + + /** + * NodeHandle is the main access point to communications with the ROS system. + * The first NodeHandle constructed will fully initialize this node, and the last + * NodeHandle destructed will close down the node. + */ + ros::NodeHandle n; + + /** + * The subscribe() call is how you tell ROS that you want to receive messages + * on a given topic. This invokes a call to the ROS + * master node, which keeps a registry of who is publishing and who + * is subscribing. Messages are passed to a callback function, here + * called chatterCallback. subscribe() returns a Subscriber object that you + * must hold on to until you want to unsubscribe. When all copies of the Subscriber + * object go out of scope, this callback will automatically be unsubscribed from + * this topic. + * + * The second parameter to the subscribe() function is the size of the message + * queue. If messages are arriving faster than they are being processed, this + * is the number of messages that will be buffered up before beginning to throw + * away the oldest ones. + */ +// %Tag(SUBSCRIBER)% + ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback); +// %EndTag(SUBSCRIBER)% + + /** + * ros::spin() will enter a loop, pumping callbacks. With this version, all + * callbacks will be called from within this thread (the main one). ros::spin() + * will exit when Ctrl-C is pressed, or the node is shutdown by the master. + */ +// %Tag(SPIN)% + ros::spin(); +// %EndTag(SPIN)% + + return 0; +} +// %EndTag(FULLTEXT)% \ No newline at end of file -- cgit v1.2.3 From 3956a2c836df9aa411815b4a4027fdcfb864f041 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Oct 2014 08:12:41 -0700 Subject: Baby steps towards PX4 ROS nodes --- CMakeLists.txt | 4 +-- include/px4.h | 47 ++++++++++++++++++++++++++++++++++++ src/examples/publisher/publisher.cpp | 10 ++++---- 3 files changed, 54 insertions(+), 7 deletions(-) create mode 100644 include/px4.h diff --git a/CMakeLists.txt b/CMakeLists.txt index d370c5a3f..144bdcbc3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -80,7 +80,7 @@ generate_messages( ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( -# INCLUDE_DIRS include + INCLUDE_DIRS include # LIBRARIES px4 # CATKIN_DEPENDS roscpp rospy std_msgs # DEPENDS system_lib @@ -92,7 +92,7 @@ catkin_package( ## Specify additional locations of header files ## Your package locations should be listed before other locations -# include_directories(include) +include_directories(include) include_directories( ${catkin_INCLUDE_DIRS} ) diff --git a/include/px4.h b/include/px4.h new file mode 100644 index 000000000..7dedd5f82 --- /dev/null +++ b/include/px4.h @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * 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 px4.h + * + * Main system header with common convenience functions + */ + +#ifdef __linux +#include "ros/ros.h" +#define px4_warnx ROS_WARN +#define px4_infox ROS_INFO +#else +#define px4_warnx warnx +#define px4_infox warnx +#endif \ No newline at end of file diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp index 2c0f84476..cc88cd543 100644 --- a/src/examples/publisher/publisher.cpp +++ b/src/examples/publisher/publisher.cpp @@ -24,9 +24,9 @@ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -// %Tag(FULLTEXT)% -// %Tag(ROS_HEADER)% -#include "ros/ros.h" + +#include + // %EndTag(ROS_HEADER)% // %Tag(MSG_HEADER)% #include "std_msgs/String.h" @@ -50,7 +50,7 @@ int main(int argc, char **argv) * part of the ROS system. */ // %Tag(INIT)% - ros::init(argc, argv, "talker"); + ros::init(argc, argv, "px4_publisher"); // %EndTag(INIT)% /** @@ -108,7 +108,7 @@ int main(int argc, char **argv) // %EndTag(FILL_MESSAGE)% // %Tag(ROSCONSOLE)% - ROS_INFO("%s", msg.data.c_str()); + px4_warnx("%s", msg.data.c_str()); // %EndTag(ROSCONSOLE)% /** -- cgit v1.2.3 From 4fdf8e1ff26d37513c003ea1e92445fae81cc2cb Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 23 Oct 2014 16:59:21 +0200 Subject: updated from remote --- ROMFS/px4fmu_common/init.d/rc.vtol_apps | 15 + ROMFS/px4fmu_common/init.d/rc.vtol_defaults | 63 ++ ROMFS/px4fmu_common/mixers/FMU_quadshot.mix | 15 + Tools/tests-host/sf0x_test.cpp | 65 ++ Tools/tests-host/st24_test.cpp | 76 ++ src/drivers/sf0x/sf0x_parser.cpp | 155 ++++ src/drivers/sf0x/sf0x_parser.h | 51 ++ src/lib/rc/module.mk | 40 + src/lib/rc/st24.c | 253 ++++++ src/lib/rc/st24.h | 163 ++++ src/modules/bottle_drop/bottle_drop.cpp | 907 +++++++++++++++++++++ src/modules/bottle_drop/bottle_drop_params.c | 131 +++ src/modules/bottle_drop/module.mk | 41 + src/modules/mc_att_control/mc_att_control_base.cpp | 283 +++++++ src/modules/mc_att_control/mc_att_control_base.h | 172 ++++ src/modules/navigator/datalinkloss.cpp | 227 ++++++ src/modules/navigator/datalinkloss.h | 98 +++ src/modules/navigator/datalinkloss_params.c | 126 +++ src/modules/navigator/enginefailure.cpp | 149 ++++ src/modules/navigator/enginefailure.h | 83 ++ src/modules/navigator/gpsfailure.cpp | 178 ++++ src/modules/navigator/gpsfailure.h | 97 +++ src/modules/navigator/gpsfailure_params.c | 97 +++ src/modules/navigator/rcloss.cpp | 182 +++++ src/modules/navigator/rcloss.h | 88 ++ src/modules/navigator/rcloss_params.c | 60 ++ src/modules/uORB/topics/multirotor_motor_limits.h | 69 ++ src/modules/uORB/topics/vtol_vehicle_status.h | 66 ++ src/modules/vtol_att_control/module.mk | 40 + .../vtol_att_control/vtol_att_control_main.cpp | 642 +++++++++++++++ 30 files changed, 4632 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/rc.vtol_apps create mode 100644 ROMFS/px4fmu_common/init.d/rc.vtol_defaults create mode 100644 ROMFS/px4fmu_common/mixers/FMU_quadshot.mix create mode 100644 Tools/tests-host/sf0x_test.cpp create mode 100644 Tools/tests-host/st24_test.cpp create mode 100644 src/drivers/sf0x/sf0x_parser.cpp create mode 100644 src/drivers/sf0x/sf0x_parser.h create mode 100644 src/lib/rc/module.mk create mode 100644 src/lib/rc/st24.c create mode 100644 src/lib/rc/st24.h create mode 100644 src/modules/bottle_drop/bottle_drop.cpp create mode 100644 src/modules/bottle_drop/bottle_drop_params.c create mode 100644 src/modules/bottle_drop/module.mk create mode 100644 src/modules/mc_att_control/mc_att_control_base.cpp create mode 100644 src/modules/mc_att_control/mc_att_control_base.h create mode 100644 src/modules/navigator/datalinkloss.cpp create mode 100644 src/modules/navigator/datalinkloss.h create mode 100644 src/modules/navigator/datalinkloss_params.c create mode 100644 src/modules/navigator/enginefailure.cpp create mode 100644 src/modules/navigator/enginefailure.h create mode 100644 src/modules/navigator/gpsfailure.cpp create mode 100644 src/modules/navigator/gpsfailure.h create mode 100644 src/modules/navigator/gpsfailure_params.c create mode 100644 src/modules/navigator/rcloss.cpp create mode 100644 src/modules/navigator/rcloss.h create mode 100644 src/modules/navigator/rcloss_params.c create mode 100644 src/modules/uORB/topics/multirotor_motor_limits.h create mode 100644 src/modules/uORB/topics/vtol_vehicle_status.h create mode 100644 src/modules/vtol_att_control/module.mk create mode 100644 src/modules/vtol_att_control/vtol_att_control_main.cpp diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps new file mode 100644 index 000000000..23ade6d78 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -0,0 +1,15 @@ +#!nsh +# +# Standard apps for vtol: +# att & pos estimator, att & pos control. +# + +attitude_estimator_ekf start +#ekf_att_pos_estimator start +position_estimator_inav start + +vtol_att_control start +mc_att_control start +mc_pos_control start +fw_att_control start +fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults new file mode 100644 index 000000000..f0ea9add0 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults @@ -0,0 +1,63 @@ +#!nsh + +set VEHICLE_TYPE vtol + +if [ $DO_AUTOCONFIG == yes ] +then + #Default parameters for MC + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.003 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.003 + param set MC_YAW_P 2.8 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.1 + param set MC_YAWRATE_D 0.0 + param set MC_YAW_FF 0.5 + + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_P 0.1 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_FF 0.5 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_P 0.1 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_FF 0.5 + param set MPC_TILTMAX_AIR 45.0 + param set MPC_TILTMAX_LND 15.0 + param set MPC_LAND_SPEED 1.0 + + param set PE_VELNE_NOISE 0.5 + param set PE_VELD_NOISE 0.7 + param set PE_POSNE_NOISE 0.5 + param set PE_POSD_NOISE 1.0 + + param set NAV_ACCEPT_RAD 2.0 + + # + # Default parameters for FW + # + param set NAV_LAND_ALT 90 + param set NAV_RTL_ALT 100 + param set NAV_RTL_LAND_T -1 + param set NAV_ACCEPT_RAD 50 + param set FW_T_HRATE_P 0.01 + param set FW_T_RLL2THR 15 + param set FW_T_SRATE_P 0.01 + param set FW_T_TIME_CONST 5 +fi + +set PWM_RATE 400 +set PWM_DISARMED 900 +set PWM_MIN 1075 +set PWM_MAX 2000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_quadshot.mix b/ROMFS/px4fmu_common/mixers/FMU_quadshot.mix new file mode 100644 index 000000000..b077ff30a --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_quadshot.mix @@ -0,0 +1,15 @@ +#!nsh +#Quadshot mixer for PX4FMU +#=========================== +R: 4v 10000 10000 10000 0 + +#mixer for the elevons +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 0 7500 7500 0 -10000 10000 +S: 1 1 8000 8000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 0 7500 7500 0 -10000 10000 +S: 1 1 -8000 -8000 0 -10000 10000 diff --git a/Tools/tests-host/sf0x_test.cpp b/Tools/tests-host/sf0x_test.cpp new file mode 100644 index 000000000..82d19fcbe --- /dev/null +++ b/Tools/tests-host/sf0x_test.cpp @@ -0,0 +1,65 @@ + +#include +#include +#include +#include +#include +#include + +#include + +int main(int argc, char *argv[]) +{ + warnx("SF0X test started"); + + int ret = 0; + + const char LINE_MAX = 20; + char _linebuf[LINE_MAX]; + _linebuf[0] = '\0'; + + const char *lines[] = {"0.01\r\n", + "0.02\r\n", + "0.03\r\n", + "0.04\r\n", + "0", + ".", + "0", + "5", + "\r", + "\n", + "0", + "3\r", + "\n" + "\r\n", + "0.06", + "\r\n" + }; + + enum SF0X_PARSE_STATE state = SF0X_PARSE_STATE0_UNSYNC; + float dist_m; + char _parserbuf[LINE_MAX]; + unsigned _parsebuf_index = 0; + + for (unsigned l = 0; l < sizeof(lines) / sizeof(lines[0]); l++) { + + printf("\n%s", _linebuf); + + int parse_ret; + + for (int i = 0; i < strlen(lines[l]); i++) { + parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m); + + if (parse_ret == 0) { + printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : ""); + } + } + + printf("%s", lines[l]); + + } + + warnx("test finished"); + + return ret; +} diff --git a/Tools/tests-host/st24_test.cpp b/Tools/tests-host/st24_test.cpp new file mode 100644 index 000000000..25a9355e2 --- /dev/null +++ b/Tools/tests-host/st24_test.cpp @@ -0,0 +1,76 @@ + +#include +#include +#include +#include +#include +#include +#include "../../src/systemcmds/tests/tests.h" + +int main(int argc, char *argv[]) +{ + warnx("ST24 test started"); + + if (argc < 2) { + errx(1, "Need a filename for the input file"); + } + + warnx("loading data from: %s", argv[1]); + + FILE *fp; + + fp = fopen(argv[1], "rt"); + + if (!fp) { + errx(1, "failed opening file"); + } + + float f; + unsigned x; + int ret; + + // Trash the first 20 lines + for (unsigned i = 0; i < 20; i++) { + char buf[200]; + (void)fgets(buf, sizeof(buf), fp); + } + + float last_time = 0; + + while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { + if (((f - last_time) * 1000 * 1000) > 3000) { + // warnx("FRAME RESET\n\n"); + } + + uint8_t b = static_cast(x); + + last_time = f; + + // Pipe the data into the parser + hrt_abstime now = hrt_absolute_time(); + + uint8_t rssi; + uint8_t rx_count; + uint16_t channel_count; + uint16_t channels[20]; + + + if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0]))) { + warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count); + + for (unsigned i = 0; i < channel_count; i++) { + + int16_t val = channels[i]; + warnx("channel %u: %d 0x%03X", i, static_cast(val), static_cast(val)); + } + } + } + + if (ret == EOF) { + warnx("Test finished, reached end of file"); + + } else { + warnx("Test aborted, errno: %d", ret); + } + +} diff --git a/src/drivers/sf0x/sf0x_parser.cpp b/src/drivers/sf0x/sf0x_parser.cpp new file mode 100644 index 000000000..8e73b0ad3 --- /dev/null +++ b/src/drivers/sf0x/sf0x_parser.cpp @@ -0,0 +1,155 @@ +/**************************************************************************** + * + * 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 sf0x_parser.cpp + * @author Lorenz Meier + * + * Driver for the Lightware SF0x laser rangefinder series + */ + +#include "sf0x_parser.h" +#include +#include + +//#define SF0X_DEBUG + +#ifdef SF0X_DEBUG +#include + +const char *parser_state[] = { + "0_UNSYNC", + "1_SYNC", + "2_GOT_DIGIT0", + "3_GOT_DOT", + "4_GOT_DIGIT1", + "5_GOT_DIGIT2", + "6_GOT_CARRIAGE_RETURN" +}; +#endif + +int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist) +{ + int ret = -1; + char *end; + + switch (*state) { + case SF0X_PARSE_STATE0_UNSYNC: + if (c == '\n') { + *state = SF0X_PARSE_STATE1_SYNC; + (*parserbuf_index) = 0; + } + + break; + + case SF0X_PARSE_STATE1_SYNC: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE2_GOT_DIGIT0; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + } + + break; + + case SF0X_PARSE_STATE2_GOT_DIGIT0: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE2_GOT_DIGIT0; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + } else if (c == '.') { + *state = SF0X_PARSE_STATE3_GOT_DOT; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + + break; + + case SF0X_PARSE_STATE3_GOT_DOT: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE4_GOT_DIGIT1; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + + break; + + case SF0X_PARSE_STATE4_GOT_DIGIT1: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE5_GOT_DIGIT2; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + + break; + + case SF0X_PARSE_STATE5_GOT_DIGIT2: + if (c == '\r') { + *state = SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN; + + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + + break; + + case SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN: + if (c == '\n') { + parserbuf[*parserbuf_index] = '\0'; + *dist = strtod(parserbuf, &end); + *state = SF0X_PARSE_STATE1_SYNC; + *parserbuf_index = 0; + ret = 0; + + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + + break; + } + +#ifdef SF0X_DEBUG + printf("state: SF0X_PARSE_STATE%s\n", parser_state[*state]); +#endif + + return ret; +} \ No newline at end of file diff --git a/src/drivers/sf0x/sf0x_parser.h b/src/drivers/sf0x/sf0x_parser.h new file mode 100644 index 000000000..20892d50e --- /dev/null +++ b/src/drivers/sf0x/sf0x_parser.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * 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 sf0x_parser.cpp + * @author Lorenz Meier + * + * Declarations of parser for the Lightware SF0x laser rangefinder series + */ + +enum SF0X_PARSE_STATE { + SF0X_PARSE_STATE0_UNSYNC = 0, + SF0X_PARSE_STATE1_SYNC, + SF0X_PARSE_STATE2_GOT_DIGIT0, + SF0X_PARSE_STATE3_GOT_DOT, + SF0X_PARSE_STATE4_GOT_DIGIT1, + SF0X_PARSE_STATE5_GOT_DIGIT2, + SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN +}; + +int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist); \ No newline at end of file diff --git a/src/lib/rc/module.mk b/src/lib/rc/module.mk new file mode 100644 index 000000000..e089c6965 --- /dev/null +++ b/src/lib/rc/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Yuntec ST24 transmitter protocol decoder +# + +SRCS = st24.c + +MAXOPTIMIZATION = -Os diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c new file mode 100644 index 000000000..e8a791b8f --- /dev/null +++ b/src/lib/rc/st24.c @@ -0,0 +1,253 @@ +/**************************************************************************** + * + * 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 st24.h + * + * RC protocol implementation for Yuneec ST24 transmitter. + * + * @author Lorenz Meier + */ + +#include +#include +#include "st24.h" + +enum ST24_DECODE_STATE { + ST24_DECODE_STATE_UNSYNCED = 0, + ST24_DECODE_STATE_GOT_STX1, + ST24_DECODE_STATE_GOT_STX2, + ST24_DECODE_STATE_GOT_LEN, + ST24_DECODE_STATE_GOT_TYPE, + ST24_DECODE_STATE_GOT_DATA +}; + +const char *decode_states[] = {"UNSYNCED", + "GOT_STX1", + "GOT_STX2", + "GOT_LEN", + "GOT_TYPE", + "GOT_DATA" + }; + +/* define range mapping here, -+100% -> 1000..2000 */ +#define ST24_RANGE_MIN 0.0f +#define ST24_RANGE_MAX 4096.0f + +#define ST24_TARGET_MIN 1000.0f +#define ST24_TARGET_MAX 2000.0f + +/* pre-calculate the floating point stuff as far as possible at compile time */ +#define ST24_SCALE_FACTOR ((ST24_TARGET_MAX - ST24_TARGET_MIN) / (ST24_RANGE_MAX - ST24_RANGE_MIN)) +#define ST24_SCALE_OFFSET (int)(ST24_TARGET_MIN - (ST24_SCALE_FACTOR * ST24_RANGE_MIN + 0.5f)) + +static enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED; +static unsigned _rxlen; + +static ReceiverFcPacket _rxpacket; + +uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) +{ + uint8_t i, crc ; + crc = 0; + + while (len--) { + for (i = 0x80; i != 0; i >>= 1) { + if ((crc & 0x80) != 0) { + crc <<= 1; + crc ^= 0x07; + + } else { + crc <<= 1; + } + + if ((*ptr & i) != 0) { + crc ^= 0x07; + } + } + + ptr++; + } + + return (crc); +} + + +int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, + uint16_t max_chan_count) +{ + + int ret = 1; + + switch (_decode_state) { + case ST24_DECODE_STATE_UNSYNCED: + if (byte == ST24_STX1) { + _decode_state = ST24_DECODE_STATE_GOT_STX1; + } else { + ret = 3; + } + + break; + + case ST24_DECODE_STATE_GOT_STX1: + if (byte == ST24_STX2) { + _decode_state = ST24_DECODE_STATE_GOT_STX2; + + } else { + _decode_state = ST24_DECODE_STATE_UNSYNCED; + } + + break; + + case ST24_DECODE_STATE_GOT_STX2: + + /* ensure no data overflow failure or hack is possible */ + if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) { + _rxpacket.length = byte; + _rxlen = 0; + _decode_state = ST24_DECODE_STATE_GOT_LEN; + + } else { + _decode_state = ST24_DECODE_STATE_UNSYNCED; + } + + break; + + case ST24_DECODE_STATE_GOT_LEN: + _rxpacket.type = byte; + _rxlen++; + _decode_state = ST24_DECODE_STATE_GOT_TYPE; + break; + + case ST24_DECODE_STATE_GOT_TYPE: + _rxpacket.st24_data[_rxlen - 1] = byte; + _rxlen++; + + if (_rxlen == (_rxpacket.length - 1)) { + _decode_state = ST24_DECODE_STATE_GOT_DATA; + } + + break; + + case ST24_DECODE_STATE_GOT_DATA: + _rxpacket.crc8 = byte; + _rxlen++; + + if (st24_common_crc8((uint8_t *) & (_rxpacket.length), _rxlen) == _rxpacket.crc8) { + + ret = 0; + + /* decode the actual packet */ + + switch (_rxpacket.type) { + + case ST24_PACKET_TYPE_CHANNELDATA12: { + ChannelData12 *d = (ChannelData12 *)_rxpacket.st24_data; + + *rssi = d->rssi; + *rx_count = d->packet_count; + + /* this can lead to rounding of the strides */ + *channel_count = (max_chan_count < 12) ? max_chan_count : 12; + + unsigned stride_count = (*channel_count * 3) / 2; + unsigned chan_index = 0; + + for (unsigned i = 0; i < stride_count; i += 3) { + channels[chan_index] = ((uint16_t)d->channel[i] << 4); + channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + + channels[chan_index] = ((uint16_t)d->channel[i + 2]); + channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + } + } + break; + + case ST24_PACKET_TYPE_CHANNELDATA24: { + ChannelData24 *d = (ChannelData24 *)&_rxpacket.st24_data; + + *rssi = d->rssi; + *rx_count = d->packet_count; + + /* this can lead to rounding of the strides */ + *channel_count = (max_chan_count < 24) ? max_chan_count : 24; + + unsigned stride_count = (*channel_count * 3) / 2; + unsigned chan_index = 0; + + for (unsigned i = 0; i < stride_count; i += 3) { + channels[chan_index] = ((uint16_t)d->channel[i] << 4); + channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + + channels[chan_index] = ((uint16_t)d->channel[i + 2]); + channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + } + } + break; + + case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: { + + // ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket.st24_data; + /* we silently ignore this data for now, as it is unused */ + ret = 2; + } + break; + + default: + ret = 2; + break; + } + + } else { + /* decoding failed */ + ret = 4; + } + + _decode_state = ST24_DECODE_STATE_UNSYNCED; + break; + } + + return ret; +} diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h new file mode 100644 index 000000000..454234601 --- /dev/null +++ b/src/lib/rc/st24.h @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * 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 st24.h + * + * RC protocol definition for Yuneec ST24 transmitter + * + * @author Lorenz Meier + */ + +#pragma once + +#include + +__BEGIN_DECLS + +#define ST24_DATA_LEN_MAX 64 +#define ST24_STX1 0x55 +#define ST24_STX2 0x55 + +enum ST24_PACKET_TYPE { + ST24_PACKET_TYPE_CHANNELDATA12 = 0, + ST24_PACKET_TYPE_CHANNELDATA24, + ST24_PACKET_TYPE_TRANSMITTERGPSDATA +}; + +#pragma pack(push, 1) +typedef struct { + uint8_t header1; ///< 0x55 for a valid packet + uint8_t header2; ///< 0x55 for a valid packet + uint8_t length; ///< length includes type, data, and crc = sizeof(type)+sizeof(data[payload_len])+sizeof(crc8) + uint8_t type; ///< from enum ST24_PACKET_TYPE + uint8_t st24_data[ST24_DATA_LEN_MAX]; + uint8_t crc8; ///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data +} ReceiverFcPacket; + +/** + * RC Channel data (12 channels). + * + * This is incoming from the ST24 + */ +typedef struct { + uint16_t t; ///< packet counter or clock + uint8_t rssi; ///< signal strength + uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate) + uint8_t channel[18]; ///< channel data, 12 channels (12 bit numbers) +} ChannelData12; + +/** + * RC Channel data (12 channels). + * + */ +typedef struct { + uint16_t t; ///< packet counter or clock + uint8_t rssi; ///< signal strength + uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate) + uint8_t channel[36]; ///< channel data, 24 channels (12 bit numbers) +} ChannelData24; + +/** + * Telemetry packet + * + * This is outgoing to the ST24 + * + * imuStatus: + * 8 bit total + * bits 0-2 for status + * - value 0 is FAILED + * - value 1 is INITIALIZING + * - value 2 is RUNNING + * - values 3 through 7 are reserved + * bits 3-7 are status for sensors (0 or 1) + * - mpu6050 + * - accelerometer + * - primary gyro x + * - primary gyro y + * - primary gyro z + * + * pressCompassStatus + * 8 bit total + * bits 0-3 for compass status + * - value 0 is FAILED + * - value 1 is INITIALIZING + * - value 2 is RUNNING + * - value 3 - 15 are reserved + * bits 4-7 for pressure status + * - value 0 is FAILED + * - value 1 is INITIALIZING + * - value 2 is RUNNING + * - value 3 - 15 are reserved + * + */ +typedef struct { + uint16_t t; ///< packet counter or clock + int32_t lat; ///< lattitude (degrees) +/- 90 deg + int32_t lon; ///< longitude (degrees) +/- 180 deg + int32_t alt; ///< 0.01m resolution, altitude (meters) + int16_t vx, vy, vz; ///< velocity 0.01m res, +/-320.00 North-East- Down + uint8_t nsat; /// + * @author Julian Oes + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +/** + * bottle_drop app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int bottle_drop_main(int argc, char *argv[]); + +class BottleDrop +{ +public: + /** + * Constructor + */ + BottleDrop(); + + /** + * Destructor, also kills task. + */ + ~BottleDrop(); + + /** + * Start the task. + * + * @return OK on success. + */ + int start(); + + /** + * Display status. + */ + void status(); + + void open_bay(); + void close_bay(); + void drop(); + void lock_release(); + +private: + bool _task_should_exit; /**< if true, task should exit */ + int _main_task; /**< handle for task */ + int _mavlink_fd; + + int _command_sub; + int _wind_estimate_sub; + struct vehicle_command_s _command; + struct vehicle_global_position_s _global_pos; + map_projection_reference_s ref; + + orb_advert_t _actuator_pub; + struct actuator_controls_s _actuators; + + bool _drop_approval; + hrt_abstime _doors_opened; + hrt_abstime _drop_time; + + float _alt_clearance; + + struct position_s { + double lat; ///< degrees + double lon; ///< degrees + float alt; ///< m + } _target_position, _drop_position; + + enum DROP_STATE { + DROP_STATE_INIT = 0, + DROP_STATE_TARGET_VALID, + DROP_STATE_TARGET_SET, + DROP_STATE_BAY_OPEN, + DROP_STATE_DROPPED, + DROP_STATE_BAY_CLOSED + } _drop_state; + + struct mission_s _onboard_mission; + orb_advert_t _onboard_mission_pub; + + void task_main(); + + void handle_command(struct vehicle_command_s *cmd); + + void answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result); + + /** + * Set the actuators + */ + int actuators_publish(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); +}; + +namespace bottle_drop +{ +BottleDrop *g_bottle_drop; +} + +BottleDrop::BottleDrop() : + + _task_should_exit(false), + _main_task(-1), + _mavlink_fd(-1), + _command_sub(-1), + _wind_estimate_sub(-1), + _command {}, + _global_pos {}, + ref {}, + _actuator_pub(-1), + _actuators {}, + _drop_approval(false), + _doors_opened(0), + _drop_time(0), + _alt_clearance(70.0f), + _target_position {}, + _drop_position {}, + _drop_state(DROP_STATE_INIT), + _onboard_mission {}, + _onboard_mission_pub(-1) +{ +} + +BottleDrop::~BottleDrop() +{ + if (_main_task != -1) { + + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_main_task); + break; + } + } while (_main_task != -1); + } + + bottle_drop::g_bottle_drop = nullptr; +} + +int +BottleDrop::start() +{ + ASSERT(_main_task == -1); + + /* start the task */ + _main_task = task_spawn_cmd("bottle_drop", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT + 15, + 2048, + (main_t)&BottleDrop::task_main_trampoline, + nullptr); + + if (_main_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + + +void +BottleDrop::status() +{ + warnx("drop state: %d", _drop_state); +} + +void +BottleDrop::open_bay() +{ + _actuators.control[0] = -1.0f; + _actuators.control[1] = 1.0f; + + if (_doors_opened == 0) { + _doors_opened = hrt_absolute_time(); + } + warnx("open doors"); + + actuators_publish(); + + usleep(500 * 1000); +} + +void +BottleDrop::close_bay() +{ + // closed door and locked survival kit + _actuators.control[0] = 1.0f; + _actuators.control[1] = -1.0f; + + _doors_opened = 0; + + actuators_publish(); + + // delay until the bay is closed + usleep(500 * 1000); +} + +void +BottleDrop::drop() +{ + + // update drop actuator, wait 0.5s until the doors are open before dropping + hrt_abstime starttime = hrt_absolute_time(); + + // force the door open if we have to + if (_doors_opened == 0) { + open_bay(); + warnx("bay not ready, forced open"); + } + + while (hrt_elapsed_time(&_doors_opened) < 500 * 1000 && hrt_elapsed_time(&starttime) < 2000000) { + usleep(50000); + warnx("delayed by door!"); + } + + _actuators.control[2] = 1.0f; + + _drop_time = hrt_absolute_time(); + actuators_publish(); + + warnx("dropping now"); + + // Give it time to drop + usleep(1000 * 1000); +} + +void +BottleDrop::lock_release() +{ + _actuators.control[2] = -1.0f; + actuators_publish(); + + warnx("closing release"); +} + +int +BottleDrop::actuators_publish() +{ + _actuators.timestamp = hrt_absolute_time(); + + // lazily publish _actuators only once available + if (_actuator_pub > 0) { + return orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators); + + } else { + _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators); + if (_actuator_pub > 0) { + return OK; + } else { + return -1; + } + } +} + +void +BottleDrop::task_main() +{ + + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(_mavlink_fd, "[bottle_drop] started"); + + _command_sub = orb_subscribe(ORB_ID(vehicle_command)); + _wind_estimate_sub = orb_subscribe(ORB_ID(wind_estimate)); + + bool updated = false; + + float z_0; // ground properties + float turn_radius; // turn radius of the UAV + float precision; // Expected precision of the UAV + + float ground_distance = _alt_clearance; // Replace by closer estimate in loop + + // constant + float g = CONSTANTS_ONE_G; // constant of gravity [m/s^2] + float m = 0.5f; // mass of bottle [kg] + float rho = 1.2f; // air density [kg/m^3] + float A = ((0.063f * 0.063f) / 4.0f * M_PI_F); // Bottle cross section [m^2] + float dt_freefall_prediction = 0.01f; // step size of the free fall prediction [s] + + // Has to be estimated by experiment + float cd = 0.86f; // Drag coefficient for a cylinder with a d/l ratio of 1/3 [] + float t_signal = + 0.084f; // Time span between sending the signal and the bottle top reaching level height with the bottom of the plane [s] + float t_door = + 0.7f; // The time the system needs to open the door + safety, is also the time the palyload needs to safely escape the shaft [s] + + + // Definition + float h_0; // height over target + float az; // acceleration in z direction[m/s^2] + float vz; // velocity in z direction [m/s] + float z; // fallen distance [m] + float h; // height over target [m] + float ax; // acceleration in x direction [m/s^2] + float vx; // ground speed in x direction [m/s] + float x; // traveled distance in x direction [m] + float vw; // wind speed [m/s] + float vrx; // relative velocity in x direction [m/s] + float v; // relative speed vector [m/s] + float Fd; // Drag force [N] + float Fdx; // Drag force in x direction [N] + float Fdz; // Drag force in z direction [N] + float x_drop, y_drop; // coordinates of the drop point in reference to the target (projection of NED) + float x_t, y_t; // coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED) + float x_l, y_l; // local position in projected coordinates + float x_f, y_f; // to-be position of the UAV after dt_runs seconds in projected coordinates + double x_f_NED, y_f_NED; // to-be position of the UAV after dt_runs seconds in NED + float distance_open_door; // The distance the UAV travels during its doors open [m] + float approach_error = 0.0f; // The error in radians between current ground vector and desired ground vector + float distance_real = 0; // The distance between the UAVs position and the drop point [m] + float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m] + + unsigned counter = 0; + + param_t param_gproperties = param_find("BD_GPROPERTIES"); + param_t param_turn_radius = param_find("BD_TURNRADIUS"); + param_t param_precision = param_find("BD_PRECISION"); + param_t param_cd = param_find("BD_OBJ_CD"); + param_t param_mass = param_find("BD_OBJ_MASS"); + param_t param_surface = param_find("BD_OBJ_SURFACE"); + + + param_get(param_precision, &precision); + param_get(param_turn_radius, &turn_radius); + param_get(param_gproperties, &z_0); + param_get(param_cd, &cd); + param_get(param_mass, &m); + param_get(param_surface, &A); + + int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + + struct parameter_update_s update; + memset(&update, 0, sizeof(update)); + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + + struct mission_item_s flight_vector_s {}; + struct mission_item_s flight_vector_e {}; + + flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT; + flight_vector_s.acceptance_radius = 50; // TODO: make parameter + flight_vector_s.autocontinue = true; + flight_vector_s.altitude_is_relative = false; + + flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT; + flight_vector_e.acceptance_radius = 50; // TODO: make parameter + flight_vector_e.autocontinue = true; + flight_vector_s.altitude_is_relative = false; + + struct wind_estimate_s wind; + + // wakeup source(s) + struct pollfd fds[1]; + + // Setup of loop + fds[0].fd = _command_sub; + fds[0].events = POLLIN; + + // Whatever state the bay is in, we want it closed on startup + lock_release(); + close_bay(); + + while (!_task_should_exit) { + + /* wait for up to 100ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50); + + /* 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); + continue; + } + + /* vehicle commands updated */ + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_command), _command_sub, &_command); + handle_command(&_command); + } + + orb_check(vehicle_global_position_sub, &updated); + if (updated) { + /* copy global position */ + orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); + } + + if (_global_pos.timestamp == 0) { + continue; + } + + const unsigned sleeptime_us = 9500; + + hrt_abstime last_run = hrt_absolute_time(); + float dt_runs = sleeptime_us / 1e6f; + + // switch to faster updates during the drop + while (_drop_state > DROP_STATE_INIT) { + + // Get wind estimate + orb_check(_wind_estimate_sub, &updated); + if (updated) { + orb_copy(ORB_ID(wind_estimate), _wind_estimate_sub, &wind); + } + + // Get vehicle position + orb_check(vehicle_global_position_sub, &updated); + if (updated) { + // copy global position + orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); + } + + // Get parameter updates + orb_check(parameter_update_sub, &updated); + if (updated) { + // copy global position + orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); + + // update all parameters + param_get(param_gproperties, &z_0); + param_get(param_turn_radius, &turn_radius); + param_get(param_precision, &precision); + } + + orb_check(_command_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_command), _command_sub, &_command); + handle_command(&_command); + } + + + float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east); + float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e); + ground_distance = _global_pos.alt - _target_position.alt; + + // Distance to drop position and angle error to approach vector + // are relevant in all states greater than target valid (which calculates these positions) + if (_drop_state > DROP_STATE_TARGET_VALID) { + distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon)); + + float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n); + float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); + + approach_error = _wrap_pi(ground_direction - approach_direction); + + if (counter % 90 == 0) { + mavlink_log_info(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error)); + } + } + + switch (_drop_state) { + + case DROP_STATE_TARGET_VALID: + { + + az = g; // acceleration in z direction[m/s^2] + vz = 0; // velocity in z direction [m/s] + z = 0; // fallen distance [m] + h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m] + h = h_0; // height over target [m] + ax = 0; // acceleration in x direction [m/s^2] + vx = groundspeed_body;// XXX project // ground speed in x direction [m/s] + x = 0; // traveled distance in x direction [m] + vw = 0; // wind speed [m/s] + vrx = 0; // relative velocity in x direction [m/s] + v = groundspeed_body; // relative speed vector [m/s] + Fd = 0; // Drag force [N] + Fdx = 0; // Drag force in x direction [N] + Fdz = 0; // Drag force in z direction [N] + + + // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x + while (h > 0.05f) { + // z-direction + vz = vz + az * dt_freefall_prediction; + z = z + vz * dt_freefall_prediction; + h = h_0 - z; + + // x-direction + vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0); + vx = vx + ax * dt_freefall_prediction; + x = x + vx * dt_freefall_prediction; + vrx = vx + vw; + + // drag force + v = sqrtf(vz * vz + vrx * vrx); + Fd = 0.5f * rho * A * cd * (v * v); + Fdx = Fd * vrx / v; + Fdz = Fd * vz / v; + + // acceleration + az = g - Fdz / m; + ax = -Fdx / m; + } + + // compute drop vector + x = groundspeed_body * t_signal + x; + + x_t = 0.0f; + y_t = 0.0f; + + float wind_direction_n, wind_direction_e; + + if (windspeed_norm < 0.5f) { // If there is no wind, an arbitrarily direction is chosen + wind_direction_n = 1.0f; + wind_direction_e = 0.0f; + + } else { + wind_direction_n = wind.windspeed_north / windspeed_norm; + wind_direction_e = wind.windspeed_east / windspeed_norm; + } + + x_drop = x_t + x * wind_direction_n; + y_drop = y_t + x * wind_direction_e; + map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon); + _drop_position.alt = _target_position.alt + _alt_clearance; + + // Compute flight vector + map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_e, + &(flight_vector_s.lat), &(flight_vector_s.lon)); + flight_vector_s.altitude = _drop_position.alt; + map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_n, y_drop - turn_radius * wind_direction_e, + &flight_vector_e.lat, &flight_vector_e.lon); + flight_vector_e.altitude = _drop_position.alt; + + // Save WPs in datamanager + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) { + warnx("ERROR: could not save onboard WP"); + } + + if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) { + warnx("ERROR: could not save onboard WP"); + } + + _onboard_mission.count = 2; + _onboard_mission.current_seq = 0; + + if (_onboard_mission_pub > 0) { + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + + } else { + _onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission); + } + + float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); + mavlink_log_critical(_mavlink_fd, "position set, approach heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_direction + M_PI_F)); + + _drop_state = DROP_STATE_TARGET_SET; + } + break; + + case DROP_STATE_TARGET_SET: + { + float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); + + if (distance_wp2 < distance_real) { + _onboard_mission.current_seq = 0; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + } else { + + // We're close enough - open the bay + distance_open_door = math::max(10.0f, 3.0f * fabsf(t_door * groundspeed_body)); + + if (isfinite(distance_real) && distance_real < distance_open_door && + fabsf(approach_error) < math::radians(20.0f)) { + open_bay(); + _drop_state = DROP_STATE_BAY_OPEN; + mavlink_log_info(_mavlink_fd, "#audio: opening bay"); + } + } + } + break; + + case DROP_STATE_BAY_OPEN: + { + if (_drop_approval) { + map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l); + x_f = x_l + _global_pos.vel_n * dt_runs; + y_f = y_l + _global_pos.vel_e * dt_runs; + map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); + future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon); + + if (isfinite(distance_real) && + (distance_real < precision) && ((distance_real < future_distance))) { + drop(); + _drop_state = DROP_STATE_DROPPED; + mavlink_log_info(_mavlink_fd, "#audio: payload dropped"); + } else { + + float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); + + if (distance_wp2 < distance_real) { + _onboard_mission.current_seq = 0; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + } + } + } + } + break; + + case DROP_STATE_DROPPED: + /* 2s after drop, reset and close everything again */ + if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) { + _drop_state = DROP_STATE_INIT; + _drop_approval = false; + lock_release(); + close_bay(); + mavlink_log_info(_mavlink_fd, "#audio: closing bay"); + + // remove onboard mission + _onboard_mission.current_seq = -1; + _onboard_mission.count = 0; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + } + break; + } + + counter++; + + // update_actuators(); + + // run at roughly 100 Hz + usleep(sleeptime_us); + + dt_runs = hrt_elapsed_time(&last_run) / 1e6f; + last_run = hrt_absolute_time(); + } + } + + warnx("exiting."); + + _main_task = -1; + _exit(0); +} + +void +BottleDrop::handle_command(struct vehicle_command_s *cmd) +{ + switch (cmd->command) { + case VEHICLE_CMD_CUSTOM_0: + /* + * param1 and param2 set to 1: open and drop + * param1 set to 1: open + * else: close (and don't drop) + */ + if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) { + open_bay(); + drop(); + mavlink_log_info(_mavlink_fd, "#audio: drop bottle"); + + } else if (cmd->param1 > 0.5f) { + open_bay(); + mavlink_log_info(_mavlink_fd, "#audio: opening bay"); + + } else { + lock_release(); + close_bay(); + mavlink_log_info(_mavlink_fd, "#audio: closing bay"); + } + + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + break; + + case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY: + + switch ((int)(cmd->param1 + 0.5f)) { + case 0: + _drop_approval = false; + mavlink_log_info(_mavlink_fd, "#audio: got drop position, no approval"); + break; + + case 1: + _drop_approval = true; + mavlink_log_info(_mavlink_fd, "#audio: got drop position and approval"); + break; + + default: + _drop_approval = false; + warnx("param1 val unknown"); + break; + } + + // XXX check all fields (2-3) + _alt_clearance = cmd->param4; + _target_position.lat = cmd->param5; + _target_position.lon = cmd->param6; + _target_position.alt = cmd->param7; + _drop_state = DROP_STATE_TARGET_VALID; + mavlink_log_info(_mavlink_fd, "got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat, + (double)_target_position.lon, (double)_target_position.alt); + map_projection_init(&ref, _target_position.lat, _target_position.lon); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + break; + + case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: + + if (cmd->param1 < 0) { + + // Clear internal states + _drop_approval = false; + _drop_state = DROP_STATE_INIT; + + // Abort if mission is present + _onboard_mission.current_seq = -1; + + if (_onboard_mission_pub > 0) { + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + } + + } else { + switch ((int)(cmd->param1 + 0.5f)) { + case 0: + _drop_approval = false; + break; + + case 1: + _drop_approval = true; + mavlink_log_info(_mavlink_fd, "#audio: got drop approval"); + break; + + default: + _drop_approval = false; + break; + // XXX handle other values + } + } + + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + break; + + default: + break; + } +} + +void +BottleDrop::answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result) +{ + switch (result) { + case VEHICLE_CMD_RESULT_ACCEPTED: + break; + + case VEHICLE_CMD_RESULT_DENIED: + mavlink_log_critical(_mavlink_fd, "#audio: command denied: %u", cmd->command); + break; + + case VEHICLE_CMD_RESULT_FAILED: + mavlink_log_critical(_mavlink_fd, "#audio: command failed: %u", cmd->command); + break; + + case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: + mavlink_log_critical(_mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command); + break; + + case VEHICLE_CMD_RESULT_UNSUPPORTED: + mavlink_log_critical(_mavlink_fd, "#audio: command unsupported: %u", cmd->command); + break; + + default: + break; + } +} + +void +BottleDrop::task_main_trampoline(int argc, char *argv[]) +{ + bottle_drop::g_bottle_drop->task_main(); +} + +static void usage() +{ + errx(1, "usage: bottle_drop {start|stop|status}"); +} + +int bottle_drop_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + } + + if (!strcmp(argv[1], "start")) { + + if (bottle_drop::g_bottle_drop != nullptr) { + errx(1, "already running"); + } + + bottle_drop::g_bottle_drop = new BottleDrop; + + if (bottle_drop::g_bottle_drop == nullptr) { + errx(1, "alloc failed"); + } + + if (OK != bottle_drop::g_bottle_drop->start()) { + delete bottle_drop::g_bottle_drop; + bottle_drop::g_bottle_drop = nullptr; + err(1, "start failed"); + } + + return 0; + } + + if (bottle_drop::g_bottle_drop == nullptr) { + errx(1, "not running"); + } + + if (!strcmp(argv[1], "stop")) { + delete bottle_drop::g_bottle_drop; + bottle_drop::g_bottle_drop = nullptr; + + } else if (!strcmp(argv[1], "status")) { + bottle_drop::g_bottle_drop->status(); + + } else if (!strcmp(argv[1], "drop")) { + bottle_drop::g_bottle_drop->drop(); + + } else if (!strcmp(argv[1], "open")) { + bottle_drop::g_bottle_drop->open_bay(); + + } else if (!strcmp(argv[1], "close")) { + bottle_drop::g_bottle_drop->close_bay(); + + } else if (!strcmp(argv[1], "lock")) { + bottle_drop::g_bottle_drop->lock_release(); + + } else { + usage(); + } + + return 0; +} diff --git a/src/modules/bottle_drop/bottle_drop_params.c b/src/modules/bottle_drop/bottle_drop_params.c new file mode 100644 index 000000000..51ebfb9a1 --- /dev/null +++ b/src/modules/bottle_drop/bottle_drop_params.c @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * 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 bottle_drop_params.c + * Bottle drop parameters + * + * @author Dominik Juchli + */ + +#include +#include + +/** + * Ground drag property + * + * This parameter encodes the ground drag coefficient and the corresponding + * decrease in wind speed from the plane altitude to ground altitude. + * + * @unit unknown + * @min 0.001 + * @max 0.1 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_GPROPERTIES, 0.03f); + +/** + * Plane turn radius + * + * The planes known minimal turn radius - use a higher value + * to make the plane maneuver more distant from the actual drop + * position. This is to ensure the wings are level during the drop. + * + * @unit meter + * @min 30.0 + * @max 500.0 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 120.0f); + +/** + * Drop precision + * + * If the system is closer than this distance on passing over the + * drop position, it will release the payload. This is a safeguard + * to prevent a drop out of the required accuracy. + * + * @unit meter + * @min 1.0 + * @max 80.0 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_PRECISION, 30.0f); + +/** + * Payload drag coefficient of the dropped object + * + * The drag coefficient (cd) is the typical drag + * constant for air. It is in general object specific, + * but the closest primitive shape to the actual object + * should give good results: + * http://en.wikipedia.org/wiki/Drag_coefficient + * + * @unit meter + * @min 0.08 + * @max 1.5 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_OBJ_CD, 0.1f); + +/** + * Payload mass + * + * A typical small toy ball: + * 0.025 kg + * + * OBC water bottle: + * 0.6 kg + * + * @unit kilogram + * @min 0.001 + * @max 5.0 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_OBJ_MASS, 0.6f); + +/** + * Payload front surface area + * + * A typical small toy ball: + * (0.045 * 0.045) / 4.0 * pi = 0.001590 m^2 + * + * OBC water bottle: + * (0.063 * 0.063) / 4.0 * pi = 0.003117 m^2 + * + * @unit m^2 + * @min 0.001 + * @max 0.5 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_OBJ_SURFACE, 0.00311724531f); diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk new file mode 100644 index 000000000..6b18fff55 --- /dev/null +++ b/src/modules/bottle_drop/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Daemon application +# + +MODULE_COMMAND = bottle_drop + +SRCS = bottle_drop.cpp \ + bottle_drop_params.c diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp new file mode 100644 index 000000000..d4270b153 --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -0,0 +1,283 @@ +/* + * mc_att_control_base.cpp + * + * Created on: Sep 25, 2014 + * Author: roman + */ + +#include "mc_att_control_base.h" +#include +#include + +#ifdef CONFIG_ARCH_ARM +#else +#include +using namespace std; +#endif + +MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : + + _task_should_exit(false), _control_task(-1), + + _actuators_0_circuit_breaker_enabled(false), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) + +{ + memset(&_v_att, 0, sizeof(_v_att)); + memset(&_v_att_sp, 0, sizeof(_v_att_sp)); + memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); + memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); + memset(&_v_control_mode, 0, sizeof(_v_control_mode)); + memset(&_actuators, 0, sizeof(_actuators)); + memset(&_armed, 0, sizeof(_armed)); + + _params.att_p.zero(); + _params.rate_p.zero(); + _params.rate_i.zero(); + _params.rate_d.zero(); + _params.yaw_ff = 0.0f; + _params.yaw_rate_max = 0.0f; + _params.man_roll_max = 0.0f; + _params.man_pitch_max = 0.0f; + _params.man_yaw_max = 0.0f; + _params.acro_rate_max.zero(); + + _rates_prev.zero(); + _rates_sp.zero(); + _rates_int.zero(); + _thrust_sp = 0.0f; + _att_control.zero(); + + _I.identity(); +} + +MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() { +} + +void MulticopterAttitudeControlBase::vehicle_attitude_setpoint_poll() { +} + +void MulticopterAttitudeControlBase::control_attitude(float dt) { + float yaw_sp_move_rate = 0.0f; + bool publish_att_sp = false; + + if (_v_control_mode.flag_control_manual_enabled) { + /* manual input, set or modify attitude setpoint */ + + if (_v_control_mode.flag_control_velocity_enabled + || _v_control_mode.flag_control_climb_rate_enabled) { + /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ + vehicle_attitude_setpoint_poll(); + } + + if (!_v_control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude stabilized mode */ + _v_att_sp.thrust = _manual_control_sp.z; + publish_att_sp = true; + } + + if (!_armed.armed) { + /* reset yaw setpoint when disarmed */ + _reset_yaw_sp = true; + } + + /* move yaw setpoint in all modes */ + if (_v_att_sp.thrust < 0.1f) { + // TODO + //if (_status.condition_landed) { + /* reset yaw setpoint if on ground */ + // reset_yaw_sp = true; + //} + } else { + /* move yaw setpoint */ + yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max; + _v_att_sp.yaw_body = _wrap_pi( + _v_att_sp.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.yaw_body - _v_att.yaw); + if (yaw_offs < -yaw_offs_max) { + _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max); + + } else if (yaw_offs > yaw_offs_max) { + _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); + } + _v_att_sp.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.yaw_body = _v_att.yaw; + _v_att_sp.R_valid = false; + publish_att_sp = true; + } + + if (!_v_control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + _v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max; + _v_att_sp.pitch_body = -_manual_control_sp.x + * _params.man_pitch_max; + _v_att_sp.R_valid = false; + publish_att_sp = true; + } + + } else { + /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ + vehicle_attitude_setpoint_poll(); + + /* reset yaw setpoint after non-manual control mode */ + _reset_yaw_sp = true; + } + + _thrust_sp = _v_att_sp.thrust; + + /* construct attitude setpoint rotation matrix */ + math::Matrix<3, 3> R_sp; + + if (_v_att_sp.R_valid) { + /* rotation matrix in _att_sp is valid, use it */ + R_sp.set(&_v_att_sp.R_body[0][0]); + + } else { + /* rotation matrix in _att_sp is not valid, use euler angles instead */ + R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, + _v_att_sp.yaw_body); + + /* copy rotation matrix back to setpoint struct */ + memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], + sizeof(_v_att_sp.R_body)); + _v_att_sp.R_valid = true; + } + +// /* 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); +// +// } else { +// _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), +// &_v_att_sp); +// } +// } + + /* rotation matrix for current state */ + math::Matrix<3, 3> R; + R.set(_v_att.R); + + /* all input data is ready, run controller itself */ + + /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ + math::Vector < 3 > R_z(R(0, 2), R(1, 2), R(2, 2)); + math::Vector < 3 > R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); + + /* axis and sin(angle) of desired rotation */ + math::Vector < 3 > e_R = R.transposed() * (R_z % R_sp_z); + + /* calculate angle error */ + float e_R_z_sin = e_R.length(); + float e_R_z_cos = R_z * R_sp_z; + + /* calculate weight for yaw control */ + float yaw_w = R_sp(2, 2) * R_sp(2, 2); + + /* calculate rotation matrix after roll/pitch only rotation */ + math::Matrix<3, 3> R_rp; + + if (e_R_z_sin > 0.0f) { + /* get axis-angle representation */ + float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); + math::Vector < 3 > e_R_z_axis = e_R / e_R_z_sin; + + e_R = e_R_z_axis * e_R_z_angle; + + /* cross product matrix for e_R_axis */ + math::Matrix<3, 3> e_R_cp; + e_R_cp.zero(); + e_R_cp(0, 1) = -e_R_z_axis(2); + e_R_cp(0, 2) = e_R_z_axis(1); + e_R_cp(1, 0) = e_R_z_axis(2); + e_R_cp(1, 2) = -e_R_z_axis(0); + e_R_cp(2, 0) = -e_R_z_axis(1); + e_R_cp(2, 1) = e_R_z_axis(0); + + /* rotation matrix for roll/pitch only rotation */ + R_rp = R + * (_I + e_R_cp * e_R_z_sin + + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + + } else { + /* zero roll/pitch rotation */ + R_rp = R; + } + + /* R_rp and R_sp has the same Z axis, calculate yaw error */ + math::Vector < 3 > R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); + math::Vector < 3 > R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); + e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; + + if (e_R_z_cos < 0.0f) { + /* for large thrust vector rotations use another rotation method: + * calculate angle and axis for R -> R_sp rotation directly */ + math::Quaternion q; + q.from_dcm(R.transposed() * R_sp); + math::Vector < 3 > e_R_d = q.imag(); + e_R_d.normalize(); + e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); + + /* use fusion of Z axis based rotation and direct rotation */ + float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; + e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; + } + + /* calculate angular rates setpoint */ + _rates_sp = _params.att_p.emult(e_R); + + /* limit yaw rate */ + _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, + _params.yaw_rate_max); + + /* feed forward yaw setpoint rate */ + _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; +} + +void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { + /* reset integral if disarmed */ + if (!_armed.armed) { + _rates_int.zero(); + } + + /* current body angular rates */ + math::Vector < 3 > rates; + rates(0) = _v_att.rollspeed; + rates(1) = _v_att.pitchspeed; + rates(2) = _v_att.yawspeed; + + /* angular rates error */ + math::Vector < 3 > rates_err = _rates_sp - rates; + _att_control = _params.rate_p.emult(rates_err) + + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; + _rates_prev = rates; + + /* update integral only if not saturated on low limit */ + if (_thrust_sp > MIN_TAKEOFF_THRUST) { + for (int i = 0; i < 3; i++) { + if (fabsf(_att_control(i)) < _thrust_sp) { + float rate_i = _rates_int(i) + + _params.rate_i(i) * rates_err(i) * dt; + + if (isfinite( + rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { + _rates_int(i) = rate_i; + } + } + } + } + +} diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h new file mode 100644 index 000000000..d75088b85 --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -0,0 +1,172 @@ +/* + * mc_att_control_base.h + * + * Created on: Sep 25, 2014 + * Author: roman + */ + +#ifndef MC_ATT_CONTROL_BASE_H_ +#define MC_ATT_CONTROL_BASE_H_ + +/* 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 + * + * 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 +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +//#include +#include + + +/** + * Multicopter attitude control app start / stop handling function + * + * @ingroup apps + */ + +#define YAW_DEADZONE 0.05f +#define MIN_TAKEOFF_THRUST 0.2f +#define RATES_I_LIMIT 0.3f + +class MulticopterAttitudeControlBase { +public: + /** + * Constructor + */ + MulticopterAttitudeControlBase(); + + /** + * Destructor, also kills the sensors task. + */ + ~MulticopterAttitudeControlBase(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + +protected: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _control_task; /**< task handle for sensor task */ + + + + + bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ + + struct vehicle_attitude_s _v_att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ + struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ + struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ + struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator controls */ + struct actuator_armed_s _armed; /**< actuator arming status */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + math::Vector<3> _rates_prev; /**< angular rates on previous step */ + math::Vector<3> _rates_sp; /**< angular rates setpoint */ + math::Vector<3> _rates_int; /**< angular rates integral error */ + float _thrust_sp; /**< thrust setpoint */ + math::Vector<3> _att_control; /**< attitude control vector */ + + math::Matrix<3, 3> _I; /**< identity matrix */ + + bool _reset_yaw_sp; /**< reset yaw setpoint flag */ + + + + struct { + math::Vector<3> att_p; /**< P gain for angular error */ + math::Vector<3> rate_p; /**< P gain for angular rate error */ + math::Vector<3> rate_i; /**< I gain for angular rate error */ + math::Vector<3> rate_d; /**< D gain for angular rate error */ + float yaw_ff; /**< yaw control feed-forward */ + float yaw_rate_max; /**< max yaw rate */ + + float man_roll_max; + float man_pitch_max; + float man_yaw_max; + math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ + } _params; + + + /** + * Attitude controller. + */ + //void control_attitude(float dt); + + /** + * Attitude rates controller. + */ + void control_attitude(float dt); + void control_attitude_rates(float dt); + + void vehicle_attitude_setpoint_poll(); //provisional + + +}; + +#endif /* MC_ATT_CONTROL_BASE_H_ */ diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp new file mode 100644 index 000000000..66f1c8c73 --- /dev/null +++ b/src/modules/navigator/datalinkloss.cpp @@ -0,0 +1,227 @@ +/**************************************************************************** + * + * 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 datalinkloss.cpp + * Helper class for Data Link Loss Mode according to the OBC rules + * + * @author Thomas Gubler + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "navigator.h" +#include "datalinkloss.h" + +#define DELAY_SIGMA 0.01f + +DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _param_commsholdwaittime(this, "CH_T"), + _param_commsholdlat(this, "CH_LAT"), + _param_commsholdlon(this, "CH_LON"), + _param_commsholdalt(this, "CH_ALT"), + _param_airfieldhomelat(this, "NAV_AH_LAT", false), + _param_airfieldhomelon(this, "NAV_AH_LON", false), + _param_airfieldhomealt(this, "NAV_AH_ALT", false), + _param_airfieldhomewaittime(this, "AH_T"), + _param_numberdatalinklosses(this, "N"), + _param_skipcommshold(this, "CHSK"), + _dll_state(DLL_STATE_NONE) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +DataLinkLoss::~DataLinkLoss() +{ +} + +void +DataLinkLoss::on_inactive() +{ + /* reset DLL state only if setpoint moved */ + if (!_navigator->get_can_loiter_at_sp()) { + _dll_state = DLL_STATE_NONE; + } +} + +void +DataLinkLoss::on_activation() +{ + _dll_state = DLL_STATE_NONE; + updateParams(); + advance_dll(); + set_dll_item(); +} + +void +DataLinkLoss::on_active() +{ + if (is_mission_item_reached()) { + updateParams(); + advance_dll(); + set_dll_item(); + } +} + +void +DataLinkLoss::set_dll_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + set_previous_pos_setpoint(); + _navigator->set_can_loiter_at_sp(false); + + switch (_dll_state) { + case DLL_STATE_FLYTOCOMMSHOLDWP: { + _mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7; + _mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _param_commsholdalt.get(); + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + break; + } + case DLL_STATE_FLYTOAIRFIELDHOMEWP: { + _mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7; + _mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _param_airfieldhomealt.get(); + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + _mission_item.time_inside = _param_airfieldhomewaittime.get() < 0.0f ? 0.0f : _param_airfieldhomewaittime.get(); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + break; + } + case DLL_STATE_TERMINATE: { + /* Request flight termination from the commander */ + _navigator->get_mission_result()->flight_termination = true; + _navigator->publish_mission_result(); + warnx("not switched to manual: request flight termination"); + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; + break; + } + default: + break; + } + + reset_mission_item_reached(); + + /* convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +DataLinkLoss::advance_dll() +{ + switch (_dll_state) { + case DLL_STATE_NONE: + /* Check the number of data link losses. If above home fly home directly */ + /* if number of data link losses limit is not reached fly to comms hold wp */ + if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { + warnx("%d data link losses, limit is %d, fly to airfield home", + _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home"); + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + } else { + if (!_param_skipcommshold.get()) { + warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold"); + _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + } else { + /* comms hold wp not active, fly to airfield home directly */ + warnx("Skipping comms hold wp. Flying directly to airfield home"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped"); + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + } + } + break; + case DLL_STATE_FLYTOCOMMSHOLDWP: + warnx("fly to airfield home"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home"); + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + break; + case DLL_STATE_FLYTOAIRFIELDHOMEWP: + _dll_state = DLL_STATE_TERMINATE; + warnx("time is up, state should have been changed manually by now"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating"); + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + break; + case DLL_STATE_TERMINATE: + warnx("dll end"); + _dll_state = DLL_STATE_END; + break; + + default: + break; + } +} diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h new file mode 100644 index 000000000..31e0e3907 --- /dev/null +++ b/src/modules/navigator/datalinkloss.h @@ -0,0 +1,98 @@ +/*************************************************************************** + * + * 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 datalinkloss.h + * Helper class for Data Link Loss Mode acording to the OBC rules + * + * @author Thomas Gubler + */ + +#ifndef NAVIGATOR_DATALINKLOSS_H +#define NAVIGATOR_DATALINKLOSS_H + +#include +#include + +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class DataLinkLoss : public MissionBlock +{ +public: + DataLinkLoss(Navigator *navigator, const char *name); + + ~DataLinkLoss(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + /* Params */ + control::BlockParamFloat _param_commsholdwaittime; + control::BlockParamInt _param_commsholdlat; // * 1e7 + control::BlockParamInt _param_commsholdlon; // * 1e7 + control::BlockParamFloat _param_commsholdalt; + control::BlockParamInt _param_airfieldhomelat; // * 1e7 + control::BlockParamInt _param_airfieldhomelon; // * 1e7 + control::BlockParamFloat _param_airfieldhomealt; + control::BlockParamFloat _param_airfieldhomewaittime; + control::BlockParamInt _param_numberdatalinklosses; + control::BlockParamInt _param_skipcommshold; + + enum DLLState { + DLL_STATE_NONE = 0, + DLL_STATE_FLYTOCOMMSHOLDWP = 1, + DLL_STATE_FLYTOAIRFIELDHOMEWP = 2, + DLL_STATE_TERMINATE = 3, + DLL_STATE_END = 4 + } _dll_state; + + /** + * Set the DLL item + */ + void set_dll_item(); + + /** + * Move to next DLL item + */ + void advance_dll(); + +}; +#endif diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c new file mode 100644 index 000000000..6780c0c88 --- /dev/null +++ b/src/modules/navigator/datalinkloss_params.c @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * 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 datalinkloss_params.c + * + * Parameters for DLL + * + * @author Thomas Gubler + */ + +#include + +#include + +/* + * Data Link Loss parameters, accessible via MAVLink + */ + +/** + * Comms hold wait time + * + * The amount of time in seconds the system should wait at the comms hold waypoint + * + * @unit seconds + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f); + +/** + * Comms hold Lat + * + * Latitude of comms hold waypoint + * + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120); + +/** + * Comms hold Lon + * + * Longitude of comms hold waypoint + * + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); + +/** + * Comms hold alt + * + * Altitude of comms hold waypoint + * + * @unit m + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f); + +/** + * Aifield hole wait time + * + * The amount of time in seconds the system should wait at the airfield home waypoint + * + * @unit seconds + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f); + +/** + * Number of allowed Datalink timeouts + * + * After more than this number of data link timeouts the aircraft returns home directly + * + * @group DLL + * @min 0 + * @max 1000 + */ +PARAM_DEFINE_INT32(NAV_DLL_N, 2); + +/** + * Skip comms hold wp + * + * If set to 1 the system will skip the comms hold wp on data link loss and will directly fly to + * airfield home + * + * @group DLL + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(NAV_DLL_CHSK, 0); diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp new file mode 100644 index 000000000..e007b208b --- /dev/null +++ b/src/modules/navigator/enginefailure.cpp @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * 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 enginefailure.cpp + * Helper class for a fixedwing engine failure mode + * + * @author Thomas Gubler + */ +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include "navigator.h" +#include "enginefailure.h" + +#define DELAY_SIGMA 0.01f + +EngineFailure::EngineFailure(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _ef_state(EF_STATE_NONE) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +EngineFailure::~EngineFailure() +{ +} + +void +EngineFailure::on_inactive() +{ + _ef_state = EF_STATE_NONE; +} + +void +EngineFailure::on_activation() +{ + _ef_state = EF_STATE_NONE; + advance_ef(); + set_ef_item(); +} + +void +EngineFailure::on_active() +{ + if (is_mission_item_reached()) { + advance_ef(); + set_ef_item(); + } +} + +void +EngineFailure::set_ef_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* make sure we have the latest params */ + updateParams(); + + set_previous_pos_setpoint(); + _navigator->set_can_loiter_at_sp(false); + + switch (_ef_state) { + case EF_STATE_LOITERDOWN: { + //XXX create mission item at ground (below?) here + + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude_is_relative = false; + //XXX setting altitude to a very low value, evaluate other options + _mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + break; + } + default: + break; + } + + reset_mission_item_reached(); + + /* convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +EngineFailure::advance_ef() +{ + switch (_ef_state) { + case EF_STATE_NONE: + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: Engine failure. Loitering down"); + _ef_state = EF_STATE_LOITERDOWN; + break; + default: + break; + } +} diff --git a/src/modules/navigator/enginefailure.h b/src/modules/navigator/enginefailure.h new file mode 100644 index 000000000..2c48c2fce --- /dev/null +++ b/src/modules/navigator/enginefailure.h @@ -0,0 +1,83 @@ +/*************************************************************************** + * + * 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 enginefailure.h + * Helper class for a fixedwing engine failure mode + * + * @author Thomas Gubler + */ + +#ifndef NAVIGATOR_ENGINEFAILURE_H +#define NAVIGATOR_ENGINEFAILURE_H + +#include +#include + +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class EngineFailure : public MissionBlock +{ +public: + EngineFailure(Navigator *navigator, const char *name); + + ~EngineFailure(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + enum EFState { + EF_STATE_NONE = 0, + EF_STATE_LOITERDOWN = 1, + } _ef_state; + + /** + * Set the DLL item + */ + void set_ef_item(); + + /** + * Move to next EF item + */ + void advance_ef(); + +}; +#endif diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp new file mode 100644 index 000000000..cd55f60b0 --- /dev/null +++ b/src/modules/navigator/gpsfailure.cpp @@ -0,0 +1,178 @@ +/**************************************************************************** + * + * 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 gpsfailure.cpp + * Helper class for gpsfailure mode according to the OBC rules + * + * @author Thomas Gubler + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "navigator.h" +#include "gpsfailure.h" + +#define DELAY_SIGMA 0.01f + +GpsFailure::GpsFailure(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _param_loitertime(this, "LT"), + _param_openlooploiter_roll(this, "R"), + _param_openlooploiter_pitch(this, "P"), + _param_openlooploiter_thrust(this, "TR"), + _gpsf_state(GPSF_STATE_NONE), + _timestamp_activation(0) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +GpsFailure::~GpsFailure() +{ +} + +void +GpsFailure::on_inactive() +{ + /* reset GPSF state only if setpoint moved */ + if (!_navigator->get_can_loiter_at_sp()) { + _gpsf_state = GPSF_STATE_NONE; + } +} + +void +GpsFailure::on_activation() +{ + _gpsf_state = GPSF_STATE_NONE; + _timestamp_activation = hrt_absolute_time(); + updateParams(); + advance_gpsf(); + set_gpsf_item(); +} + +void +GpsFailure::on_active() +{ + + switch (_gpsf_state) { + case GPSF_STATE_LOITER: { + /* Position controller does not run in this mode: + * navigator has to publish an attitude setpoint */ + _navigator->get_att_sp()->roll_body = M_DEG_TO_RAD_F * _param_openlooploiter_roll.get(); + _navigator->get_att_sp()->pitch_body = M_DEG_TO_RAD_F * _param_openlooploiter_pitch.get(); + _navigator->get_att_sp()->thrust = _param_openlooploiter_thrust.get(); + _navigator->publish_att_sp(); + + /* Measure time */ + hrt_abstime elapsed = hrt_elapsed_time(&_timestamp_activation); + + //warnx("open loop loiter, posctl enabled %u, elapsed %.1fs, thrust %.2f", + //_navigator->get_control_mode()->flag_control_position_enabled, elapsed * 1e-6, (double)_param_openlooploiter_thrust.get()); + if (elapsed > _param_loitertime.get() * 1e6f) { + /* no recovery, adavance the state machine */ + warnx("gps not recovered, switch to next state"); + advance_gpsf(); + } + break; + } + case GPSF_STATE_TERMINATE: + set_gpsf_item(); + advance_gpsf(); + break; + default: + break; + } +} + +void +GpsFailure::set_gpsf_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* Set pos sp triplet to invalid to stop pos controller */ + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; + + switch (_gpsf_state) { + case GPSF_STATE_TERMINATE: { + /* Request flight termination from the commander */ + _navigator->get_mission_result()->flight_termination = true; + _navigator->publish_mission_result(); + warnx("gps fail: request flight termination"); + } + default: + break; + } + + reset_mission_item_reached(); + _navigator->set_position_setpoint_triplet_updated(); +} + +void +GpsFailure::advance_gpsf() +{ + updateParams(); + + switch (_gpsf_state) { + case GPSF_STATE_NONE: + _gpsf_state = GPSF_STATE_LOITER; + warnx("gpsf loiter"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: open loop loiter"); + break; + case GPSF_STATE_LOITER: + _gpsf_state = GPSF_STATE_TERMINATE; + warnx("gpsf terminate"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no gps recovery, termination"); + warnx("mavlink sent"); + break; + case GPSF_STATE_TERMINATE: + warnx("gpsf end"); + _gpsf_state = GPSF_STATE_END; + default: + break; + } +} diff --git a/src/modules/navigator/gpsfailure.h b/src/modules/navigator/gpsfailure.h new file mode 100644 index 000000000..e9e72babf --- /dev/null +++ b/src/modules/navigator/gpsfailure.h @@ -0,0 +1,97 @@ +/*************************************************************************** + * + * 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 gpsfailure.h + * Helper class for Data Link Loss Mode according to the OBC rules + * + * @author Thomas Gubler + */ + +#ifndef NAVIGATOR_GPSFAILURE_H +#define NAVIGATOR_GPSFAILURE_H + +#include +#include + +#include +#include +#include + +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class GpsFailure : public MissionBlock +{ +public: + GpsFailure(Navigator *navigator, const char *name); + + ~GpsFailure(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + /* Params */ + control::BlockParamFloat _param_loitertime; + control::BlockParamFloat _param_openlooploiter_roll; + control::BlockParamFloat _param_openlooploiter_pitch; + control::BlockParamFloat _param_openlooploiter_thrust; + + enum GPSFState { + GPSF_STATE_NONE = 0, + GPSF_STATE_LOITER = 1, + GPSF_STATE_TERMINATE = 2, + GPSF_STATE_END = 3, + } _gpsf_state; + + hrt_abstime _timestamp_activation; //*< timestamp when this mode was activated */ + + /** + * Set the GPSF item + */ + void set_gpsf_item(); + + /** + * Move to next GPSF item + */ + void advance_gpsf(); + +}; +#endif diff --git a/src/modules/navigator/gpsfailure_params.c b/src/modules/navigator/gpsfailure_params.c new file mode 100644 index 000000000..39d179eed --- /dev/null +++ b/src/modules/navigator/gpsfailure_params.c @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * 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 gpsfailure_params.c + * + * Parameters for GPSF navigation mode + * + * @author Thomas Gubler + */ + +#include + +#include + +/* + * GPS Failure Navigation Mode parameters, accessible via MAVLink + */ + +/** + * Loiter time + * + * The amount of time in seconds the system should do open loop loiter and wait for gps recovery + * before it goes into flight termination. + * + * @unit seconds + * @min 0.0 + * @group GPSF + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f); + +/** + * Open loop loiter roll + * + * Roll in degrees during the open loop loiter + * + * @unit deg + * @min 0.0 + * @max 30.0 + * @group GPSF + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f); + +/** + * Open loop loiter pitch + * + * Pitch in degrees during the open loop loiter + * + * @unit deg + * @min -30.0 + * @max 30.0 + * @group GPSF + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f); + +/** + * Open loop loiter thrust + * + * Thrust value which is set during the open loop loiter + * + * @min 0.0 + * @max 1.0 + * @group GPSF + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.7f); + + diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp new file mode 100644 index 000000000..5564a1c42 --- /dev/null +++ b/src/modules/navigator/rcloss.cpp @@ -0,0 +1,182 @@ +/**************************************************************************** + * + * 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 rcloss.cpp + * Helper class for RC Loss Mode according to the OBC rules + * + * @author Thomas Gubler + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "navigator.h" +#include "datalinkloss.h" + +#define DELAY_SIGMA 0.01f + +RCLoss::RCLoss(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _param_loitertime(this, "LT"), + _rcl_state(RCL_STATE_NONE) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +RCLoss::~RCLoss() +{ +} + +void +RCLoss::on_inactive() +{ + /* reset RCL state only if setpoint moved */ + if (!_navigator->get_can_loiter_at_sp()) { + _rcl_state = RCL_STATE_NONE; + } +} + +void +RCLoss::on_activation() +{ + _rcl_state = RCL_STATE_NONE; + updateParams(); + advance_rcl(); + set_rcl_item(); +} + +void +RCLoss::on_active() +{ + if (is_mission_item_reached()) { + updateParams(); + advance_rcl(); + set_rcl_item(); + } +} + +void +RCLoss::set_rcl_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + set_previous_pos_setpoint(); + _navigator->set_can_loiter_at_sp(false); + + switch (_rcl_state) { + case RCL_STATE_LOITER: { + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = _navigator->get_global_position()->alt; + _mission_item.altitude_is_relative = false; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = _param_loitertime.get() < 0.0f ? 0.0f : _param_loitertime.get(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + break; + } + case RCL_STATE_TERMINATE: { + /* Request flight termination from the commander */ + _navigator->get_mission_result()->flight_termination = true; + _navigator->publish_mission_result(); + warnx("rc not recovered: request flight termination"); + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; + break; + } + default: + break; + } + + reset_mission_item_reached(); + + /* convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +RCLoss::advance_rcl() +{ + switch (_rcl_state) { + case RCL_STATE_NONE: + if (_param_loitertime.get() > 0.0f) { + warnx("RC loss, OBC mode, loiter"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, loitering"); + _rcl_state = RCL_STATE_LOITER; + } else { + warnx("RC loss, OBC mode, slip loiter, terminate"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating"); + _rcl_state = RCL_STATE_TERMINATE; + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + } + break; + case RCL_STATE_LOITER: + _rcl_state = RCL_STATE_TERMINATE; + warnx("time is up, no RC regain, terminating"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating"); + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + break; + case RCL_STATE_TERMINATE: + warnx("rcl end"); + _rcl_state = RCL_STATE_END; + break; + default: + break; + } +} diff --git a/src/modules/navigator/rcloss.h b/src/modules/navigator/rcloss.h new file mode 100644 index 000000000..bcb74d877 --- /dev/null +++ b/src/modules/navigator/rcloss.h @@ -0,0 +1,88 @@ +/*************************************************************************** + * + * 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 rcloss.h + * Helper class for RC Loss Mode acording to the OBC rules + * + * @author Thomas Gubler + */ + +#ifndef NAVIGATOR_RCLOSS_H +#define NAVIGATOR_RCLOSS_H + +#include +#include + +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class RCLoss : public MissionBlock +{ +public: + RCLoss(Navigator *navigator, const char *name); + + ~RCLoss(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + /* Params */ + control::BlockParamFloat _param_loitertime; + + enum RCLState { + RCL_STATE_NONE = 0, + RCL_STATE_LOITER = 1, + RCL_STATE_TERMINATE = 2, + RCL_STATE_END = 3 + } _rcl_state; + + /** + * Set the RCL item + */ + void set_rcl_item(); + + /** + * Move to next RCL item + */ + void advance_rcl(); + +}; +#endif diff --git a/src/modules/navigator/rcloss_params.c b/src/modules/navigator/rcloss_params.c new file mode 100644 index 000000000..f1a01c73b --- /dev/null +++ b/src/modules/navigator/rcloss_params.c @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * 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 rcloss_params.c + * + * Parameters for RC Loss (OBC) + * + * @author Thomas Gubler + */ + +#include + +#include + +/* + * OBC RC Loss mode parameters, accessible via MAVLink + */ + +/** + * Loiter Time + * + * The amount of time in seconds the system should loiter at current position before termination + * Set to -1 to make the system skip loitering + * + * @unit seconds + * @min -1.0 + * @group RCL + */ +PARAM_DEFINE_FLOAT(NAV_RCL_LT, 120.0f); diff --git a/src/modules/uORB/topics/multirotor_motor_limits.h b/src/modules/uORB/topics/multirotor_motor_limits.h new file mode 100644 index 000000000..44c51e4d9 --- /dev/null +++ b/src/modules/uORB/topics/multirotor_motor_limits.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 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 multirotor_motor_limits.h + * + * Definition of multirotor_motor_limits topic + */ + +#ifndef MULTIROTOR_MOTOR_LIMITS_H_ +#define MULTIROTOR_MOTOR_LIMITS_H_ + +#include "../uORB.h" +#include + +/** + * @addtogroup topics + * @{ + */ + +/** + * Motor limits + */ +struct multirotor_motor_limits_s { + uint8_t roll_pitch : 1; // roll/pitch limit reached + uint8_t yaw : 1; // yaw limit reached + uint8_t throttle_lower : 1; // lower throttle limit reached + uint8_t throttle_upper : 1; // upper throttle limit reached + uint8_t reserved : 4; +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(multirotor_motor_limits); + +#endif diff --git a/src/modules/uORB/topics/vtol_vehicle_status.h b/src/modules/uORB/topics/vtol_vehicle_status.h new file mode 100644 index 000000000..24ecca9fa --- /dev/null +++ b/src/modules/uORB/topics/vtol_vehicle_status.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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 vtol_status.h + * + * Vtol status topic + * + */ + +#ifndef TOPIC_VTOL_STATUS_H +#define TOPIC_VTOL_STATUS_H + +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/* Indicates in which mode the vtol aircraft is in */ +struct vtol_vehicle_status_s { + + uint64_t timestamp; /**< Microseconds since system boot */ + bool vtol_in_rw_mode; /*true: vtol vehicle is in rotating wing mode */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vtol_vehicle_status); + +#endif diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk new file mode 100644 index 000000000..c349c2340 --- /dev/null +++ b/src/modules/vtol_att_control/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# VTOL attitude controller +# + +MODULE_COMMAND = vtol_att_control + +SRCS = vtol_att_control_main.cpp diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp new file mode 100644 index 000000000..38fa4eec1 --- /dev/null +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -0,0 +1,642 @@ +/**************************************************************************** + * + * 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 VTOL_att_control_main.cpp + * Implementation of an attitude controller for VTOL airframes. This module receives data + * from both the fixed wing- and the multicopter attitude controllers and processes it. + * It computes the correct actuator controls depending on which mode the vehicle is in (hover,forward- + * flight or transition). It also publishes the resulting controls on the actuator controls topics. + * + * @author Roman Bapst + * @author Lorenz Meier + * @author Thomas Gubler + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "drivers/drv_pwm_output.h" +#include +#include + +#include + +#include + + +extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); + +class VtolAttitudeControl +{ +public: + + VtolAttitudeControl(); + ~VtolAttitudeControl(); + + int start(); /* start the task and return OK on success */ + + +private: +//******************flags & handlers****************************************************** + bool _task_should_exit; + int _control_task; //task handle for VTOL attitude controller + + /* handlers for subscriptions */ + int _v_att_sub; //vehicle attitude subscription + int _v_att_sp_sub; //vehicle attitude setpoint subscription + int _v_rates_sp_sub; //vehicle rates setpoint subscription + int _v_control_mode_sub; //vehicle control mode subscription + int _params_sub; //parameter updates subscription + int _manual_control_sp_sub; //manual control setpoint subscription + int _armed_sub; //arming status subscription + + int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs + int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs + + //handlers for publishers + orb_advert_t _actuators_0_pub; //input for the mixer (roll,pitch,yaw,thrust) + orb_advert_t _actuators_1_pub; + orb_advert_t _vtol_vehicle_status_pub; +//*******************data containers*********************************************************** + struct vehicle_attitude_s _v_att; //vehicle attitude + struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint + struct vehicle_rates_setpoint_s _v_rates_sp; //vehicle rates setpoint + struct manual_control_setpoint_s _manual_control_sp; //manual control setpoint + struct vehicle_control_mode_s _v_control_mode; //vehicle control mode + struct vtol_vehicle_status_s _vtol_vehicle_status; + struct actuator_controls_s _actuators_out_0; //actuator controls going to the mc mixer + struct actuator_controls_s _actuators_out_1; //actuator controls going to the fw mixer (used for elevons) + struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control + struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control + struct actuator_armed_s _armed; //actuator arming status + + struct { + float min_pwm_mc; //pwm value for idle in mc mode + } _params; + + struct { + param_t min_pwm_mc; + } _params_handles; + + perf_counter_t _loop_perf; /**< loop performance counter */ + + /* for multicopters it is usual to have a non-zero idle speed of the engines + * for fixed wings we want to have an idle speed of zero since we do not want + * to waste energy when gliding. */ + bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" + +//*****************Member functions*********************************************************************** + + void task_main(); //main task + static void task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create. + + void vehicle_control_mode_poll(); //Check for changes in vehicle control mode. + void vehicle_manual_poll(); //Check for changes in manual inputs. + void arming_status_poll(); //Check for arming status updates. + void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output + void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output + void parameters_update_poll(); //Check if parameters have changed + int parameters_update(); //Update local paraemter cache + void fill_mc_att_control_output(); //write mc_att_control results to actuator message + void fill_fw_att_control_output(); //write fw_att_control results to actuator message + void set_idle_fw(); + void set_idle_mc(); +}; + +namespace VTOL_att_control +{ +VtolAttitudeControl *g_control; +} + +/** +* Constructor +*/ +VtolAttitudeControl::VtolAttitudeControl() : + _task_should_exit(false), + _control_task(-1), + + //init subscription handlers + _v_att_sub(-1), + _v_att_sp_sub(-1), + _v_control_mode_sub(-1), + _params_sub(-1), + _manual_control_sp_sub(-1), + _armed_sub(-1), + + //init publication handlers + _actuators_0_pub(-1), + _actuators_1_pub(-1), + _vtol_vehicle_status_pub(-1), + + _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) +{ + + flag_idle_mc = true; /*assume we always start in mc mode for a VTOL airframe */ + + memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); + _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ + memset(&_v_att, 0, sizeof(_v_att)); + memset(&_v_att_sp, 0, sizeof(_v_att_sp)); + memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); + memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); + memset(&_v_control_mode, 0, sizeof(_v_control_mode)); + memset(&_vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); + memset(&_actuators_out_0, 0, sizeof(_actuators_out_0)); + memset(&_actuators_out_1, 0, sizeof(_actuators_out_1)); + memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in)); + memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in)); + memset(&_armed, 0, sizeof(_armed)); + + _params_handles.min_pwm_mc = param_find("PWM_MIN"); + + /* fetch initial parameter values */ + parameters_update(); +} + +/** +* Destructor +*/ +VtolAttitudeControl::~VtolAttitudeControl() +{ + if (_control_task != -1) { + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_control_task); + break; + } + } while (_control_task != -1); + } + + VTOL_att_control::g_control = nullptr; +} + +/** +* Check for changes in vehicle control mode. +*/ +void VtolAttitudeControl::vehicle_control_mode_poll() +{ + bool updated; + + /* Check if vehicle control mode has changed */ + orb_check(_v_control_mode_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); + } +} + +/** +* Check for changes in manual inputs. +*/ +void VtolAttitudeControl::vehicle_manual_poll() +{ + bool updated; + + /* get pilots inputs */ + orb_check(_manual_control_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); + } +} +/** +* Check for arming status updates. +*/ +void VtolAttitudeControl::arming_status_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + } +} + +/** +* Check for inputs from mc attitude controller. +*/ +void VtolAttitudeControl::actuator_controls_mc_poll() +{ + bool updated; + orb_check(_actuator_inputs_mc, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc , &_actuators_mc_in); + } +} + +/** +* Check for inputs from fw attitude controller. +*/ +void VtolAttitudeControl::actuator_controls_fw_poll() +{ + bool updated; + orb_check(_actuator_inputs_fw, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw , &_actuators_fw_in); + } +} + +/** +* Check for parameter updates. +*/ +void +VtolAttitudeControl::parameters_update_poll() +{ + bool updated; + + /* Check if parameters have changed */ + orb_check(_params_sub, &updated); + + if (updated) { + struct parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); + parameters_update(); + } +} + +/** +* Update parameters. +*/ +int +VtolAttitudeControl::parameters_update() +{ + /* idle pwm */ + float v; + param_get(_params_handles.min_pwm_mc, &v); + _params.min_pwm_mc = v; + + return OK; +} + +/** +* Prepare message to acutators with data from mc attitude controller. +*/ +void VtolAttitudeControl::fill_mc_att_control_output() +{ + _actuators_out_0.control[0] = _actuators_mc_in.control[0]; + _actuators_out_0.control[1] = _actuators_mc_in.control[1]; + _actuators_out_0.control[2] = _actuators_mc_in.control[2]; + _actuators_out_0.control[3] = _actuators_mc_in.control[3]; + //set neutral position for elevons + _actuators_out_1.control[0] = 0; //roll elevon + _actuators_out_1.control[1] = 0; //pitch elevon +} + +/** +* Prepare message to acutators with data from fw attitude controller. +*/ +void VtolAttitudeControl::fill_fw_att_control_output() +{ + /*For the first test in fw mode, only use engines for thrust!!!*/ + _actuators_out_0.control[0] = 0; + _actuators_out_0.control[1] = 0; + _actuators_out_0.control[2] = 0; + _actuators_out_0.control[3] = _actuators_fw_in.control[3]; + /*controls for the elevons */ + _actuators_out_1.control[0] = _actuators_fw_in.control[0]; /*roll elevon*/ + _actuators_out_1.control[1] = _actuators_fw_in.control[1]; /*pitch elevon */ +} + +/** +* Adjust idle speed for fw mode. +*/ +void VtolAttitudeControl::set_idle_fw() +{ + int ret; + char *dev = PWM_OUTPUT_DEVICE_PATH; + int fd = open(dev, 0); + + if (fd < 0) {err(1, "can't open %s", dev);} + + unsigned pwm_value = PWM_LOWEST_MIN; + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + + for (unsigned i = 0; i < 4; i++) { + + pwm_values.values[i] = pwm_value; + pwm_values.channel_count++; + } + + ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) {errx(ret, "failed setting min values");} + + close(fd); +} + +/** +* Adjust idle speed for mc mode. +*/ +void VtolAttitudeControl::set_idle_mc() +{ + int ret; + unsigned servo_count; + char *dev = PWM_OUTPUT_DEVICE_PATH; + int fd = open(dev, 0); + + if (fd < 0) {err(1, "can't open %s", dev);} + + ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + unsigned pwm_value = 1100; + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + + for (unsigned i = 0; i < 4; i++) { + pwm_values.values[i] = pwm_value; + pwm_values.channel_count++; + } + + ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) {errx(ret, "failed setting min values");} + + close(fd); +} + +void +VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) +{ + VTOL_att_control::g_control->task_main(); +} + +void VtolAttitudeControl::task_main() +{ + warnx("started"); + fflush(stdout); + + /* do subscriptions */ + _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _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)); + + _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); + _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); + + parameters_update(); /*initialize parameter cache/* + + /* wakeup source*/ + struct pollfd fds[3]; /*input_mc, input_fw, parameters*/ + + fds[0].fd = _actuator_inputs_mc; + fds[0].events = POLLIN; + fds[1].fd = _actuator_inputs_fw; + fds[1].events = POLLIN; + fds[2].fd = _params_sub; + fds[2].events = POLLIN; + + while (!_task_should_exit) { + /*Advertise/Publish vtol vehicle status*/ + if (_vtol_vehicle_status_pub > 0) { + orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status); + + } else { + _vtol_vehicle_status.timestamp = hrt_absolute_time(); + _vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status); + } + + /* wait for up to 100ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + + /* timed out - periodic check for _task_should_exit */ + if (pret == 0) { + continue; + } + + /* 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; + } + + if (fds[2].revents & POLLIN) { //parameters were updated, read them now + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + /* update parameters from storage */ + parameters_update(); + } + + vehicle_control_mode_poll(); //Check for changes in vehicle control mode. + vehicle_manual_poll(); //Check for changes in manual inputs. + arming_status_poll(); //Check for arming status updates. + actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output + actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output + parameters_update_poll(); + + if (_manual_control_sp.aux1 <= 0.0f) { /* vehicle is in mc mode */ + _vtol_vehicle_status.vtol_in_rw_mode = true; + + if (!flag_idle_mc) { /* we want to adjust idle speed for mc mode */ + set_idle_mc(); + flag_idle_mc = true; + } + + /* got data from mc_att_controller */ + if (fds[0].revents & POLLIN) { + vehicle_manual_poll(); /* update remote input */ + orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); + + fill_mc_att_control_output(); + + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); + + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); + } + + if (_actuators_1_pub > 0) { + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); + + } else { + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); + } + } + } + + if (_manual_control_sp.aux1 >= 0.0f) { /* vehicle is in fw mode */ + _vtol_vehicle_status.vtol_in_rw_mode = false; + + if (flag_idle_mc) { /* we want to adjust idle speed for fixed wing mode */ + set_idle_fw(); + flag_idle_mc = false; + } + + if (fds[1].revents & POLLIN) { /* got data from fw_att_controller */ + orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in); + vehicle_manual_poll(); //update remote input + + fill_fw_att_control_output(); + + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); + + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); + } + + if (_actuators_1_pub > 0) { + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); + + } else { + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); + } + } + } + } + + warnx("exit"); + _control_task = -1; + _exit(0); +} + +int +VtolAttitudeControl::start() +{ + ASSERT(_control_task == -1); + + /* start the task */ + _control_task = task_spawn_cmd("vtol_att_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 2048, + (main_t)&VtolAttitudeControl::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + + +int vtol_att_control_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "usage: vtol_att_control {start|stop|status}"); + } + + if (!strcmp(argv[1], "start")) { + + if (VTOL_att_control::g_control != nullptr) { + errx(1, "already running"); + } + + VTOL_att_control::g_control = new VtolAttitudeControl; + + if (VTOL_att_control::g_control == nullptr) { + errx(1, "alloc failed"); + } + + if (OK != VTOL_att_control::g_control->start()) { + delete VTOL_att_control::g_control; + VTOL_att_control::g_control = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (VTOL_att_control::g_control == nullptr) { + errx(1, "not running"); + } + + delete VTOL_att_control::g_control; + VTOL_att_control::g_control = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (VTOL_att_control::g_control) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} -- cgit v1.2.3 From b168ba92e0be91aec9db5aaccbcc427cfd067c7b Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 29 Oct 2014 15:57:45 +0100 Subject: added setter functions into base class. used when integrating the base class into a gazebo environment --- src/modules/mc_att_control/mc_att_control_base.cpp | 57 ++++++++++++++++++++++ src/modules/mc_att_control/mc_att_control_base.h | 7 +++ 2 files changed, 64 insertions(+) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index d4270b153..d39bae4dc 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -281,3 +281,60 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { } } + +void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion& attitude) { + // check if this is consistent !!! + math::Vector<3> quat; + quat(0) = attitude(0); + quat(1) = attitude(1); + quat(2) = attitude(2); + quat(3) = attitude(3); + + _v_att.q[0] = quat(0); + _v_att.q[1] = quat(1); + _v_att.q[2] = quat(2); + _v_att.q[3] = quat(3); + + math::Matrix<3,3> Rot = quat.to_dcm(); + _v_att.R[0][0] = Rot(0,0); + _v_att.R[1][0] = Rot(1,0); + _v_att.R[2][0] = Rot(2,0); + _v_att.R[0][1] = Rot(0,1); + _v_att.R[1][1] = Rot(1,1); + _v_att.R[2][1] = Rot(2,1); + _v_att.R[0][2] = Rot(0,2); + _v_att.R[1][2] = Rot(1,2); + _v_att.R[2][2] = Rot(2,2); + + _v_att.R_valid = true; + +} + +void MulticopterAttitudeControlBase::set_attitude_rates(const Eigen::Vector3d& angular_rate) { + // check if this is consistent !!! + _v_att.rollspeed = angular_rate(0); + _v_att.pitchspeed = angular_rate(1); + _v_att.yawspeed = angular_rate(2); + +} + +void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) { + _v_att_sp.roll_body = control_attitude_thrust_reference(0); + _v_att_sp.pitch_body = control_attitude_thrust_reference(1); + _v_att_sp.yaw_body = control_attitude_thrust_reference(2); + _v_att_sp.thrust = control_attitude_thrust_reference(3); + + // setup rotation matrix + math::Matrix<3,3> Rot_sp; + Rot_sp = R.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body); + _v_att_sp.R_body[0][0] = Rot_sp(0,0); + _v_att_sp.R_body[1][0] = Rot_sp(1,0); + _v_att_sp.R_body[2][0] = Rot_sp(2,0); + _v_att_sp.R_body[0][1] = Rot_sp(0,1); + _v_att_sp.R_body[1][1] = Rot_sp(1,1); + _v_att_sp.R_body[2][1] = Rot_sp(2,1); + _v_att_sp.R_body[0][2] = Rot_sp(0,2); + _v_att_sp.R_body[1][2] = Rot_sp(1,2); + _v_att_sp.R_body[2][2] = Rot_sp(2,2); + +} diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index d75088b85..995838bb2 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -166,6 +166,13 @@ protected: void vehicle_attitude_setpoint_poll(); //provisional + // setters and getters for interface with euroc-gazebo simulator + void set_attitude(const Eigen::Quaternion& attitude); + void set_attitude_rates(const Eigen::Vector3d& angular_rate); + void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); + + + }; -- cgit v1.2.3 From 6ecefe7930d851a033b4fa414366dec223560a50 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 4 Nov 2014 22:52:30 +0100 Subject: Fix include guards for ROS, needs cleanups --- include/px4.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/px4.h b/include/px4.h index 7dedd5f82..abc10276f 100644 --- a/include/px4.h +++ b/include/px4.h @@ -37,11 +37,11 @@ * Main system header with common convenience functions */ -#ifdef __linux +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) #include "ros/ros.h" #define px4_warnx ROS_WARN #define px4_infox ROS_INFO #else #define px4_warnx warnx #define px4_infox warnx -#endif \ No newline at end of file +#endif -- cgit v1.2.3 From c51ec3411850d9de0794d4e584838d4a137dafcf Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 5 Nov 2014 08:50:20 +0100 Subject: added initialisation of parameters, added assignment of actuator controls, cleaned up --- src/modules/mc_att_control/mc_att_control_base.cpp | 53 ++++++++++++++++++---- 1 file changed, 45 insertions(+), 8 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index d39bae4dc..fee117458 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -51,6 +51,27 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : _att_control.zero(); _I.identity(); + + // setup standard gains + _params.att_p(0) = 5.0; + _params.rate_p(0) = 0.05; + _params.rate_i(0) = 0.0; + _params.rate_d(0) = 0.003; + /* pitch gains */ + _params.att_p(1) = 5.0; + _params.rate_p(1) = 0.05; + _params.rate_i(1) = 0.0; + _params.rate_d(1) = 0.003; + /* yaw gains */ + _params.att_p(2) = 2.8; + _params.rate_p(2) = 0.2; + _params.rate_i(2) = 0.1; + _params.rate_d(2) = 0.0; + _params.yaw_rate_max = 0.5; + _params.yaw_ff = 0.5; + _params.man_roll_max = 0.6; + _params.man_pitch_max = 0.6; + _params.man_yaw_max = 0.6; } MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() { @@ -282,13 +303,21 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { } -void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion& attitude) { +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.timestamp = hrt_absolute_time(); +} + +void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion attitude) { // check if this is consistent !!! - math::Vector<3> quat; - quat(0) = attitude(0); - quat(1) = attitude(1); - quat(2) = attitude(2); - quat(3) = attitude(3); + math::Quaternion quat; + quat(0) = (float)attitude.w(); + quat(1) = (float)attitude.x(); + quat(2) = (float)attitude.y(); + quat(3) = (float)attitude.z(); _v_att.q[0] = quat(0); _v_att.q[1] = quat(1); @@ -322,11 +351,11 @@ void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4 _v_att_sp.roll_body = control_attitude_thrust_reference(0); _v_att_sp.pitch_body = control_attitude_thrust_reference(1); _v_att_sp.yaw_body = control_attitude_thrust_reference(2); - _v_att_sp.thrust = control_attitude_thrust_reference(3); + _v_att_sp.thrust = (control_attitude_thrust_referenc(3) -30)*(-1)/30; // setup rotation matrix math::Matrix<3,3> Rot_sp; - Rot_sp = R.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body); + Rot_sp.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body); _v_att_sp.R_body[0][0] = Rot_sp(0,0); _v_att_sp.R_body[1][0] = Rot_sp(1,0); _v_att_sp.R_body[2][0] = Rot_sp(2,0); @@ -338,3 +367,11 @@ void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4 _v_att_sp.R_body[2][2] = Rot_sp(2,2); } + +void MulticopterAttitudeControlBase::get_mixer_input(Eigen::Vector4d& motor_inputs) { + motor_inputs(0) = _actuators.control[0]; + motor_inputs(1) = _actuators.control[1]; + motor_inputs(2) = _actuators.control[2]; + motor_inputs(3) = _actuators.control[3]; +} + -- cgit v1.2.3 From 48cdbf3d0cb8aed7dcdddde872a64096e7c9a595 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 5 Nov 2014 08:51:12 +0100 Subject: added more setter and getter functions --- src/modules/mc_att_control/mc_att_control_base.h | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 995838bb2..8f1cf015e 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -76,6 +76,8 @@ #include //#include #include +#include + /** @@ -105,6 +107,14 @@ public: * * @return OK on success. */ + void control_attitude(float dt); + void control_attitude_rates(float dt); + // setters and getters for interface with euroc-gazebo simulator + void set_attitude(const Eigen::Quaternion attitude); + void set_attitude_rates(const Eigen::Vector3d& angular_rate); + void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); + void get_mixer_input(Eigen::Vector4d& motor_inputs); + void set_actuator_controls(); protected: @@ -161,15 +171,10 @@ protected: /** * Attitude rates controller. */ - void control_attitude(float dt); - void control_attitude_rates(float dt); - + void vehicle_attitude_setpoint_poll(); //provisional - // setters and getters for interface with euroc-gazebo simulator - void set_attitude(const Eigen::Quaternion& attitude); - void set_attitude_rates(const Eigen::Vector3d& angular_rate); - void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); + -- cgit v1.2.3 From 1e9f431cf1d93b64f26fb9c3e42329e7237bad0e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 5 Nov 2014 15:03:00 +0100 Subject: ros example: send rc_channels message --- CMakeLists.txt | 1 + include/px4.h | 6 ++++++ package.xml | 8 ++++---- src/examples/publisher/publisher.cpp | 16 ++++++++-------- src/examples/subscriber/subscriber.cpp | 9 +++++---- 5 files changed, 24 insertions(+), 16 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 144bdcbc3..de2907e6a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -84,6 +84,7 @@ catkin_package( # LIBRARIES px4 # CATKIN_DEPENDS roscpp rospy std_msgs # DEPENDS system_lib + CATKIN_DEPENDS message_runtime ) ########### diff --git a/include/px4.h b/include/px4.h index abc10276f..4684cd256 100644 --- a/include/px4.h +++ b/include/px4.h @@ -38,10 +38,16 @@ */ #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/* + * Building for running within the ROS environment + */ #include "ros/ros.h" #define px4_warnx ROS_WARN #define px4_infox ROS_INFO #else +/* + * Building for NuttX + */ #define px4_warnx warnx #define px4_infox warnx #endif diff --git a/package.xml b/package.xml index 3575119be..bc23cdd18 100644 --- a/package.xml +++ b/package.xml @@ -4,7 +4,7 @@ 1.0.0 The PX4 Flight Stack package - + Lorenz Meier @@ -32,11 +32,11 @@ - + message_generation - + message_runtime catkin @@ -56,4 +56,4 @@ - \ No newline at end of file + diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp index cc88cd543..53fe2e905 100644 --- a/src/examples/publisher/publisher.cpp +++ b/src/examples/publisher/publisher.cpp @@ -33,6 +33,7 @@ // %EndTag(MSG_HEADER)% #include +#include /** * This tutorial demonstrates simple sending of messages over the ROS system. @@ -80,7 +81,7 @@ int main(int argc, char **argv) * buffer up before throwing some away. */ // %Tag(PUBLISHER)% - ros::Publisher chatter_pub = n.advertise("chatter", 1000); + ros::Publisher rc_channels_pub = n.advertise("rc_channels", 1000); // %EndTag(PUBLISHER)% // %Tag(LOOP_RATE)% @@ -100,15 +101,14 @@ int main(int argc, char **argv) * This is a message object. You stuff it with data, and then publish it. */ // %Tag(FILL_MESSAGE)% - std_msgs::String msg; + px4::rc_channels msg; - std::stringstream ss; - ss << "hello world " << count; - msg.data = ss.str(); + ros::Time time = ros::Time::now(); + msg.timestamp_last_valid = time.sec * 1e6 + time.nsec; // %EndTag(FILL_MESSAGE)% // %Tag(ROSCONSOLE)% - px4_warnx("%s", msg.data.c_str()); + px4_warnx("%lu", msg.timestamp_last_valid); // %EndTag(ROSCONSOLE)% /** @@ -118,7 +118,7 @@ int main(int argc, char **argv) * in the constructor above. */ // %Tag(PUBLISH)% - chatter_pub.publish(msg); + rc_channels_pub.publish(msg); // %EndTag(PUBLISH)% // %Tag(SPINONCE)% @@ -134,4 +134,4 @@ int main(int argc, char **argv) return 0; } -// %EndTag(FULLTEXT)% \ No newline at end of file +// %EndTag(FULLTEXT)% diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index e46055306..da69e6e25 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -28,14 +28,15 @@ // %Tag(FULLTEXT)% #include "ros/ros.h" #include "std_msgs/String.h" +#include "px4/rc_channels.h" /** * This tutorial demonstrates simple receipt of messages over the ROS system. */ // %Tag(CALLBACK)% -void chatterCallback(const std_msgs::String::ConstPtr& msg) +void rc_channels_callback(const px4::rc_channels::ConstPtr& msg) { - ROS_INFO("I heard: [%s]", msg->data.c_str()); + ROS_INFO("I heard: [%lu]", msg->timestamp_last_valid); } // %EndTag(CALLBACK)% @@ -76,7 +77,7 @@ int main(int argc, char **argv) * away the oldest ones. */ // %Tag(SUBSCRIBER)% - ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback); + ros::Subscriber sub = n.subscribe("rc_channels", 1000, rc_channels_callback); // %EndTag(SUBSCRIBER)% /** @@ -90,4 +91,4 @@ int main(int argc, char **argv) return 0; } -// %EndTag(FULLTEXT)% \ No newline at end of file +// %EndTag(FULLTEXT)% -- cgit v1.2.3 From b5e34eac4faae4b0bc605c6d2d36ed65740c781d Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 5 Nov 2014 15:40:22 +0100 Subject: fixed typo --- src/modules/mc_att_control/mc_att_control_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index fee117458..b07a1a6be 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -351,7 +351,7 @@ void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4 _v_att_sp.roll_body = control_attitude_thrust_reference(0); _v_att_sp.pitch_body = control_attitude_thrust_reference(1); _v_att_sp.yaw_body = control_attitude_thrust_reference(2); - _v_att_sp.thrust = (control_attitude_thrust_referenc(3) -30)*(-1)/30; + _v_att_sp.thrust = (control_attitude_thrust_reference(3) -30)*(-1)/30; // setup rotation matrix math::Matrix<3,3> Rot_sp; -- cgit v1.2.3 From f36f54c621cb5b36d345c5a26f72e89fc948fa65 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Nov 2014 11:57:34 +0100 Subject: Restructuring of generic middleware support files, wrapping of the main ROS calls, skeletons for publishers / subscribers --- CMakeLists.txt | 35 +++++---- include/px4.h | 53 ------------- makefiles/config_px4fmu-v2_test.mk | 11 +-- platforms/empty.c | 3 - src/examples/publisher/module.mk | 42 ++++++++++ src/examples/publisher/publisher.cpp | 138 ++++++++++++--------------------- src/examples/subscriber/module.mk | 42 ++++++++++ src/examples/subscriber/subscriber.cpp | 99 +++++++++++------------ src/include/px4.h | 60 ++++++++++++++ src/platforms/empty.c | 3 + src/platforms/nuttx/module.mk | 42 ++++++++++ src/platforms/nuttx/px4_nuttx_impl.cpp | 75 ++++++++++++++++++ src/platforms/nuttx/px4_publisher.cpp | 40 ++++++++++ src/platforms/nuttx/px4_subscriber.cpp | 40 ++++++++++ src/platforms/px4_defines.h | 51 ++++++++++++ src/platforms/px4_middleware.h | 95 +++++++++++++++++++++++ src/platforms/px4_nodehandle.h | 46 +++++++++++ src/platforms/px4_publisher.h | 46 +++++++++++ src/platforms/px4_subscriber.h | 46 +++++++++++ src/platforms/ros/px4_publisher.cpp | 40 ++++++++++ src/platforms/ros/px4_ros_impl.cpp | 71 +++++++++++++++++ src/platforms/ros/px4_subscriber.cpp | 40 ++++++++++ 22 files changed, 901 insertions(+), 217 deletions(-) delete mode 100644 include/px4.h delete mode 100644 platforms/empty.c create mode 100644 src/examples/publisher/module.mk create mode 100644 src/examples/subscriber/module.mk create mode 100644 src/include/px4.h create mode 100644 src/platforms/empty.c create mode 100644 src/platforms/nuttx/module.mk create mode 100644 src/platforms/nuttx/px4_nuttx_impl.cpp create mode 100644 src/platforms/nuttx/px4_publisher.cpp create mode 100644 src/platforms/nuttx/px4_subscriber.cpp create mode 100644 src/platforms/px4_defines.h create mode 100644 src/platforms/px4_middleware.h create mode 100644 src/platforms/px4_nodehandle.h create mode 100644 src/platforms/px4_publisher.h create mode 100644 src/platforms/px4_subscriber.h create mode 100644 src/platforms/ros/px4_publisher.cpp create mode 100644 src/platforms/ros/px4_ros_impl.cpp create mode 100644 src/platforms/ros/px4_subscriber.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index de2907e6a..36c2ffeff 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -80,10 +80,10 @@ generate_messages( ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( - INCLUDE_DIRS include -# LIBRARIES px4 -# CATKIN_DEPENDS roscpp rospy std_msgs -# DEPENDS system_lib + INCLUDE_DIRS src/include + LIBRARIES px4 + CATKIN_DEPENDS roscpp rospy std_msgs + DEPENDS system_lib CATKIN_DEPENDS message_runtime ) @@ -93,15 +93,22 @@ catkin_package( ## Specify additional locations of header files ## Your package locations should be listed before other locations -include_directories(include) include_directories( ${catkin_INCLUDE_DIRS} + src/platforms + src/include ) ## Declare a cpp library -# add_library(px4 -# src/${PROJECT_NAME}/px4test.cpp # src/platforms/ros/ros.cpp -# ) +add_library(px4 + src/platforms/ros/px4_ros_impl.cpp + src/platforms/ros/px4_publisher.cpp + src/platforms/ros/px4_subscriber.cpp +) + +target_link_libraries(px4 + ${catkin_LIBRARIES} +) ## Declare a test publisher add_executable(publisher src/examples/publisher/publisher.cpp) @@ -113,6 +120,7 @@ add_dependencies(publisher px4_generate_messages_cpp) ## Specify libraries to link a library or executable target against target_link_libraries(publisher ${catkin_LIBRARIES} + px4 ) ## Declare a test subscriber @@ -125,6 +133,7 @@ add_dependencies(subscriber px4_generate_messages_cpp) ## Specify libraries to link a library or executable target against target_link_libraries(subscriber ${catkin_LIBRARIES} + px4 ) ############# @@ -142,11 +151,11 @@ target_link_libraries(subscriber # ) ## Mark executables and/or libraries for installation -# install(TARGETS px4 mc_attitude_control -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) +install(TARGETS px4 publisher subscriber + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) ## Mark cpp header files for installation # install(DIRECTORY include/${PROJECT_NAME}/ diff --git a/include/px4.h b/include/px4.h deleted file mode 100644 index 4684cd256..000000000 --- a/include/px4.h +++ /dev/null @@ -1,53 +0,0 @@ -/**************************************************************************** - * - * 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 px4.h - * - * Main system header with common convenience functions - */ - -#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) -/* - * Building for running within the ROS environment - */ -#include "ros/ros.h" -#define px4_warnx ROS_WARN -#define px4_infox ROS_INFO -#else -/* - * Building for NuttX - */ -#define px4_warnx warnx -#define px4_infox warnx -#endif diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 2f4d9d6a4..67934b8e4 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -34,9 +34,11 @@ MODULES += systemcmds/mtd MODULES += systemcmds/ver # -# Testing modules +# Example modules # MODULES += examples/matlab_csv_serial +MODULES += examples/subscriber +MODULES += examples/publisher # # Library modules @@ -44,16 +46,15 @@ MODULES += examples/matlab_csv_serial MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/uORB -LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/conversion +LIBRARIES += lib/mathlib/CMSIS +MODULES += platforms/nuttx # -# Libraries +# Tests # -LIBRARIES += lib/mathlib/CMSIS - MODULES += modules/unit_test MODULES += modules/mavlink/mavlink_tests MODULES += modules/commander/commander_tests diff --git a/platforms/empty.c b/platforms/empty.c deleted file mode 100644 index 139531354..000000000 --- a/platforms/empty.c +++ /dev/null @@ -1,3 +0,0 @@ -/* - * This is an empty C source file, used when building default firmware configurations. - */ diff --git a/src/examples/publisher/module.mk b/src/examples/publisher/module.mk new file mode 100644 index 000000000..0fc4316ec --- /dev/null +++ b/src/examples/publisher/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Publisher Example Application +# + +MODULE_COMMAND = publisher + +SRCS = publisher.cpp + +MODULE_STACKSIZE = 1200 diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp index 53fe2e905..91e063162 100644 --- a/src/examples/publisher/publisher.cpp +++ b/src/examples/publisher/publisher.cpp @@ -26,112 +26,70 @@ */ #include - -// %EndTag(ROS_HEADER)% -// %Tag(MSG_HEADER)% -#include "std_msgs/String.h" -// %EndTag(MSG_HEADER)% +#include #include -#include /** - * This tutorial demonstrates simple sending of messages over the ROS system. + * This tutorial demonstrates simple sending of messages over the PX4 middleware system. */ int main(int argc, char **argv) { - /** - * The ros::init() function needs to see argc and argv so that it can perform - * any ROS arguments and name remapping that were provided at the command line. For programmatic - * remappings you can use a different version of init() which takes remappings - * directly, but for most command-line programs, passing argc and argv is the easiest - * way to do it. The third argument to init() is the name of the node. - * - * You must call one of the versions of ros::init() before using any other - * part of the ROS system. - */ -// %Tag(INIT)% - ros::init(argc, argv, "px4_publisher"); -// %EndTag(INIT)% - /** - * NodeHandle is the main access point to communications with the ROS system. - * The first NodeHandle constructed will fully initialize this node, and the last - * NodeHandle destructed will close down the node. - */ -// %Tag(NODEHANDLE)% - ros::NodeHandle n; -// %EndTag(NODEHANDLE)% + px4::init(argc, argv, "px4_publisher"); + + ros::NodeHandle n; - /** - * The advertise() function is how you tell ROS that you want to - * publish on a given topic name. This invokes a call to the ROS - * master node, which keeps a registry of who is publishing and who - * is subscribing. After this advertise() call is made, the master - * node will notify anyone who is trying to subscribe to this topic name, - * and they will in turn negotiate a peer-to-peer connection with this - * node. advertise() returns a Publisher object which allows you to - * publish messages on that topic through a call to publish(). Once - * all copies of the returned Publisher object are destroyed, the topic - * will be automatically unadvertised. - * - * The second parameter to advertise() is the size of the message queue - * used for publishing messages. If messages are published more quickly - * than we can send them, the number here specifies how many messages to - * buffer up before throwing some away. - */ -// %Tag(PUBLISHER)% - ros::Publisher rc_channels_pub = n.advertise("rc_channels", 1000); -// %EndTag(PUBLISHER)% + /** + * The advertise() function is how you tell ROS that you want to + * publish on a given topic name. This invokes a call to the ROS + * master node, which keeps a registry of who is publishing and who + * is subscribing. After this advertise() call is made, the master + * node will notify anyone who is trying to subscribe to this topic name, + * and they will in turn negotiate a peer-to-peer connection with this + * node. advertise() returns a Publisher object which allows you to + * publish messages on that topic through a call to publish(). Once + * all copies of the returned Publisher object are destroyed, the topic + * will be automatically unadvertised. + * + * The second parameter to advertise() is the size of the message queue + * used for publishing messages. If messages are published more quickly + * than we can send them, the number here specifies how many messages to + * buffer up before throwing some away. + */ + ros::Publisher rc_channels_pub = n.advertise("rc_channels", 1000); -// %Tag(LOOP_RATE)% - ros::Rate loop_rate(10); -// %EndTag(LOOP_RATE)% + px4::Rate loop_rate(10); - /** - * A count of how many messages we have sent. This is used to create - * a unique string for each message. - */ -// %Tag(ROS_OK)% - int count = 0; - while (ros::ok()) - { -// %EndTag(ROS_OK)% - /** - * This is a message object. You stuff it with data, and then publish it. - */ -// %Tag(FILL_MESSAGE)% - px4::rc_channels msg; + /** + * A count of how many messages we have sent. This is used to create + * a unique string for each message. + */ + int count = 0; - ros::Time time = ros::Time::now(); - msg.timestamp_last_valid = time.sec * 1e6 + time.nsec; -// %EndTag(FILL_MESSAGE)% + while (px4::ok()) { + /** + * This is a message object. You stuff it with data, and then publish it. + */ + px4::rc_channels msg; -// %Tag(ROSCONSOLE)% - px4_warnx("%lu", msg.timestamp_last_valid); -// %EndTag(ROSCONSOLE)% + msg.timestamp_last_valid = px4::get_time_micros(); + PX4_INFO("%lu", msg.timestamp_last_valid); - /** - * The publish() function is how you send messages. The parameter - * is the message object. The type of this object must agree with the type - * given as a template parameter to the advertise<>() call, as was done - * in the constructor above. - */ -// %Tag(PUBLISH)% - rc_channels_pub.publish(msg); -// %EndTag(PUBLISH)% + /** + * The publish() function is how you send messages. The parameter + * is the message object. The type of this object must agree with the type + * given as a template parameter to the advertise<>() call, as was done + * in the constructor above. + */ + rc_channels_pub.publish(msg); -// %Tag(SPINONCE)% - ros::spinOnce(); -// %EndTag(SPINONCE)% + px4::spin_once(); -// %Tag(RATE_SLEEP)% - loop_rate.sleep(); -// %EndTag(RATE_SLEEP)% - ++count; - } + loop_rate.sleep(); + ++count; + } - return 0; + return 0; } -// %EndTag(FULLTEXT)% diff --git a/src/examples/subscriber/module.mk b/src/examples/subscriber/module.mk new file mode 100644 index 000000000..44d3d03a9 --- /dev/null +++ b/src/examples/subscriber/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Subscriber Example Application +# + +MODULE_COMMAND = subscriber + +SRCS = subscriber.cpp + +MODULE_STACKSIZE = 1200 diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index da69e6e25..bf16bf84e 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -25,70 +25,63 @@ * POSSIBILITY OF SUCH DAMAGE. */ -// %Tag(FULLTEXT)% -#include "ros/ros.h" -#include "std_msgs/String.h" +#include #include "px4/rc_channels.h" /** - * This tutorial demonstrates simple receipt of messages over the ROS system. + * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. */ -// %Tag(CALLBACK)% -void rc_channels_callback(const px4::rc_channels::ConstPtr& msg) +void rc_channels_callback(const px4::rc_channels::ConstPtr &msg) { - ROS_INFO("I heard: [%lu]", msg->timestamp_last_valid); + PX4_INFO("I heard: [%lu]", msg->timestamp_last_valid); } -// %EndTag(CALLBACK)% + +PX4_MAIN_FUNCTION(subscriber) int main(int argc, char **argv) { - /** - * The ros::init() function needs to see argc and argv so that it can perform - * any ROS arguments and name remapping that were provided at the command line. For programmatic - * remappings you can use a different version of init() which takes remappings - * directly, but for most command-line programs, passing argc and argv is the easiest - * way to do it. The third argument to init() is the name of the node. - * - * You must call one of the versions of ros::init() before using any other - * part of the ROS system. - */ - ros::init(argc, argv, "listener"); + /** + * The ros::init() function needs to see argc and argv so that it can perform + * any ROS arguments and name remapping that were provided at the command line. For programmatic + * remappings you can use a different version of init() which takes remappings + * directly, but for most command-line programs, passing argc and argv is the easiest + * way to do it. The third argument to init() is the name of the node. + * + * You must call one of the versions of px4::init() before using any other + * part of the PX4/ ROS system. + */ + px4::init(argc, argv, "listener"); - /** - * NodeHandle is the main access point to communications with the ROS system. - * The first NodeHandle constructed will fully initialize this node, and the last - * NodeHandle destructed will close down the node. - */ - ros::NodeHandle n; + /** + * NodeHandle is the main access point to communications with the ROS system. + * The first NodeHandle constructed will fully initialize this node, and the last + * NodeHandle destructed will close down the node. + */ + ros::NodeHandle n; - /** - * The subscribe() call is how you tell ROS that you want to receive messages - * on a given topic. This invokes a call to the ROS - * master node, which keeps a registry of who is publishing and who - * is subscribing. Messages are passed to a callback function, here - * called chatterCallback. subscribe() returns a Subscriber object that you - * must hold on to until you want to unsubscribe. When all copies of the Subscriber - * object go out of scope, this callback will automatically be unsubscribed from - * this topic. - * - * The second parameter to the subscribe() function is the size of the message - * queue. If messages are arriving faster than they are being processed, this - * is the number of messages that will be buffered up before beginning to throw - * away the oldest ones. - */ -// %Tag(SUBSCRIBER)% - ros::Subscriber sub = n.subscribe("rc_channels", 1000, rc_channels_callback); -// %EndTag(SUBSCRIBER)% + /** + * The subscribe() call is how you tell ROS that you want to receive messages + * on a given topic. This invokes a call to the ROS + * master node, which keeps a registry of who is publishing and who + * is subscribing. Messages are passed to a callback function, here + * called chatterCallback. subscribe() returns a Subscriber object that you + * must hold on to until you want to unsubscribe. When all copies of the Subscriber + * object go out of scope, this callback will automatically be unsubscribed from + * this topic. + * + * The second parameter to the subscribe() function is the size of the message + * queue. If messages are arriving faster than they are being processed, this + * is the number of messages that will be buffered up before beginning to throw + * away the oldest ones. + */ + ros::Subscriber sub = n.subscribe("rc_channels", 1000, rc_channels_callback); - /** - * ros::spin() will enter a loop, pumping callbacks. With this version, all - * callbacks will be called from within this thread (the main one). ros::spin() - * will exit when Ctrl-C is pressed, or the node is shutdown by the master. - */ -// %Tag(SPIN)% - ros::spin(); -// %EndTag(SPIN)% + /** + * 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. + */ + px4::spin(); - return 0; + return 0; } -// %EndTag(FULLTEXT)% diff --git a/src/include/px4.h b/src/include/px4.h new file mode 100644 index 000000000..0aba2ee77 --- /dev/null +++ b/src/include/px4.h @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * 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 px4.h + * + * Main system header with common convenience functions + */ + +#pragma once + +#include + +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/* + * Building for running within the ROS environment + */ +#include "ros/ros.h" +#define PX4_WARN ROS_WARN +#define PX4_INFO ROS_INFO +#else +/* + * Building for NuttX + */ +#define PX4_WARN warnx +#define PX4_INFO warnx +#endif + +#include "../platforms/px4_defines.h" +#include "../platforms/px4_middleware.h" diff --git a/src/platforms/empty.c b/src/platforms/empty.c new file mode 100644 index 000000000..139531354 --- /dev/null +++ b/src/platforms/empty.c @@ -0,0 +1,3 @@ +/* + * This is an empty C source file, used when building default firmware configurations. + */ diff --git a/src/platforms/nuttx/module.mk b/src/platforms/nuttx/module.mk new file mode 100644 index 000000000..128f0e734 --- /dev/null +++ b/src/platforms/nuttx/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# NuttX / uORB adapter library +# + +SRCS = px4_nuttx_impl.cpp \ + px4_publisher.cpp \ + px4_subscriber.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/platforms/nuttx/px4_nuttx_impl.cpp b/src/platforms/nuttx/px4_nuttx_impl.cpp new file mode 100644 index 000000000..3a6529716 --- /dev/null +++ b/src/platforms/nuttx/px4_nuttx_impl.cpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * 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 px4_nuttx_impl.cpp + * + * PX4 Middleware Wrapper NuttX Implementation + */ + +#include + +extern bool task_should_exit; + +namespace px4 +{ + +void init(int argc, char *argv[], const char *process_name) +{ + px4_warn("process: %s", process_name); + return 0; +} + +uint64_t get_time_micros() +{ + return hrt_absolute_time(); +} + +bool ok() +{ + return !task_should_exit; +} + +void spin_once() +{ + // XXX check linked list of topics with orb_check() here + +} + +void spin() +{ + // XXX block waiting for updated topics here + +} + +} diff --git a/src/platforms/nuttx/px4_publisher.cpp b/src/platforms/nuttx/px4_publisher.cpp new file mode 100644 index 000000000..ab6035b22 --- /dev/null +++ b/src/platforms/nuttx/px4_publisher.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * 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 px4_publisher.cpp + * + * PX4 Middleware Wrapper for Publisher + */ + + diff --git a/src/platforms/nuttx/px4_subscriber.cpp b/src/platforms/nuttx/px4_subscriber.cpp new file mode 100644 index 000000000..088c08fdb --- /dev/null +++ b/src/platforms/nuttx/px4_subscriber.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * 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 px4_subscriber.cpp + * + * PX4 Middleware Wrapper Subscriber + */ + + diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h new file mode 100644 index 000000000..48234766f --- /dev/null +++ b/src/platforms/px4_defines.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * 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 px4_defines.h + * + * Generally used magic defines + */ + +#pragma once + +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/* + * Building for running within the ROS environment + */ +#define __EXPORT +#define PX4_MAIN_FUNCTION(_prefix) +#else +#include +#define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##main(int argc, char **argv)() { return main(argc, argv); } +#endif diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h new file mode 100644 index 000000000..d1c0656af --- /dev/null +++ b/src/platforms/px4_middleware.h @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * 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 px4_middleware.h + * + * PX4 generic middleware wrapper + */ + +#pragma once + +#include + +namespace px4 +{ + +void init(int argc, char *argv[], const char *process_name); + +uint64_t get_time_micros(); + +bool ok(); + +void spin_once(); + +void spin(); + +class Rate +{ + +public: + explicit Rate(unsigned rate_hz) { sleep_interval = 1e6 / rate_hz; } + + void sleep() { usleep(sleep_interval); } + +private: + uint64_t sleep_interval; + +}; + +// /** +// * A limiter/ saturation. +// * The output of update is the input, bounded +// * by min/max. +// */ +// class __EXPORT BlockLimit : public Block +// { +// public: +// // methods +// BlockLimit(SuperBlock *parent, const char *name) : +// Block(parent, name), +// _min(this, "MIN"), +// _max(this, "MAX") +// {}; +// virtual ~BlockLimit() {}; +// float update(float input); +// // accessors +// float getMin() { return _min.get(); } +// float getMax() { return _max.get(); } +// protected: +// // attributes +// control::BlockParamFloat _min; +// control::BlockParamFloat _max; +// }; + +} // namespace px4 diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h new file mode 100644 index 000000000..d278828b7 --- /dev/null +++ b/src/platforms/px4_nodehandle.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * 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 px4_nodehandle.h + * + * PX4 Middleware Wrapper Node Handle + */ + +namespace px4 +{ +class NodeHandle +{ + +}; +} diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h new file mode 100644 index 000000000..1b0952155 --- /dev/null +++ b/src/platforms/px4_publisher.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * 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 px4_nodehandle.h + * + * PX4 Middleware Wrapper Node Handle + */ + +namespace px4 +{ +class Publisher +{ + +}; +} diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h new file mode 100644 index 000000000..8759f8b05 --- /dev/null +++ b/src/platforms/px4_subscriber.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * 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 px4_subscriber.h + * + * PX4 Middleware Wrapper Subscriber + */ + +namespace px4 +{ +class Subscriber +{ + +}; +} diff --git a/src/platforms/ros/px4_publisher.cpp b/src/platforms/ros/px4_publisher.cpp new file mode 100644 index 000000000..ab6035b22 --- /dev/null +++ b/src/platforms/ros/px4_publisher.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * 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 px4_publisher.cpp + * + * PX4 Middleware Wrapper for Publisher + */ + + diff --git a/src/platforms/ros/px4_ros_impl.cpp b/src/platforms/ros/px4_ros_impl.cpp new file mode 100644 index 000000000..eda17e5a9 --- /dev/null +++ b/src/platforms/ros/px4_ros_impl.cpp @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * 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 px4_ros_impl.cpp + * + * PX4 Middleware Wrapper ROS Implementation + */ + +#include + +namespace px4 +{ + +void init(int argc, char *argv[], const char *process_name) +{ + ros::init(argc, argv, process_name); +} + +uint64_t get_time_micros() +{ + ros::Time time = ros::Time::now(); + return time.sec * 1e6 + time.nsec / 1000; +} + +bool ok() +{ + return ros::ok(); +} + +void spin_once() +{ + ros::spinOnce(); +} + +void spin() +{ + ros::spin(); +} + +} diff --git a/src/platforms/ros/px4_subscriber.cpp b/src/platforms/ros/px4_subscriber.cpp new file mode 100644 index 000000000..088c08fdb --- /dev/null +++ b/src/platforms/ros/px4_subscriber.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * 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 px4_subscriber.cpp + * + * PX4 Middleware Wrapper Subscriber + */ + + -- cgit v1.2.3 From c9a7850b93643eee7a7ddb145972c4cb36f5db66 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Nov 2014 16:10:01 +0100 Subject: do not include eigen lib from local source --- src/lib/mathlib/math/Matrix.hpp | 2 +- src/lib/mathlib/math/Vector.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 34fc4604a..832be66db 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -50,7 +50,7 @@ #include "../CMSIS/Include/arm_math.h" #else #include -#include +#include #define M_PI_2_F 1.570769 #endif diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 51a84d27e..b0b03980d 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -50,7 +50,7 @@ #include "../CMSIS/Include/arm_math.h" #else #include -#include +#include #endif namespace math -- cgit v1.2.3 From f72f0db9963049879eb0dce9a9ef37baadd45dd2 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Nov 2014 16:10:43 +0100 Subject: include correct eigen lib header --- src/modules/mc_att_control/mc_att_control_base.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 8f1cf015e..5a08bba79 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -76,7 +76,7 @@ #include //#include #include -#include +//#include -- cgit v1.2.3 From a2ec2ec857f5b7584628c2a923c85d0b62b082dd Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Nov 2014 16:58:56 +0100 Subject: cleaned up --- src/modules/mc_att_control/mc_att_control_base.h | 58 +++--------------------- 1 file changed, 6 insertions(+), 52 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 5a08bba79..515fb0c14 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -1,14 +1,7 @@ -/* - * mc_att_control_base.h - * - * Created on: Sep 25, 2014 - * Author: roman - */ - #ifndef MC_ATT_CONTROL_BASE_H_ #define MC_ATT_CONTROL_BASE_H_ -/* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. +/* 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 @@ -40,21 +33,10 @@ ****************************************************************************/ /** - * @file mc_att_control_main.cpp - * Multicopter attitude controller. - * - * @author Tobias Naegeli - * @author Lorenz Meier - * @author Anton Babushkin + * @file mc_att_control_base.h + * + * @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 @@ -74,18 +56,10 @@ #include #include #include -//#include #include -//#include -/** - * Multicopter attitude control app start / stop handling function - * - * @ingroup apps - */ - #define YAW_DEADZONE 0.05f #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f @@ -98,7 +72,7 @@ public: MulticopterAttitudeControlBase(); /** - * Destructor, also kills the sensors task. + * Destructor */ ~MulticopterAttitudeControlBase(); @@ -109,6 +83,7 @@ public: */ void control_attitude(float dt); void control_attitude_rates(float dt); + // setters and getters for interface with euroc-gazebo simulator void set_attitude(const Eigen::Quaternion attitude); void set_attitude_rates(const Eigen::Vector3d& angular_rate); @@ -120,10 +95,6 @@ protected: bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ - - - - bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ struct vehicle_attitude_s _v_att; /**< vehicle attitude */ @@ -146,8 +117,6 @@ protected: bool _reset_yaw_sp; /**< reset yaw setpoint flag */ - - struct { math::Vector<3> att_p; /**< P gain for angular error */ math::Vector<3> rate_p; /**< P gain for angular rate error */ @@ -161,24 +130,9 @@ protected: float man_yaw_max; math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ } _params; - - - /** - * Attitude controller. - */ - //void control_attitude(float dt); - - /** - * Attitude rates controller. - */ void vehicle_attitude_setpoint_poll(); //provisional - - - - - }; #endif /* MC_ATT_CONTROL_BASE_H_ */ -- cgit v1.2.3 From 0acdf5927b9e2dd2cc7375008af2d00cd7f9ba0e Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Nov 2014 17:01:59 +0100 Subject: cleaned up --- src/modules/mc_att_control/mc_att_control_base.cpp | 44 +++++++++++++++++----- 1 file changed, 35 insertions(+), 9 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index b07a1a6be..baf2bfe65 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -1,8 +1,39 @@ -/* - * mc_att_control_base.cpp +/* 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_base.h + * + * @author Roman Bapst * - * Created on: Sep 25, 2014 - * Author: roman */ #include "mc_att_control_base.h" @@ -308,11 +339,9 @@ void MulticopterAttitudeControlBase::set_actuator_controls() { _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(); } void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion attitude) { - // check if this is consistent !!! math::Quaternion quat; quat(0) = (float)attitude.w(); quat(1) = (float)attitude.x(); @@ -336,7 +365,6 @@ void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion Date: Mon, 10 Nov 2014 17:06:14 +0100 Subject: cleaned up --- src/modules/fw_att_control/fw_att_control_base.cpp | 39 ++++++++++++-- src/modules/fw_att_control/fw_att_control_base.h | 59 +++++++++++++++------- 2 files changed, 75 insertions(+), 23 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp index 7218e4e9b..d8ba15969 100644 --- a/src/modules/fw_att_control/fw_att_control_base.cpp +++ b/src/modules/fw_att_control/fw_att_control_base.cpp @@ -1,8 +1,39 @@ -/* - * fw_att_control_base.cpp +/* 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_base.cpp + * + * @author Roman Bapst * - * Created on: Sep 24, 2014 - * Author: roman */ #include "fw_att_control_base.h" diff --git a/src/modules/fw_att_control/fw_att_control_base.h b/src/modules/fw_att_control/fw_att_control_base.h index 6e071fe20..6b2efc46b 100644 --- a/src/modules/fw_att_control/fw_att_control_base.h +++ b/src/modules/fw_att_control/fw_att_control_base.h @@ -1,13 +1,44 @@ -/* - * fw_att_control_base.h - * - * Created on: Sep 24, 2014 - * Author: roman - */ - #ifndef FW_ATT_CONTROL_BASE_H_ #define FW_ATT_CONTROL_BASE_H_ +/* 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 fw_att_control_base.h + * + * @author Roman Bapst + * + */ + #include #include #include @@ -22,8 +53,8 @@ #include #include #include -#include +#include #include class FixedwingAttitudeControlBase @@ -35,7 +66,7 @@ public: FixedwingAttitudeControlBase(); /** - * Destructor, also kills the sensors task. + * Destructor */ ~FixedwingAttitudeControlBase(); @@ -46,8 +77,6 @@ protected: bool _task_running; /**< if true, task is running in its mainloop */ int _control_task; /**< task handle for sensor task */ - - struct vehicle_attitude_s _att; /**< vehicle attitude */ struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ @@ -107,20 +136,12 @@ protected: } _parameters; /**< local copies of interesting parameters */ - - - ECL_RollController _roll_ctrl; ECL_PitchController _pitch_ctrl; ECL_YawController _yaw_ctrl; void control_attitude(); - - - }; - - #endif /* FW_ATT_CONTROL_BASE_H_ */ -- cgit v1.2.3 From 403fe886845b8e3da150bdc45e47c4e4b29cea96 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Nov 2014 17:24:48 +0100 Subject: deleted folder that has found its way into the repo --- build/CATKIN_IGNORE | 0 build/CMakeCache.txt | 450 --------------------- build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake | 56 --- build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake | 57 --- .../2.8.12.2/CMakeDetermineCompilerABI_C.bin | Bin 8547 -> 0 bytes .../2.8.12.2/CMakeDetermineCompilerABI_CXX.bin | Bin 8560 -> 0 bytes build/CMakeFiles/2.8.12.2/CMakeSystem.cmake | 15 - .../2.8.12.2/CompilerIdC/CMakeCCompilerId.c | 389 ------------------ build/CMakeFiles/2.8.12.2/CompilerIdC/a.out | Bin 8643 -> 0 bytes .../2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp | 377 ----------------- build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out | Bin 8652 -> 0 bytes build/CMakeFiles/CMakeDirectoryInformation.cmake | 16 - build/CMakeFiles/CMakeError.log | 53 --- build/CMakeFiles/CMakeOutput.log | 293 -------------- build/CMakeFiles/CMakeRuleHashes.txt | 5 - build/CMakeFiles/Makefile.cmake | 150 ------- build/CMakeFiles/Makefile2 | 266 ------------ build/CMakeFiles/TargetDirectories.txt | 6 - .../clean_test_results.dir/DependInfo.cmake | 20 - build/CMakeFiles/clean_test_results.dir/build.make | 66 --- .../clean_test_results.dir/cmake_clean.cmake | 8 - .../clean_test_results.dir/progress.make | 1 - build/CMakeFiles/cmake.check_cache | 1 - build/CMakeFiles/doxygen.dir/DependInfo.cmake | 20 - build/CMakeFiles/doxygen.dir/build.make | 65 --- build/CMakeFiles/doxygen.dir/cmake_clean.cmake | 8 - build/CMakeFiles/doxygen.dir/progress.make | 1 - build/CMakeFiles/progress.marks | 1 - build/CMakeFiles/run_tests.dir/DependInfo.cmake | 20 - build/CMakeFiles/run_tests.dir/build.make | 65 --- build/CMakeFiles/run_tests.dir/cmake_clean.cmake | 8 - build/CMakeFiles/run_tests.dir/progress.make | 1 - build/CMakeFiles/tests.dir/DependInfo.cmake | 20 - build/CMakeFiles/tests.dir/build.make | 65 --- build/CMakeFiles/tests.dir/cmake_clean.cmake | 8 - build/CMakeFiles/tests.dir/progress.make | 1 - build/CTestTestfile.cmake | 7 - build/Makefile | 262 ------------ .../catkin/catkin_generated/version/package.cmake | 9 - build/catkin_generated/env_cached.sh | 16 - build/catkin_generated/generate_cached_setup.py | 29 -- build/catkin_generated/installspace/.rosinstall | 2 - build/catkin_generated/installspace/_setup_util.py | 287 ------------- build/catkin_generated/installspace/env.sh | 16 - build/catkin_generated/installspace/setup.bash | 8 - build/catkin_generated/installspace/setup.sh | 87 ---- build/catkin_generated/installspace/setup.zsh | 8 - build/catkin_generated/order_packages.cmake | 10 - build/catkin_generated/order_packages.py | 5 - build/catkin_generated/setup_cached.sh | 20 - .../Project/interrogate_setup_dot_py.py.stamp | 250 ------------ .../stamps/Project/order_packages.cmake.em.stamp | 56 --- .../stamps/Project/package.xml.stamp | 37 -- build/catkin_make.cache | 1 - build/cmake_install.cmake | 140 ------- .../CMakeFiles/CMakeDirectoryInformation.cmake | 16 - build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake | 27 -- build/gtest/CMakeFiles/gtest.dir/build.make | 102 ----- build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake | 10 - build/gtest/CMakeFiles/gtest.dir/depend.make | 2 - build/gtest/CMakeFiles/gtest.dir/flags.make | 8 - build/gtest/CMakeFiles/gtest.dir/link.txt | 1 - build/gtest/CMakeFiles/gtest.dir/progress.make | 2 - .../CMakeFiles/gtest_main.dir/DependInfo.cmake | 28 -- build/gtest/CMakeFiles/gtest_main.dir/build.make | 103 ----- .../CMakeFiles/gtest_main.dir/cmake_clean.cmake | 10 - build/gtest/CMakeFiles/gtest_main.dir/depend.make | 2 - build/gtest/CMakeFiles/gtest_main.dir/flags.make | 8 - build/gtest/CMakeFiles/gtest_main.dir/link.txt | 1 - .../gtest/CMakeFiles/gtest_main.dir/progress.make | 2 - build/gtest/CMakeFiles/progress.marks | 1 - build/gtest/CTestTestfile.cmake | 6 - build/gtest/Makefile | 262 ------------ build/gtest/cmake_install.cmake | 34 -- 74 files changed, 4387 deletions(-) delete mode 100644 build/CATKIN_IGNORE delete mode 100644 build/CMakeCache.txt delete mode 100644 build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake delete mode 100644 build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake delete mode 100755 build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin delete mode 100755 build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin delete mode 100644 build/CMakeFiles/2.8.12.2/CMakeSystem.cmake delete mode 100644 build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c delete mode 100755 build/CMakeFiles/2.8.12.2/CompilerIdC/a.out delete mode 100644 build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp delete mode 100755 build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out delete mode 100644 build/CMakeFiles/CMakeDirectoryInformation.cmake delete mode 100644 build/CMakeFiles/CMakeError.log delete mode 100644 build/CMakeFiles/CMakeOutput.log delete mode 100644 build/CMakeFiles/CMakeRuleHashes.txt delete mode 100644 build/CMakeFiles/Makefile.cmake delete mode 100644 build/CMakeFiles/Makefile2 delete mode 100644 build/CMakeFiles/TargetDirectories.txt delete mode 100644 build/CMakeFiles/clean_test_results.dir/DependInfo.cmake delete mode 100644 build/CMakeFiles/clean_test_results.dir/build.make delete mode 100644 build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake delete mode 100644 build/CMakeFiles/clean_test_results.dir/progress.make delete mode 100644 build/CMakeFiles/cmake.check_cache delete mode 100644 build/CMakeFiles/doxygen.dir/DependInfo.cmake delete mode 100644 build/CMakeFiles/doxygen.dir/build.make delete mode 100644 build/CMakeFiles/doxygen.dir/cmake_clean.cmake delete mode 100644 build/CMakeFiles/doxygen.dir/progress.make delete mode 100644 build/CMakeFiles/progress.marks delete mode 100644 build/CMakeFiles/run_tests.dir/DependInfo.cmake delete mode 100644 build/CMakeFiles/run_tests.dir/build.make delete mode 100644 build/CMakeFiles/run_tests.dir/cmake_clean.cmake delete mode 100644 build/CMakeFiles/run_tests.dir/progress.make delete mode 100644 build/CMakeFiles/tests.dir/DependInfo.cmake delete mode 100644 build/CMakeFiles/tests.dir/build.make delete mode 100644 build/CMakeFiles/tests.dir/cmake_clean.cmake delete mode 100644 build/CMakeFiles/tests.dir/progress.make delete mode 100644 build/CTestTestfile.cmake delete mode 100644 build/Makefile delete mode 100644 build/catkin/catkin_generated/version/package.cmake delete mode 100755 build/catkin_generated/env_cached.sh delete mode 100644 build/catkin_generated/generate_cached_setup.py delete mode 100644 build/catkin_generated/installspace/.rosinstall delete mode 100755 build/catkin_generated/installspace/_setup_util.py delete mode 100755 build/catkin_generated/installspace/env.sh delete mode 100644 build/catkin_generated/installspace/setup.bash delete mode 100644 build/catkin_generated/installspace/setup.sh delete mode 100644 build/catkin_generated/installspace/setup.zsh delete mode 100644 build/catkin_generated/order_packages.cmake delete mode 100644 build/catkin_generated/order_packages.py delete mode 100755 build/catkin_generated/setup_cached.sh delete mode 100644 build/catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp delete mode 100644 build/catkin_generated/stamps/Project/order_packages.cmake.em.stamp delete mode 100644 build/catkin_generated/stamps/Project/package.xml.stamp delete mode 100644 build/catkin_make.cache delete mode 100644 build/cmake_install.cmake delete mode 100644 build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake delete mode 100644 build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake delete mode 100644 build/gtest/CMakeFiles/gtest.dir/build.make delete mode 100644 build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake delete mode 100644 build/gtest/CMakeFiles/gtest.dir/depend.make delete mode 100644 build/gtest/CMakeFiles/gtest.dir/flags.make delete mode 100644 build/gtest/CMakeFiles/gtest.dir/link.txt delete mode 100644 build/gtest/CMakeFiles/gtest.dir/progress.make delete mode 100644 build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake delete mode 100644 build/gtest/CMakeFiles/gtest_main.dir/build.make delete mode 100644 build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake delete mode 100644 build/gtest/CMakeFiles/gtest_main.dir/depend.make delete mode 100644 build/gtest/CMakeFiles/gtest_main.dir/flags.make delete mode 100644 build/gtest/CMakeFiles/gtest_main.dir/link.txt delete mode 100644 build/gtest/CMakeFiles/gtest_main.dir/progress.make delete mode 100644 build/gtest/CMakeFiles/progress.marks delete mode 100644 build/gtest/CTestTestfile.cmake delete mode 100644 build/gtest/Makefile delete mode 100644 build/gtest/cmake_install.cmake diff --git a/build/CATKIN_IGNORE b/build/CATKIN_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/build/CMakeCache.txt b/build/CMakeCache.txt deleted file mode 100644 index 8fc5c4b4a..000000000 --- a/build/CMakeCache.txt +++ /dev/null @@ -1,450 +0,0 @@ -# This is the CMakeCache file. -# For build in directory: /home/roman/src/Firmware/build -# It was generated by CMake: /usr/bin/cmake -# You can edit this file to change values found and used by cmake. -# If you do not want to change any of the values, simply exit the editor. -# If you do want to change a value, simply edit, save, and exit the editor. -# The syntax for the file is as follows: -# KEY:TYPE=VALUE -# KEY is the name of a variable in the cache. -# TYPE is a hint to GUIs for the type of VALUE, DO NOT EDIT TYPE!. -# VALUE is the current value for the KEY. - -######################## -# EXTERNAL cache entries -######################## - -//Build shared libraries (DLLs). -BUILD_SHARED_LIBS:BOOL=ON - -//List of ';' separated packages to exclude -CATKIN_BLACKLIST_PACKAGES:STRING= - -//catkin devel space -CATKIN_DEVEL_PREFIX:PATH=/home/roman/src/Firmware/devel - -//Catkin enable testing -CATKIN_ENABLE_TESTING:BOOL=ON - -//Catkin skip testing -CATKIN_SKIP_TESTING:BOOL=OFF - -//List of ';' separated packages to build -CATKIN_WHITELIST_PACKAGES:STRING= - -//Path to a program. -CMAKE_AR:FILEPATH=/usr/bin/ar - -//Choose the type of build, options are: None(CMAKE_CXX_FLAGS or -// CMAKE_C_FLAGS used) Debug Release RelWithDebInfo MinSizeRel. -CMAKE_BUILD_TYPE:STRING= - -//Enable/Disable color output during build. -CMAKE_COLOR_MAKEFILE:BOOL=ON - -//CXX compiler. -CMAKE_CXX_COMPILER:FILEPATH=/usr/bin/c++ - -//Flags used by the compiler during all build types. -CMAKE_CXX_FLAGS:STRING= - -//Flags used by the compiler during debug builds. -CMAKE_CXX_FLAGS_DEBUG:STRING=-g - -//Flags used by the compiler during release minsize builds. -CMAKE_CXX_FLAGS_MINSIZEREL:STRING=-Os -DNDEBUG - -//Flags used by the compiler during release builds (/MD /Ob1 /Oi -// /Ot /Oy /Gs will produce slightly less optimized but smaller -// files). -CMAKE_CXX_FLAGS_RELEASE:STRING=-O3 -DNDEBUG - -//Flags used by the compiler during Release with Debug Info builds. -CMAKE_CXX_FLAGS_RELWITHDEBINFO:STRING=-O2 -g -DNDEBUG - -//C compiler. -CMAKE_C_COMPILER:FILEPATH=/usr/bin/cc - -//Flags used by the compiler during all build types. -CMAKE_C_FLAGS:STRING= - -//Flags used by the compiler during debug builds. -CMAKE_C_FLAGS_DEBUG:STRING=-g - -//Flags used by the compiler during release minsize builds. -CMAKE_C_FLAGS_MINSIZEREL:STRING=-Os -DNDEBUG - -//Flags used by the compiler during release builds (/MD /Ob1 /Oi -// /Ot /Oy /Gs will produce slightly less optimized but smaller -// files). -CMAKE_C_FLAGS_RELEASE:STRING=-O3 -DNDEBUG - -//Flags used by the compiler during Release with Debug Info builds. -CMAKE_C_FLAGS_RELWITHDEBINFO:STRING=-O2 -g -DNDEBUG - -//Flags used by the linker. -CMAKE_EXE_LINKER_FLAGS:STRING=' ' - -//Flags used by the linker during debug builds. -CMAKE_EXE_LINKER_FLAGS_DEBUG:STRING= - -//Flags used by the linker during release minsize builds. -CMAKE_EXE_LINKER_FLAGS_MINSIZEREL:STRING= - -//Flags used by the linker during release builds. -CMAKE_EXE_LINKER_FLAGS_RELEASE:STRING= - -//Flags used by the linker during Release with Debug Info builds. -CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO:STRING= - -//Enable/Disable output of compile commands during generation. -CMAKE_EXPORT_COMPILE_COMMANDS:BOOL=OFF - -//Install path prefix, prepended onto install directories. -CMAKE_INSTALL_PREFIX:PATH=/home/roman/src/Firmware/install - -//Path to a program. -CMAKE_LINKER:FILEPATH=/usr/bin/ld - -//Path to a program. -CMAKE_MAKE_PROGRAM:FILEPATH=/usr/bin/make - -//Flags used by the linker during the creation of modules. -CMAKE_MODULE_LINKER_FLAGS:STRING=' ' - -//Flags used by the linker during debug builds. -CMAKE_MODULE_LINKER_FLAGS_DEBUG:STRING= - -//Flags used by the linker during release minsize builds. -CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL:STRING= - -//Flags used by the linker during release builds. -CMAKE_MODULE_LINKER_FLAGS_RELEASE:STRING= - -//Flags used by the linker during Release with Debug Info builds. -CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO:STRING= - -//Path to a program. -CMAKE_NM:FILEPATH=/usr/bin/nm - -//Path to a program. -CMAKE_OBJCOPY:FILEPATH=/usr/bin/objcopy - -//Path to a program. -CMAKE_OBJDUMP:FILEPATH=/usr/bin/objdump - -//Value Computed by CMake -CMAKE_PROJECT_NAME:STATIC=Project - -//Path to a program. -CMAKE_RANLIB:FILEPATH=/usr/bin/ranlib - -//Flags used by the linker during the creation of dll's. -CMAKE_SHARED_LINKER_FLAGS:STRING=' ' - -//Flags used by the linker during debug builds. -CMAKE_SHARED_LINKER_FLAGS_DEBUG:STRING= - -//Flags used by the linker during release minsize builds. -CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL:STRING= - -//Flags used by the linker during release builds. -CMAKE_SHARED_LINKER_FLAGS_RELEASE:STRING= - -//Flags used by the linker during Release with Debug Info builds. -CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO:STRING= - -//If set, runtime paths are not added when installing shared libraries, -// but are added when building. -CMAKE_SKIP_INSTALL_RPATH:BOOL=NO - -//If set, runtime paths are not added when using shared libraries. -CMAKE_SKIP_RPATH:BOOL=NO - -//Flags used by the linker during the creation of static libraries. -CMAKE_STATIC_LINKER_FLAGS:STRING= - -//Flags used by the linker during debug builds. -CMAKE_STATIC_LINKER_FLAGS_DEBUG:STRING= - -//Flags used by the linker during release minsize builds. -CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL:STRING= - -//Flags used by the linker during release builds. -CMAKE_STATIC_LINKER_FLAGS_RELEASE:STRING= - -//Flags used by the linker during Release with Debug Info builds. -CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO:STRING= - -//Path to a program. -CMAKE_STRIP:FILEPATH=/usr/bin/strip - -//If true, cmake will use relative paths in makefiles and projects. -CMAKE_USE_RELATIVE_PATHS:BOOL=OFF - -//If this value is on, makefiles will be generated without the -// .SILENT directive, and all commands will be echoed to the console -// during the make. This is useful for debugging only. With Visual -// Studio IDE projects all commands are done without /nologo. -CMAKE_VERBOSE_MAKEFILE:BOOL=FALSE - -//Path to a program. -DOXYGEN_EXECUTABLE:FILEPATH=DOXYGEN_EXECUTABLE-NOTFOUND - -//Path to a program. -EMPY_EXECUTABLE:FILEPATH=/usr/bin/empy - -//Empy script -EMPY_SCRIPT:STRING=/usr/bin/empy - -//Path to a file. -GTEST_INCLUDE_DIR:PATH=/usr/include - -//Path to a library. -GTEST_LIBRARY:FILEPATH=GTEST_LIBRARY-NOTFOUND - -//Path to a library. -GTEST_LIBRARY_DEBUG:FILEPATH=GTEST_LIBRARY_DEBUG-NOTFOUND - -//Path to a library. -GTEST_MAIN_LIBRARY:FILEPATH=GTEST_MAIN_LIBRARY-NOTFOUND - -//Path to a library. -GTEST_MAIN_LIBRARY_DEBUG:FILEPATH=GTEST_MAIN_LIBRARY_DEBUG-NOTFOUND - -//lsb_release executable was found -LSB_FOUND:BOOL=TRUE - -//Path to a program. -LSB_RELEASE_EXECUTABLE:FILEPATH=/usr/bin/lsb_release - -//Path to a program. -NOSETESTS:FILEPATH=/usr/bin/nosetests-2.7 - -//Path to a program. -PYTHON_EXECUTABLE:FILEPATH=/usr/bin/python - -//Specify specific Python version to use ('major.minor' or 'major') -PYTHON_VERSION:STRING= - -//Value Computed by CMake -Project_BINARY_DIR:STATIC=/home/roman/src/Firmware/build - -//Value Computed by CMake -Project_SOURCE_DIR:STATIC=/home/roman/src/Firmware/src - -//Path to a library. -RT_LIBRARY:FILEPATH=/usr/lib/x86_64-linux-gnu/librt.so - -//Enable debian style python package layout -SETUPTOOLS_DEB_LAYOUT:BOOL=ON - -//LSB Distrib tag -UBUNTU:BOOL=TRUE - -//LSB Distrib - codename tag -UBUNTU_TRUSTY:BOOL=TRUE - -//Path to a file. -_CATKIN_GTEST_INCLUDE:FILEPATH=/usr/include/gtest/gtest.h - -//Path to a file. -_CATKIN_GTEST_SRC:FILEPATH=/usr/src/gtest/src/gtest.cc - -//The directory containing a CMake configuration file for catkin. -catkin_DIR:PATH=/opt/ros/indigo/share/catkin/cmake - -//Value Computed by CMake -gtest_BINARY_DIR:STATIC=/home/roman/src/Firmware/build/gtest - -//Dependencies for the target -gtest_LIB_DEPENDS:STATIC=general;-lpthread; - -//Value Computed by CMake -gtest_SOURCE_DIR:STATIC=/usr/src/gtest - -//Build gtest's sample programs. -gtest_build_samples:BOOL=OFF - -//Build all of gtest's own tests. -gtest_build_tests:BOOL=OFF - -//Disable uses of pthreads in gtest. -gtest_disable_pthreads:BOOL=OFF - -//Use shared (DLL) run-time lib even when Google Test is built -// as static lib. -gtest_force_shared_crt:BOOL=OFF - -//Dependencies for the target -gtest_main_LIB_DEPENDS:STATIC=general;-lpthread;general;gtest; - - -######################## -# INTERNAL cache entries -######################## - -//catkin environment -CATKIN_ENV:INTERNAL=/home/roman/src/Firmware/build/catkin_generated/env_cached.sh -CATKIN_TEST_RESULTS_DIR:INTERNAL=/home/roman/src/Firmware/build/test_results -//ADVANCED property for variable: CMAKE_AR -CMAKE_AR-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_BUILD_TOOL -CMAKE_BUILD_TOOL-ADVANCED:INTERNAL=1 -//What is the target build tool cmake is generating for. -CMAKE_BUILD_TOOL:INTERNAL=/usr/bin/make -//This is the directory where this CMakeCache.txt was created -CMAKE_CACHEFILE_DIR:INTERNAL=/home/roman/src/Firmware/build -//Major version of cmake used to create the current loaded cache -CMAKE_CACHE_MAJOR_VERSION:INTERNAL=2 -//Minor version of cmake used to create the current loaded cache -CMAKE_CACHE_MINOR_VERSION:INTERNAL=8 -//Patch version of cmake used to create the current loaded cache -CMAKE_CACHE_PATCH_VERSION:INTERNAL=12 -//ADVANCED property for variable: CMAKE_COLOR_MAKEFILE -CMAKE_COLOR_MAKEFILE-ADVANCED:INTERNAL=1 -//Path to CMake executable. -CMAKE_COMMAND:INTERNAL=/usr/bin/cmake -//Path to cpack program executable. -CMAKE_CPACK_COMMAND:INTERNAL=/usr/bin/cpack -//Path to ctest program executable. -CMAKE_CTEST_COMMAND:INTERNAL=/usr/bin/ctest -//ADVANCED property for variable: CMAKE_CXX_COMPILER -CMAKE_CXX_COMPILER-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_CXX_FLAGS -CMAKE_CXX_FLAGS-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_CXX_FLAGS_DEBUG -CMAKE_CXX_FLAGS_DEBUG-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_CXX_FLAGS_MINSIZEREL -CMAKE_CXX_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_CXX_FLAGS_RELEASE -CMAKE_CXX_FLAGS_RELEASE-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_CXX_FLAGS_RELWITHDEBINFO -CMAKE_CXX_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_C_COMPILER -CMAKE_C_COMPILER-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_C_FLAGS -CMAKE_C_FLAGS-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_C_FLAGS_DEBUG -CMAKE_C_FLAGS_DEBUG-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_C_FLAGS_MINSIZEREL -CMAKE_C_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_C_FLAGS_RELEASE -CMAKE_C_FLAGS_RELEASE-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_C_FLAGS_RELWITHDEBINFO -CMAKE_C_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 -//Executable file format -CMAKE_EXECUTABLE_FORMAT:INTERNAL=ELF -//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS -CMAKE_EXE_LINKER_FLAGS-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_DEBUG -CMAKE_EXE_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_MINSIZEREL -CMAKE_EXE_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_RELEASE -CMAKE_EXE_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO -CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_EXPORT_COMPILE_COMMANDS -CMAKE_EXPORT_COMPILE_COMMANDS-ADVANCED:INTERNAL=1 -//Name of generator. -CMAKE_GENERATOR:INTERNAL=Unix Makefiles -//Name of generator toolset. -CMAKE_GENERATOR_TOOLSET:INTERNAL= -//Have symbol pthread_create -CMAKE_HAVE_LIBC_CREATE:INTERNAL= -//Have library pthreads -CMAKE_HAVE_PTHREADS_CREATE:INTERNAL= -//Have library pthread -CMAKE_HAVE_PTHREAD_CREATE:INTERNAL=1 -//Have include pthread.h -CMAKE_HAVE_PTHREAD_H:INTERNAL=1 -//Start directory with the top level CMakeLists.txt file for this -// project -CMAKE_HOME_DIRECTORY:INTERNAL=/home/roman/src/Firmware/src -//Install .so files without execute permission. -CMAKE_INSTALL_SO_NO_EXE:INTERNAL=1 -//ADVANCED property for variable: CMAKE_LINKER -CMAKE_LINKER-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_MAKE_PROGRAM -CMAKE_MAKE_PROGRAM-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS -CMAKE_MODULE_LINKER_FLAGS-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_DEBUG -CMAKE_MODULE_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL -CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_RELEASE -CMAKE_MODULE_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO -CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_NM -CMAKE_NM-ADVANCED:INTERNAL=1 -//number of local generators -CMAKE_NUMBER_OF_LOCAL_GENERATORS:INTERNAL=2 -//ADVANCED property for variable: CMAKE_OBJCOPY -CMAKE_OBJCOPY-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_OBJDUMP -CMAKE_OBJDUMP-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_RANLIB -CMAKE_RANLIB-ADVANCED:INTERNAL=1 -//Path to CMake installation. -CMAKE_ROOT:INTERNAL=/usr/share/cmake-2.8 -//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS -CMAKE_SHARED_LINKER_FLAGS-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_DEBUG -CMAKE_SHARED_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL -CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_RELEASE -CMAKE_SHARED_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO -CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_SKIP_INSTALL_RPATH -CMAKE_SKIP_INSTALL_RPATH-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_SKIP_RPATH -CMAKE_SKIP_RPATH-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS -CMAKE_STATIC_LINKER_FLAGS-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_DEBUG -CMAKE_STATIC_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL -CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_RELEASE -CMAKE_STATIC_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO -CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_STRIP -CMAKE_STRIP-ADVANCED:INTERNAL=1 -//uname command -CMAKE_UNAME:INTERNAL=/bin/uname -//ADVANCED property for variable: CMAKE_USE_RELATIVE_PATHS -CMAKE_USE_RELATIVE_PATHS-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: CMAKE_VERBOSE_MAKEFILE -CMAKE_VERBOSE_MAKEFILE-ADVANCED:INTERNAL=1 -//Details about finding PythonInterp -FIND_PACKAGE_MESSAGE_DETAILS_PythonInterp:INTERNAL=[/usr/bin/python][v2.7.6()] -//Details about finding Threads -FIND_PACKAGE_MESSAGE_DETAILS_Threads:INTERNAL=[TRUE][v()] -GTEST_FROM_SOURCE_FOUND:INTERNAL=TRUE -GTEST_FROM_SOURCE_INCLUDE_DIRS:INTERNAL=/usr/include -GTEST_FROM_SOURCE_LIBRARIES:INTERNAL=gtest -GTEST_FROM_SOURCE_LIBRARY_DIRS:INTERNAL=/home/roman/src/Firmware/build/gtest -GTEST_FROM_SOURCE_MAIN_LIBRARIES:INTERNAL=gtest_main -//ADVANCED property for variable: GTEST_INCLUDE_DIR -GTEST_INCLUDE_DIR-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: GTEST_LIBRARY -GTEST_LIBRARY-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: GTEST_LIBRARY_DEBUG -GTEST_LIBRARY_DEBUG-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: GTEST_MAIN_LIBRARY -GTEST_MAIN_LIBRARY-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: GTEST_MAIN_LIBRARY_DEBUG -GTEST_MAIN_LIBRARY_DEBUG-ADVANCED:INTERNAL=1 -//ADVANCED property for variable: PYTHON_EXECUTABLE -PYTHON_EXECUTABLE-ADVANCED:INTERNAL=1 -//This needs to be in PYTHONPATH when 'setup.py install' is called. -// And it needs to match. But setuptools won't tell us where -// it will install things. -PYTHON_INSTALL_DIR:INTERNAL=lib/python2.7/dist-packages - diff --git a/build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake b/build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake deleted file mode 100644 index 83254ce49..000000000 --- a/build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake +++ /dev/null @@ -1,56 +0,0 @@ -set(CMAKE_C_COMPILER "/usr/bin/cc") -set(CMAKE_C_COMPILER_ARG1 "") -set(CMAKE_C_COMPILER_ID "GNU") -set(CMAKE_C_COMPILER_VERSION "4.8.2") -set(CMAKE_C_PLATFORM_ID "Linux") - -set(CMAKE_AR "/usr/bin/ar") -set(CMAKE_RANLIB "/usr/bin/ranlib") -set(CMAKE_LINKER "/usr/bin/ld") -set(CMAKE_COMPILER_IS_GNUCC 1) -set(CMAKE_C_COMPILER_LOADED 1) -set(CMAKE_C_COMPILER_WORKS TRUE) -set(CMAKE_C_ABI_COMPILED TRUE) -set(CMAKE_COMPILER_IS_MINGW ) -set(CMAKE_COMPILER_IS_CYGWIN ) -if(CMAKE_COMPILER_IS_CYGWIN) - set(CYGWIN 1) - set(UNIX 1) -endif() - -set(CMAKE_C_COMPILER_ENV_VAR "CC") - -if(CMAKE_COMPILER_IS_MINGW) - set(MINGW 1) -endif() -set(CMAKE_C_COMPILER_ID_RUN 1) -set(CMAKE_C_SOURCE_FILE_EXTENSIONS c) -set(CMAKE_C_IGNORE_EXTENSIONS h;H;o;O;obj;OBJ;def;DEF;rc;RC) -set(CMAKE_C_LINKER_PREFERENCE 10) - -# Save compiler ABI information. -set(CMAKE_C_SIZEOF_DATA_PTR "8") -set(CMAKE_C_COMPILER_ABI "ELF") -set(CMAKE_C_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") - -if(CMAKE_C_SIZEOF_DATA_PTR) - set(CMAKE_SIZEOF_VOID_P "${CMAKE_C_SIZEOF_DATA_PTR}") -endif() - -if(CMAKE_C_COMPILER_ABI) - set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_C_COMPILER_ABI}") -endif() - -if(CMAKE_C_LIBRARY_ARCHITECTURE) - set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") -endif() - - - - -set(CMAKE_C_IMPLICIT_LINK_LIBRARIES "c") -set(CMAKE_C_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib") -set(CMAKE_C_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "") - - - diff --git a/build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake b/build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake deleted file mode 100644 index c4373d570..000000000 --- a/build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake +++ /dev/null @@ -1,57 +0,0 @@ -set(CMAKE_CXX_COMPILER "/usr/bin/c++") -set(CMAKE_CXX_COMPILER_ARG1 "") -set(CMAKE_CXX_COMPILER_ID "GNU") -set(CMAKE_CXX_COMPILER_VERSION "4.8.2") -set(CMAKE_CXX_PLATFORM_ID "Linux") - -set(CMAKE_AR "/usr/bin/ar") -set(CMAKE_RANLIB "/usr/bin/ranlib") -set(CMAKE_LINKER "/usr/bin/ld") -set(CMAKE_COMPILER_IS_GNUCXX 1) -set(CMAKE_CXX_COMPILER_LOADED 1) -set(CMAKE_CXX_COMPILER_WORKS TRUE) -set(CMAKE_CXX_ABI_COMPILED TRUE) -set(CMAKE_COMPILER_IS_MINGW ) -set(CMAKE_COMPILER_IS_CYGWIN ) -if(CMAKE_COMPILER_IS_CYGWIN) - set(CYGWIN 1) - set(UNIX 1) -endif() - -set(CMAKE_CXX_COMPILER_ENV_VAR "CXX") - -if(CMAKE_COMPILER_IS_MINGW) - set(MINGW 1) -endif() -set(CMAKE_CXX_COMPILER_ID_RUN 1) -set(CMAKE_CXX_IGNORE_EXTENSIONS inl;h;hpp;HPP;H;o;O;obj;OBJ;def;DEF;rc;RC) -set(CMAKE_CXX_SOURCE_FILE_EXTENSIONS C;M;c++;cc;cpp;cxx;m;mm;CPP) -set(CMAKE_CXX_LINKER_PREFERENCE 30) -set(CMAKE_CXX_LINKER_PREFERENCE_PROPAGATES 1) - -# Save compiler ABI information. -set(CMAKE_CXX_SIZEOF_DATA_PTR "8") -set(CMAKE_CXX_COMPILER_ABI "ELF") -set(CMAKE_CXX_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") - -if(CMAKE_CXX_SIZEOF_DATA_PTR) - set(CMAKE_SIZEOF_VOID_P "${CMAKE_CXX_SIZEOF_DATA_PTR}") -endif() - -if(CMAKE_CXX_COMPILER_ABI) - set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_CXX_COMPILER_ABI}") -endif() - -if(CMAKE_CXX_LIBRARY_ARCHITECTURE) - set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") -endif() - - - - -set(CMAKE_CXX_IMPLICIT_LINK_LIBRARIES "stdc++;m;c") -set(CMAKE_CXX_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib") -set(CMAKE_CXX_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "") - - - diff --git a/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin b/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin deleted file mode 100755 index 2f2ebe478..000000000 Binary files a/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin and /dev/null differ diff --git a/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin b/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin deleted file mode 100755 index 16c737f26..000000000 Binary files a/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin and /dev/null differ diff --git a/build/CMakeFiles/2.8.12.2/CMakeSystem.cmake b/build/CMakeFiles/2.8.12.2/CMakeSystem.cmake deleted file mode 100644 index 0616b7c1e..000000000 --- a/build/CMakeFiles/2.8.12.2/CMakeSystem.cmake +++ /dev/null @@ -1,15 +0,0 @@ -set(CMAKE_HOST_SYSTEM "Linux-3.13.0-24-generic") -set(CMAKE_HOST_SYSTEM_NAME "Linux") -set(CMAKE_HOST_SYSTEM_VERSION "3.13.0-24-generic") -set(CMAKE_HOST_SYSTEM_PROCESSOR "x86_64") - - - -set(CMAKE_SYSTEM "Linux-3.13.0-24-generic") -set(CMAKE_SYSTEM_NAME "Linux") -set(CMAKE_SYSTEM_VERSION "3.13.0-24-generic") -set(CMAKE_SYSTEM_PROCESSOR "x86_64") - -set(CMAKE_CROSSCOMPILING "FALSE") - -set(CMAKE_SYSTEM_LOADED 1) diff --git a/build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c b/build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c deleted file mode 100644 index cba81d4a6..000000000 --- a/build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c +++ /dev/null @@ -1,389 +0,0 @@ -#ifdef __cplusplus -# error "A C++ compiler has been selected for C." -#endif - -/* Version number components: V=Version, R=Revision, P=Patch - Version date components: YYYY=Year, MM=Month, DD=Day */ - -#if defined(__18CXX) -# define ID_VOID_MAIN -#endif - -#if defined(__INTEL_COMPILER) || defined(__ICC) -# define COMPILER_ID "Intel" - /* __INTEL_COMPILER = VRP */ -# define COMPILER_VERSION_MAJOR DEC(__INTEL_COMPILER/100) -# define COMPILER_VERSION_MINOR DEC(__INTEL_COMPILER/10 % 10) -# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER % 10) -# if defined(__INTEL_COMPILER_BUILD_DATE) - /* __INTEL_COMPILER_BUILD_DATE = YYYYMMDD */ -# define COMPILER_VERSION_TWEAK DEC(__INTEL_COMPILER_BUILD_DATE) -# endif - -#elif defined(__PATHCC__) -# define COMPILER_ID "PathScale" -# define COMPILER_VERSION_MAJOR DEC(__PATHCC__) -# define COMPILER_VERSION_MINOR DEC(__PATHCC_MINOR__) -# if defined(__PATHCC_PATCHLEVEL__) -# define COMPILER_VERSION_PATCH DEC(__PATHCC_PATCHLEVEL__) -# endif - -#elif defined(__clang__) -# define COMPILER_ID "Clang" -# define COMPILER_VERSION_MAJOR DEC(__clang_major__) -# define COMPILER_VERSION_MINOR DEC(__clang_minor__) -# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__) - -#elif defined(__BORLANDC__) && defined(__CODEGEARC_VERSION__) -# define COMPILER_ID "Embarcadero" -# define COMPILER_VERSION_MAJOR HEX(__CODEGEARC_VERSION__>>24 & 0x00FF) -# define COMPILER_VERSION_MINOR HEX(__CODEGEARC_VERSION__>>16 & 0x00FF) -# define COMPILER_VERSION_PATCH HEX(__CODEGEARC_VERSION__ & 0xFFFF) - -#elif defined(__BORLANDC__) -# define COMPILER_ID "Borland" - /* __BORLANDC__ = 0xVRR */ -# define COMPILER_VERSION_MAJOR HEX(__BORLANDC__>>8) -# define COMPILER_VERSION_MINOR HEX(__BORLANDC__ & 0xFF) - -#elif defined(__WATCOMC__) -# define COMPILER_ID "Watcom" - /* __WATCOMC__ = VVRR */ -# define COMPILER_VERSION_MAJOR DEC(__WATCOMC__ / 100) -# define COMPILER_VERSION_MINOR DEC(__WATCOMC__ % 100) - -#elif defined(__SUNPRO_C) -# define COMPILER_ID "SunPro" -# if __SUNPRO_C >= 0x5100 - /* __SUNPRO_C = 0xVRRP */ -# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_C>>12) -# define COMPILER_VERSION_MINOR HEX(__SUNPRO_C>>4 & 0xFF) -# define COMPILER_VERSION_PATCH HEX(__SUNPRO_C & 0xF) -# else - /* __SUNPRO_C = 0xVRP */ -# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_C>>8) -# define COMPILER_VERSION_MINOR HEX(__SUNPRO_C>>4 & 0xF) -# define COMPILER_VERSION_PATCH HEX(__SUNPRO_C & 0xF) -# endif - -#elif defined(__HP_cc) -# define COMPILER_ID "HP" - /* __HP_cc = VVRRPP */ -# define COMPILER_VERSION_MAJOR DEC(__HP_cc/10000) -# define COMPILER_VERSION_MINOR DEC(__HP_cc/100 % 100) -# define COMPILER_VERSION_PATCH DEC(__HP_cc % 100) - -#elif defined(__DECC) -# define COMPILER_ID "Compaq" - /* __DECC_VER = VVRRTPPPP */ -# define COMPILER_VERSION_MAJOR DEC(__DECC_VER/10000000) -# define COMPILER_VERSION_MINOR DEC(__DECC_VER/100000 % 100) -# define COMPILER_VERSION_PATCH DEC(__DECC_VER % 10000) - -#elif defined(__IBMC__) -# if defined(__COMPILER_VER__) -# define COMPILER_ID "zOS" -# else -# if __IBMC__ >= 800 -# define COMPILER_ID "XL" -# else -# define COMPILER_ID "VisualAge" -# endif - /* __IBMC__ = VRP */ -# define COMPILER_VERSION_MAJOR DEC(__IBMC__/100) -# define COMPILER_VERSION_MINOR DEC(__IBMC__/10 % 10) -# define COMPILER_VERSION_PATCH DEC(__IBMC__ % 10) -# endif - -#elif defined(__PGI) -# define COMPILER_ID "PGI" -# define COMPILER_VERSION_MAJOR DEC(__PGIC__) -# define COMPILER_VERSION_MINOR DEC(__PGIC_MINOR__) -# if defined(__PGIC_PATCHLEVEL__) -# define COMPILER_VERSION_PATCH DEC(__PGIC_PATCHLEVEL__) -# endif - -#elif defined(_CRAYC) -# define COMPILER_ID "Cray" -# define COMPILER_VERSION_MAJOR DEC(_RELEASE) -# define COMPILER_VERSION_MINOR DEC(_RELEASE_MINOR) - -#elif defined(__TI_COMPILER_VERSION__) -# define COMPILER_ID "TI" - /* __TI_COMPILER_VERSION__ = VVVRRRPPP */ -# define COMPILER_VERSION_MAJOR DEC(__TI_COMPILER_VERSION__/1000000) -# define COMPILER_VERSION_MINOR DEC(__TI_COMPILER_VERSION__/1000 % 1000) -# define COMPILER_VERSION_PATCH DEC(__TI_COMPILER_VERSION__ % 1000) - -#elif defined(__TINYC__) -# define COMPILER_ID "TinyCC" - -#elif defined(__SCO_VERSION__) -# define COMPILER_ID "SCO" - -#elif defined(__GNUC__) -# define COMPILER_ID "GNU" -# define COMPILER_VERSION_MAJOR DEC(__GNUC__) -# define COMPILER_VERSION_MINOR DEC(__GNUC_MINOR__) -# if defined(__GNUC_PATCHLEVEL__) -# define COMPILER_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) -# endif - -#elif defined(_MSC_VER) -# define COMPILER_ID "MSVC" - /* _MSC_VER = VVRR */ -# define COMPILER_VERSION_MAJOR DEC(_MSC_VER / 100) -# define COMPILER_VERSION_MINOR DEC(_MSC_VER % 100) -# if defined(_MSC_FULL_VER) -# if _MSC_VER >= 1400 - /* _MSC_FULL_VER = VVRRPPPPP */ -# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 100000) -# else - /* _MSC_FULL_VER = VVRRPPPP */ -# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 10000) -# endif -# endif -# if defined(_MSC_BUILD) -# define COMPILER_VERSION_TWEAK DEC(_MSC_BUILD) -# endif - -/* Analog VisualDSP++ >= 4.5.6 */ -#elif defined(__VISUALDSPVERSION__) -# define COMPILER_ID "ADSP" - /* __VISUALDSPVERSION__ = 0xVVRRPP00 */ -# define COMPILER_VERSION_MAJOR HEX(__VISUALDSPVERSION__>>24) -# define COMPILER_VERSION_MINOR HEX(__VISUALDSPVERSION__>>16 & 0xFF) -# define COMPILER_VERSION_PATCH HEX(__VISUALDSPVERSION__>>8 & 0xFF) - -/* Analog VisualDSP++ < 4.5.6 */ -#elif defined(__ADSPBLACKFIN__) || defined(__ADSPTS__) || defined(__ADSP21000__) -# define COMPILER_ID "ADSP" - -/* IAR Systems compiler for embedded systems. - http://www.iar.com */ -#elif defined(__IAR_SYSTEMS_ICC__ ) || defined(__IAR_SYSTEMS_ICC) -# define COMPILER_ID "IAR" - -/* sdcc, the small devices C compiler for embedded systems, - http://sdcc.sourceforge.net */ -#elif defined(SDCC) -# define COMPILER_ID "SDCC" - /* SDCC = VRP */ -# define COMPILER_VERSION_MAJOR DEC(SDCC/100) -# define COMPILER_VERSION_MINOR DEC(SDCC/10 % 10) -# define COMPILER_VERSION_PATCH DEC(SDCC % 10) - -#elif defined(_SGI_COMPILER_VERSION) || defined(_COMPILER_VERSION) -# define COMPILER_ID "MIPSpro" -# if defined(_SGI_COMPILER_VERSION) - /* _SGI_COMPILER_VERSION = VRP */ -# define COMPILER_VERSION_MAJOR DEC(_SGI_COMPILER_VERSION/100) -# define COMPILER_VERSION_MINOR DEC(_SGI_COMPILER_VERSION/10 % 10) -# define COMPILER_VERSION_PATCH DEC(_SGI_COMPILER_VERSION % 10) -# else - /* _COMPILER_VERSION = VRP */ -# define COMPILER_VERSION_MAJOR DEC(_COMPILER_VERSION/100) -# define COMPILER_VERSION_MINOR DEC(_COMPILER_VERSION/10 % 10) -# define COMPILER_VERSION_PATCH DEC(_COMPILER_VERSION % 10) -# endif - -/* This compiler is either not known or is too old to define an - identification macro. Try to identify the platform and guess that - it is the native compiler. */ -#elif defined(__sgi) -# define COMPILER_ID "MIPSpro" - -#elif defined(__hpux) || defined(__hpua) -# define COMPILER_ID "HP" - -#else /* unknown compiler */ -# define COMPILER_ID "" - -#endif - -/* Construct the string literal in pieces to prevent the source from - getting matched. Store it in a pointer rather than an array - because some compilers will just produce instructions to fill the - array rather than assigning a pointer to a static array. */ -char const* info_compiler = "INFO" ":" "compiler[" COMPILER_ID "]"; - -/* Identify known platforms by name. */ -#if defined(__linux) || defined(__linux__) || defined(linux) -# define PLATFORM_ID "Linux" - -#elif defined(__CYGWIN__) -# define PLATFORM_ID "Cygwin" - -#elif defined(__MINGW32__) -# define PLATFORM_ID "MinGW" - -#elif defined(__APPLE__) -# define PLATFORM_ID "Darwin" - -#elif defined(_WIN32) || defined(__WIN32__) || defined(WIN32) -# define PLATFORM_ID "Windows" - -#elif defined(__FreeBSD__) || defined(__FreeBSD) -# define PLATFORM_ID "FreeBSD" - -#elif defined(__NetBSD__) || defined(__NetBSD) -# define PLATFORM_ID "NetBSD" - -#elif defined(__OpenBSD__) || defined(__OPENBSD) -# define PLATFORM_ID "OpenBSD" - -#elif defined(__sun) || defined(sun) -# define PLATFORM_ID "SunOS" - -#elif defined(_AIX) || defined(__AIX) || defined(__AIX__) || defined(__aix) || defined(__aix__) -# define PLATFORM_ID "AIX" - -#elif defined(__sgi) || defined(__sgi__) || defined(_SGI) -# define PLATFORM_ID "IRIX" - -#elif defined(__hpux) || defined(__hpux__) -# define PLATFORM_ID "HP-UX" - -#elif defined(__HAIKU__) -# define PLATFORM_ID "Haiku" - -#elif defined(__BeOS) || defined(__BEOS__) || defined(_BEOS) -# define PLATFORM_ID "BeOS" - -#elif defined(__QNX__) || defined(__QNXNTO__) -# define PLATFORM_ID "QNX" - -#elif defined(__tru64) || defined(_tru64) || defined(__TRU64__) -# define PLATFORM_ID "Tru64" - -#elif defined(__riscos) || defined(__riscos__) -# define PLATFORM_ID "RISCos" - -#elif defined(__sinix) || defined(__sinix__) || defined(__SINIX__) -# define PLATFORM_ID "SINIX" - -#elif defined(__UNIX_SV__) -# define PLATFORM_ID "UNIX_SV" - -#elif defined(__bsdos__) -# define PLATFORM_ID "BSDOS" - -#elif defined(_MPRAS) || defined(MPRAS) -# define PLATFORM_ID "MP-RAS" - -#elif defined(__osf) || defined(__osf__) -# define PLATFORM_ID "OSF1" - -#elif defined(_SCO_SV) || defined(SCO_SV) || defined(sco_sv) -# define PLATFORM_ID "SCO_SV" - -#elif defined(__ultrix) || defined(__ultrix__) || defined(_ULTRIX) -# define PLATFORM_ID "ULTRIX" - -#elif defined(__XENIX__) || defined(_XENIX) || defined(XENIX) -# define PLATFORM_ID "Xenix" - -#else /* unknown platform */ -# define PLATFORM_ID "" - -#endif - -/* For windows compilers MSVC and Intel we can determine - the architecture of the compiler being used. This is because - the compilers do not have flags that can change the architecture, - but rather depend on which compiler is being used -*/ -#if defined(_WIN32) && defined(_MSC_VER) -# if defined(_M_IA64) -# define ARCHITECTURE_ID "IA64" - -# elif defined(_M_X64) || defined(_M_AMD64) -# define ARCHITECTURE_ID "x64" - -# elif defined(_M_IX86) -# define ARCHITECTURE_ID "X86" - -# elif defined(_M_ARM) -# define ARCHITECTURE_ID "ARM" - -# elif defined(_M_MIPS) -# define ARCHITECTURE_ID "MIPS" - -# elif defined(_M_SH) -# define ARCHITECTURE_ID "SHx" - -# else /* unknown architecture */ -# define ARCHITECTURE_ID "" -# endif - -#else -# define ARCHITECTURE_ID "" -#endif - -/* Convert integer to decimal digit literals. */ -#define DEC(n) \ - ('0' + (((n) / 10000000)%10)), \ - ('0' + (((n) / 1000000)%10)), \ - ('0' + (((n) / 100000)%10)), \ - ('0' + (((n) / 10000)%10)), \ - ('0' + (((n) / 1000)%10)), \ - ('0' + (((n) / 100)%10)), \ - ('0' + (((n) / 10)%10)), \ - ('0' + ((n) % 10)) - -/* Convert integer to hex digit literals. */ -#define HEX(n) \ - ('0' + ((n)>>28 & 0xF)), \ - ('0' + ((n)>>24 & 0xF)), \ - ('0' + ((n)>>20 & 0xF)), \ - ('0' + ((n)>>16 & 0xF)), \ - ('0' + ((n)>>12 & 0xF)), \ - ('0' + ((n)>>8 & 0xF)), \ - ('0' + ((n)>>4 & 0xF)), \ - ('0' + ((n) & 0xF)) - -/* Construct a string literal encoding the version number components. */ -#ifdef COMPILER_VERSION_MAJOR -char const info_version[] = { - 'I', 'N', 'F', 'O', ':', - 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','[', - COMPILER_VERSION_MAJOR, -# ifdef COMPILER_VERSION_MINOR - '.', COMPILER_VERSION_MINOR, -# ifdef COMPILER_VERSION_PATCH - '.', COMPILER_VERSION_PATCH, -# ifdef COMPILER_VERSION_TWEAK - '.', COMPILER_VERSION_TWEAK, -# endif -# endif -# endif - ']','\0'}; -#endif - -/* Construct the string literal in pieces to prevent the source from - getting matched. Store it in a pointer rather than an array - because some compilers will just produce instructions to fill the - array rather than assigning a pointer to a static array. */ -char const* info_platform = "INFO" ":" "platform[" PLATFORM_ID "]"; -char const* info_arch = "INFO" ":" "arch[" ARCHITECTURE_ID "]"; - - - -/*--------------------------------------------------------------------------*/ - -#ifdef ID_VOID_MAIN -void main() {} -#else -int main(int argc, char* argv[]) -{ - int require = 0; - require += info_compiler[argc]; - require += info_platform[argc]; - require += info_arch[argc]; -#ifdef COMPILER_VERSION_MAJOR - require += info_version[argc]; -#endif - (void)argv; - return require; -} -#endif diff --git a/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out b/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out deleted file mode 100755 index 33a3d2b58..000000000 Binary files a/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out and /dev/null differ diff --git a/build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp b/build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp deleted file mode 100644 index e8220b26e..000000000 --- a/build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp +++ /dev/null @@ -1,377 +0,0 @@ -/* This source file must have a .cpp extension so that all C++ compilers - recognize the extension without flags. Borland does not know .cxx for - example. */ -#ifndef __cplusplus -# error "A C compiler has been selected for C++." -#endif - -/* Version number components: V=Version, R=Revision, P=Patch - Version date components: YYYY=Year, MM=Month, DD=Day */ - -#if defined(__COMO__) -# define COMPILER_ID "Comeau" - /* __COMO_VERSION__ = VRR */ -# define COMPILER_VERSION_MAJOR DEC(__COMO_VERSION__ / 100) -# define COMPILER_VERSION_MINOR DEC(__COMO_VERSION__ % 100) - -#elif defined(__INTEL_COMPILER) || defined(__ICC) -# define COMPILER_ID "Intel" - /* __INTEL_COMPILER = VRP */ -# define COMPILER_VERSION_MAJOR DEC(__INTEL_COMPILER/100) -# define COMPILER_VERSION_MINOR DEC(__INTEL_COMPILER/10 % 10) -# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER % 10) -# if defined(__INTEL_COMPILER_BUILD_DATE) - /* __INTEL_COMPILER_BUILD_DATE = YYYYMMDD */ -# define COMPILER_VERSION_TWEAK DEC(__INTEL_COMPILER_BUILD_DATE) -# endif - -#elif defined(__PATHCC__) -# define COMPILER_ID "PathScale" -# define COMPILER_VERSION_MAJOR DEC(__PATHCC__) -# define COMPILER_VERSION_MINOR DEC(__PATHCC_MINOR__) -# if defined(__PATHCC_PATCHLEVEL__) -# define COMPILER_VERSION_PATCH DEC(__PATHCC_PATCHLEVEL__) -# endif - -#elif defined(__clang__) -# define COMPILER_ID "Clang" -# define COMPILER_VERSION_MAJOR DEC(__clang_major__) -# define COMPILER_VERSION_MINOR DEC(__clang_minor__) -# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__) - -#elif defined(__BORLANDC__) && defined(__CODEGEARC_VERSION__) -# define COMPILER_ID "Embarcadero" -# define COMPILER_VERSION_MAJOR HEX(__CODEGEARC_VERSION__>>24 & 0x00FF) -# define COMPILER_VERSION_MINOR HEX(__CODEGEARC_VERSION__>>16 & 0x00FF) -# define COMPILER_VERSION_PATCH HEX(__CODEGEARC_VERSION__ & 0xFFFF) - -#elif defined(__BORLANDC__) -# define COMPILER_ID "Borland" - /* __BORLANDC__ = 0xVRR */ -# define COMPILER_VERSION_MAJOR HEX(__BORLANDC__>>8) -# define COMPILER_VERSION_MINOR HEX(__BORLANDC__ & 0xFF) - -#elif defined(__WATCOMC__) -# define COMPILER_ID "Watcom" - /* __WATCOMC__ = VVRR */ -# define COMPILER_VERSION_MAJOR DEC(__WATCOMC__ / 100) -# define COMPILER_VERSION_MINOR DEC(__WATCOMC__ % 100) - -#elif defined(__SUNPRO_CC) -# define COMPILER_ID "SunPro" -# if __SUNPRO_CC >= 0x5100 - /* __SUNPRO_CC = 0xVRRP */ -# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_CC>>12) -# define COMPILER_VERSION_MINOR HEX(__SUNPRO_CC>>4 & 0xFF) -# define COMPILER_VERSION_PATCH HEX(__SUNPRO_CC & 0xF) -# else - /* __SUNPRO_CC = 0xVRP */ -# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_CC>>8) -# define COMPILER_VERSION_MINOR HEX(__SUNPRO_CC>>4 & 0xF) -# define COMPILER_VERSION_PATCH HEX(__SUNPRO_CC & 0xF) -# endif - -#elif defined(__HP_aCC) -# define COMPILER_ID "HP" - /* __HP_aCC = VVRRPP */ -# define COMPILER_VERSION_MAJOR DEC(__HP_aCC/10000) -# define COMPILER_VERSION_MINOR DEC(__HP_aCC/100 % 100) -# define COMPILER_VERSION_PATCH DEC(__HP_aCC % 100) - -#elif defined(__DECCXX) -# define COMPILER_ID "Compaq" - /* __DECCXX_VER = VVRRTPPPP */ -# define COMPILER_VERSION_MAJOR DEC(__DECCXX_VER/10000000) -# define COMPILER_VERSION_MINOR DEC(__DECCXX_VER/100000 % 100) -# define COMPILER_VERSION_PATCH DEC(__DECCXX_VER % 10000) - -#elif defined(__IBMCPP__) -# if defined(__COMPILER_VER__) -# define COMPILER_ID "zOS" -# else -# if __IBMCPP__ >= 800 -# define COMPILER_ID "XL" -# else -# define COMPILER_ID "VisualAge" -# endif - /* __IBMCPP__ = VRP */ -# define COMPILER_VERSION_MAJOR DEC(__IBMCPP__/100) -# define COMPILER_VERSION_MINOR DEC(__IBMCPP__/10 % 10) -# define COMPILER_VERSION_PATCH DEC(__IBMCPP__ % 10) -# endif - -#elif defined(__PGI) -# define COMPILER_ID "PGI" -# define COMPILER_VERSION_MAJOR DEC(__PGIC__) -# define COMPILER_VERSION_MINOR DEC(__PGIC_MINOR__) -# if defined(__PGIC_PATCHLEVEL__) -# define COMPILER_VERSION_PATCH DEC(__PGIC_PATCHLEVEL__) -# endif - -#elif defined(_CRAYC) -# define COMPILER_ID "Cray" -# define COMPILER_VERSION_MAJOR DEC(_RELEASE) -# define COMPILER_VERSION_MINOR DEC(_RELEASE_MINOR) - -#elif defined(__TI_COMPILER_VERSION__) -# define COMPILER_ID "TI" - /* __TI_COMPILER_VERSION__ = VVVRRRPPP */ -# define COMPILER_VERSION_MAJOR DEC(__TI_COMPILER_VERSION__/1000000) -# define COMPILER_VERSION_MINOR DEC(__TI_COMPILER_VERSION__/1000 % 1000) -# define COMPILER_VERSION_PATCH DEC(__TI_COMPILER_VERSION__ % 1000) - -#elif defined(__SCO_VERSION__) -# define COMPILER_ID "SCO" - -#elif defined(__GNUC__) -# define COMPILER_ID "GNU" -# define COMPILER_VERSION_MAJOR DEC(__GNUC__) -# define COMPILER_VERSION_MINOR DEC(__GNUC_MINOR__) -# if defined(__GNUC_PATCHLEVEL__) -# define COMPILER_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) -# endif - -#elif defined(_MSC_VER) -# define COMPILER_ID "MSVC" - /* _MSC_VER = VVRR */ -# define COMPILER_VERSION_MAJOR DEC(_MSC_VER / 100) -# define COMPILER_VERSION_MINOR DEC(_MSC_VER % 100) -# if defined(_MSC_FULL_VER) -# if _MSC_VER >= 1400 - /* _MSC_FULL_VER = VVRRPPPPP */ -# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 100000) -# else - /* _MSC_FULL_VER = VVRRPPPP */ -# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 10000) -# endif -# endif -# if defined(_MSC_BUILD) -# define COMPILER_VERSION_TWEAK DEC(_MSC_BUILD) -# endif - -/* Analog VisualDSP++ >= 4.5.6 */ -#elif defined(__VISUALDSPVERSION__) -# define COMPILER_ID "ADSP" - /* __VISUALDSPVERSION__ = 0xVVRRPP00 */ -# define COMPILER_VERSION_MAJOR HEX(__VISUALDSPVERSION__>>24) -# define COMPILER_VERSION_MINOR HEX(__VISUALDSPVERSION__>>16 & 0xFF) -# define COMPILER_VERSION_PATCH HEX(__VISUALDSPVERSION__>>8 & 0xFF) - -/* Analog VisualDSP++ < 4.5.6 */ -#elif defined(__ADSPBLACKFIN__) || defined(__ADSPTS__) || defined(__ADSP21000__) -# define COMPILER_ID "ADSP" - -/* IAR Systems compiler for embedded systems. - http://www.iar.com */ -#elif defined(__IAR_SYSTEMS_ICC__ ) || defined(__IAR_SYSTEMS_ICC) -# define COMPILER_ID "IAR" - -#elif defined(_SGI_COMPILER_VERSION) || defined(_COMPILER_VERSION) -# define COMPILER_ID "MIPSpro" -# if defined(_SGI_COMPILER_VERSION) - /* _SGI_COMPILER_VERSION = VRP */ -# define COMPILER_VERSION_MAJOR DEC(_SGI_COMPILER_VERSION/100) -# define COMPILER_VERSION_MINOR DEC(_SGI_COMPILER_VERSION/10 % 10) -# define COMPILER_VERSION_PATCH DEC(_SGI_COMPILER_VERSION % 10) -# else - /* _COMPILER_VERSION = VRP */ -# define COMPILER_VERSION_MAJOR DEC(_COMPILER_VERSION/100) -# define COMPILER_VERSION_MINOR DEC(_COMPILER_VERSION/10 % 10) -# define COMPILER_VERSION_PATCH DEC(_COMPILER_VERSION % 10) -# endif - -/* This compiler is either not known or is too old to define an - identification macro. Try to identify the platform and guess that - it is the native compiler. */ -#elif defined(__sgi) -# define COMPILER_ID "MIPSpro" - -#elif defined(__hpux) || defined(__hpua) -# define COMPILER_ID "HP" - -#else /* unknown compiler */ -# define COMPILER_ID "" - -#endif - -/* Construct the string literal in pieces to prevent the source from - getting matched. Store it in a pointer rather than an array - because some compilers will just produce instructions to fill the - array rather than assigning a pointer to a static array. */ -char const* info_compiler = "INFO" ":" "compiler[" COMPILER_ID "]"; - -/* Identify known platforms by name. */ -#if defined(__linux) || defined(__linux__) || defined(linux) -# define PLATFORM_ID "Linux" - -#elif defined(__CYGWIN__) -# define PLATFORM_ID "Cygwin" - -#elif defined(__MINGW32__) -# define PLATFORM_ID "MinGW" - -#elif defined(__APPLE__) -# define PLATFORM_ID "Darwin" - -#elif defined(_WIN32) || defined(__WIN32__) || defined(WIN32) -# define PLATFORM_ID "Windows" - -#elif defined(__FreeBSD__) || defined(__FreeBSD) -# define PLATFORM_ID "FreeBSD" - -#elif defined(__NetBSD__) || defined(__NetBSD) -# define PLATFORM_ID "NetBSD" - -#elif defined(__OpenBSD__) || defined(__OPENBSD) -# define PLATFORM_ID "OpenBSD" - -#elif defined(__sun) || defined(sun) -# define PLATFORM_ID "SunOS" - -#elif defined(_AIX) || defined(__AIX) || defined(__AIX__) || defined(__aix) || defined(__aix__) -# define PLATFORM_ID "AIX" - -#elif defined(__sgi) || defined(__sgi__) || defined(_SGI) -# define PLATFORM_ID "IRIX" - -#elif defined(__hpux) || defined(__hpux__) -# define PLATFORM_ID "HP-UX" - -#elif defined(__HAIKU__) -# define PLATFORM_ID "Haiku" - -#elif defined(__BeOS) || defined(__BEOS__) || defined(_BEOS) -# define PLATFORM_ID "BeOS" - -#elif defined(__QNX__) || defined(__QNXNTO__) -# define PLATFORM_ID "QNX" - -#elif defined(__tru64) || defined(_tru64) || defined(__TRU64__) -# define PLATFORM_ID "Tru64" - -#elif defined(__riscos) || defined(__riscos__) -# define PLATFORM_ID "RISCos" - -#elif defined(__sinix) || defined(__sinix__) || defined(__SINIX__) -# define PLATFORM_ID "SINIX" - -#elif defined(__UNIX_SV__) -# define PLATFORM_ID "UNIX_SV" - -#elif defined(__bsdos__) -# define PLATFORM_ID "BSDOS" - -#elif defined(_MPRAS) || defined(MPRAS) -# define PLATFORM_ID "MP-RAS" - -#elif defined(__osf) || defined(__osf__) -# define PLATFORM_ID "OSF1" - -#elif defined(_SCO_SV) || defined(SCO_SV) || defined(sco_sv) -# define PLATFORM_ID "SCO_SV" - -#elif defined(__ultrix) || defined(__ultrix__) || defined(_ULTRIX) -# define PLATFORM_ID "ULTRIX" - -#elif defined(__XENIX__) || defined(_XENIX) || defined(XENIX) -# define PLATFORM_ID "Xenix" - -#else /* unknown platform */ -# define PLATFORM_ID "" - -#endif - -/* For windows compilers MSVC and Intel we can determine - the architecture of the compiler being used. This is because - the compilers do not have flags that can change the architecture, - but rather depend on which compiler is being used -*/ -#if defined(_WIN32) && defined(_MSC_VER) -# if defined(_M_IA64) -# define ARCHITECTURE_ID "IA64" - -# elif defined(_M_X64) || defined(_M_AMD64) -# define ARCHITECTURE_ID "x64" - -# elif defined(_M_IX86) -# define ARCHITECTURE_ID "X86" - -# elif defined(_M_ARM) -# define ARCHITECTURE_ID "ARM" - -# elif defined(_M_MIPS) -# define ARCHITECTURE_ID "MIPS" - -# elif defined(_M_SH) -# define ARCHITECTURE_ID "SHx" - -# else /* unknown architecture */ -# define ARCHITECTURE_ID "" -# endif - -#else -# define ARCHITECTURE_ID "" -#endif - -/* Convert integer to decimal digit literals. */ -#define DEC(n) \ - ('0' + (((n) / 10000000)%10)), \ - ('0' + (((n) / 1000000)%10)), \ - ('0' + (((n) / 100000)%10)), \ - ('0' + (((n) / 10000)%10)), \ - ('0' + (((n) / 1000)%10)), \ - ('0' + (((n) / 100)%10)), \ - ('0' + (((n) / 10)%10)), \ - ('0' + ((n) % 10)) - -/* Convert integer to hex digit literals. */ -#define HEX(n) \ - ('0' + ((n)>>28 & 0xF)), \ - ('0' + ((n)>>24 & 0xF)), \ - ('0' + ((n)>>20 & 0xF)), \ - ('0' + ((n)>>16 & 0xF)), \ - ('0' + ((n)>>12 & 0xF)), \ - ('0' + ((n)>>8 & 0xF)), \ - ('0' + ((n)>>4 & 0xF)), \ - ('0' + ((n) & 0xF)) - -/* Construct a string literal encoding the version number components. */ -#ifdef COMPILER_VERSION_MAJOR -char const info_version[] = { - 'I', 'N', 'F', 'O', ':', - 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','[', - COMPILER_VERSION_MAJOR, -# ifdef COMPILER_VERSION_MINOR - '.', COMPILER_VERSION_MINOR, -# ifdef COMPILER_VERSION_PATCH - '.', COMPILER_VERSION_PATCH, -# ifdef COMPILER_VERSION_TWEAK - '.', COMPILER_VERSION_TWEAK, -# endif -# endif -# endif - ']','\0'}; -#endif - -/* Construct the string literal in pieces to prevent the source from - getting matched. Store it in a pointer rather than an array - because some compilers will just produce instructions to fill the - array rather than assigning a pointer to a static array. */ -char const* info_platform = "INFO" ":" "platform[" PLATFORM_ID "]"; -char const* info_arch = "INFO" ":" "arch[" ARCHITECTURE_ID "]"; - - - -/*--------------------------------------------------------------------------*/ - -int main(int argc, char* argv[]) -{ - int require = 0; - require += info_compiler[argc]; - require += info_platform[argc]; -#ifdef COMPILER_VERSION_MAJOR - require += info_version[argc]; -#endif - (void)argv; - return require; -} diff --git a/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out b/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out deleted file mode 100755 index db35dbde0..000000000 Binary files a/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out and /dev/null differ diff --git a/build/CMakeFiles/CMakeDirectoryInformation.cmake b/build/CMakeFiles/CMakeDirectoryInformation.cmake deleted file mode 100644 index b2cc9b7e7..000000000 --- a/build/CMakeFiles/CMakeDirectoryInformation.cmake +++ /dev/null @@ -1,16 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -# Relative path conversion top directories. -SET(CMAKE_RELATIVE_PATH_TOP_SOURCE "/home/roman/src/Firmware/src") -SET(CMAKE_RELATIVE_PATH_TOP_BINARY "/home/roman/src/Firmware/build") - -# Force unix paths in dependencies. -SET(CMAKE_FORCE_UNIX_PATHS 1) - - -# The C and CXX include file regular expressions for this directory. -SET(CMAKE_C_INCLUDE_REGEX_SCAN "^.*$") -SET(CMAKE_C_INCLUDE_REGEX_COMPLAIN "^$") -SET(CMAKE_CXX_INCLUDE_REGEX_SCAN ${CMAKE_C_INCLUDE_REGEX_SCAN}) -SET(CMAKE_CXX_INCLUDE_REGEX_COMPLAIN ${CMAKE_C_INCLUDE_REGEX_COMPLAIN}) diff --git a/build/CMakeFiles/CMakeError.log b/build/CMakeFiles/CMakeError.log deleted file mode 100644 index 9ab6f1e9a..000000000 --- a/build/CMakeFiles/CMakeError.log +++ /dev/null @@ -1,53 +0,0 @@ -Determining if the pthread_create exist failed with the following output: -Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp - -Run Build Command:/usr/bin/make "cmTryCompileExec1424100880/fast" -/usr/bin/make -f CMakeFiles/cmTryCompileExec1424100880.dir/build.make CMakeFiles/cmTryCompileExec1424100880.dir/build -make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' -/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 -Building C object CMakeFiles/cmTryCompileExec1424100880.dir/CheckSymbolExists.c.o -/usr/bin/cc -o CMakeFiles/cmTryCompileExec1424100880.dir/CheckSymbolExists.c.o -c /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CheckSymbolExists.c -Linking C executable cmTryCompileExec1424100880 -/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec1424100880.dir/link.txt --verbose=1 -/usr/bin/cc CMakeFiles/cmTryCompileExec1424100880.dir/CheckSymbolExists.c.o -o cmTryCompileExec1424100880 -rdynamic -CMakeFiles/cmTryCompileExec1424100880.dir/CheckSymbolExists.c.o: In function `main': -CheckSymbolExists.c:(.text+0x16): undefined reference to `pthread_create' -collect2: error: ld returned 1 exit status -make[1]: *** [cmTryCompileExec1424100880] Error 1 -make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' -make: *** [cmTryCompileExec1424100880/fast] Error 2 - -File /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CheckSymbolExists.c: -/* */ -#include - -int main(int argc, char** argv) -{ - (void)argv; -#ifndef pthread_create - return ((int*)(&pthread_create))[argc]; -#else - (void)argc; - return 0; -#endif -} - -Determining if the function pthread_create exists in the pthreads failed with the following output: -Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp - -Run Build Command:/usr/bin/make "cmTryCompileExec26988121/fast" -/usr/bin/make -f CMakeFiles/cmTryCompileExec26988121.dir/build.make CMakeFiles/cmTryCompileExec26988121.dir/build -make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' -/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 -Building C object CMakeFiles/cmTryCompileExec26988121.dir/CheckFunctionExists.c.o -/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create -o CMakeFiles/cmTryCompileExec26988121.dir/CheckFunctionExists.c.o -c /usr/share/cmake-2.8/Modules/CheckFunctionExists.c -Linking C executable cmTryCompileExec26988121 -/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec26988121.dir/link.txt --verbose=1 -/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create CMakeFiles/cmTryCompileExec26988121.dir/CheckFunctionExists.c.o -o cmTryCompileExec26988121 -rdynamic -lpthreads -/usr/bin/ld: cannot find -lpthreads -collect2: error: ld returned 1 exit status -make[1]: *** [cmTryCompileExec26988121] Error 1 -make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' -make: *** [cmTryCompileExec26988121/fast] Error 2 - - diff --git a/build/CMakeFiles/CMakeOutput.log b/build/CMakeFiles/CMakeOutput.log deleted file mode 100644 index a3757fc34..000000000 --- a/build/CMakeFiles/CMakeOutput.log +++ /dev/null @@ -1,293 +0,0 @@ -The system is: Linux - 3.13.0-24-generic - x86_64 -Compiling the C compiler identification source file "CMakeCCompilerId.c" succeeded. -Compiler: /usr/bin/cc -Build flags: -Id flags: - -The output was: -0 - - -Compilation of the C compiler identification source "CMakeCCompilerId.c" produced "a.out" - -The C compiler identification is GNU, found in "/home/roman/src/Firmware/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out" - -Compiling the CXX compiler identification source file "CMakeCXXCompilerId.cpp" succeeded. -Compiler: /usr/bin/c++ -Build flags: -Id flags: - -The output was: -0 - - -Compilation of the CXX compiler identification source "CMakeCXXCompilerId.cpp" produced "a.out" - -The CXX compiler identification is GNU, found in "/home/roman/src/Firmware/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out" - -Determining if the C compiler works passed with the following output: -Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp - -Run Build Command:/usr/bin/make "cmTryCompileExec4189733644/fast" -/usr/bin/make -f CMakeFiles/cmTryCompileExec4189733644.dir/build.make CMakeFiles/cmTryCompileExec4189733644.dir/build -make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' -/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 -Building C object CMakeFiles/cmTryCompileExec4189733644.dir/testCCompiler.c.o -/usr/bin/cc -o CMakeFiles/cmTryCompileExec4189733644.dir/testCCompiler.c.o -c /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/testCCompiler.c -Linking C executable cmTryCompileExec4189733644 -/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec4189733644.dir/link.txt --verbose=1 -/usr/bin/cc CMakeFiles/cmTryCompileExec4189733644.dir/testCCompiler.c.o -o cmTryCompileExec4189733644 -rdynamic -make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' - - -Detecting C compiler ABI info compiled with the following output: -Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp - -Run Build Command:/usr/bin/make "cmTryCompileExec2944435992/fast" -/usr/bin/make -f CMakeFiles/cmTryCompileExec2944435992.dir/build.make CMakeFiles/cmTryCompileExec2944435992.dir/build -make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' -/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 -Building C object CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -/usr/bin/cc -o CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -c /usr/share/cmake-2.8/Modules/CMakeCCompilerABI.c -Linking C executable cmTryCompileExec2944435992 -/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec2944435992.dir/link.txt --verbose=1 -/usr/bin/cc -v CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -o cmTryCompileExec2944435992 -rdynamic -Using built-in specs. -COLLECT_GCC=/usr/bin/cc -COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/4.8/lto-wrapper -Target: x86_64-linux-gnu -Configured with: ../src/configure -v --with-pkgversion='Ubuntu 4.8.2-19ubuntu1' --with-bugurl=file:///usr/share/doc/gcc-4.8/README.Bugs --enable-languages=c,c++,java,go,d,fortran,objc,obj-c++ --prefix=/usr --program-suffix=-4.8 --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --with-gxx-include-dir=/usr/include/c++/4.8 --libdir=/usr/lib --enable-nls --with-sysroot=/ --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --enable-gnu-unique-object --disable-libmudflap --enable-plugin --with-system-zlib --disable-browser-plugin --enable-java-awt=gtk --enable-gtk-cairo --with-java-home=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64/jre --enable-java-home --with-jvm-root-dir=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64 --with-jvm-jar-dir=/usr/lib/jvm-exports/java-1.5.0-gcj-4.8-amd64 --with-arch-directory=amd64 --with-ecj-jar=/usr/share/java/eclipse-ecj.jar --enable-objc-gc --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --with-tune=generic --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu -Thread model: posix -gcc version 4.8.2 (Ubuntu 4.8.2-19ubuntu1) -COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/ -LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../:/lib/:/usr/lib/ -COLLECT_GCC_OPTIONS='-v' '-o' 'cmTryCompileExec2944435992' '-rdynamic' '-mtune=generic' '-march=x86-64' - /usr/lib/gcc/x86_64-linux-gnu/4.8/collect2 --sysroot=/ --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -z relro -o cmTryCompileExec2944435992 /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o -L/usr/lib/gcc/x86_64-linux-gnu/4.8 -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../.. CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -lgcc --as-needed -lgcc_s --no-as-needed -lc -lgcc --as-needed -lgcc_s --no-as-needed /usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o -make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' - - -Parsed C implicit link information from above output: - link line regex: [^( *|.*[/\])(ld|([^/\]+-)?ld|collect2)[^/\]*( |$)] - ignore line: [Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp] - ignore line: [] - ignore line: [Run Build Command:/usr/bin/make "cmTryCompileExec2944435992/fast"] - ignore line: [/usr/bin/make -f CMakeFiles/cmTryCompileExec2944435992.dir/build.make CMakeFiles/cmTryCompileExec2944435992.dir/build] - ignore line: [make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'] - ignore line: [/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1] - ignore line: [Building C object CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o] - ignore line: [/usr/bin/cc -o CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -c /usr/share/cmake-2.8/Modules/CMakeCCompilerABI.c] - ignore line: [Linking C executable cmTryCompileExec2944435992] - ignore line: [/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec2944435992.dir/link.txt --verbose=1] - ignore line: [/usr/bin/cc -v CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -o cmTryCompileExec2944435992 -rdynamic ] - ignore line: [Using built-in specs.] - ignore line: [COLLECT_GCC=/usr/bin/cc] - ignore line: [COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/4.8/lto-wrapper] - ignore line: [Target: x86_64-linux-gnu] - ignore line: [Configured with: ../src/configure -v --with-pkgversion='Ubuntu 4.8.2-19ubuntu1' --with-bugurl=file:///usr/share/doc/gcc-4.8/README.Bugs --enable-languages=c,c++,java,go,d,fortran,objc,obj-c++ --prefix=/usr --program-suffix=-4.8 --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --with-gxx-include-dir=/usr/include/c++/4.8 --libdir=/usr/lib --enable-nls --with-sysroot=/ --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --enable-gnu-unique-object --disable-libmudflap --enable-plugin --with-system-zlib --disable-browser-plugin --enable-java-awt=gtk --enable-gtk-cairo --with-java-home=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64/jre --enable-java-home --with-jvm-root-dir=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64 --with-jvm-jar-dir=/usr/lib/jvm-exports/java-1.5.0-gcj-4.8-amd64 --with-arch-directory=amd64 --with-ecj-jar=/usr/share/java/eclipse-ecj.jar --enable-objc-gc --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --with-tune=generic --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu] - ignore line: [Thread model: posix] - ignore line: [gcc version 4.8.2 (Ubuntu 4.8.2-19ubuntu1) ] - ignore line: [COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/] - ignore line: [LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../:/lib/:/usr/lib/] - ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'cmTryCompileExec2944435992' '-rdynamic' '-mtune=generic' '-march=x86-64'] - link line: [ /usr/lib/gcc/x86_64-linux-gnu/4.8/collect2 --sysroot=/ --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -z relro -o cmTryCompileExec2944435992 /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o -L/usr/lib/gcc/x86_64-linux-gnu/4.8 -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../.. CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -lgcc --as-needed -lgcc_s --no-as-needed -lc -lgcc --as-needed -lgcc_s --no-as-needed /usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o] - arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/collect2] ==> ignore - arg [--sysroot=/] ==> ignore - arg [--build-id] ==> ignore - arg [--eh-frame-hdr] ==> ignore - arg [-m] ==> ignore - arg [elf_x86_64] ==> ignore - arg [--hash-style=gnu] ==> ignore - arg [--as-needed] ==> ignore - arg [-export-dynamic] ==> ignore - arg [-dynamic-linker] ==> ignore - arg [/lib64/ld-linux-x86-64.so.2] ==> ignore - arg [-zrelro] ==> ignore - arg [-o] ==> ignore - arg [cmTryCompileExec2944435992] ==> ignore - arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o] ==> ignore - arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o] ==> ignore - arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o] ==> ignore - arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8] - arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] - arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] - arg [-L/lib/x86_64-linux-gnu] ==> dir [/lib/x86_64-linux-gnu] - arg [-L/lib/../lib] ==> dir [/lib/../lib] - arg [-L/usr/lib/x86_64-linux-gnu] ==> dir [/usr/lib/x86_64-linux-gnu] - arg [-L/usr/lib/../lib] ==> dir [/usr/lib/../lib] - arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] - arg [CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o] ==> ignore - arg [-lgcc] ==> lib [gcc] - arg [--as-needed] ==> ignore - arg [-lgcc_s] ==> lib [gcc_s] - arg [--no-as-needed] ==> ignore - arg [-lc] ==> lib [c] - arg [-lgcc] ==> lib [gcc] - arg [--as-needed] ==> ignore - arg [-lgcc_s] ==> lib [gcc_s] - arg [--no-as-needed] ==> ignore - arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o] ==> ignore - arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o] ==> ignore - remove lib [gcc] - remove lib [gcc_s] - remove lib [gcc] - remove lib [gcc_s] - collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8] ==> [/usr/lib/gcc/x86_64-linux-gnu/4.8] - collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu] - collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] ==> [/usr/lib] - collapse library dir [/lib/x86_64-linux-gnu] ==> [/lib/x86_64-linux-gnu] - collapse library dir [/lib/../lib] ==> [/lib] - collapse library dir [/usr/lib/x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu] - collapse library dir [/usr/lib/../lib] ==> [/usr/lib] - collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] ==> [/usr/lib] - implicit libs: [c] - implicit dirs: [/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib] - implicit fwks: [] - - -Determining if the CXX compiler works passed with the following output: -Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp - -Run Build Command:/usr/bin/make "cmTryCompileExec191173331/fast" -/usr/bin/make -f CMakeFiles/cmTryCompileExec191173331.dir/build.make CMakeFiles/cmTryCompileExec191173331.dir/build -make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' -/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 -Building CXX object CMakeFiles/cmTryCompileExec191173331.dir/testCXXCompiler.cxx.o -/usr/bin/c++ -o CMakeFiles/cmTryCompileExec191173331.dir/testCXXCompiler.cxx.o -c /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/testCXXCompiler.cxx -Linking CXX executable cmTryCompileExec191173331 -/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec191173331.dir/link.txt --verbose=1 -/usr/bin/c++ CMakeFiles/cmTryCompileExec191173331.dir/testCXXCompiler.cxx.o -o cmTryCompileExec191173331 -rdynamic -make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' - - -Detecting CXX compiler ABI info compiled with the following output: -Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp - -Run Build Command:/usr/bin/make "cmTryCompileExec843003076/fast" -/usr/bin/make -f CMakeFiles/cmTryCompileExec843003076.dir/build.make CMakeFiles/cmTryCompileExec843003076.dir/build -make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' -/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 -Building CXX object CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -/usr/bin/c++ -o CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -c /usr/share/cmake-2.8/Modules/CMakeCXXCompilerABI.cpp -Linking CXX executable cmTryCompileExec843003076 -/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec843003076.dir/link.txt --verbose=1 -/usr/bin/c++ -v CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -o cmTryCompileExec843003076 -rdynamic -Using built-in specs. -COLLECT_GCC=/usr/bin/c++ -COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/4.8/lto-wrapper -Target: x86_64-linux-gnu -Configured with: ../src/configure -v --with-pkgversion='Ubuntu 4.8.2-19ubuntu1' --with-bugurl=file:///usr/share/doc/gcc-4.8/README.Bugs --enable-languages=c,c++,java,go,d,fortran,objc,obj-c++ --prefix=/usr --program-suffix=-4.8 --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --with-gxx-include-dir=/usr/include/c++/4.8 --libdir=/usr/lib --enable-nls --with-sysroot=/ --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --enable-gnu-unique-object --disable-libmudflap --enable-plugin --with-system-zlib --disable-browser-plugin --enable-java-awt=gtk --enable-gtk-cairo --with-java-home=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64/jre --enable-java-home --with-jvm-root-dir=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64 --with-jvm-jar-dir=/usr/lib/jvm-exports/java-1.5.0-gcj-4.8-amd64 --with-arch-directory=amd64 --with-ecj-jar=/usr/share/java/eclipse-ecj.jar --enable-objc-gc --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --with-tune=generic --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu -Thread model: posix -gcc version 4.8.2 (Ubuntu 4.8.2-19ubuntu1) -COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/ -LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../:/lib/:/usr/lib/ -COLLECT_GCC_OPTIONS='-v' '-o' 'cmTryCompileExec843003076' '-rdynamic' '-shared-libgcc' '-mtune=generic' '-march=x86-64' - /usr/lib/gcc/x86_64-linux-gnu/4.8/collect2 --sysroot=/ --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -z relro -o cmTryCompileExec843003076 /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o -L/usr/lib/gcc/x86_64-linux-gnu/4.8 -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../.. CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -lstdc++ -lm -lgcc_s -lgcc -lc -lgcc_s -lgcc /usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o -make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' - - -Parsed CXX implicit link information from above output: - link line regex: [^( *|.*[/\])(ld|([^/\]+-)?ld|collect2)[^/\]*( |$)] - ignore line: [Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp] - ignore line: [] - ignore line: [Run Build Command:/usr/bin/make "cmTryCompileExec843003076/fast"] - ignore line: [/usr/bin/make -f CMakeFiles/cmTryCompileExec843003076.dir/build.make CMakeFiles/cmTryCompileExec843003076.dir/build] - ignore line: [make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'] - ignore line: [/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1] - ignore line: [Building CXX object CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o] - ignore line: [/usr/bin/c++ -o CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -c /usr/share/cmake-2.8/Modules/CMakeCXXCompilerABI.cpp] - ignore line: [Linking CXX executable cmTryCompileExec843003076] - ignore line: [/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec843003076.dir/link.txt --verbose=1] - ignore line: [/usr/bin/c++ -v CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -o cmTryCompileExec843003076 -rdynamic ] - ignore line: [Using built-in specs.] - ignore line: [COLLECT_GCC=/usr/bin/c++] - ignore line: [COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/4.8/lto-wrapper] - ignore line: [Target: x86_64-linux-gnu] - ignore line: [Configured with: ../src/configure -v --with-pkgversion='Ubuntu 4.8.2-19ubuntu1' --with-bugurl=file:///usr/share/doc/gcc-4.8/README.Bugs --enable-languages=c,c++,java,go,d,fortran,objc,obj-c++ --prefix=/usr --program-suffix=-4.8 --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --with-gxx-include-dir=/usr/include/c++/4.8 --libdir=/usr/lib --enable-nls --with-sysroot=/ --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --enable-gnu-unique-object --disable-libmudflap --enable-plugin --with-system-zlib --disable-browser-plugin --enable-java-awt=gtk --enable-gtk-cairo --with-java-home=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64/jre --enable-java-home --with-jvm-root-dir=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64 --with-jvm-jar-dir=/usr/lib/jvm-exports/java-1.5.0-gcj-4.8-amd64 --with-arch-directory=amd64 --with-ecj-jar=/usr/share/java/eclipse-ecj.jar --enable-objc-gc --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --with-tune=generic --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu] - ignore line: [Thread model: posix] - ignore line: [gcc version 4.8.2 (Ubuntu 4.8.2-19ubuntu1) ] - ignore line: [COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/] - ignore line: [LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../:/lib/:/usr/lib/] - ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'cmTryCompileExec843003076' '-rdynamic' '-shared-libgcc' '-mtune=generic' '-march=x86-64'] - link line: [ /usr/lib/gcc/x86_64-linux-gnu/4.8/collect2 --sysroot=/ --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -z relro -o cmTryCompileExec843003076 /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o -L/usr/lib/gcc/x86_64-linux-gnu/4.8 -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../.. CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -lstdc++ -lm -lgcc_s -lgcc -lc -lgcc_s -lgcc /usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o] - arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/collect2] ==> ignore - arg [--sysroot=/] ==> ignore - arg [--build-id] ==> ignore - arg [--eh-frame-hdr] ==> ignore - arg [-m] ==> ignore - arg [elf_x86_64] ==> ignore - arg [--hash-style=gnu] ==> ignore - arg [--as-needed] ==> ignore - arg [-export-dynamic] ==> ignore - arg [-dynamic-linker] ==> ignore - arg [/lib64/ld-linux-x86-64.so.2] ==> ignore - arg [-zrelro] ==> ignore - arg [-o] ==> ignore - arg [cmTryCompileExec843003076] ==> ignore - arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o] ==> ignore - arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o] ==> ignore - arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o] ==> ignore - arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8] - arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] - arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] - arg [-L/lib/x86_64-linux-gnu] ==> dir [/lib/x86_64-linux-gnu] - arg [-L/lib/../lib] ==> dir [/lib/../lib] - arg [-L/usr/lib/x86_64-linux-gnu] ==> dir [/usr/lib/x86_64-linux-gnu] - arg [-L/usr/lib/../lib] ==> dir [/usr/lib/../lib] - arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] - arg [CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o] ==> ignore - arg [-lstdc++] ==> lib [stdc++] - arg [-lm] ==> lib [m] - arg [-lgcc_s] ==> lib [gcc_s] - arg [-lgcc] ==> lib [gcc] - arg [-lc] ==> lib [c] - arg [-lgcc_s] ==> lib [gcc_s] - arg [-lgcc] ==> lib [gcc] - arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o] ==> ignore - arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o] ==> ignore - remove lib [gcc_s] - remove lib [gcc] - remove lib [gcc_s] - remove lib [gcc] - collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8] ==> [/usr/lib/gcc/x86_64-linux-gnu/4.8] - collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu] - collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] ==> [/usr/lib] - collapse library dir [/lib/x86_64-linux-gnu] ==> [/lib/x86_64-linux-gnu] - collapse library dir [/lib/../lib] ==> [/lib] - collapse library dir [/usr/lib/x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu] - collapse library dir [/usr/lib/../lib] ==> [/usr/lib] - collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] ==> [/usr/lib] - implicit libs: [stdc++;m;c] - implicit dirs: [/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib] - implicit fwks: [] - - -Determining if files pthread.h exist passed with the following output: -Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp - -Run Build Command:/usr/bin/make "cmTryCompileExec3897003384/fast" -/usr/bin/make -f CMakeFiles/cmTryCompileExec3897003384.dir/build.make CMakeFiles/cmTryCompileExec3897003384.dir/build -make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' -/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 -Building C object CMakeFiles/cmTryCompileExec3897003384.dir/CheckIncludeFiles.c.o -/usr/bin/cc -o CMakeFiles/cmTryCompileExec3897003384.dir/CheckIncludeFiles.c.o -c /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CheckIncludeFiles.c -Linking C executable cmTryCompileExec3897003384 -/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec3897003384.dir/link.txt --verbose=1 -/usr/bin/cc CMakeFiles/cmTryCompileExec3897003384.dir/CheckIncludeFiles.c.o -o cmTryCompileExec3897003384 -rdynamic -make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' - - -Determining if the function pthread_create exists in the pthread passed with the following output: -Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp - -Run Build Command:/usr/bin/make "cmTryCompileExec4077710905/fast" -/usr/bin/make -f CMakeFiles/cmTryCompileExec4077710905.dir/build.make CMakeFiles/cmTryCompileExec4077710905.dir/build -make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' -/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1 -Building C object CMakeFiles/cmTryCompileExec4077710905.dir/CheckFunctionExists.c.o -/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create -o CMakeFiles/cmTryCompileExec4077710905.dir/CheckFunctionExists.c.o -c /usr/share/cmake-2.8/Modules/CheckFunctionExists.c -Linking C executable cmTryCompileExec4077710905 -/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec4077710905.dir/link.txt --verbose=1 -/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create CMakeFiles/cmTryCompileExec4077710905.dir/CheckFunctionExists.c.o -o cmTryCompileExec4077710905 -rdynamic -lpthread -make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp' - - diff --git a/build/CMakeFiles/CMakeRuleHashes.txt b/build/CMakeFiles/CMakeRuleHashes.txt deleted file mode 100644 index b1bfe30c0..000000000 --- a/build/CMakeFiles/CMakeRuleHashes.txt +++ /dev/null @@ -1,5 +0,0 @@ -# Hashes of file build rules. -353bd50eb4c4b757d9a3d734d52d4f76 CMakeFiles/clean_test_results -d83f452b18a5909d95cdb786c10abffb CMakeFiles/doxygen -d83f452b18a5909d95cdb786c10abffb CMakeFiles/run_tests -d83f452b18a5909d95cdb786c10abffb CMakeFiles/tests diff --git a/build/CMakeFiles/Makefile.cmake b/build/CMakeFiles/Makefile.cmake deleted file mode 100644 index d232bacff..000000000 --- a/build/CMakeFiles/Makefile.cmake +++ /dev/null @@ -1,150 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -# The generator used is: -SET(CMAKE_DEPENDS_GENERATOR "Unix Makefiles") - -# The top level Makefile was generated from the following files: -SET(CMAKE_MAKEFILE_DEPENDS - "CMakeCache.txt" - "CMakeFiles/2.8.12.2/CMakeCCompiler.cmake" - "CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake" - "CMakeFiles/2.8.12.2/CMakeSystem.cmake" - "catkin/catkin_generated/version/package.cmake" - "catkin_generated/order_packages.cmake" - "/home/roman/src/Firmware/src/CMakeLists.txt" - "/opt/ros/indigo/share/catkin/cmake/../package.xml" - "/opt/ros/indigo/share/catkin/cmake/all.cmake" - "/opt/ros/indigo/share/catkin/cmake/assert.cmake" - "/opt/ros/indigo/share/catkin/cmake/atomic_configure_file.cmake" - "/opt/ros/indigo/share/catkin/cmake/catkinConfig-version.cmake" - "/opt/ros/indigo/share/catkin/cmake/catkinConfig.cmake" - "/opt/ros/indigo/share/catkin/cmake/catkin_add_env_hooks.cmake" - "/opt/ros/indigo/share/catkin/cmake/catkin_destinations.cmake" - "/opt/ros/indigo/share/catkin/cmake/catkin_generate_environment.cmake" - "/opt/ros/indigo/share/catkin/cmake/catkin_install_python.cmake" - "/opt/ros/indigo/share/catkin/cmake/catkin_libraries.cmake" - "/opt/ros/indigo/share/catkin/cmake/catkin_metapackage.cmake" - "/opt/ros/indigo/share/catkin/cmake/catkin_package.cmake" - "/opt/ros/indigo/share/catkin/cmake/catkin_package_xml.cmake" - "/opt/ros/indigo/share/catkin/cmake/catkin_python_setup.cmake" - "/opt/ros/indigo/share/catkin/cmake/catkin_workspace.cmake" - "/opt/ros/indigo/share/catkin/cmake/debug_message.cmake" - "/opt/ros/indigo/share/catkin/cmake/em/order_packages.cmake.em" - "/opt/ros/indigo/share/catkin/cmake/em_expand.cmake" - "/opt/ros/indigo/share/catkin/cmake/empy.cmake" - "/opt/ros/indigo/share/catkin/cmake/env-hooks/05.catkin-test-results.sh.develspace.in" - "/opt/ros/indigo/share/catkin/cmake/find_program_required.cmake" - "/opt/ros/indigo/share/catkin/cmake/interrogate_setup_dot_py.py" - "/opt/ros/indigo/share/catkin/cmake/legacy.cmake" - "/opt/ros/indigo/share/catkin/cmake/list_append_deduplicate.cmake" - "/opt/ros/indigo/share/catkin/cmake/list_append_unique.cmake" - "/opt/ros/indigo/share/catkin/cmake/list_insert_in_workspace_order.cmake" - "/opt/ros/indigo/share/catkin/cmake/platform/lsb.cmake" - "/opt/ros/indigo/share/catkin/cmake/platform/ubuntu.cmake" - "/opt/ros/indigo/share/catkin/cmake/platform/windows.cmake" - "/opt/ros/indigo/share/catkin/cmake/python.cmake" - "/opt/ros/indigo/share/catkin/cmake/safe_execute_process.cmake" - "/opt/ros/indigo/share/catkin/cmake/stamp.cmake" - "/opt/ros/indigo/share/catkin/cmake/string_starts_with.cmake" - "/opt/ros/indigo/share/catkin/cmake/templates/_setup_util.py.in" - "/opt/ros/indigo/share/catkin/cmake/templates/env.sh.in" - "/opt/ros/indigo/share/catkin/cmake/templates/generate_cached_setup.py.in" - "/opt/ros/indigo/share/catkin/cmake/templates/order_packages.context.py.in" - "/opt/ros/indigo/share/catkin/cmake/templates/rosinstall.in" - "/opt/ros/indigo/share/catkin/cmake/templates/setup.bash.in" - "/opt/ros/indigo/share/catkin/cmake/templates/setup.sh.in" - "/opt/ros/indigo/share/catkin/cmake/templates/setup.zsh.in" - "/opt/ros/indigo/share/catkin/cmake/test/catkin_download_test_data.cmake" - "/opt/ros/indigo/share/catkin/cmake/test/gtest.cmake" - "/opt/ros/indigo/share/catkin/cmake/test/nosetests.cmake" - "/opt/ros/indigo/share/catkin/cmake/test/tests.cmake" - "/opt/ros/indigo/share/catkin/cmake/tools/doxygen.cmake" - "/opt/ros/indigo/share/catkin/cmake/tools/libraries.cmake" - "/opt/ros/indigo/share/catkin/cmake/tools/rt.cmake" - "/usr/share/cmake-2.8/Modules/CMakeCCompiler.cmake.in" - "/usr/share/cmake-2.8/Modules/CMakeCCompilerABI.c" - "/usr/share/cmake-2.8/Modules/CMakeCInformation.cmake" - "/usr/share/cmake-2.8/Modules/CMakeCXXCompiler.cmake.in" - "/usr/share/cmake-2.8/Modules/CMakeCXXCompilerABI.cpp" - "/usr/share/cmake-2.8/Modules/CMakeCXXInformation.cmake" - "/usr/share/cmake-2.8/Modules/CMakeClDeps.cmake" - "/usr/share/cmake-2.8/Modules/CMakeCommonLanguageInclude.cmake" - "/usr/share/cmake-2.8/Modules/CMakeConfigurableFile.in" - "/usr/share/cmake-2.8/Modules/CMakeDetermineCCompiler.cmake" - "/usr/share/cmake-2.8/Modules/CMakeDetermineCXXCompiler.cmake" - "/usr/share/cmake-2.8/Modules/CMakeDetermineCompiler.cmake" - "/usr/share/cmake-2.8/Modules/CMakeDetermineCompilerABI.cmake" - "/usr/share/cmake-2.8/Modules/CMakeDetermineCompilerId.cmake" - "/usr/share/cmake-2.8/Modules/CMakeDetermineSystem.cmake" - "/usr/share/cmake-2.8/Modules/CMakeFindBinUtils.cmake" - "/usr/share/cmake-2.8/Modules/CMakeGenericSystem.cmake" - "/usr/share/cmake-2.8/Modules/CMakeParseArguments.cmake" - "/usr/share/cmake-2.8/Modules/CMakeParseImplicitLinkInfo.cmake" - "/usr/share/cmake-2.8/Modules/CMakeSystem.cmake.in" - "/usr/share/cmake-2.8/Modules/CMakeSystemSpecificInformation.cmake" - "/usr/share/cmake-2.8/Modules/CMakeTestCCompiler.cmake" - "/usr/share/cmake-2.8/Modules/CMakeTestCXXCompiler.cmake" - "/usr/share/cmake-2.8/Modules/CMakeTestCompilerCommon.cmake" - "/usr/share/cmake-2.8/Modules/CMakeUnixFindMake.cmake" - "/usr/share/cmake-2.8/Modules/CheckFunctionExists.c" - "/usr/share/cmake-2.8/Modules/CheckIncludeFiles.cmake" - "/usr/share/cmake-2.8/Modules/CheckLibraryExists.cmake" - "/usr/share/cmake-2.8/Modules/CheckSymbolExists.cmake" - "/usr/share/cmake-2.8/Modules/Compiler/GNU-C.cmake" - "/usr/share/cmake-2.8/Modules/Compiler/GNU-CXX.cmake" - "/usr/share/cmake-2.8/Modules/Compiler/GNU.cmake" - "/usr/share/cmake-2.8/Modules/FindGTest.cmake" - "/usr/share/cmake-2.8/Modules/FindPackageHandleStandardArgs.cmake" - "/usr/share/cmake-2.8/Modules/FindPackageMessage.cmake" - "/usr/share/cmake-2.8/Modules/FindPythonInterp.cmake" - "/usr/share/cmake-2.8/Modules/FindThreads.cmake" - "/usr/share/cmake-2.8/Modules/MultiArchCross.cmake" - "/usr/share/cmake-2.8/Modules/Platform/Linux-CXX.cmake" - "/usr/share/cmake-2.8/Modules/Platform/Linux-GNU-C.cmake" - "/usr/share/cmake-2.8/Modules/Platform/Linux-GNU-CXX.cmake" - "/usr/share/cmake-2.8/Modules/Platform/Linux-GNU.cmake" - "/usr/share/cmake-2.8/Modules/Platform/Linux.cmake" - "/usr/share/cmake-2.8/Modules/Platform/UnixPaths.cmake" - "/usr/src/gtest/CMakeLists.txt" - "/usr/src/gtest/cmake/internal_utils.cmake" - ) - -# The corresponding makefile is: -SET(CMAKE_MAKEFILE_OUTPUTS - "Makefile" - "CMakeFiles/cmake.check_cache" - ) - -# Byproducts of CMake generate step: -SET(CMAKE_MAKEFILE_PRODUCTS - "CMakeFiles/2.8.12.2/CMakeSystem.cmake" - "CMakeFiles/2.8.12.2/CMakeCCompiler.cmake" - "CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake" - "CMakeFiles/2.8.12.2/CMakeCCompiler.cmake" - "CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake" - "catkin_generated/stamps/Project/package.xml.stamp" - "catkin_generated/installspace/_setup_util.py" - "catkin_generated/installspace/env.sh" - "catkin_generated/installspace/setup.bash" - "catkin_generated/installspace/setup.sh" - "catkin_generated/installspace/setup.zsh" - "catkin_generated/installspace/.rosinstall" - "catkin_generated/generate_cached_setup.py" - "catkin_generated/env_cached.sh" - "catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp" - "catkin_generated/order_packages.py" - "catkin_generated/stamps/Project/order_packages.cmake.em.stamp" - "CMakeFiles/CMakeDirectoryInformation.cmake" - "gtest/CMakeFiles/CMakeDirectoryInformation.cmake" - ) - -# Dependency information for all targets: -SET(CMAKE_DEPEND_INFO_FILES - "CMakeFiles/clean_test_results.dir/DependInfo.cmake" - "CMakeFiles/doxygen.dir/DependInfo.cmake" - "CMakeFiles/run_tests.dir/DependInfo.cmake" - "CMakeFiles/tests.dir/DependInfo.cmake" - "gtest/CMakeFiles/gtest.dir/DependInfo.cmake" - "gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake" - ) diff --git a/build/CMakeFiles/Makefile2 b/build/CMakeFiles/Makefile2 deleted file mode 100644 index 2a11fe8ef..000000000 --- a/build/CMakeFiles/Makefile2 +++ /dev/null @@ -1,266 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -# Default target executed when no arguments are given to make. -default_target: all -.PHONY : default_target - -# The main recursive all target -all: -.PHONY : all - -# The main recursive preinstall target -preinstall: -.PHONY : preinstall - -#============================================================================= -# Special targets provided by cmake. - -# Disable implicit rules so canonical targets will work. -.SUFFIXES: - -# Remove some rules from gmake that .SUFFIXES does not remove. -SUFFIXES = - -.SUFFIXES: .hpux_make_needs_suffix_list - -# Suppress display of executed commands. -$(VERBOSE).SILENT: - -# A target that is always out of date. -cmake_force: -.PHONY : cmake_force - -#============================================================================= -# Set environment variables for the build. - -# The shell in which to execute make rules. -SHELL = /bin/sh - -# The CMake executable. -CMAKE_COMMAND = /usr/bin/cmake - -# The command to remove a file. -RM = /usr/bin/cmake -E remove -f - -# Escaping for special characters. -EQUALS = = - -# The top-level source directory on which CMake was run. -CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src - -# The top-level build directory on which CMake was run. -CMAKE_BINARY_DIR = /home/roman/src/Firmware/build - -#============================================================================= -# Target rules for target CMakeFiles/clean_test_results.dir - -# All Build rule for target. -CMakeFiles/clean_test_results.dir/all: - $(MAKE) -f CMakeFiles/clean_test_results.dir/build.make CMakeFiles/clean_test_results.dir/depend - $(MAKE) -f CMakeFiles/clean_test_results.dir/build.make CMakeFiles/clean_test_results.dir/build - $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles - @echo "Built target clean_test_results" -.PHONY : CMakeFiles/clean_test_results.dir/all - -# Build rule for subdir invocation for target. -CMakeFiles/clean_test_results.dir/rule: cmake_check_build_system - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 - $(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/clean_test_results.dir/all - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 -.PHONY : CMakeFiles/clean_test_results.dir/rule - -# Convenience name for target. -clean_test_results: CMakeFiles/clean_test_results.dir/rule -.PHONY : clean_test_results - -# clean rule for target. -CMakeFiles/clean_test_results.dir/clean: - $(MAKE) -f CMakeFiles/clean_test_results.dir/build.make CMakeFiles/clean_test_results.dir/clean -.PHONY : CMakeFiles/clean_test_results.dir/clean - -# clean rule for target. -clean: CMakeFiles/clean_test_results.dir/clean -.PHONY : clean - -#============================================================================= -# Target rules for target CMakeFiles/doxygen.dir - -# All Build rule for target. -CMakeFiles/doxygen.dir/all: - $(MAKE) -f CMakeFiles/doxygen.dir/build.make CMakeFiles/doxygen.dir/depend - $(MAKE) -f CMakeFiles/doxygen.dir/build.make CMakeFiles/doxygen.dir/build - $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles - @echo "Built target doxygen" -.PHONY : CMakeFiles/doxygen.dir/all - -# Build rule for subdir invocation for target. -CMakeFiles/doxygen.dir/rule: cmake_check_build_system - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 - $(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/doxygen.dir/all - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 -.PHONY : CMakeFiles/doxygen.dir/rule - -# Convenience name for target. -doxygen: CMakeFiles/doxygen.dir/rule -.PHONY : doxygen - -# clean rule for target. -CMakeFiles/doxygen.dir/clean: - $(MAKE) -f CMakeFiles/doxygen.dir/build.make CMakeFiles/doxygen.dir/clean -.PHONY : CMakeFiles/doxygen.dir/clean - -# clean rule for target. -clean: CMakeFiles/doxygen.dir/clean -.PHONY : clean - -#============================================================================= -# Target rules for target CMakeFiles/run_tests.dir - -# All Build rule for target. -CMakeFiles/run_tests.dir/all: - $(MAKE) -f CMakeFiles/run_tests.dir/build.make CMakeFiles/run_tests.dir/depend - $(MAKE) -f CMakeFiles/run_tests.dir/build.make CMakeFiles/run_tests.dir/build - $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles - @echo "Built target run_tests" -.PHONY : CMakeFiles/run_tests.dir/all - -# Build rule for subdir invocation for target. -CMakeFiles/run_tests.dir/rule: cmake_check_build_system - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 - $(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/run_tests.dir/all - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 -.PHONY : CMakeFiles/run_tests.dir/rule - -# Convenience name for target. -run_tests: CMakeFiles/run_tests.dir/rule -.PHONY : run_tests - -# clean rule for target. -CMakeFiles/run_tests.dir/clean: - $(MAKE) -f CMakeFiles/run_tests.dir/build.make CMakeFiles/run_tests.dir/clean -.PHONY : CMakeFiles/run_tests.dir/clean - -# clean rule for target. -clean: CMakeFiles/run_tests.dir/clean -.PHONY : clean - -#============================================================================= -# Target rules for target CMakeFiles/tests.dir - -# All Build rule for target. -CMakeFiles/tests.dir/all: - $(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/depend - $(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/build - $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles - @echo "Built target tests" -.PHONY : CMakeFiles/tests.dir/all - -# Build rule for subdir invocation for target. -CMakeFiles/tests.dir/rule: cmake_check_build_system - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 - $(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/tests.dir/all - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 -.PHONY : CMakeFiles/tests.dir/rule - -# Convenience name for target. -tests: CMakeFiles/tests.dir/rule -.PHONY : tests - -# clean rule for target. -CMakeFiles/tests.dir/clean: - $(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/clean -.PHONY : CMakeFiles/tests.dir/clean - -# clean rule for target. -clean: CMakeFiles/tests.dir/clean -.PHONY : clean - -#============================================================================= -# Directory level rules for directory gtest - -# Convenience name for "all" pass in the directory. -gtest/all: -.PHONY : gtest/all - -# Convenience name for "clean" pass in the directory. -gtest/clean: gtest/CMakeFiles/gtest.dir/clean -gtest/clean: gtest/CMakeFiles/gtest_main.dir/clean -.PHONY : gtest/clean - -# Convenience name for "preinstall" pass in the directory. -gtest/preinstall: -.PHONY : gtest/preinstall - -#============================================================================= -# Target rules for target gtest/CMakeFiles/gtest.dir - -# All Build rule for target. -gtest/CMakeFiles/gtest.dir/all: - $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/depend - $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/build - $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles 1 - @echo "Built target gtest" -.PHONY : gtest/CMakeFiles/gtest.dir/all - -# Build rule for subdir invocation for target. -gtest/CMakeFiles/gtest.dir/rule: cmake_check_build_system - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 1 - $(MAKE) -f CMakeFiles/Makefile2 gtest/CMakeFiles/gtest.dir/all - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 -.PHONY : gtest/CMakeFiles/gtest.dir/rule - -# Convenience name for target. -gtest: gtest/CMakeFiles/gtest.dir/rule -.PHONY : gtest - -# clean rule for target. -gtest/CMakeFiles/gtest.dir/clean: - $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/clean -.PHONY : gtest/CMakeFiles/gtest.dir/clean - -# clean rule for target. -clean: gtest/CMakeFiles/gtest.dir/clean -.PHONY : clean - -#============================================================================= -# Target rules for target gtest/CMakeFiles/gtest_main.dir - -# All Build rule for target. -gtest/CMakeFiles/gtest_main.dir/all: gtest/CMakeFiles/gtest.dir/all - $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/depend - $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/build - $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles 2 - @echo "Built target gtest_main" -.PHONY : gtest/CMakeFiles/gtest_main.dir/all - -# Build rule for subdir invocation for target. -gtest/CMakeFiles/gtest_main.dir/rule: cmake_check_build_system - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 2 - $(MAKE) -f CMakeFiles/Makefile2 gtest/CMakeFiles/gtest_main.dir/all - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 -.PHONY : gtest/CMakeFiles/gtest_main.dir/rule - -# Convenience name for target. -gtest_main: gtest/CMakeFiles/gtest_main.dir/rule -.PHONY : gtest_main - -# clean rule for target. -gtest/CMakeFiles/gtest_main.dir/clean: - $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/clean -.PHONY : gtest/CMakeFiles/gtest_main.dir/clean - -# clean rule for target. -clean: gtest/CMakeFiles/gtest_main.dir/clean -.PHONY : clean - -#============================================================================= -# Special targets to cleanup operation of make. - -# Special rule to run CMake to check the build system integrity. -# No rule that depends on this can have commands that come from listfiles -# because they might be regenerated. -cmake_check_build_system: - $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 -.PHONY : cmake_check_build_system - diff --git a/build/CMakeFiles/TargetDirectories.txt b/build/CMakeFiles/TargetDirectories.txt deleted file mode 100644 index 552e0c540..000000000 --- a/build/CMakeFiles/TargetDirectories.txt +++ /dev/null @@ -1,6 +0,0 @@ -/home/roman/src/Firmware/build/CMakeFiles/clean_test_results.dir -/home/roman/src/Firmware/build/CMakeFiles/doxygen.dir -/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest.dir -/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest_main.dir -/home/roman/src/Firmware/build/CMakeFiles/run_tests.dir -/home/roman/src/Firmware/build/CMakeFiles/tests.dir diff --git a/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake b/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake deleted file mode 100644 index 7aff3a53c..000000000 --- a/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake +++ /dev/null @@ -1,20 +0,0 @@ -# The set of languages for which implicit dependencies are needed: -SET(CMAKE_DEPENDS_LANGUAGES - ) -# The set of files for implicit dependencies of each language: - -# Preprocessor definitions for this target. -SET(CMAKE_TARGET_DEFINITIONS - "ROS_BUILD_SHARED_LIBS=1" - ) - -# Targets to which this target links. -SET(CMAKE_TARGET_LINKED_INFO_FILES - ) - -# The include file search paths: -SET(CMAKE_C_TARGET_INCLUDE_PATH - ) -SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) -SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) -SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) diff --git a/build/CMakeFiles/clean_test_results.dir/build.make b/build/CMakeFiles/clean_test_results.dir/build.make deleted file mode 100644 index b3776d0df..000000000 --- a/build/CMakeFiles/clean_test_results.dir/build.make +++ /dev/null @@ -1,66 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -#============================================================================= -# Special targets provided by cmake. - -# Disable implicit rules so canonical targets will work. -.SUFFIXES: - -# Remove some rules from gmake that .SUFFIXES does not remove. -SUFFIXES = - -.SUFFIXES: .hpux_make_needs_suffix_list - -# Suppress display of executed commands. -$(VERBOSE).SILENT: - -# A target that is always out of date. -cmake_force: -.PHONY : cmake_force - -#============================================================================= -# Set environment variables for the build. - -# The shell in which to execute make rules. -SHELL = /bin/sh - -# The CMake executable. -CMAKE_COMMAND = /usr/bin/cmake - -# The command to remove a file. -RM = /usr/bin/cmake -E remove -f - -# Escaping for special characters. -EQUALS = = - -# The top-level source directory on which CMake was run. -CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src - -# The top-level build directory on which CMake was run. -CMAKE_BINARY_DIR = /home/roman/src/Firmware/build - -# Utility rule file for clean_test_results. - -# Include the progress variables for this target. -include CMakeFiles/clean_test_results.dir/progress.make - -CMakeFiles/clean_test_results: - /usr/bin/cmake -E remove_directory /home/roman/src/Firmware/build/test_results - -clean_test_results: CMakeFiles/clean_test_results -clean_test_results: CMakeFiles/clean_test_results.dir/build.make -.PHONY : clean_test_results - -# Rule to build all files generated by this target. -CMakeFiles/clean_test_results.dir/build: clean_test_results -.PHONY : CMakeFiles/clean_test_results.dir/build - -CMakeFiles/clean_test_results.dir/clean: - $(CMAKE_COMMAND) -P CMakeFiles/clean_test_results.dir/cmake_clean.cmake -.PHONY : CMakeFiles/clean_test_results.dir/clean - -CMakeFiles/clean_test_results.dir/depend: - cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /home/roman/src/Firmware/src /home/roman/src/Firmware/build /home/roman/src/Firmware/build /home/roman/src/Firmware/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake --color=$(COLOR) -.PHONY : CMakeFiles/clean_test_results.dir/depend - diff --git a/build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake b/build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake deleted file mode 100644 index 46c1cb338..000000000 --- a/build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake +++ /dev/null @@ -1,8 +0,0 @@ -FILE(REMOVE_RECURSE - "CMakeFiles/clean_test_results" -) - -# Per-language clean rules from dependency scanning. -FOREACH(lang) - INCLUDE(CMakeFiles/clean_test_results.dir/cmake_clean_${lang}.cmake OPTIONAL) -ENDFOREACH(lang) diff --git a/build/CMakeFiles/clean_test_results.dir/progress.make b/build/CMakeFiles/clean_test_results.dir/progress.make deleted file mode 100644 index 8b1378917..000000000 --- a/build/CMakeFiles/clean_test_results.dir/progress.make +++ /dev/null @@ -1 +0,0 @@ - diff --git a/build/CMakeFiles/cmake.check_cache b/build/CMakeFiles/cmake.check_cache deleted file mode 100644 index 3dccd7317..000000000 --- a/build/CMakeFiles/cmake.check_cache +++ /dev/null @@ -1 +0,0 @@ -# This file is generated by cmake for dependency checking of the CMakeCache.txt file diff --git a/build/CMakeFiles/doxygen.dir/DependInfo.cmake b/build/CMakeFiles/doxygen.dir/DependInfo.cmake deleted file mode 100644 index 7aff3a53c..000000000 --- a/build/CMakeFiles/doxygen.dir/DependInfo.cmake +++ /dev/null @@ -1,20 +0,0 @@ -# The set of languages for which implicit dependencies are needed: -SET(CMAKE_DEPENDS_LANGUAGES - ) -# The set of files for implicit dependencies of each language: - -# Preprocessor definitions for this target. -SET(CMAKE_TARGET_DEFINITIONS - "ROS_BUILD_SHARED_LIBS=1" - ) - -# Targets to which this target links. -SET(CMAKE_TARGET_LINKED_INFO_FILES - ) - -# The include file search paths: -SET(CMAKE_C_TARGET_INCLUDE_PATH - ) -SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) -SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) -SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) diff --git a/build/CMakeFiles/doxygen.dir/build.make b/build/CMakeFiles/doxygen.dir/build.make deleted file mode 100644 index f5cc02bb3..000000000 --- a/build/CMakeFiles/doxygen.dir/build.make +++ /dev/null @@ -1,65 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -#============================================================================= -# Special targets provided by cmake. - -# Disable implicit rules so canonical targets will work. -.SUFFIXES: - -# Remove some rules from gmake that .SUFFIXES does not remove. -SUFFIXES = - -.SUFFIXES: .hpux_make_needs_suffix_list - -# Suppress display of executed commands. -$(VERBOSE).SILENT: - -# A target that is always out of date. -cmake_force: -.PHONY : cmake_force - -#============================================================================= -# Set environment variables for the build. - -# The shell in which to execute make rules. -SHELL = /bin/sh - -# The CMake executable. -CMAKE_COMMAND = /usr/bin/cmake - -# The command to remove a file. -RM = /usr/bin/cmake -E remove -f - -# Escaping for special characters. -EQUALS = = - -# The top-level source directory on which CMake was run. -CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src - -# The top-level build directory on which CMake was run. -CMAKE_BINARY_DIR = /home/roman/src/Firmware/build - -# Utility rule file for doxygen. - -# Include the progress variables for this target. -include CMakeFiles/doxygen.dir/progress.make - -CMakeFiles/doxygen: - -doxygen: CMakeFiles/doxygen -doxygen: CMakeFiles/doxygen.dir/build.make -.PHONY : doxygen - -# Rule to build all files generated by this target. -CMakeFiles/doxygen.dir/build: doxygen -.PHONY : CMakeFiles/doxygen.dir/build - -CMakeFiles/doxygen.dir/clean: - $(CMAKE_COMMAND) -P CMakeFiles/doxygen.dir/cmake_clean.cmake -.PHONY : CMakeFiles/doxygen.dir/clean - -CMakeFiles/doxygen.dir/depend: - cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /home/roman/src/Firmware/src /home/roman/src/Firmware/build /home/roman/src/Firmware/build /home/roman/src/Firmware/build/CMakeFiles/doxygen.dir/DependInfo.cmake --color=$(COLOR) -.PHONY : CMakeFiles/doxygen.dir/depend - diff --git a/build/CMakeFiles/doxygen.dir/cmake_clean.cmake b/build/CMakeFiles/doxygen.dir/cmake_clean.cmake deleted file mode 100644 index 3cf72d90f..000000000 --- a/build/CMakeFiles/doxygen.dir/cmake_clean.cmake +++ /dev/null @@ -1,8 +0,0 @@ -FILE(REMOVE_RECURSE - "CMakeFiles/doxygen" -) - -# Per-language clean rules from dependency scanning. -FOREACH(lang) - INCLUDE(CMakeFiles/doxygen.dir/cmake_clean_${lang}.cmake OPTIONAL) -ENDFOREACH(lang) diff --git a/build/CMakeFiles/doxygen.dir/progress.make b/build/CMakeFiles/doxygen.dir/progress.make deleted file mode 100644 index 8b1378917..000000000 --- a/build/CMakeFiles/doxygen.dir/progress.make +++ /dev/null @@ -1 +0,0 @@ - diff --git a/build/CMakeFiles/progress.marks b/build/CMakeFiles/progress.marks deleted file mode 100644 index 573541ac9..000000000 --- a/build/CMakeFiles/progress.marks +++ /dev/null @@ -1 +0,0 @@ -0 diff --git a/build/CMakeFiles/run_tests.dir/DependInfo.cmake b/build/CMakeFiles/run_tests.dir/DependInfo.cmake deleted file mode 100644 index 7aff3a53c..000000000 --- a/build/CMakeFiles/run_tests.dir/DependInfo.cmake +++ /dev/null @@ -1,20 +0,0 @@ -# The set of languages for which implicit dependencies are needed: -SET(CMAKE_DEPENDS_LANGUAGES - ) -# The set of files for implicit dependencies of each language: - -# Preprocessor definitions for this target. -SET(CMAKE_TARGET_DEFINITIONS - "ROS_BUILD_SHARED_LIBS=1" - ) - -# Targets to which this target links. -SET(CMAKE_TARGET_LINKED_INFO_FILES - ) - -# The include file search paths: -SET(CMAKE_C_TARGET_INCLUDE_PATH - ) -SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) -SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) -SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) diff --git a/build/CMakeFiles/run_tests.dir/build.make b/build/CMakeFiles/run_tests.dir/build.make deleted file mode 100644 index 3907c7573..000000000 --- a/build/CMakeFiles/run_tests.dir/build.make +++ /dev/null @@ -1,65 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -#============================================================================= -# Special targets provided by cmake. - -# Disable implicit rules so canonical targets will work. -.SUFFIXES: - -# Remove some rules from gmake that .SUFFIXES does not remove. -SUFFIXES = - -.SUFFIXES: .hpux_make_needs_suffix_list - -# Suppress display of executed commands. -$(VERBOSE).SILENT: - -# A target that is always out of date. -cmake_force: -.PHONY : cmake_force - -#============================================================================= -# Set environment variables for the build. - -# The shell in which to execute make rules. -SHELL = /bin/sh - -# The CMake executable. -CMAKE_COMMAND = /usr/bin/cmake - -# The command to remove a file. -RM = /usr/bin/cmake -E remove -f - -# Escaping for special characters. -EQUALS = = - -# The top-level source directory on which CMake was run. -CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src - -# The top-level build directory on which CMake was run. -CMAKE_BINARY_DIR = /home/roman/src/Firmware/build - -# Utility rule file for run_tests. - -# Include the progress variables for this target. -include CMakeFiles/run_tests.dir/progress.make - -CMakeFiles/run_tests: - -run_tests: CMakeFiles/run_tests -run_tests: CMakeFiles/run_tests.dir/build.make -.PHONY : run_tests - -# Rule to build all files generated by this target. -CMakeFiles/run_tests.dir/build: run_tests -.PHONY : CMakeFiles/run_tests.dir/build - -CMakeFiles/run_tests.dir/clean: - $(CMAKE_COMMAND) -P CMakeFiles/run_tests.dir/cmake_clean.cmake -.PHONY : CMakeFiles/run_tests.dir/clean - -CMakeFiles/run_tests.dir/depend: - cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /home/roman/src/Firmware/src /home/roman/src/Firmware/build /home/roman/src/Firmware/build /home/roman/src/Firmware/build/CMakeFiles/run_tests.dir/DependInfo.cmake --color=$(COLOR) -.PHONY : CMakeFiles/run_tests.dir/depend - diff --git a/build/CMakeFiles/run_tests.dir/cmake_clean.cmake b/build/CMakeFiles/run_tests.dir/cmake_clean.cmake deleted file mode 100644 index 45a3e057b..000000000 --- a/build/CMakeFiles/run_tests.dir/cmake_clean.cmake +++ /dev/null @@ -1,8 +0,0 @@ -FILE(REMOVE_RECURSE - "CMakeFiles/run_tests" -) - -# Per-language clean rules from dependency scanning. -FOREACH(lang) - INCLUDE(CMakeFiles/run_tests.dir/cmake_clean_${lang}.cmake OPTIONAL) -ENDFOREACH(lang) diff --git a/build/CMakeFiles/run_tests.dir/progress.make b/build/CMakeFiles/run_tests.dir/progress.make deleted file mode 100644 index 8b1378917..000000000 --- a/build/CMakeFiles/run_tests.dir/progress.make +++ /dev/null @@ -1 +0,0 @@ - diff --git a/build/CMakeFiles/tests.dir/DependInfo.cmake b/build/CMakeFiles/tests.dir/DependInfo.cmake deleted file mode 100644 index 7aff3a53c..000000000 --- a/build/CMakeFiles/tests.dir/DependInfo.cmake +++ /dev/null @@ -1,20 +0,0 @@ -# The set of languages for which implicit dependencies are needed: -SET(CMAKE_DEPENDS_LANGUAGES - ) -# The set of files for implicit dependencies of each language: - -# Preprocessor definitions for this target. -SET(CMAKE_TARGET_DEFINITIONS - "ROS_BUILD_SHARED_LIBS=1" - ) - -# Targets to which this target links. -SET(CMAKE_TARGET_LINKED_INFO_FILES - ) - -# The include file search paths: -SET(CMAKE_C_TARGET_INCLUDE_PATH - ) -SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) -SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) -SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) diff --git a/build/CMakeFiles/tests.dir/build.make b/build/CMakeFiles/tests.dir/build.make deleted file mode 100644 index 720d53000..000000000 --- a/build/CMakeFiles/tests.dir/build.make +++ /dev/null @@ -1,65 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -#============================================================================= -# Special targets provided by cmake. - -# Disable implicit rules so canonical targets will work. -.SUFFIXES: - -# Remove some rules from gmake that .SUFFIXES does not remove. -SUFFIXES = - -.SUFFIXES: .hpux_make_needs_suffix_list - -# Suppress display of executed commands. -$(VERBOSE).SILENT: - -# A target that is always out of date. -cmake_force: -.PHONY : cmake_force - -#============================================================================= -# Set environment variables for the build. - -# The shell in which to execute make rules. -SHELL = /bin/sh - -# The CMake executable. -CMAKE_COMMAND = /usr/bin/cmake - -# The command to remove a file. -RM = /usr/bin/cmake -E remove -f - -# Escaping for special characters. -EQUALS = = - -# The top-level source directory on which CMake was run. -CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src - -# The top-level build directory on which CMake was run. -CMAKE_BINARY_DIR = /home/roman/src/Firmware/build - -# Utility rule file for tests. - -# Include the progress variables for this target. -include CMakeFiles/tests.dir/progress.make - -CMakeFiles/tests: - -tests: CMakeFiles/tests -tests: CMakeFiles/tests.dir/build.make -.PHONY : tests - -# Rule to build all files generated by this target. -CMakeFiles/tests.dir/build: tests -.PHONY : CMakeFiles/tests.dir/build - -CMakeFiles/tests.dir/clean: - $(CMAKE_COMMAND) -P CMakeFiles/tests.dir/cmake_clean.cmake -.PHONY : CMakeFiles/tests.dir/clean - -CMakeFiles/tests.dir/depend: - cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /home/roman/src/Firmware/src /home/roman/src/Firmware/build /home/roman/src/Firmware/build /home/roman/src/Firmware/build/CMakeFiles/tests.dir/DependInfo.cmake --color=$(COLOR) -.PHONY : CMakeFiles/tests.dir/depend - diff --git a/build/CMakeFiles/tests.dir/cmake_clean.cmake b/build/CMakeFiles/tests.dir/cmake_clean.cmake deleted file mode 100644 index a0424cfc7..000000000 --- a/build/CMakeFiles/tests.dir/cmake_clean.cmake +++ /dev/null @@ -1,8 +0,0 @@ -FILE(REMOVE_RECURSE - "CMakeFiles/tests" -) - -# Per-language clean rules from dependency scanning. -FOREACH(lang) - INCLUDE(CMakeFiles/tests.dir/cmake_clean_${lang}.cmake OPTIONAL) -ENDFOREACH(lang) diff --git a/build/CMakeFiles/tests.dir/progress.make b/build/CMakeFiles/tests.dir/progress.make deleted file mode 100644 index 8b1378917..000000000 --- a/build/CMakeFiles/tests.dir/progress.make +++ /dev/null @@ -1 +0,0 @@ - diff --git a/build/CTestTestfile.cmake b/build/CTestTestfile.cmake deleted file mode 100644 index 7b5534824..000000000 --- a/build/CTestTestfile.cmake +++ /dev/null @@ -1,7 +0,0 @@ -# CMake generated Testfile for -# Source directory: /home/roman/src/Firmware/src -# Build directory: /home/roman/src/Firmware/build -# -# This file includes the relevant testing commands required for -# testing this directory and lists subdirectories to be tested as well. -SUBDIRS(gtest) diff --git a/build/Makefile b/build/Makefile deleted file mode 100644 index c7d796cc4..000000000 --- a/build/Makefile +++ /dev/null @@ -1,262 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -# Default target executed when no arguments are given to make. -default_target: all -.PHONY : default_target - -#============================================================================= -# Special targets provided by cmake. - -# Disable implicit rules so canonical targets will work. -.SUFFIXES: - -# Remove some rules from gmake that .SUFFIXES does not remove. -SUFFIXES = - -.SUFFIXES: .hpux_make_needs_suffix_list - -# Suppress display of executed commands. -$(VERBOSE).SILENT: - -# A target that is always out of date. -cmake_force: -.PHONY : cmake_force - -#============================================================================= -# Set environment variables for the build. - -# The shell in which to execute make rules. -SHELL = /bin/sh - -# The CMake executable. -CMAKE_COMMAND = /usr/bin/cmake - -# The command to remove a file. -RM = /usr/bin/cmake -E remove -f - -# Escaping for special characters. -EQUALS = = - -# The top-level source directory on which CMake was run. -CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src - -# The top-level build directory on which CMake was run. -CMAKE_BINARY_DIR = /home/roman/src/Firmware/build - -#============================================================================= -# Targets provided globally by CMake. - -# Special rule for the target edit_cache -edit_cache: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running interactive CMake command-line interface..." - /usr/bin/cmake -i . -.PHONY : edit_cache - -# Special rule for the target edit_cache -edit_cache/fast: edit_cache -.PHONY : edit_cache/fast - -# Special rule for the target install -install: preinstall - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." - /usr/bin/cmake -P cmake_install.cmake -.PHONY : install - -# Special rule for the target install -install/fast: preinstall/fast - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." - /usr/bin/cmake -P cmake_install.cmake -.PHONY : install/fast - -# Special rule for the target install/local -install/local: preinstall - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..." - /usr/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake -.PHONY : install/local - -# Special rule for the target install/local -install/local/fast: install/local -.PHONY : install/local/fast - -# Special rule for the target install/strip -install/strip: preinstall - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..." - /usr/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake -.PHONY : install/strip - -# Special rule for the target install/strip -install/strip/fast: install/strip -.PHONY : install/strip/fast - -# Special rule for the target list_install_components -list_install_components: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Available install components are: \"Unspecified\"" -.PHONY : list_install_components - -# Special rule for the target list_install_components -list_install_components/fast: list_install_components -.PHONY : list_install_components/fast - -# Special rule for the target rebuild_cache -rebuild_cache: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..." - /usr/bin/cmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) -.PHONY : rebuild_cache - -# Special rule for the target rebuild_cache -rebuild_cache/fast: rebuild_cache -.PHONY : rebuild_cache/fast - -# Special rule for the target test -test: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running tests..." - /usr/bin/ctest --force-new-ctest-process $(ARGS) -.PHONY : test - -# Special rule for the target test -test/fast: test -.PHONY : test/fast - -# The main all target -all: cmake_check_build_system - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles /home/roman/src/Firmware/build/CMakeFiles/progress.marks - $(MAKE) -f CMakeFiles/Makefile2 all - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 -.PHONY : all - -# The main clean target -clean: - $(MAKE) -f CMakeFiles/Makefile2 clean -.PHONY : clean - -# The main clean target -clean/fast: clean -.PHONY : clean/fast - -# Prepare targets for installation. -preinstall: all - $(MAKE) -f CMakeFiles/Makefile2 preinstall -.PHONY : preinstall - -# Prepare targets for installation. -preinstall/fast: - $(MAKE) -f CMakeFiles/Makefile2 preinstall -.PHONY : preinstall/fast - -# clear depends -depend: - $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1 -.PHONY : depend - -#============================================================================= -# Target rules for targets named clean_test_results - -# Build rule for target. -clean_test_results: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 clean_test_results -.PHONY : clean_test_results - -# fast build rule for target. -clean_test_results/fast: - $(MAKE) -f CMakeFiles/clean_test_results.dir/build.make CMakeFiles/clean_test_results.dir/build -.PHONY : clean_test_results/fast - -#============================================================================= -# Target rules for targets named doxygen - -# Build rule for target. -doxygen: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 doxygen -.PHONY : doxygen - -# fast build rule for target. -doxygen/fast: - $(MAKE) -f CMakeFiles/doxygen.dir/build.make CMakeFiles/doxygen.dir/build -.PHONY : doxygen/fast - -#============================================================================= -# Target rules for targets named run_tests - -# Build rule for target. -run_tests: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 run_tests -.PHONY : run_tests - -# fast build rule for target. -run_tests/fast: - $(MAKE) -f CMakeFiles/run_tests.dir/build.make CMakeFiles/run_tests.dir/build -.PHONY : run_tests/fast - -#============================================================================= -# Target rules for targets named tests - -# Build rule for target. -tests: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 tests -.PHONY : tests - -# fast build rule for target. -tests/fast: - $(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/build -.PHONY : tests/fast - -#============================================================================= -# Target rules for targets named gtest - -# Build rule for target. -gtest: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 gtest -.PHONY : gtest - -# fast build rule for target. -gtest/fast: - $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/build -.PHONY : gtest/fast - -#============================================================================= -# Target rules for targets named gtest_main - -# Build rule for target. -gtest_main: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 gtest_main -.PHONY : gtest_main - -# fast build rule for target. -gtest_main/fast: - $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/build -.PHONY : gtest_main/fast - -# Help Target -help: - @echo "The following are some of the valid targets for this Makefile:" - @echo "... all (the default if no target is provided)" - @echo "... clean" - @echo "... depend" - @echo "... clean_test_results" - @echo "... doxygen" - @echo "... edit_cache" - @echo "... install" - @echo "... install/local" - @echo "... install/strip" - @echo "... list_install_components" - @echo "... rebuild_cache" - @echo "... run_tests" - @echo "... test" - @echo "... tests" - @echo "... gtest" - @echo "... gtest_main" -.PHONY : help - - - -#============================================================================= -# Special targets to cleanup operation of make. - -# Special rule to run CMake to check the build system integrity. -# No rule that depends on this can have commands that come from listfiles -# because they might be regenerated. -cmake_check_build_system: - $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 -.PHONY : cmake_check_build_system - diff --git a/build/catkin/catkin_generated/version/package.cmake b/build/catkin/catkin_generated/version/package.cmake deleted file mode 100644 index 1cbfc826b..000000000 --- a/build/catkin/catkin_generated/version/package.cmake +++ /dev/null @@ -1,9 +0,0 @@ -set(_CATKIN_CURRENT_PACKAGE "catkin") -set(catkin_VERSION "0.6.9") -set(catkin_BUILD_DEPENDS_python-catkin-pkg_VERSION_GTE "0.2.2") -set(catkin_BUILD_DEPENDS "python-empy" "python-argparse" "python-catkin-pkg") -set(catkin_DEPRECATED "") -set(catkin_RUN_DEPENDS "python-argparse" "python-catkin-pkg" "gtest" "python-empy" "python-nose") -set(catkin_MAINTAINER "Dirk Thomas ") -set(catkin_BUILDTOOL_DEPENDS "cmake") -set(catkin_RUN_DEPENDS_python-catkin-pkg_VERSION_GTE "0.2.2") \ No newline at end of file diff --git a/build/catkin_generated/env_cached.sh b/build/catkin_generated/env_cached.sh deleted file mode 100755 index d6be91db5..000000000 --- a/build/catkin_generated/env_cached.sh +++ /dev/null @@ -1,16 +0,0 @@ -#!/usr/bin/env sh -# generated from catkin/cmake/templates/env.sh.in - -if [ $# -eq 0 ] ; then - /bin/echo "Usage: env.sh COMMANDS" - /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually." - exit 1 -fi - -# ensure to not use different shell type which was set before -CATKIN_SHELL=sh - -# source setup_cached.sh from same directory as this file -_CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd) -. "$_CATKIN_SETUP_DIR/setup_cached.sh" -exec "$@" diff --git a/build/catkin_generated/generate_cached_setup.py b/build/catkin_generated/generate_cached_setup.py deleted file mode 100644 index 7a56cdb7f..000000000 --- a/build/catkin_generated/generate_cached_setup.py +++ /dev/null @@ -1,29 +0,0 @@ -from __future__ import print_function -import argparse -import os -import stat -import sys - -# find the import for catkin's python package - either from source space or from an installed underlay -if os.path.exists(os.path.join('/opt/ros/indigo/share/catkin/cmake', 'catkinConfig.cmake.in')): - sys.path.insert(0, os.path.join('/opt/ros/indigo/share/catkin/cmake', '..', 'python')) -try: - from catkin.environment_cache import generate_environment_script -except ImportError: - # search for catkin package in all workspaces and prepend to path - for workspace in "/home/roman/catkin_ws/devel;/opt/ros/indigo".split(';'): - python_path = os.path.join(workspace, 'lib/python2.7/dist-packages') - if os.path.isdir(os.path.join(python_path, 'catkin')): - sys.path.insert(0, python_path) - break - from catkin.environment_cache import generate_environment_script - -code = generate_environment_script('/home/roman/src/Firmware/devel/env.sh') - -output_filename = '/home/roman/src/Firmware/build/catkin_generated/setup_cached.sh' -with open(output_filename, 'w') as f: - #print('Generate script for cached setup "%s"' % output_filename) - f.write('\n'.join(code)) - -mode = os.stat(output_filename).st_mode -os.chmod(output_filename, mode | stat.S_IXUSR) diff --git a/build/catkin_generated/installspace/.rosinstall b/build/catkin_generated/installspace/.rosinstall deleted file mode 100644 index a959c71ef..000000000 --- a/build/catkin_generated/installspace/.rosinstall +++ /dev/null @@ -1,2 +0,0 @@ -- setup-file: - local-name: /home/roman/src/Firmware/install/setup.sh diff --git a/build/catkin_generated/installspace/_setup_util.py b/build/catkin_generated/installspace/_setup_util.py deleted file mode 100755 index 8db644140..000000000 --- a/build/catkin_generated/installspace/_setup_util.py +++ /dev/null @@ -1,287 +0,0 @@ -#!/usr/bin/python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * 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. -# * Neither the name of Willow Garage, Inc. 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. - -'''This file generates shell code for the setup.SHELL scripts to set environment variables''' - -from __future__ import print_function -import argparse -import copy -import errno -import os -import platform -import sys - -CATKIN_MARKER_FILE = '.catkin' - -system = platform.system() -IS_DARWIN = (system == 'Darwin') -IS_WINDOWS = (system == 'Windows') - -# subfolder of workspace prepended to CMAKE_PREFIX_PATH -ENV_VAR_SUBFOLDERS = { - 'CMAKE_PREFIX_PATH': '', - 'CPATH': 'include', - 'LD_LIBRARY_PATH' if not IS_DARWIN else 'DYLD_LIBRARY_PATH': ['lib', os.path.join('lib', 'x86_64-linux-gnu')], - 'PATH': 'bin', - 'PKG_CONFIG_PATH': [os.path.join('lib', 'pkgconfig'), os.path.join('lib', 'x86_64-linux-gnu', 'pkgconfig')], - 'PYTHONPATH': 'lib/python2.7/dist-packages', -} - - -def rollback_env_variables(environ, env_var_subfolders): - ''' - Generate shell code to reset environment variables - by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH. - This does not cover modifications performed by environment hooks. - ''' - lines = [] - unmodified_environ = copy.copy(environ) - for key in sorted(env_var_subfolders.keys()): - subfolders = env_var_subfolders[key] - if not isinstance(subfolders, list): - subfolders = [subfolders] - for subfolder in subfolders: - value = _rollback_env_variable(unmodified_environ, key, subfolder) - if value is not None: - environ[key] = value - lines.append(assignment(key, value)) - if lines: - lines.insert(0, comment('reset environment variables by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH')) - return lines - - -def _rollback_env_variable(environ, name, subfolder): - ''' - For each catkin workspace in CMAKE_PREFIX_PATH remove the first entry from env[NAME] matching workspace + subfolder. - - :param subfolder: str '' or subfoldername that may start with '/' - :returns: the updated value of the environment variable. - ''' - value = environ[name] if name in environ else '' - env_paths = [path for path in value.split(os.pathsep) if path] - value_modified = False - if subfolder: - if subfolder.startswith(os.path.sep) or (os.path.altsep and subfolder.startswith(os.path.altsep)): - subfolder = subfolder[1:] - if subfolder.endswith(os.path.sep) or (os.path.altsep and subfolder.endswith(os.path.altsep)): - subfolder = subfolder[:-1] - for ws_path in _get_workspaces(environ, include_fuerte=True, include_non_existing=True): - path_to_find = os.path.join(ws_path, subfolder) if subfolder else ws_path - path_to_remove = None - for env_path in env_paths: - env_path_clean = env_path[:-1] if env_path and env_path[-1] in [os.path.sep, os.path.altsep] else env_path - if env_path_clean == path_to_find: - path_to_remove = env_path - break - if path_to_remove: - env_paths.remove(path_to_remove) - value_modified = True - new_value = os.pathsep.join(env_paths) - return new_value if value_modified else None - - -def _get_workspaces(environ, include_fuerte=False, include_non_existing=False): - ''' - Based on CMAKE_PREFIX_PATH return all catkin workspaces. - - :param include_fuerte: The flag if paths starting with '/opt/ros/fuerte' should be considered workspaces, ``bool`` - ''' - # get all cmake prefix paths - env_name = 'CMAKE_PREFIX_PATH' - value = environ[env_name] if env_name in environ else '' - paths = [path for path in value.split(os.pathsep) if path] - # remove non-workspace paths - workspaces = [path for path in paths if os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE)) or (include_fuerte and path.startswith('/opt/ros/fuerte')) or (include_non_existing and not os.path.exists(path))] - return workspaces - - -def prepend_env_variables(environ, env_var_subfolders, workspaces): - ''' - Generate shell code to prepend environment variables - for the all workspaces. - ''' - lines = [] - lines.append(comment('prepend folders of workspaces to environment variables')) - - paths = [path for path in workspaces.split(os.pathsep) if path] - - prefix = _prefix_env_variable(environ, 'CMAKE_PREFIX_PATH', paths, '') - lines.append(prepend(environ, 'CMAKE_PREFIX_PATH', prefix)) - - for key in sorted([key for key in env_var_subfolders.keys() if key != 'CMAKE_PREFIX_PATH']): - subfolder = env_var_subfolders[key] - prefix = _prefix_env_variable(environ, key, paths, subfolder) - lines.append(prepend(environ, key, prefix)) - return lines - - -def _prefix_env_variable(environ, name, paths, subfolders): - ''' - Return the prefix to prepend to the environment variable NAME, adding any path in NEW_PATHS_STR without creating duplicate or empty items. - ''' - value = environ[name] if name in environ else '' - environ_paths = [path for path in value.split(os.pathsep) if path] - checked_paths = [] - for path in paths: - if not isinstance(subfolders, list): - subfolders = [subfolders] - for subfolder in subfolders: - path_tmp = path - if subfolder: - path_tmp = os.path.join(path_tmp, subfolder) - # exclude any path already in env and any path we already added - if path_tmp not in environ_paths and path_tmp not in checked_paths: - checked_paths.append(path_tmp) - prefix_str = os.pathsep.join(checked_paths) - if prefix_str != '' and environ_paths: - prefix_str += os.pathsep - return prefix_str - - -def assignment(key, value): - if not IS_WINDOWS: - return 'export %s="%s"' % (key, value) - else: - return 'set %s=%s' % (key, value) - - -def comment(msg): - if not IS_WINDOWS: - return '# %s' % msg - else: - return 'REM %s' % msg - - -def prepend(environ, key, prefix): - if key not in environ or not environ[key]: - return assignment(key, prefix) - if not IS_WINDOWS: - return 'export %s="%s$%s"' % (key, prefix, key) - else: - return 'set %s=%s%%%s%%' % (key, prefix, key) - - -def find_env_hooks(environ, cmake_prefix_path): - ''' - Generate shell code with found environment hooks - for the all workspaces. - ''' - lines = [] - lines.append(comment('found environment hooks in workspaces')) - - generic_env_hooks = [] - generic_env_hooks_workspace = [] - specific_env_hooks = [] - specific_env_hooks_workspace = [] - generic_env_hooks_by_filename = {} - specific_env_hooks_by_filename = {} - generic_env_hook_ext = 'bat' if IS_WINDOWS else 'sh' - specific_env_hook_ext = environ['CATKIN_SHELL'] if not IS_WINDOWS and 'CATKIN_SHELL' in environ and environ['CATKIN_SHELL'] else None - # remove non-workspace paths - workspaces = [path for path in cmake_prefix_path.split(os.pathsep) if path and os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE))] - for workspace in reversed(workspaces): - env_hook_dir = os.path.join(workspace, 'etc', 'catkin', 'profile.d') - if os.path.isdir(env_hook_dir): - for filename in sorted(os.listdir(env_hook_dir)): - if filename.endswith('.%s' % generic_env_hook_ext): - # remove previous env hook with same name if present - if filename in generic_env_hooks_by_filename: - i = generic_env_hooks.index(generic_env_hooks_by_filename[filename]) - generic_env_hooks.pop(i) - generic_env_hooks_workspace.pop(i) - # append env hook - generic_env_hooks.append(os.path.join(env_hook_dir, filename)) - generic_env_hooks_workspace.append(workspace) - generic_env_hooks_by_filename[filename] = generic_env_hooks[-1] - elif specific_env_hook_ext is not None and filename.endswith('.%s' % specific_env_hook_ext): - # remove previous env hook with same name if present - if filename in specific_env_hooks_by_filename: - i = specific_env_hooks.index(specific_env_hooks_by_filename[filename]) - specific_env_hooks.pop(i) - specific_env_hooks_workspace.pop(i) - # append env hook - specific_env_hooks.append(os.path.join(env_hook_dir, filename)) - specific_env_hooks_workspace.append(workspace) - specific_env_hooks_by_filename[filename] = specific_env_hooks[-1] - env_hooks = generic_env_hooks + specific_env_hooks - env_hooks_workspace = generic_env_hooks_workspace + specific_env_hooks_workspace - count = len(env_hooks) - lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_COUNT', count)) - for i in range(count): - lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d' % i, env_hooks[i])) - lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d_WORKSPACE' % i, env_hooks_workspace[i])) - return lines - - -def _parse_arguments(args=None): - parser = argparse.ArgumentParser(description='Generates code blocks for the setup.SHELL script.') - parser.add_argument('--extend', action='store_true', help='Skip unsetting previous environment variables to extend context') - return parser.parse_known_args(args=args)[0] - - -if __name__ == '__main__': - try: - try: - args = _parse_arguments() - except Exception as e: - print(e, file=sys.stderr) - sys.exit(1) - - # environment at generation time - CMAKE_PREFIX_PATH = '/home/roman/catkin_ws/devel;/opt/ros/indigo'.split(';') - # prepend current workspace if not already part of CPP - base_path = os.path.dirname(__file__) - if base_path not in CMAKE_PREFIX_PATH: - CMAKE_PREFIX_PATH.insert(0, base_path) - CMAKE_PREFIX_PATH = os.pathsep.join(CMAKE_PREFIX_PATH) - - environ = dict(os.environ) - lines = [] - if not args.extend: - lines += rollback_env_variables(environ, ENV_VAR_SUBFOLDERS) - lines += prepend_env_variables(environ, ENV_VAR_SUBFOLDERS, CMAKE_PREFIX_PATH) - lines += find_env_hooks(environ, CMAKE_PREFIX_PATH) - print('\n'.join(lines)) - - # need to explicitly flush the output - sys.stdout.flush() - except IOError as e: - # and catch potantial "broken pipe" if stdout is not writable - # which can happen when piping the output to a file but the disk is full - if e.errno == errno.EPIPE: - print(e, file=sys.stderr) - sys.exit(2) - raise - - sys.exit(0) diff --git a/build/catkin_generated/installspace/env.sh b/build/catkin_generated/installspace/env.sh deleted file mode 100755 index 8aa9d244a..000000000 --- a/build/catkin_generated/installspace/env.sh +++ /dev/null @@ -1,16 +0,0 @@ -#!/usr/bin/env sh -# generated from catkin/cmake/templates/env.sh.in - -if [ $# -eq 0 ] ; then - /bin/echo "Usage: env.sh COMMANDS" - /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually." - exit 1 -fi - -# ensure to not use different shell type which was set before -CATKIN_SHELL=sh - -# source setup.sh from same directory as this file -_CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd) -. "$_CATKIN_SETUP_DIR/setup.sh" -exec "$@" diff --git a/build/catkin_generated/installspace/setup.bash b/build/catkin_generated/installspace/setup.bash deleted file mode 100644 index ff47af8f3..000000000 --- a/build/catkin_generated/installspace/setup.bash +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env bash -# generated from catkin/cmake/templates/setup.bash.in - -CATKIN_SHELL=bash - -# source setup.sh from same directory as this file -_CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd) -. "$_CATKIN_SETUP_DIR/setup.sh" diff --git a/build/catkin_generated/installspace/setup.sh b/build/catkin_generated/installspace/setup.sh deleted file mode 100644 index 95e1571be..000000000 --- a/build/catkin_generated/installspace/setup.sh +++ /dev/null @@ -1,87 +0,0 @@ -#!/usr/bin/env sh -# generated from catkin/cmake/template/setup.sh.in - -# Sets various environment variables and sources additional environment hooks. -# It tries it's best to undo changes from a previously sourced setup file before. -# Supported command line options: -# --extend: skips the undoing of changes from a previously sourced setup file - -# since this file is sourced either use the provided _CATKIN_SETUP_DIR -# or fall back to the destination set at configure time -: ${_CATKIN_SETUP_DIR:=/home/roman/src/Firmware/install} -_SETUP_UTIL="$_CATKIN_SETUP_DIR/_setup_util.py" -unset _CATKIN_SETUP_DIR - -if [ ! -f "$_SETUP_UTIL" ]; then - echo "Missing Python script: $_SETUP_UTIL" - return 22 -fi - -# detect if running on Darwin platform -_UNAME=`uname -s` -_IS_DARWIN=0 -if [ "$_UNAME" = "Darwin" ]; then - _IS_DARWIN=1 -fi -unset _UNAME - -# make sure to export all environment variables -export CMAKE_PREFIX_PATH -export CPATH -if [ $_IS_DARWIN -eq 0 ]; then - export LD_LIBRARY_PATH -else - export DYLD_LIBRARY_PATH -fi -unset _IS_DARWIN -export PATH -export PKG_CONFIG_PATH -export PYTHONPATH - -# remember type of shell if not already set -if [ -z "$CATKIN_SHELL" ]; then - CATKIN_SHELL=sh -fi - -# invoke Python script to generate necessary exports of environment variables -_SETUP_TMP=`mktemp /tmp/setup.sh.XXXXXXXXXX` -if [ $? -ne 0 -o ! -f "$_SETUP_TMP" ]; then - echo "Could not create temporary file: $_SETUP_TMP" - return 1 -fi -CATKIN_SHELL=$CATKIN_SHELL "$_SETUP_UTIL" $@ > $_SETUP_TMP -_RC=$? -if [ $_RC -ne 0 ]; then - if [ $_RC -eq 2 ]; then - echo "Could not write the output of '$_SETUP_UTIL' to temporary file '$_SETUP_TMP': may be the disk if full?" - else - echo "Failed to run '\"$_SETUP_UTIL\" $@': return code $_RC" - fi - unset _RC - unset _SETUP_UTIL - rm -f $_SETUP_TMP - unset _SETUP_TMP - return 1 -fi -unset _RC -unset _SETUP_UTIL -. $_SETUP_TMP -rm -f $_SETUP_TMP -unset _SETUP_TMP - -# source all environment hooks -_i=0 -while [ $_i -lt $_CATKIN_ENVIRONMENT_HOOKS_COUNT ]; do - eval _envfile=\$_CATKIN_ENVIRONMENT_HOOKS_$_i - unset _CATKIN_ENVIRONMENT_HOOKS_$_i - eval _envfile_workspace=\$_CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE - unset _CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE - # set workspace for environment hook - CATKIN_ENV_HOOK_WORKSPACE=$_envfile_workspace - . "$_envfile" - unset CATKIN_ENV_HOOK_WORKSPACE - _i=$((_i + 1)) -done -unset _i - -unset _CATKIN_ENVIRONMENT_HOOKS_COUNT diff --git a/build/catkin_generated/installspace/setup.zsh b/build/catkin_generated/installspace/setup.zsh deleted file mode 100644 index b66071766..000000000 --- a/build/catkin_generated/installspace/setup.zsh +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env zsh -# generated from catkin/cmake/templates/setup.zsh.in - -CATKIN_SHELL=zsh -_CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd) -emulate sh # emulate POSIX -. "$_CATKIN_SETUP_DIR/setup.sh" -emulate zsh # back to zsh mode diff --git a/build/catkin_generated/order_packages.cmake b/build/catkin_generated/order_packages.cmake deleted file mode 100644 index 6b0e2dff9..000000000 --- a/build/catkin_generated/order_packages.cmake +++ /dev/null @@ -1,10 +0,0 @@ -# generated from catkin/cmake/em/order_packages.cmake.em - -set(CATKIN_ORDERED_PACKAGES "") -set(CATKIN_ORDERED_PACKAGE_PATHS "") -set(CATKIN_ORDERED_PACKAGES_IS_META "") -set(CATKIN_ORDERED_PACKAGES_BUILD_TYPE "") - -set(CATKIN_MESSAGE_GENERATORS ) - -set(CATKIN_METAPACKAGE_CMAKE_TEMPLATE "/usr/lib/pymodules/python2.7/catkin_pkg/templates/metapackage.cmake.in") diff --git a/build/catkin_generated/order_packages.py b/build/catkin_generated/order_packages.py deleted file mode 100644 index 5cb176580..000000000 --- a/build/catkin_generated/order_packages.py +++ /dev/null @@ -1,5 +0,0 @@ -# generated from catkin/cmake/template/order_packages.context.py.in -source_root_dir = "/home/roman/src/Firmware/src" -whitelisted_packages = "".split(';') if "" != "" else [] -blacklisted_packages = "".split(';') if "" != "" else [] -underlay_workspaces = "/home/roman/catkin_ws/devel;/opt/ros/indigo".split(';') if "/home/roman/catkin_ws/devel;/opt/ros/indigo" != "" else [] diff --git a/build/catkin_generated/setup_cached.sh b/build/catkin_generated/setup_cached.sh deleted file mode 100755 index e03bce4ba..000000000 --- a/build/catkin_generated/setup_cached.sh +++ /dev/null @@ -1,20 +0,0 @@ -#!/usr/bin/env sh -# generated from catkin/python/catkin/environment_cache.py - -# based on a snapshot of the environment before and after calling the setup script -# it emulates the modifications of the setup script without recurring computations - -# new environment variables - -# modified environment variables -export CATKIN_TEST_RESULTS_DIR="/home/roman/src/Firmware/build/test_results" -export CMAKE_PREFIX_PATH="/home/roman/src/Firmware/devel:$CMAKE_PREFIX_PATH" -export CPATH="/home/roman/src/Firmware/devel/include:$CPATH" -export LD_LIBRARY_PATH="/home/roman/src/Firmware/devel/lib:/home/roman/src/Firmware/devel/lib/x86_64-linux-gnu:/home/roman/catkin_ws/devel/lib/x86_64-linux-gnu:/opt/ros/indigo/lib/x86_64-linux-gnu:/home/roman/catkin_ws/devel/lib:/opt/ros/indigo/lib" -export PATH="/home/roman/src/Firmware/devel/bin:$PATH" -export PKG_CONFIG_PATH="/home/roman/src/Firmware/devel/lib/pkgconfig:/home/roman/src/Firmware/devel/lib/x86_64-linux-gnu/pkgconfig:$PKG_CONFIG_PATH" -export PWD="/home/roman/src/Firmware/build" -export PYTHONPATH="/home/roman/src/Firmware/devel/lib/python2.7/dist-packages:$PYTHONPATH" -export ROSLISP_PACKAGE_DIRECTORIES="/home/roman/src/Firmware/devel/share/common-lisp:$ROSLISP_PACKAGE_DIRECTORIES" -export ROS_PACKAGE_PATH="/home/roman/src/Firmware/src:$ROS_PACKAGE_PATH" -export ROS_TEST_RESULTS_DIR="/home/roman/src/Firmware/build/test_results" \ No newline at end of file diff --git a/build/catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp b/build/catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp deleted file mode 100644 index 26882f02a..000000000 --- a/build/catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp +++ /dev/null @@ -1,250 +0,0 @@ -#!/usr/bin/env python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * 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. -# * Neither the name of Willow Garage, Inc. 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. - -from __future__ import print_function -import os -import sys - -import distutils.core -try: - import setuptools -except ImportError: - pass - -from argparse import ArgumentParser - - -def _get_locations(pkgs, package_dir): - """ - based on setuptools logic and the package_dir dict, builds a dict - of location roots for each pkg in pkgs. - See http://docs.python.org/distutils/setupscript.html - - :returns: a dict {pkgname: root} for each pkgname in pkgs (and each of their parents) - """ - # package_dir contains a dict {package_name: relativepath} - # Example {'': 'src', 'foo': 'lib', 'bar': 'lib2'} - # - # '' means where to look for any package unless a parent package - # is listed so package bar.pot is expected at lib2/bar/pot, - # whereas package sup.dee is expected at src/sup/dee - # - # if package_dir does not state anything about a package, - # setuptool expects the package folder to be in the root of the - # project - locations = {} - allprefix = package_dir.get('', '') - for pkg in pkgs: - parent_location = None - splits = pkg.split('.') - # we iterate over compound name from parent to child - # so once we found parent, children just append to their parent - for key_len in range(len(splits)): - key = '.'.join(splits[:key_len + 1]) - if key not in locations: - if key in package_dir: - locations[key] = package_dir[key] - elif parent_location is not None: - locations[key] = parent_location - else: - locations[key] = allprefix - parent_location = locations[key] - return locations - - -def generate_cmake_file(package_name, version, scripts, package_dir, pkgs, modules): - """ - Generates lines to add to a cmake file which will set variables - - :param version: str, format 'int.int.int' - :param scripts: [list of str]: relative paths to scripts - :param package_dir: {modulename: path} - :pkgs: [list of str] python_packages declared in catkin package - :modules: [list of str] python modules - """ - prefix = '%s_SETUP_PY' % package_name - result = [] - result.append(r'set(%s_VERSION "%s")' % (prefix, version)) - result.append(r'set(%s_SCRIPTS "%s")' % (prefix, ';'.join(scripts))) - - # Remove packages with '.' separators. - # - # setuptools allows specifying submodules in other folders than - # their parent - # - # The symlink approach of catkin does not work with such submodules. - # In the common case, this does not matter as the submodule is - # within the containing module. We verify this assumption, and if - # it passes, we remove submodule packages. - locations = _get_locations(pkgs, package_dir) - for pkgname, location in locations.items(): - if not '.' in pkgname: - continue - splits = pkgname.split('.') - # hack: ignore write-combining setup.py files for msg and srv files - if splits[1] in ['msg', 'srv']: - continue - # check every child has the same root folder as its parent - parent_name = '.'.join(splits[:1]) - if location != locations[parent_name]: - raise RuntimeError( - "catkin_export_python does not support setup.py files that combine across multiple directories: %s in %s, %s in %s" % (pkgname, location, parent_name, locations[parent_name])) - - # If checks pass, remove all submodules - pkgs = [p for p in pkgs if '.' not in p] - - resolved_pkgs = [] - for pkg in pkgs: - resolved_pkgs += [os.path.join(locations[pkg], pkg)] - - result.append(r'set(%s_PACKAGES "%s")' % (prefix, ';'.join(pkgs))) - result.append(r'set(%s_PACKAGE_DIRS "%s")' % (prefix, ';'.join(resolved_pkgs).replace("\\", "/"))) - - # skip modules which collide with package names - filtered_modules = [] - for modname in modules: - splits = modname.split('.') - # check all parents too - equals_package = [('.'.join(splits[:-i]) in locations) for i in range(len(splits))] - if any(equals_package): - continue - filtered_modules.append(modname) - module_locations = _get_locations(filtered_modules, package_dir) - - result.append(r'set(%s_MODULES "%s")' % (prefix, ';'.join(['%s.py' % m.replace('.', '/') for m in filtered_modules]))) - result.append(r'set(%s_MODULE_DIRS "%s")' % (prefix, ';'.join([module_locations[m] for m in filtered_modules]).replace("\\", "/"))) - - return result - - -def _create_mock_setup_function(package_name, outfile): - """ - Creates a function to call instead of distutils.core.setup or - setuptools.setup, which just captures some args and writes them - into a file that can be used from cmake - - :param package_name: name of the package - :param outfile: filename that cmake will use afterwards - :returns: a function to replace disutils.core.setup and setuptools.setup - """ - - def setup(*args, **kwargs): - ''' - Checks kwargs and writes a scriptfile - ''' - if 'version' not in kwargs: - sys.stderr.write("\n*** Unable to find 'version' in setup.py of %s\n" % package_name) - raise RuntimeError("version not found in setup.py") - version = kwargs['version'] - package_dir = kwargs.get('package_dir', {}) - - pkgs = kwargs.get('packages', []) - scripts = kwargs.get('scripts', []) - modules = kwargs.get('py_modules', []) - - unsupported_args = [ - 'entry_points', - 'exclude_package_data', - 'ext_modules ', - 'ext_package', - 'include_package_data', - 'namespace_packages', - 'setup_requires', - 'use_2to3', - 'zip_safe'] - used_unsupported_args = [arg for arg in unsupported_args if arg in kwargs] - if used_unsupported_args: - sys.stderr.write("*** Arguments %s to setup() not supported in catkin devel space in setup.py of %s\n" % (used_unsupported_args, package_name)) - - result = generate_cmake_file(package_name=package_name, - version=version, - scripts=scripts, - package_dir=package_dir, - pkgs=pkgs, - modules=modules) - with open(outfile, 'w') as out: - out.write('\n'.join(result)) - - return setup - - -def main(): - """ - Script main, parses arguments and invokes Dummy.setup indirectly. - """ - parser = ArgumentParser(description='Utility to read setup.py values from cmake macros. Creates a file with CMake set commands setting variables.') - parser.add_argument('package_name', help='Name of catkin package') - parser.add_argument('setupfile_path', help='Full path to setup.py') - parser.add_argument('outfile', help='Where to write result to') - - args = parser.parse_args() - - # print("%s" % sys.argv) - # PACKAGE_NAME = sys.argv[1] - # OUTFILE = sys.argv[3] - # print("Interrogating setup.py for package %s into %s " % (PACKAGE_NAME, OUTFILE), - # file=sys.stderr) - - # print("executing %s" % args.setupfile_path) - - # be sure you're in the directory containing - # setup.py so the sys.path manipulation works, - # so the import of __version__ works - os.chdir(os.path.dirname(os.path.abspath(args.setupfile_path))) - - # patch setup() function of distutils and setuptools for the - # context of evaluating setup.py - try: - fake_setup = _create_mock_setup_function(package_name=args.package_name, - outfile=args.outfile) - - distutils_backup = distutils.core.setup - distutils.core.setup = fake_setup - try: - setuptools_backup = setuptools.setup - setuptools.setup = fake_setup - except NameError: - pass - - with open(args.setupfile_path, 'r') as fh: - exec(fh.read()) - finally: - distutils.core.setup = distutils_backup - try: - setuptools.setup = setuptools_backup - except NameError: - pass - -if __name__ == '__main__': - main() diff --git a/build/catkin_generated/stamps/Project/order_packages.cmake.em.stamp b/build/catkin_generated/stamps/Project/order_packages.cmake.em.stamp deleted file mode 100644 index 087d4d802..000000000 --- a/build/catkin_generated/stamps/Project/order_packages.cmake.em.stamp +++ /dev/null @@ -1,56 +0,0 @@ -# generated from catkin/cmake/em/order_packages.cmake.em -@{ -import os -try: - from catkin_pkg.cmake import get_metapackage_cmake_template_path -except ImportError as e: - raise RuntimeError('ImportError: "from catkin_pkg.cmake import get_metapackage_cmake_template_path" failed: %s\nMake sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.' % e) -try: - from catkin_pkg.topological_order import topological_order -except ImportError as e: - raise RuntimeError('ImportError: "from catkin_pkg.topological_order import topological_order" failed: %s\nMake sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.' % e) -try: - from catkin_pkg.package import InvalidPackage -except ImportError as e: - raise RuntimeError('ImportError: "from catkin_pkg.package import InvalidPackage" failed: %s\nMake sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.' % e) -# vars defined in order_packages.context.py.in -try: - ordered_packages = topological_order(os.path.normpath(source_root_dir), whitelisted=whitelisted_packages, blacklisted=blacklisted_packages, underlay_workspaces=underlay_workspaces) -except InvalidPackage as e: - print('message(FATAL_ERROR "%s")' % ('%s' % e).replace('"', '\\"')) - ordered_packages = [] -fatal_error = False -}@ - -set(CATKIN_ORDERED_PACKAGES "") -set(CATKIN_ORDERED_PACKAGE_PATHS "") -set(CATKIN_ORDERED_PACKAGES_IS_META "") -set(CATKIN_ORDERED_PACKAGES_BUILD_TYPE "") -@[for path, package in ordered_packages]@ -@[if path is None]@ -message(FATAL_ERROR "Circular dependency in subset of packages:\n@package") -@{ -fatal_error = True -}@ -@[elif package.name != 'catkin']@ -list(APPEND CATKIN_ORDERED_PACKAGES "@(package.name)") -list(APPEND CATKIN_ORDERED_PACKAGE_PATHS "@(path.replace('\\','/'))") -list(APPEND CATKIN_ORDERED_PACKAGES_IS_META "@(str('metapackage' in [e.tagname for e in package.exports]))") -list(APPEND CATKIN_ORDERED_PACKAGES_BUILD_TYPE "@(str([e.content for e in package.exports if e.tagname == 'build_type'][0]) if 'build_type' in [e.tagname for e in package.exports] else 'catkin')") -@{ -deprecated = [e for e in package.exports if e.tagname == 'deprecated'] -}@ -@[if deprecated]@ -message("WARNING: Package '@(package.name)' is deprecated@(' (%s)' % deprecated[0].content if deprecated[0].content else '')") -@[end if]@ -@[end if]@ -@[end for]@ - -@[if not fatal_error]@ -@{ -message_generators = [package.name for (_, package) in ordered_packages if 'message_generator' in [e.tagname for e in package.exports]] -}@ -set(CATKIN_MESSAGE_GENERATORS @(' '.join(message_generators))) -@[end if]@ - -set(CATKIN_METAPACKAGE_CMAKE_TEMPLATE "@(get_metapackage_cmake_template_path().replace('\\','/'))") diff --git a/build/catkin_generated/stamps/Project/package.xml.stamp b/build/catkin_generated/stamps/Project/package.xml.stamp deleted file mode 100644 index 7c2708ac3..000000000 --- a/build/catkin_generated/stamps/Project/package.xml.stamp +++ /dev/null @@ -1,37 +0,0 @@ - - - catkin - 0.6.9 - Low-level build system macros and infrastructure for ROS. - Dirk Thomas - BSD - - http://www.ros.org/wiki/catkin - https://github.com/ros/catkin/issues - https://github.com/ros/catkin - - Troy Straszheim - Morten Kjaergaard - Brian Gerkey - Dirk Thomas - - cmake - cmake - - python-argparse - python-catkin-pkg - - python-empy - - gtest - python-empy - python-nose - - python-mock - python-nose - - - - - - diff --git a/build/catkin_make.cache b/build/catkin_make.cache deleted file mode 100644 index 8b1378917..000000000 --- a/build/catkin_make.cache +++ /dev/null @@ -1 +0,0 @@ - diff --git a/build/cmake_install.cmake b/build/cmake_install.cmake deleted file mode 100644 index 982fd705c..000000000 --- a/build/cmake_install.cmake +++ /dev/null @@ -1,140 +0,0 @@ -# Install script for directory: /home/roman/src/Firmware/src - -# Set the install prefix -IF(NOT DEFINED CMAKE_INSTALL_PREFIX) - SET(CMAKE_INSTALL_PREFIX "/home/roman/src/Firmware/install") -ENDIF(NOT DEFINED CMAKE_INSTALL_PREFIX) -STRING(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}") - -# Set the install configuration name. -IF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) - IF(BUILD_TYPE) - STRING(REGEX REPLACE "^[^A-Za-z0-9_]+" "" - CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}") - ELSE(BUILD_TYPE) - SET(CMAKE_INSTALL_CONFIG_NAME "") - ENDIF(BUILD_TYPE) - MESSAGE(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"") -ENDIF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) - -# Set the component getting installed. -IF(NOT CMAKE_INSTALL_COMPONENT) - IF(COMPONENT) - MESSAGE(STATUS "Install component: \"${COMPONENT}\"") - SET(CMAKE_INSTALL_COMPONENT "${COMPONENT}") - ELSE(COMPONENT) - SET(CMAKE_INSTALL_COMPONENT) - ENDIF(COMPONENT) -ENDIF(NOT CMAKE_INSTALL_COMPONENT) - -# Install shared libraries without execute permission? -IF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) - SET(CMAKE_INSTALL_SO_NO_EXE "1") -ENDIF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) - -IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - - if (NOT EXISTS "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}") - file(MAKE_DIRECTORY "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}") - endif() - if (NOT EXISTS "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/.catkin") - file(WRITE "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/.catkin" "") - endif() -ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - -IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES - "/home/roman/src/Firmware/install/_setup_util.py") - IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) - message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") - ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) - IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) - message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") - ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) -FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE PROGRAM FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/_setup_util.py") -ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - -IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES - "/home/roman/src/Firmware/install/env.sh") - IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) - message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") - ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) - IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) - message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") - ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) -FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE PROGRAM FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/env.sh") -ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - -IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES - "/home/roman/src/Firmware/install/setup.bash") - IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) - message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") - ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) - IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) - message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") - ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) -FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE FILE FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/setup.bash") -ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - -IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES - "/home/roman/src/Firmware/install/setup.sh") - IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) - message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") - ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) - IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) - message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") - ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) -FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE FILE FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/setup.sh") -ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - -IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES - "/home/roman/src/Firmware/install/setup.zsh") - IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) - message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") - ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) - IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) - message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") - ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) -FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE FILE FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/setup.zsh") -ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - -IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES - "/home/roman/src/Firmware/install/.rosinstall") - IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) - message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") - ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) - IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) - message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") - ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) -FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE FILE FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/.rosinstall") -ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - -IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - FILE(INSTALL DESTINATION "${CMAKE_INSTALL_PREFIX}/etc/catkin/profile.d" TYPE FILE FILES "/opt/ros/indigo/share/catkin/cmake/env-hooks/05.catkin_make.bash") -ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - -IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - FILE(INSTALL DESTINATION "${CMAKE_INSTALL_PREFIX}/etc/catkin/profile.d" TYPE FILE FILES "/opt/ros/indigo/share/catkin/cmake/env-hooks/05.catkin_make_isolated.bash") -ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified") - -IF(NOT CMAKE_INSTALL_LOCAL_ONLY) - # Include the install script for each subdirectory. - INCLUDE("/home/roman/src/Firmware/build/gtest/cmake_install.cmake") - -ENDIF(NOT CMAKE_INSTALL_LOCAL_ONLY) - -IF(CMAKE_INSTALL_COMPONENT) - SET(CMAKE_INSTALL_MANIFEST "install_manifest_${CMAKE_INSTALL_COMPONENT}.txt") -ELSE(CMAKE_INSTALL_COMPONENT) - SET(CMAKE_INSTALL_MANIFEST "install_manifest.txt") -ENDIF(CMAKE_INSTALL_COMPONENT) - -FILE(WRITE "/home/roman/src/Firmware/build/${CMAKE_INSTALL_MANIFEST}" "") -FOREACH(file ${CMAKE_INSTALL_MANIFEST_FILES}) - FILE(APPEND "/home/roman/src/Firmware/build/${CMAKE_INSTALL_MANIFEST}" "${file}\n") -ENDFOREACH(file) diff --git a/build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake b/build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake deleted file mode 100644 index 06b7c63c1..000000000 --- a/build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake +++ /dev/null @@ -1,16 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -# Relative path conversion top directories. -SET(CMAKE_RELATIVE_PATH_TOP_SOURCE "/usr/src/gtest") -SET(CMAKE_RELATIVE_PATH_TOP_BINARY "/home/roman/src/Firmware/build") - -# Force unix paths in dependencies. -SET(CMAKE_FORCE_UNIX_PATHS 1) - - -# The C and CXX include file regular expressions for this directory. -SET(CMAKE_C_INCLUDE_REGEX_SCAN "^.*$") -SET(CMAKE_C_INCLUDE_REGEX_COMPLAIN "^$") -SET(CMAKE_CXX_INCLUDE_REGEX_SCAN ${CMAKE_C_INCLUDE_REGEX_SCAN}) -SET(CMAKE_CXX_INCLUDE_REGEX_COMPLAIN ${CMAKE_C_INCLUDE_REGEX_COMPLAIN}) diff --git a/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake b/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake deleted file mode 100644 index 40234804c..000000000 --- a/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake +++ /dev/null @@ -1,27 +0,0 @@ -# The set of languages for which implicit dependencies are needed: -SET(CMAKE_DEPENDS_LANGUAGES - "CXX" - ) -# The set of files for implicit dependencies of each language: -SET(CMAKE_DEPENDS_CHECK_CXX - "/usr/src/gtest/src/gtest-all.cc" "/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o" - ) -SET(CMAKE_CXX_COMPILER_ID "GNU") - -# Preprocessor definitions for this target. -SET(CMAKE_TARGET_DEFINITIONS - "GTEST_CREATE_SHARED_LIBRARY=1" - ) - -# Targets to which this target links. -SET(CMAKE_TARGET_LINKED_INFO_FILES - ) - -# The include file search paths: -SET(CMAKE_C_TARGET_INCLUDE_PATH - "/usr/src/gtest/include" - "/usr/src/gtest" - ) -SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) -SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) -SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) diff --git a/build/gtest/CMakeFiles/gtest.dir/build.make b/build/gtest/CMakeFiles/gtest.dir/build.make deleted file mode 100644 index 744feec6d..000000000 --- a/build/gtest/CMakeFiles/gtest.dir/build.make +++ /dev/null @@ -1,102 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -#============================================================================= -# Special targets provided by cmake. - -# Disable implicit rules so canonical targets will work. -.SUFFIXES: - -# Remove some rules from gmake that .SUFFIXES does not remove. -SUFFIXES = - -.SUFFIXES: .hpux_make_needs_suffix_list - -# Suppress display of executed commands. -$(VERBOSE).SILENT: - -# A target that is always out of date. -cmake_force: -.PHONY : cmake_force - -#============================================================================= -# Set environment variables for the build. - -# The shell in which to execute make rules. -SHELL = /bin/sh - -# The CMake executable. -CMAKE_COMMAND = /usr/bin/cmake - -# The command to remove a file. -RM = /usr/bin/cmake -E remove -f - -# Escaping for special characters. -EQUALS = = - -# The top-level source directory on which CMake was run. -CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src - -# The top-level build directory on which CMake was run. -CMAKE_BINARY_DIR = /home/roman/src/Firmware/build - -# Include any dependencies generated for this target. -include gtest/CMakeFiles/gtest.dir/depend.make - -# Include the progress variables for this target. -include gtest/CMakeFiles/gtest.dir/progress.make - -# Include the compile flags for this target's objects. -include gtest/CMakeFiles/gtest.dir/flags.make - -gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o: gtest/CMakeFiles/gtest.dir/flags.make -gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o: /usr/src/gtest/src/gtest-all.cc - $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles $(CMAKE_PROGRESS_1) - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Building CXX object gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o" - cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++ $(CXX_DEFINES) $(CXX_FLAGS) -o CMakeFiles/gtest.dir/src/gtest-all.cc.o -c /usr/src/gtest/src/gtest-all.cc - -gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.i: cmake_force - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/gtest.dir/src/gtest-all.cc.i" - cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++ $(CXX_DEFINES) $(CXX_FLAGS) -E /usr/src/gtest/src/gtest-all.cc > CMakeFiles/gtest.dir/src/gtest-all.cc.i - -gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.s: cmake_force - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/gtest.dir/src/gtest-all.cc.s" - cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++ $(CXX_DEFINES) $(CXX_FLAGS) -S /usr/src/gtest/src/gtest-all.cc -o CMakeFiles/gtest.dir/src/gtest-all.cc.s - -gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.requires: -.PHONY : gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.requires - -gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.provides: gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.requires - $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.provides.build -.PHONY : gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.provides - -gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.provides.build: gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o - -# Object files for target gtest -gtest_OBJECTS = \ -"CMakeFiles/gtest.dir/src/gtest-all.cc.o" - -# External object files for target gtest -gtest_EXTERNAL_OBJECTS = - -gtest/libgtest.so: gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o -gtest/libgtest.so: gtest/CMakeFiles/gtest.dir/build.make -gtest/libgtest.so: gtest/CMakeFiles/gtest.dir/link.txt - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --red --bold "Linking CXX shared library libgtest.so" - cd /home/roman/src/Firmware/build/gtest && $(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/gtest.dir/link.txt --verbose=$(VERBOSE) - -# Rule to build all files generated by this target. -gtest/CMakeFiles/gtest.dir/build: gtest/libgtest.so -.PHONY : gtest/CMakeFiles/gtest.dir/build - -gtest/CMakeFiles/gtest.dir/requires: gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.requires -.PHONY : gtest/CMakeFiles/gtest.dir/requires - -gtest/CMakeFiles/gtest.dir/clean: - cd /home/roman/src/Firmware/build/gtest && $(CMAKE_COMMAND) -P CMakeFiles/gtest.dir/cmake_clean.cmake -.PHONY : gtest/CMakeFiles/gtest.dir/clean - -gtest/CMakeFiles/gtest.dir/depend: - cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /usr/src/gtest /home/roman/src/Firmware/build /home/roman/src/Firmware/build/gtest /home/roman/src/Firmware/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake --color=$(COLOR) -.PHONY : gtest/CMakeFiles/gtest.dir/depend - diff --git a/build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake b/build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake deleted file mode 100644 index 015a1ee75..000000000 --- a/build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake +++ /dev/null @@ -1,10 +0,0 @@ -FILE(REMOVE_RECURSE - "CMakeFiles/gtest.dir/src/gtest-all.cc.o" - "libgtest.pdb" - "libgtest.so" -) - -# Per-language clean rules from dependency scanning. -FOREACH(lang CXX) - INCLUDE(CMakeFiles/gtest.dir/cmake_clean_${lang}.cmake OPTIONAL) -ENDFOREACH(lang) diff --git a/build/gtest/CMakeFiles/gtest.dir/depend.make b/build/gtest/CMakeFiles/gtest.dir/depend.make deleted file mode 100644 index 37ac348db..000000000 --- a/build/gtest/CMakeFiles/gtest.dir/depend.make +++ /dev/null @@ -1,2 +0,0 @@ -# Empty dependencies file for gtest. -# This may be replaced when dependencies are built. diff --git a/build/gtest/CMakeFiles/gtest.dir/flags.make b/build/gtest/CMakeFiles/gtest.dir/flags.make deleted file mode 100644 index fa55300ac..000000000 --- a/build/gtest/CMakeFiles/gtest.dir/flags.make +++ /dev/null @@ -1,8 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -# compile CXX with /usr/bin/c++ -CXX_FLAGS = -fPIC -I/usr/src/gtest/include -I/usr/src/gtest -Wall -Wshadow -DGTEST_HAS_PTHREAD=1 -fexceptions -Wextra - -CXX_DEFINES = -DGTEST_CREATE_SHARED_LIBRARY=1 -Dgtest_EXPORTS - diff --git a/build/gtest/CMakeFiles/gtest.dir/link.txt b/build/gtest/CMakeFiles/gtest.dir/link.txt deleted file mode 100644 index 8745cedd2..000000000 --- a/build/gtest/CMakeFiles/gtest.dir/link.txt +++ /dev/null @@ -1 +0,0 @@ -/usr/bin/c++ -fPIC -shared -Wl,-soname,libgtest.so -o libgtest.so CMakeFiles/gtest.dir/src/gtest-all.cc.o -L/home/roman/src/Firmware/build/gtest/src -lpthread -Wl,-rpath,/home/roman/src/Firmware/build/gtest/src diff --git a/build/gtest/CMakeFiles/gtest.dir/progress.make b/build/gtest/CMakeFiles/gtest.dir/progress.make deleted file mode 100644 index 781c7de27..000000000 --- a/build/gtest/CMakeFiles/gtest.dir/progress.make +++ /dev/null @@ -1,2 +0,0 @@ -CMAKE_PROGRESS_1 = 1 - diff --git a/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake b/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake deleted file mode 100644 index 9f3beee6e..000000000 --- a/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake +++ /dev/null @@ -1,28 +0,0 @@ -# The set of languages for which implicit dependencies are needed: -SET(CMAKE_DEPENDS_LANGUAGES - "CXX" - ) -# The set of files for implicit dependencies of each language: -SET(CMAKE_DEPENDS_CHECK_CXX - "/usr/src/gtest/src/gtest_main.cc" "/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o" - ) -SET(CMAKE_CXX_COMPILER_ID "GNU") - -# Preprocessor definitions for this target. -SET(CMAKE_TARGET_DEFINITIONS - "GTEST_CREATE_SHARED_LIBRARY=1" - ) - -# Targets to which this target links. -SET(CMAKE_TARGET_LINKED_INFO_FILES - "/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake" - ) - -# The include file search paths: -SET(CMAKE_C_TARGET_INCLUDE_PATH - "/usr/src/gtest/include" - "/usr/src/gtest" - ) -SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) -SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) -SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) diff --git a/build/gtest/CMakeFiles/gtest_main.dir/build.make b/build/gtest/CMakeFiles/gtest_main.dir/build.make deleted file mode 100644 index 67a7ec018..000000000 --- a/build/gtest/CMakeFiles/gtest_main.dir/build.make +++ /dev/null @@ -1,103 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -#============================================================================= -# Special targets provided by cmake. - -# Disable implicit rules so canonical targets will work. -.SUFFIXES: - -# Remove some rules from gmake that .SUFFIXES does not remove. -SUFFIXES = - -.SUFFIXES: .hpux_make_needs_suffix_list - -# Suppress display of executed commands. -$(VERBOSE).SILENT: - -# A target that is always out of date. -cmake_force: -.PHONY : cmake_force - -#============================================================================= -# Set environment variables for the build. - -# The shell in which to execute make rules. -SHELL = /bin/sh - -# The CMake executable. -CMAKE_COMMAND = /usr/bin/cmake - -# The command to remove a file. -RM = /usr/bin/cmake -E remove -f - -# Escaping for special characters. -EQUALS = = - -# The top-level source directory on which CMake was run. -CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src - -# The top-level build directory on which CMake was run. -CMAKE_BINARY_DIR = /home/roman/src/Firmware/build - -# Include any dependencies generated for this target. -include gtest/CMakeFiles/gtest_main.dir/depend.make - -# Include the progress variables for this target. -include gtest/CMakeFiles/gtest_main.dir/progress.make - -# Include the compile flags for this target's objects. -include gtest/CMakeFiles/gtest_main.dir/flags.make - -gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o: gtest/CMakeFiles/gtest_main.dir/flags.make -gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o: /usr/src/gtest/src/gtest_main.cc - $(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles $(CMAKE_PROGRESS_1) - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Building CXX object gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o" - cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++ $(CXX_DEFINES) $(CXX_FLAGS) -o CMakeFiles/gtest_main.dir/src/gtest_main.cc.o -c /usr/src/gtest/src/gtest_main.cc - -gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.i: cmake_force - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/gtest_main.dir/src/gtest_main.cc.i" - cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++ $(CXX_DEFINES) $(CXX_FLAGS) -E /usr/src/gtest/src/gtest_main.cc > CMakeFiles/gtest_main.dir/src/gtest_main.cc.i - -gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.s: cmake_force - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/gtest_main.dir/src/gtest_main.cc.s" - cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++ $(CXX_DEFINES) $(CXX_FLAGS) -S /usr/src/gtest/src/gtest_main.cc -o CMakeFiles/gtest_main.dir/src/gtest_main.cc.s - -gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.requires: -.PHONY : gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.requires - -gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.provides: gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.requires - $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.provides.build -.PHONY : gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.provides - -gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.provides.build: gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o - -# Object files for target gtest_main -gtest_main_OBJECTS = \ -"CMakeFiles/gtest_main.dir/src/gtest_main.cc.o" - -# External object files for target gtest_main -gtest_main_EXTERNAL_OBJECTS = - -gtest/libgtest_main.so: gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o -gtest/libgtest_main.so: gtest/CMakeFiles/gtest_main.dir/build.make -gtest/libgtest_main.so: gtest/libgtest.so -gtest/libgtest_main.so: gtest/CMakeFiles/gtest_main.dir/link.txt - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --red --bold "Linking CXX shared library libgtest_main.so" - cd /home/roman/src/Firmware/build/gtest && $(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/gtest_main.dir/link.txt --verbose=$(VERBOSE) - -# Rule to build all files generated by this target. -gtest/CMakeFiles/gtest_main.dir/build: gtest/libgtest_main.so -.PHONY : gtest/CMakeFiles/gtest_main.dir/build - -gtest/CMakeFiles/gtest_main.dir/requires: gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.requires -.PHONY : gtest/CMakeFiles/gtest_main.dir/requires - -gtest/CMakeFiles/gtest_main.dir/clean: - cd /home/roman/src/Firmware/build/gtest && $(CMAKE_COMMAND) -P CMakeFiles/gtest_main.dir/cmake_clean.cmake -.PHONY : gtest/CMakeFiles/gtest_main.dir/clean - -gtest/CMakeFiles/gtest_main.dir/depend: - cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /usr/src/gtest /home/roman/src/Firmware/build /home/roman/src/Firmware/build/gtest /home/roman/src/Firmware/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake --color=$(COLOR) -.PHONY : gtest/CMakeFiles/gtest_main.dir/depend - diff --git a/build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake b/build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake deleted file mode 100644 index c8fe83819..000000000 --- a/build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake +++ /dev/null @@ -1,10 +0,0 @@ -FILE(REMOVE_RECURSE - "CMakeFiles/gtest_main.dir/src/gtest_main.cc.o" - "libgtest_main.pdb" - "libgtest_main.so" -) - -# Per-language clean rules from dependency scanning. -FOREACH(lang CXX) - INCLUDE(CMakeFiles/gtest_main.dir/cmake_clean_${lang}.cmake OPTIONAL) -ENDFOREACH(lang) diff --git a/build/gtest/CMakeFiles/gtest_main.dir/depend.make b/build/gtest/CMakeFiles/gtest_main.dir/depend.make deleted file mode 100644 index 1d67c1ab5..000000000 --- a/build/gtest/CMakeFiles/gtest_main.dir/depend.make +++ /dev/null @@ -1,2 +0,0 @@ -# Empty dependencies file for gtest_main. -# This may be replaced when dependencies are built. diff --git a/build/gtest/CMakeFiles/gtest_main.dir/flags.make b/build/gtest/CMakeFiles/gtest_main.dir/flags.make deleted file mode 100644 index 93ab70f12..000000000 --- a/build/gtest/CMakeFiles/gtest_main.dir/flags.make +++ /dev/null @@ -1,8 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -# compile CXX with /usr/bin/c++ -CXX_FLAGS = -fPIC -I/usr/src/gtest/include -I/usr/src/gtest -Wall -Wshadow -DGTEST_HAS_PTHREAD=1 -fexceptions -Wextra - -CXX_DEFINES = -DGTEST_CREATE_SHARED_LIBRARY=1 -Dgtest_main_EXPORTS - diff --git a/build/gtest/CMakeFiles/gtest_main.dir/link.txt b/build/gtest/CMakeFiles/gtest_main.dir/link.txt deleted file mode 100644 index d35b7f61a..000000000 --- a/build/gtest/CMakeFiles/gtest_main.dir/link.txt +++ /dev/null @@ -1 +0,0 @@ -/usr/bin/c++ -fPIC -shared -Wl,-soname,libgtest_main.so -o libgtest_main.so CMakeFiles/gtest_main.dir/src/gtest_main.cc.o -L/home/roman/src/Firmware/build/gtest/src -lpthread libgtest.so -lpthread -Wl,-rpath,/home/roman/src/Firmware/build/gtest/src:/home/roman/src/Firmware/build/gtest diff --git a/build/gtest/CMakeFiles/gtest_main.dir/progress.make b/build/gtest/CMakeFiles/gtest_main.dir/progress.make deleted file mode 100644 index 164e1d26c..000000000 --- a/build/gtest/CMakeFiles/gtest_main.dir/progress.make +++ /dev/null @@ -1,2 +0,0 @@ -CMAKE_PROGRESS_1 = 2 - diff --git a/build/gtest/CMakeFiles/progress.marks b/build/gtest/CMakeFiles/progress.marks deleted file mode 100644 index 573541ac9..000000000 --- a/build/gtest/CMakeFiles/progress.marks +++ /dev/null @@ -1 +0,0 @@ -0 diff --git a/build/gtest/CTestTestfile.cmake b/build/gtest/CTestTestfile.cmake deleted file mode 100644 index bd4b57a8b..000000000 --- a/build/gtest/CTestTestfile.cmake +++ /dev/null @@ -1,6 +0,0 @@ -# CMake generated Testfile for -# Source directory: /usr/src/gtest -# Build directory: /home/roman/src/Firmware/build/gtest -# -# This file includes the relevant testing commands required for -# testing this directory and lists subdirectories to be tested as well. diff --git a/build/gtest/Makefile b/build/gtest/Makefile deleted file mode 100644 index bc8c23a00..000000000 --- a/build/gtest/Makefile +++ /dev/null @@ -1,262 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -# Default target executed when no arguments are given to make. -default_target: all -.PHONY : default_target - -#============================================================================= -# Special targets provided by cmake. - -# Disable implicit rules so canonical targets will work. -.SUFFIXES: - -# Remove some rules from gmake that .SUFFIXES does not remove. -SUFFIXES = - -.SUFFIXES: .hpux_make_needs_suffix_list - -# Suppress display of executed commands. -$(VERBOSE).SILENT: - -# A target that is always out of date. -cmake_force: -.PHONY : cmake_force - -#============================================================================= -# Set environment variables for the build. - -# The shell in which to execute make rules. -SHELL = /bin/sh - -# The CMake executable. -CMAKE_COMMAND = /usr/bin/cmake - -# The command to remove a file. -RM = /usr/bin/cmake -E remove -f - -# Escaping for special characters. -EQUALS = = - -# The top-level source directory on which CMake was run. -CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src - -# The top-level build directory on which CMake was run. -CMAKE_BINARY_DIR = /home/roman/src/Firmware/build - -#============================================================================= -# Targets provided globally by CMake. - -# Special rule for the target edit_cache -edit_cache: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running interactive CMake command-line interface..." - /usr/bin/cmake -i . -.PHONY : edit_cache - -# Special rule for the target edit_cache -edit_cache/fast: edit_cache -.PHONY : edit_cache/fast - -# Special rule for the target install -install: preinstall - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." - /usr/bin/cmake -P cmake_install.cmake -.PHONY : install - -# Special rule for the target install -install/fast: preinstall/fast - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..." - /usr/bin/cmake -P cmake_install.cmake -.PHONY : install/fast - -# Special rule for the target install/local -install/local: preinstall - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..." - /usr/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake -.PHONY : install/local - -# Special rule for the target install/local -install/local/fast: install/local -.PHONY : install/local/fast - -# Special rule for the target install/strip -install/strip: preinstall - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..." - /usr/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake -.PHONY : install/strip - -# Special rule for the target install/strip -install/strip/fast: install/strip -.PHONY : install/strip/fast - -# Special rule for the target list_install_components -list_install_components: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Available install components are: \"Unspecified\"" -.PHONY : list_install_components - -# Special rule for the target list_install_components -list_install_components/fast: list_install_components -.PHONY : list_install_components/fast - -# Special rule for the target rebuild_cache -rebuild_cache: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..." - /usr/bin/cmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) -.PHONY : rebuild_cache - -# Special rule for the target rebuild_cache -rebuild_cache/fast: rebuild_cache -.PHONY : rebuild_cache/fast - -# Special rule for the target test -test: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running tests..." - /usr/bin/ctest --force-new-ctest-process $(ARGS) -.PHONY : test - -# Special rule for the target test -test/fast: test -.PHONY : test/fast - -# The main all target -all: cmake_check_build_system - cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles /home/roman/src/Firmware/build/gtest/CMakeFiles/progress.marks - cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/all - $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0 -.PHONY : all - -# The main clean target -clean: - cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/clean -.PHONY : clean - -# The main clean target -clean/fast: clean -.PHONY : clean/fast - -# Prepare targets for installation. -preinstall: all - cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/preinstall -.PHONY : preinstall - -# Prepare targets for installation. -preinstall/fast: - cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/preinstall -.PHONY : preinstall/fast - -# clear depends -depend: - cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1 -.PHONY : depend - -# Convenience name for target. -gtest/CMakeFiles/gtest.dir/rule: - cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/CMakeFiles/gtest.dir/rule -.PHONY : gtest/CMakeFiles/gtest.dir/rule - -# Convenience name for target. -gtest: gtest/CMakeFiles/gtest.dir/rule -.PHONY : gtest - -# fast build rule for target. -gtest/fast: - cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/build -.PHONY : gtest/fast - -# Convenience name for target. -gtest/CMakeFiles/gtest_main.dir/rule: - cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/CMakeFiles/gtest_main.dir/rule -.PHONY : gtest/CMakeFiles/gtest_main.dir/rule - -# Convenience name for target. -gtest_main: gtest/CMakeFiles/gtest_main.dir/rule -.PHONY : gtest_main - -# fast build rule for target. -gtest_main/fast: - cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/build -.PHONY : gtest_main/fast - -src/gtest-all.o: src/gtest-all.cc.o -.PHONY : src/gtest-all.o - -# target to build an object file -src/gtest-all.cc.o: - cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o -.PHONY : src/gtest-all.cc.o - -src/gtest-all.i: src/gtest-all.cc.i -.PHONY : src/gtest-all.i - -# target to preprocess a source file -src/gtest-all.cc.i: - cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.i -.PHONY : src/gtest-all.cc.i - -src/gtest-all.s: src/gtest-all.cc.s -.PHONY : src/gtest-all.s - -# target to generate assembly for a file -src/gtest-all.cc.s: - cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.s -.PHONY : src/gtest-all.cc.s - -src/gtest_main.o: src/gtest_main.cc.o -.PHONY : src/gtest_main.o - -# target to build an object file -src/gtest_main.cc.o: - cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o -.PHONY : src/gtest_main.cc.o - -src/gtest_main.i: src/gtest_main.cc.i -.PHONY : src/gtest_main.i - -# target to preprocess a source file -src/gtest_main.cc.i: - cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.i -.PHONY : src/gtest_main.cc.i - -src/gtest_main.s: src/gtest_main.cc.s -.PHONY : src/gtest_main.s - -# target to generate assembly for a file -src/gtest_main.cc.s: - cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.s -.PHONY : src/gtest_main.cc.s - -# Help Target -help: - @echo "The following are some of the valid targets for this Makefile:" - @echo "... all (the default if no target is provided)" - @echo "... clean" - @echo "... depend" - @echo "... edit_cache" - @echo "... gtest" - @echo "... gtest_main" - @echo "... install" - @echo "... install/local" - @echo "... install/strip" - @echo "... list_install_components" - @echo "... rebuild_cache" - @echo "... test" - @echo "... src/gtest-all.o" - @echo "... src/gtest-all.i" - @echo "... src/gtest-all.s" - @echo "... src/gtest_main.o" - @echo "... src/gtest_main.i" - @echo "... src/gtest_main.s" -.PHONY : help - - - -#============================================================================= -# Special targets to cleanup operation of make. - -# Special rule to run CMake to check the build system integrity. -# No rule that depends on this can have commands that come from listfiles -# because they might be regenerated. -cmake_check_build_system: - cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 -.PHONY : cmake_check_build_system - diff --git a/build/gtest/cmake_install.cmake b/build/gtest/cmake_install.cmake deleted file mode 100644 index f51b65651..000000000 --- a/build/gtest/cmake_install.cmake +++ /dev/null @@ -1,34 +0,0 @@ -# Install script for directory: /usr/src/gtest - -# Set the install prefix -IF(NOT DEFINED CMAKE_INSTALL_PREFIX) - SET(CMAKE_INSTALL_PREFIX "/home/roman/src/Firmware/install") -ENDIF(NOT DEFINED CMAKE_INSTALL_PREFIX) -STRING(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}") - -# Set the install configuration name. -IF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) - IF(BUILD_TYPE) - STRING(REGEX REPLACE "^[^A-Za-z0-9_]+" "" - CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}") - ELSE(BUILD_TYPE) - SET(CMAKE_INSTALL_CONFIG_NAME "") - ENDIF(BUILD_TYPE) - MESSAGE(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"") -ENDIF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) - -# Set the component getting installed. -IF(NOT CMAKE_INSTALL_COMPONENT) - IF(COMPONENT) - MESSAGE(STATUS "Install component: \"${COMPONENT}\"") - SET(CMAKE_INSTALL_COMPONENT "${COMPONENT}") - ELSE(COMPONENT) - SET(CMAKE_INSTALL_COMPONENT) - ENDIF(COMPONENT) -ENDIF(NOT CMAKE_INSTALL_COMPONENT) - -# Install shared libraries without execute permission? -IF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) - SET(CMAKE_INSTALL_SO_NO_EXE "1") -ENDIF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) - -- cgit v1.2.3 From 7954540f45b662ade0ad4038cf936934d3d1eef7 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Nov 2014 17:52:43 +0100 Subject: removed files which should not be in here --- ROMFS/px4fmu_common/init.d/13000_quadshot | 14 - .../px4fmu_common/init.d/Roman_mavlink_stream_conf | 12 - ROMFS/px4fmu_common/init.d/rc.vtol_apps | 15 - ROMFS/px4fmu_common/init.d/rc.vtol_defaults | 63 -- ROMFS/px4fmu_common/mixers/FMU_quadshot.mix | 15 - src/modules/vtol_att_control/module.mk | 40 -- .../vtol_att_control/vtol_att_control_main.cpp | 642 --------------------- 7 files changed, 801 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/13000_quadshot delete mode 100644 ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf delete mode 100644 ROMFS/px4fmu_common/init.d/rc.vtol_apps delete mode 100644 ROMFS/px4fmu_common/init.d/rc.vtol_defaults delete mode 100644 ROMFS/px4fmu_common/mixers/FMU_quadshot.mix delete mode 100644 src/modules/vtol_att_control/module.mk delete mode 100644 src/modules/vtol_att_control/vtol_att_control_main.cpp diff --git a/ROMFS/px4fmu_common/init.d/13000_quadshot b/ROMFS/px4fmu_common/init.d/13000_quadshot deleted file mode 100644 index 8ee306a38..000000000 --- a/ROMFS/px4fmu_common/init.d/13000_quadshot +++ /dev/null @@ -1,14 +0,0 @@ -#!nsh -# -# Generic quadshot configuration file -# -# Roman Bapst -# - -sh /etc/init.d/rc.mc_defaults - -set MIXER FMU_quadshot - -set PWM_OUTPUTS 1234 -set PWM_MIN 1070 -set PWM_MAX 2000 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf b/ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf deleted file mode 100644 index d26e4a372..000000000 --- a/ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf +++ /dev/null @@ -1,12 +0,0 @@ -#!nsh -# Configure stream for Mavlink instance on TELEM2 because it is annoying always removing the SDcard -# -#usleep 100000 -#mavlink stream -d /dev/ttyS2 -s ATTITUDE_CONTROLS -r 50 -#usleep 100000 -#mavlink stream -d /dev/ttyS2 -s RC_CHANNELS_RAW -r 50 -#usleep 100000 -#mavlink stream -d /dev/ttyS2 -s VFR_HUD -r 50 -usleep 100000 -mavlink stream -d /dev/ttyS2 -s MANUAL_CONTROL -r 50 -echo "Added additional streams on TELEM2" \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps deleted file mode 100644 index 23ade6d78..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ /dev/null @@ -1,15 +0,0 @@ -#!nsh -# -# Standard apps for vtol: -# att & pos estimator, att & pos control. -# - -attitude_estimator_ekf start -#ekf_att_pos_estimator start -position_estimator_inav start - -vtol_att_control start -mc_att_control start -mc_pos_control start -fw_att_control start -fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults deleted file mode 100644 index f0ea9add0..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults +++ /dev/null @@ -1,63 +0,0 @@ -#!nsh - -set VEHICLE_TYPE vtol - -if [ $DO_AUTOCONFIG == yes ] -then - #Default parameters for MC - param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.1 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.003 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.1 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.003 - param set MC_YAW_P 2.8 - param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.1 - param set MC_YAWRATE_D 0.0 - param set MC_YAW_FF 0.5 - - param set MPC_THR_MAX 1.0 - param set MPC_THR_MIN 0.1 - param set MPC_XY_P 1.0 - param set MPC_XY_VEL_P 0.1 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_D 0.01 - param set MPC_XY_VEL_MAX 5 - param set MPC_XY_FF 0.5 - param set MPC_Z_P 1.0 - param set MPC_Z_VEL_P 0.1 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_D 0.0 - param set MPC_Z_VEL_MAX 3 - param set MPC_Z_FF 0.5 - param set MPC_TILTMAX_AIR 45.0 - param set MPC_TILTMAX_LND 15.0 - param set MPC_LAND_SPEED 1.0 - - param set PE_VELNE_NOISE 0.5 - param set PE_VELD_NOISE 0.7 - param set PE_POSNE_NOISE 0.5 - param set PE_POSD_NOISE 1.0 - - param set NAV_ACCEPT_RAD 2.0 - - # - # Default parameters for FW - # - param set NAV_LAND_ALT 90 - param set NAV_RTL_ALT 100 - param set NAV_RTL_LAND_T -1 - param set NAV_ACCEPT_RAD 50 - param set FW_T_HRATE_P 0.01 - param set FW_T_RLL2THR 15 - param set FW_T_SRATE_P 0.01 - param set FW_T_TIME_CONST 5 -fi - -set PWM_RATE 400 -set PWM_DISARMED 900 -set PWM_MIN 1075 -set PWM_MAX 2000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_quadshot.mix b/ROMFS/px4fmu_common/mixers/FMU_quadshot.mix deleted file mode 100644 index b077ff30a..000000000 --- a/ROMFS/px4fmu_common/mixers/FMU_quadshot.mix +++ /dev/null @@ -1,15 +0,0 @@ -#!nsh -#Quadshot mixer for PX4FMU -#=========================== -R: 4v 10000 10000 10000 0 - -#mixer for the elevons -M: 2 -O: 10000 10000 0 -10000 10000 -S: 1 0 7500 7500 0 -10000 10000 -S: 1 1 8000 8000 0 -10000 10000 - -M: 2 -O: 10000 10000 0 -10000 10000 -S: 1 0 7500 7500 0 -10000 10000 -S: 1 1 -8000 -8000 0 -10000 10000 diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk deleted file mode 100644 index c349c2340..000000000 --- a/src/modules/vtol_att_control/module.mk +++ /dev/null @@ -1,40 +0,0 @@ -############################################################################ -# -# 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. -# -############################################################################ - -# -# VTOL attitude controller -# - -MODULE_COMMAND = vtol_att_control - -SRCS = vtol_att_control_main.cpp diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp deleted file mode 100644 index 38fa4eec1..000000000 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ /dev/null @@ -1,642 +0,0 @@ -/**************************************************************************** - * - * 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 VTOL_att_control_main.cpp - * Implementation of an attitude controller for VTOL airframes. This module receives data - * from both the fixed wing- and the multicopter attitude controllers and processes it. - * It computes the correct actuator controls depending on which mode the vehicle is in (hover,forward- - * flight or transition). It also publishes the resulting controls on the actuator controls topics. - * - * @author Roman Bapst - * @author Lorenz Meier - * @author Thomas Gubler - * - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "drivers/drv_pwm_output.h" -#include -#include - -#include - -#include - - -extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); - -class VtolAttitudeControl -{ -public: - - VtolAttitudeControl(); - ~VtolAttitudeControl(); - - int start(); /* start the task and return OK on success */ - - -private: -//******************flags & handlers****************************************************** - bool _task_should_exit; - int _control_task; //task handle for VTOL attitude controller - - /* handlers for subscriptions */ - int _v_att_sub; //vehicle attitude subscription - int _v_att_sp_sub; //vehicle attitude setpoint subscription - int _v_rates_sp_sub; //vehicle rates setpoint subscription - int _v_control_mode_sub; //vehicle control mode subscription - int _params_sub; //parameter updates subscription - int _manual_control_sp_sub; //manual control setpoint subscription - int _armed_sub; //arming status subscription - - int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs - int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs - - //handlers for publishers - orb_advert_t _actuators_0_pub; //input for the mixer (roll,pitch,yaw,thrust) - orb_advert_t _actuators_1_pub; - orb_advert_t _vtol_vehicle_status_pub; -//*******************data containers*********************************************************** - struct vehicle_attitude_s _v_att; //vehicle attitude - struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint - struct vehicle_rates_setpoint_s _v_rates_sp; //vehicle rates setpoint - struct manual_control_setpoint_s _manual_control_sp; //manual control setpoint - struct vehicle_control_mode_s _v_control_mode; //vehicle control mode - struct vtol_vehicle_status_s _vtol_vehicle_status; - struct actuator_controls_s _actuators_out_0; //actuator controls going to the mc mixer - struct actuator_controls_s _actuators_out_1; //actuator controls going to the fw mixer (used for elevons) - struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control - struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control - struct actuator_armed_s _armed; //actuator arming status - - struct { - float min_pwm_mc; //pwm value for idle in mc mode - } _params; - - struct { - param_t min_pwm_mc; - } _params_handles; - - perf_counter_t _loop_perf; /**< loop performance counter */ - - /* for multicopters it is usual to have a non-zero idle speed of the engines - * for fixed wings we want to have an idle speed of zero since we do not want - * to waste energy when gliding. */ - bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" - -//*****************Member functions*********************************************************************** - - void task_main(); //main task - static void task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create. - - void vehicle_control_mode_poll(); //Check for changes in vehicle control mode. - void vehicle_manual_poll(); //Check for changes in manual inputs. - void arming_status_poll(); //Check for arming status updates. - void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output - void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output - void parameters_update_poll(); //Check if parameters have changed - int parameters_update(); //Update local paraemter cache - void fill_mc_att_control_output(); //write mc_att_control results to actuator message - void fill_fw_att_control_output(); //write fw_att_control results to actuator message - void set_idle_fw(); - void set_idle_mc(); -}; - -namespace VTOL_att_control -{ -VtolAttitudeControl *g_control; -} - -/** -* Constructor -*/ -VtolAttitudeControl::VtolAttitudeControl() : - _task_should_exit(false), - _control_task(-1), - - //init subscription handlers - _v_att_sub(-1), - _v_att_sp_sub(-1), - _v_control_mode_sub(-1), - _params_sub(-1), - _manual_control_sp_sub(-1), - _armed_sub(-1), - - //init publication handlers - _actuators_0_pub(-1), - _actuators_1_pub(-1), - _vtol_vehicle_status_pub(-1), - - _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) -{ - - flag_idle_mc = true; /*assume we always start in mc mode for a VTOL airframe */ - - memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); - _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ - memset(&_v_att, 0, sizeof(_v_att)); - memset(&_v_att_sp, 0, sizeof(_v_att_sp)); - memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); - memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); - memset(&_v_control_mode, 0, sizeof(_v_control_mode)); - memset(&_vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); - memset(&_actuators_out_0, 0, sizeof(_actuators_out_0)); - memset(&_actuators_out_1, 0, sizeof(_actuators_out_1)); - memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in)); - memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in)); - memset(&_armed, 0, sizeof(_armed)); - - _params_handles.min_pwm_mc = param_find("PWM_MIN"); - - /* fetch initial parameter values */ - parameters_update(); -} - -/** -* Destructor -*/ -VtolAttitudeControl::~VtolAttitudeControl() -{ - if (_control_task != -1) { - /* task wakes up every 100ms or so at the longest */ - _task_should_exit = true; - - /* wait for a second for the task to quit at our request */ - unsigned i = 0; - - do { - /* wait 20ms */ - usleep(20000); - - /* if we have given up, kill it */ - if (++i > 50) { - task_delete(_control_task); - break; - } - } while (_control_task != -1); - } - - VTOL_att_control::g_control = nullptr; -} - -/** -* Check for changes in vehicle control mode. -*/ -void VtolAttitudeControl::vehicle_control_mode_poll() -{ - bool updated; - - /* Check if vehicle control mode has changed */ - orb_check(_v_control_mode_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); - } -} - -/** -* Check for changes in manual inputs. -*/ -void VtolAttitudeControl::vehicle_manual_poll() -{ - bool updated; - - /* get pilots inputs */ - orb_check(_manual_control_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); - } -} -/** -* Check for arming status updates. -*/ -void VtolAttitudeControl::arming_status_poll() -{ - /* check if there is a new setpoint */ - bool updated; - orb_check(_armed_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - } -} - -/** -* Check for inputs from mc attitude controller. -*/ -void VtolAttitudeControl::actuator_controls_mc_poll() -{ - bool updated; - orb_check(_actuator_inputs_mc, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc , &_actuators_mc_in); - } -} - -/** -* Check for inputs from fw attitude controller. -*/ -void VtolAttitudeControl::actuator_controls_fw_poll() -{ - bool updated; - orb_check(_actuator_inputs_fw, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw , &_actuators_fw_in); - } -} - -/** -* Check for parameter updates. -*/ -void -VtolAttitudeControl::parameters_update_poll() -{ - bool updated; - - /* Check if parameters have changed */ - orb_check(_params_sub, &updated); - - if (updated) { - struct parameter_update_s param_update; - orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); - parameters_update(); - } -} - -/** -* Update parameters. -*/ -int -VtolAttitudeControl::parameters_update() -{ - /* idle pwm */ - float v; - param_get(_params_handles.min_pwm_mc, &v); - _params.min_pwm_mc = v; - - return OK; -} - -/** -* Prepare message to acutators with data from mc attitude controller. -*/ -void VtolAttitudeControl::fill_mc_att_control_output() -{ - _actuators_out_0.control[0] = _actuators_mc_in.control[0]; - _actuators_out_0.control[1] = _actuators_mc_in.control[1]; - _actuators_out_0.control[2] = _actuators_mc_in.control[2]; - _actuators_out_0.control[3] = _actuators_mc_in.control[3]; - //set neutral position for elevons - _actuators_out_1.control[0] = 0; //roll elevon - _actuators_out_1.control[1] = 0; //pitch elevon -} - -/** -* Prepare message to acutators with data from fw attitude controller. -*/ -void VtolAttitudeControl::fill_fw_att_control_output() -{ - /*For the first test in fw mode, only use engines for thrust!!!*/ - _actuators_out_0.control[0] = 0; - _actuators_out_0.control[1] = 0; - _actuators_out_0.control[2] = 0; - _actuators_out_0.control[3] = _actuators_fw_in.control[3]; - /*controls for the elevons */ - _actuators_out_1.control[0] = _actuators_fw_in.control[0]; /*roll elevon*/ - _actuators_out_1.control[1] = _actuators_fw_in.control[1]; /*pitch elevon */ -} - -/** -* Adjust idle speed for fw mode. -*/ -void VtolAttitudeControl::set_idle_fw() -{ - int ret; - char *dev = PWM_OUTPUT_DEVICE_PATH; - int fd = open(dev, 0); - - if (fd < 0) {err(1, "can't open %s", dev);} - - unsigned pwm_value = PWM_LOWEST_MIN; - struct pwm_output_values pwm_values; - memset(&pwm_values, 0, sizeof(pwm_values)); - - for (unsigned i = 0; i < 4; i++) { - - pwm_values.values[i] = pwm_value; - pwm_values.channel_count++; - } - - ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); - - if (ret != OK) {errx(ret, "failed setting min values");} - - close(fd); -} - -/** -* Adjust idle speed for mc mode. -*/ -void VtolAttitudeControl::set_idle_mc() -{ - int ret; - unsigned servo_count; - char *dev = PWM_OUTPUT_DEVICE_PATH; - int fd = open(dev, 0); - - if (fd < 0) {err(1, "can't open %s", dev);} - - ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); - unsigned pwm_value = 1100; - struct pwm_output_values pwm_values; - memset(&pwm_values, 0, sizeof(pwm_values)); - - for (unsigned i = 0; i < 4; i++) { - pwm_values.values[i] = pwm_value; - pwm_values.channel_count++; - } - - ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); - - if (ret != OK) {errx(ret, "failed setting min values");} - - close(fd); -} - -void -VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) -{ - VTOL_att_control::g_control->task_main(); -} - -void VtolAttitudeControl::task_main() -{ - warnx("started"); - fflush(stdout); - - /* do subscriptions */ - _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - _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)); - - _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); - _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); - - parameters_update(); /*initialize parameter cache/* - - /* wakeup source*/ - struct pollfd fds[3]; /*input_mc, input_fw, parameters*/ - - fds[0].fd = _actuator_inputs_mc; - fds[0].events = POLLIN; - fds[1].fd = _actuator_inputs_fw; - fds[1].events = POLLIN; - fds[2].fd = _params_sub; - fds[2].events = POLLIN; - - while (!_task_should_exit) { - /*Advertise/Publish vtol vehicle status*/ - if (_vtol_vehicle_status_pub > 0) { - orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status); - - } else { - _vtol_vehicle_status.timestamp = hrt_absolute_time(); - _vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status); - } - - /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - - - /* timed out - periodic check for _task_should_exit */ - if (pret == 0) { - continue; - } - - /* 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; - } - - if (fds[2].revents & POLLIN) { //parameters were updated, read them now - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); - - /* update parameters from storage */ - parameters_update(); - } - - vehicle_control_mode_poll(); //Check for changes in vehicle control mode. - vehicle_manual_poll(); //Check for changes in manual inputs. - arming_status_poll(); //Check for arming status updates. - actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output - actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output - parameters_update_poll(); - - if (_manual_control_sp.aux1 <= 0.0f) { /* vehicle is in mc mode */ - _vtol_vehicle_status.vtol_in_rw_mode = true; - - if (!flag_idle_mc) { /* we want to adjust idle speed for mc mode */ - set_idle_mc(); - flag_idle_mc = true; - } - - /* got data from mc_att_controller */ - if (fds[0].revents & POLLIN) { - vehicle_manual_poll(); /* update remote input */ - orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); - - fill_mc_att_control_output(); - - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); - - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); - } - - if (_actuators_1_pub > 0) { - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); - - } else { - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); - } - } - } - - if (_manual_control_sp.aux1 >= 0.0f) { /* vehicle is in fw mode */ - _vtol_vehicle_status.vtol_in_rw_mode = false; - - if (flag_idle_mc) { /* we want to adjust idle speed for fixed wing mode */ - set_idle_fw(); - flag_idle_mc = false; - } - - if (fds[1].revents & POLLIN) { /* got data from fw_att_controller */ - orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in); - vehicle_manual_poll(); //update remote input - - fill_fw_att_control_output(); - - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); - - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); - } - - if (_actuators_1_pub > 0) { - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); - - } else { - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); - } - } - } - } - - warnx("exit"); - _control_task = -1; - _exit(0); -} - -int -VtolAttitudeControl::start() -{ - ASSERT(_control_task == -1); - - /* start the task */ - _control_task = task_spawn_cmd("vtol_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 10, - 2048, - (main_t)&VtolAttitudeControl::task_main_trampoline, - nullptr); - - if (_control_task < 0) { - warn("task start failed"); - return -errno; - } - - return OK; -} - - -int vtol_att_control_main(int argc, char *argv[]) -{ - if (argc < 1) { - errx(1, "usage: vtol_att_control {start|stop|status}"); - } - - if (!strcmp(argv[1], "start")) { - - if (VTOL_att_control::g_control != nullptr) { - errx(1, "already running"); - } - - VTOL_att_control::g_control = new VtolAttitudeControl; - - if (VTOL_att_control::g_control == nullptr) { - errx(1, "alloc failed"); - } - - if (OK != VTOL_att_control::g_control->start()) { - delete VTOL_att_control::g_control; - VTOL_att_control::g_control = nullptr; - err(1, "start failed"); - } - - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - if (VTOL_att_control::g_control == nullptr) { - errx(1, "not running"); - } - - delete VTOL_att_control::g_control; - VTOL_att_control::g_control = nullptr; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (VTOL_att_control::g_control) { - errx(0, "running"); - - } else { - errx(1, "not running"); - } - } - - warnx("unrecognized command"); - return 1; -} -- cgit v1.2.3 From 4915c15036604b56f0d1fa4a42a44c9e6cacbe64 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Nov 2014 17:58:33 +0100 Subject: removed files and code segments which should not be there and removed --- ROMFS/px4fmu_common/init.d/rc.autostart | 9 ---- ROMFS/px4fmu_common/init.d/rcS | 1 - src/modules/uORB/topics/vtol_vehicle_status.h | 66 --------------------------- 3 files changed, 76 deletions(-) delete mode 100644 src/modules/uORB/topics/vtol_vehicle_status.h diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 01aa8ed13..78778d806 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -16,7 +16,6 @@ # 10000 .. 10999 Wide arm / H frame # 11000 .. 11999 Hexa Cox # 12000 .. 12999 Octo Cox -# 13000 .. 13999 Vtol # # Simulation setups @@ -239,11 +238,3 @@ if param compare SYS_AUTOSTART 12001 then sh /etc/init.d/12001_octo_cox fi - -# -# Quadshot -# - if param compare SYS_AUTOSTART 13000 - then - sh /etc/init.d/13000_quadshot - fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 39172eea4..ea04ece34 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -589,7 +589,6 @@ then then echo "[init] Starting addons script: $EXTRAS_FILE" sh $EXTRAS_FILE - sh /etc/init.d/Roman_mavlink_stream_conf else echo "[init] No addons script: $EXTRAS_FILE" fi diff --git a/src/modules/uORB/topics/vtol_vehicle_status.h b/src/modules/uORB/topics/vtol_vehicle_status.h deleted file mode 100644 index 24ecca9fa..000000000 --- a/src/modules/uORB/topics/vtol_vehicle_status.h +++ /dev/null @@ -1,66 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 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 vtol_status.h - * - * Vtol status topic - * - */ - -#ifndef TOPIC_VTOL_STATUS_H -#define TOPIC_VTOL_STATUS_H - -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/* Indicates in which mode the vtol aircraft is in */ -struct vtol_vehicle_status_s { - - uint64_t timestamp; /**< Microseconds since system boot */ - bool vtol_in_rw_mode; /*true: vtol vehicle is in rotating wing mode */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vtol_vehicle_status); - -#endif -- cgit v1.2.3 From 7e350555ea634013f083bccc2f5a8108fc5ef9f9 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 12:36:05 +0100 Subject: checked out from PX4 master to avoid eclipse formatting which happened in the past --- src/modules/fw_att_control/fw_att_control_main.cpp | 414 ++++++++++----------- 1 file changed, 203 insertions(+), 211 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 80b58ec71..e770c11a2 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -83,7 +83,8 @@ */ extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]); -class FixedwingAttitudeControl { +class FixedwingAttitudeControl +{ public: /** * Constructor @@ -100,56 +101,54 @@ public: * * @return OK on success. */ - int start(); + int start(); /** * Task status * * @return true if the mainloop is running */ - bool task_running() { - return _task_running; - } + bool task_running() { return _task_running; } private: - bool _task_should_exit; /**< if true, sensor task should exit */ - bool _task_running; /**< if true, task is running in its mainloop */ - int _control_task; /**< task handle for sensor task */ - - int _att_sub; /**< vehicle attitude subscription */ - int _accel_sub; /**< accelerometer subscription */ - int _att_sp_sub; /**< vehicle attitude setpoint */ - int _attitude_sub; /**< raw rc channels data subscription */ - int _airspeed_sub; /**< airspeed subscription */ - int _vcontrol_mode_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_sub; /**< notification of manual control updates */ - int _global_pos_sub; /**< global position subscription */ - int _vehicle_status_sub; /**< vehicle status subscription */ - - orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ - orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ - orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ - orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */ - - struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct accel_report _accel; /**< body frame accelerations */ - struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ - struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ - struct actuator_controls_s _actuators; /**< actuator control inputs */ - struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ - struct vehicle_global_position_s _global_pos; /**< global position */ - struct vehicle_status_s _vehicle_status; /**< vehicle status */ - - perf_counter_t _loop_perf; /**< loop performance counter */ - perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ - perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ - - bool _setpoint_valid; /**< flag if the position control setpoint is valid */ - bool _debug; /**< if set to true, print debug output */ + bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_running; /**< if true, task is running in its mainloop */ + int _control_task; /**< task handle for sensor task */ + + int _att_sub; /**< vehicle attitude subscription */ + int _accel_sub; /**< accelerometer subscription */ + int _att_sp_sub; /**< vehicle attitude setpoint */ + int _attitude_sub; /**< raw rc channels data subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _vcontrol_mode_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ + int _global_pos_sub; /**< global position subscription */ + int _vehicle_status_sub; /**< vehicle status subscription */ + + orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ + orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ + orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ + orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct accel_report _accel; /**< body frame accelerations */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ + struct vehicle_global_position_s _global_pos; /**< global position */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ + perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ + + bool _setpoint_valid; /**< flag if the position control setpoint is valid */ + bool _debug; /**< if set to true, print debug output */ struct { float tconst; @@ -183,14 +182,14 @@ private: float trim_roll; float trim_pitch; float trim_yaw; - float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ - float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ - float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ - float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ - float man_roll_max; /**< Max Roll in rad */ - float man_pitch_max; /**< Max Pitch in rad */ + float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ + float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ + float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ + float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ + float man_roll_max; /**< Max Roll in rad */ + float man_pitch_max; /**< Max Pitch in rad */ - } _parameters; /**< local copies of interesting parameters */ + } _parameters; /**< local copies of interesting parameters */ struct { @@ -229,71 +228,75 @@ private: param_t pitchsp_offset_deg; param_t man_roll_max; param_t man_pitch_max; - } _parameter_handles; /**< handles for interesting parameters */ + } _parameter_handles; /**< handles for interesting parameters */ + + + ECL_RollController _roll_ctrl; + ECL_PitchController _pitch_ctrl; + ECL_YawController _yaw_ctrl; - ECL_RollController _roll_ctrl; - ECL_PitchController _pitch_ctrl; - ECL_YawController _yaw_ctrl; /** * Update our local parameter cache. */ - int parameters_update(); + int parameters_update(); /** * Update control outputs * */ - void control_update(); + void control_update(); /** * Check for changes in vehicle control mode. */ - void vehicle_control_mode_poll(); + void vehicle_control_mode_poll(); /** * Check for changes in manual inputs. */ - void vehicle_manual_poll(); + void vehicle_manual_poll(); + /** * Check for airspeed updates. */ - void vehicle_airspeed_poll(); + void vehicle_airspeed_poll(); /** * Check for accel updates. */ - void vehicle_accel_poll(); + void vehicle_accel_poll(); /** * Check for set triplet updates. */ - void vehicle_setpoint_poll(); + void vehicle_setpoint_poll(); /** * Check for global position updates. */ - void global_pos_poll(); + void global_pos_poll(); /** * Check for vehicle status updates. */ - void vehicle_status_poll(); + void vehicle_status_poll(); /** * Shim for calling task_main from task_create. */ - static void task_main_trampoline(int argc, char *argv[]); + static void task_main_trampoline(int argc, char *argv[]); /** * Main sensor collection task. */ - void task_main(); + void task_main(); }; -namespace att_control { +namespace att_control +{ /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -301,28 +304,39 @@ namespace att_control { #endif static const int ERROR = -1; -FixedwingAttitudeControl *g_control = nullptr; +FixedwingAttitudeControl *g_control = nullptr; } FixedwingAttitudeControl::FixedwingAttitudeControl() : - _task_should_exit(false), _task_running(false), _control_task(-1), - - /* subscriptions */ - _att_sub(-1), _accel_sub(-1), _airspeed_sub(-1), _vcontrol_mode_sub(-1), _params_sub( - -1), _manual_sub(-1), _global_pos_sub(-1), _vehicle_status_sub( - -1), - - /* publications */ - _rate_sp_pub(-1), _attitude_sp_pub(-1), _actuators_0_pub(-1), _actuators_1_pub( - -1), - - /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), _nonfinite_input_perf( - perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf( - perf_alloc(PC_COUNT, "fw att control nonfinite output")), - /* states */ - _setpoint_valid(false), _debug(false) { + _task_should_exit(false), + _task_running(false), + _control_task(-1), + +/* subscriptions */ + _att_sub(-1), + _accel_sub(-1), + _airspeed_sub(-1), + _vcontrol_mode_sub(-1), + _params_sub(-1), + _manual_sub(-1), + _global_pos_sub(-1), + _vehicle_status_sub(-1), + +/* publications */ + _rate_sp_pub(-1), + _attitude_sp_pub(-1), + _actuators_0_pub(-1), + _actuators_1_pub(-1), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")), + _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), +/* states */ + _setpoint_valid(false), + _debug(false) +{ /* safely initialize structs */ _att = {}; _accel = {}; @@ -335,6 +349,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _global_pos = {}; _vehicle_status = {}; + _parameter_handles.tconst = param_find("FW_ATT_TC"); _parameter_handles.p_p = param_find("FW_PR_P"); _parameter_handles.p_i = param_find("FW_PR_I"); @@ -375,7 +390,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : parameters_update(); } -FixedwingAttitudeControl::~FixedwingAttitudeControl() { +FixedwingAttitudeControl::~FixedwingAttitudeControl() +{ if (_control_task != -1) { /* task wakes up every 100ms or so at the longest */ @@ -403,7 +419,9 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl() { att_control::g_control = nullptr; } -int FixedwingAttitudeControl::parameters_update() { +int +FixedwingAttitudeControl::parameters_update() +{ param_get(_parameter_handles.tconst, &(_parameters.tconst)); param_get(_parameter_handles.p_p, &(_parameters.p_p)); @@ -411,26 +429,21 @@ int FixedwingAttitudeControl::parameters_update() { param_get(_parameter_handles.p_ff, &(_parameters.p_ff)); param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos)); param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg)); - param_get(_parameter_handles.p_integrator_max, - &(_parameters.p_integrator_max)); - param_get(_parameter_handles.p_roll_feedforward, - &(_parameters.p_roll_feedforward)); + param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max)); + param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward)); param_get(_parameter_handles.r_p, &(_parameters.r_p)); param_get(_parameter_handles.r_i, &(_parameters.r_i)); param_get(_parameter_handles.r_ff, &(_parameters.r_ff)); - param_get(_parameter_handles.r_integrator_max, - &(_parameters.r_integrator_max)); + param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max)); param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax)); param_get(_parameter_handles.y_p, &(_parameters.y_p)); param_get(_parameter_handles.y_i, &(_parameters.y_i)); param_get(_parameter_handles.y_ff, &(_parameters.y_ff)); - param_get(_parameter_handles.y_integrator_max, - &(_parameters.y_integrator_max)); - param_get(_parameter_handles.y_coordinated_min_speed, - &(_parameters.y_coordinated_min_speed)); + param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max)); + param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed)); param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax)); param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); @@ -440,19 +453,16 @@ int FixedwingAttitudeControl::parameters_update() { param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll)); param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch)); param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw)); - param_get(_parameter_handles.rollsp_offset_deg, - &(_parameters.rollsp_offset_deg)); - param_get(_parameter_handles.pitchsp_offset_deg, - &(_parameters.pitchsp_offset_deg)); - _parameters.rollsp_offset_rad = math::radians( - _parameters.rollsp_offset_deg); - _parameters.pitchsp_offset_rad = math::radians( - _parameters.pitchsp_offset_deg); + param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg)); + param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg)); + _parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg); + _parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg); param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max)); param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max)); _parameters.man_roll_max = math::radians(_parameters.man_roll_max); _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); + /* pitch control parameters */ _pitch_ctrl.set_time_constant(_parameters.tconst); _pitch_ctrl.set_k_p(_parameters.p_p); @@ -482,7 +492,9 @@ int FixedwingAttitudeControl::parameters_update() { return OK; } -void FixedwingAttitudeControl::vehicle_control_mode_poll() { +void +FixedwingAttitudeControl::vehicle_control_mode_poll() +{ bool vcontrol_mode_updated; /* Check HIL state if vehicle status has changed */ @@ -490,12 +502,13 @@ void FixedwingAttitudeControl::vehicle_control_mode_poll() { if (vcontrol_mode_updated) { - orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, - &_vcontrol_mode); + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode); } } -void FixedwingAttitudeControl::vehicle_manual_poll() { +void +FixedwingAttitudeControl::vehicle_manual_poll() +{ bool manual_updated; /* get pilots inputs */ @@ -507,7 +520,9 @@ void FixedwingAttitudeControl::vehicle_manual_poll() { } } -void FixedwingAttitudeControl::vehicle_airspeed_poll() { +void +FixedwingAttitudeControl::vehicle_airspeed_poll() +{ /* check if there is a new position */ bool airspeed_updated; orb_check(_airspeed_sub, &airspeed_updated); @@ -518,7 +533,9 @@ void FixedwingAttitudeControl::vehicle_airspeed_poll() { } } -void FixedwingAttitudeControl::vehicle_accel_poll() { +void +FixedwingAttitudeControl::vehicle_accel_poll() +{ /* check if there is a new position */ bool accel_updated; orb_check(_accel_sub, &accel_updated); @@ -528,7 +545,9 @@ void FixedwingAttitudeControl::vehicle_accel_poll() { } } -void FixedwingAttitudeControl::vehicle_setpoint_poll() { +void +FixedwingAttitudeControl::vehicle_setpoint_poll() +{ /* check if there is a new setpoint */ bool att_sp_updated; orb_check(_att_sp_sub, &att_sp_updated); @@ -539,18 +558,21 @@ void FixedwingAttitudeControl::vehicle_setpoint_poll() { } } -void FixedwingAttitudeControl::global_pos_poll() { +void +FixedwingAttitudeControl::global_pos_poll() +{ /* check if there is a new global position */ bool global_pos_updated; orb_check(_global_pos_sub, &global_pos_updated); if (global_pos_updated) { - orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, - &_global_pos); + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); } } -void FixedwingAttitudeControl::vehicle_status_poll() { +void +FixedwingAttitudeControl::vehicle_status_poll() +{ /* check if there is new status information */ bool vehicle_status_updated; orb_check(_vehicle_status_sub, &vehicle_status_updated); @@ -560,11 +582,15 @@ void FixedwingAttitudeControl::vehicle_status_poll() { } } -void FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) { +void +FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) +{ att_control::g_control->task_main(); } -void FixedwingAttitudeControl::task_main() { +void +FixedwingAttitudeControl::task_main() +{ /* inform about start */ warnx("Initializing.."); @@ -641,6 +667,7 @@ void FixedwingAttitudeControl::task_main() { /* only run controller if attitude changed */ if (fds[1].revents & POLLIN) { + static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); @@ -694,8 +721,8 @@ void FixedwingAttitudeControl::task_main() { float airspeed; /* if airspeed is not updating, we assume the normal average speed */ - if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) - || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { + if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) || + hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; if (nonfinite) { perf_count(_nonfinite_input_perf); @@ -713,9 +740,7 @@ void FixedwingAttitudeControl::task_main() { * Forcing the scaling to this value allows reasonable handheld tests. */ - float airspeed_scaling = _parameters.airspeed_trim - / ((airspeed < _parameters.airspeed_min) ? - _parameters.airspeed_min : airspeed); + float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed); float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; @@ -731,8 +756,7 @@ void FixedwingAttitudeControl::task_main() { !_vcontrol_mode.flag_control_manual_enabled) { /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; - pitch_sp = _att_sp.pitch_body - + _parameters.pitchsp_offset_rad; + pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ @@ -758,12 +782,10 @@ void FixedwingAttitudeControl::task_main() { * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ - roll_sp = (_manual.y * _parameters.man_roll_max - - _parameters.trim_roll) - + _parameters.rollsp_offset_rad; - pitch_sp = -(_manual.x * _parameters.man_pitch_max - - _parameters.trim_pitch) - + _parameters.pitchsp_offset_rad; + roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) + + _parameters.rollsp_offset_rad; + pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch) + + _parameters.pitchsp_offset_rad; throttle_sp = _manual.z; _actuators.control[4] = _manual.flaps; @@ -782,13 +804,11 @@ void FixedwingAttitudeControl::task_main() { /* lazily publish the setpoint only once available */ if (_attitude_sp_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), - _attitude_sp_pub, &att_sp); + orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); } else { /* advertise and publish */ - _attitude_sp_pub = orb_advertise( - ORB_ID(vehicle_attitude_setpoint), &att_sp); + _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); } } @@ -803,17 +823,11 @@ void FixedwingAttitudeControl::task_main() { float speed_body_u = 0.0f; float speed_body_v = 0.0f; float speed_body_w = 0.0f; - if (_att.R_valid) { - speed_body_u = _att.R[0][0] * _global_pos.vel_n - + _att.R[1][0] * _global_pos.vel_e - + _att.R[2][0] * _global_pos.vel_d; - speed_body_v = _att.R[0][1] * _global_pos.vel_n - + _att.R[1][1] * _global_pos.vel_e - + _att.R[2][1] * _global_pos.vel_d; - speed_body_w = _att.R[0][2] * _global_pos.vel_n - + _att.R[1][2] * _global_pos.vel_e - + _att.R[2][2] * _global_pos.vel_d; - } else { + if(_att.R_valid) { + speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d; + speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; + speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; + } else { if (_debug && loop_counter % 10 == 0) { warnx("Did not get a valid R\n"); } @@ -822,81 +836,63 @@ void FixedwingAttitudeControl::task_main() { /* Run attitude controllers */ if (isfinite(roll_sp) && isfinite(pitch_sp)) { _roll_ctrl.control_attitude(roll_sp, _att.roll); - _pitch_ctrl.control_attitude(pitch_sp, _att.roll, - _att.pitch, airspeed); + _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed); _yaw_ctrl.control_attitude(_att.roll, _att.pitch, speed_body_u, speed_body_v, speed_body_w, - _roll_ctrl.get_desired_rate(), - _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude + _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, _att.rollspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, - airspeed, airspeed_scaling, lock_integrator); - _actuators.control[0] = - (isfinite(roll_u)) ? - roll_u + _parameters.trim_roll : - _parameters.trim_roll; + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); + _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; if (!isfinite(roll_u)) { _roll_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { - warnx("roll_u %.4f", (double) roll_u); + warnx("roll_u %.4f", (double)roll_u); } } - float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, - _att.pitch, _att.pitchspeed, _att.yawspeed, + float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, - airspeed, airspeed_scaling, lock_integrator); - _actuators.control[1] = - (isfinite(pitch_u)) ? - pitch_u + _parameters.trim_pitch : - _parameters.trim_pitch; + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); + _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; if (!isfinite(pitch_u)) { _pitch_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { - warnx( - "pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," - " airspeed %.4f, airspeed_scaling %.4f," - " roll_sp %.4f, pitch_sp %.4f," - " _roll_ctrl.get_desired_rate() %.4f," - " _pitch_ctrl.get_desired_rate() %.4f" - " att_sp.roll_body %.4f", - (double) pitch_u, - (double) _yaw_ctrl.get_desired_rate(), - (double) airspeed, - (double) airspeed_scaling, (double) roll_sp, - (double) pitch_sp, - (double) _roll_ctrl.get_desired_rate(), - (double) _pitch_ctrl.get_desired_rate(), - (double) _att_sp.roll_body); + warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," + " airspeed %.4f, airspeed_scaling %.4f," + " roll_sp %.4f, pitch_sp %.4f," + " _roll_ctrl.get_desired_rate() %.4f," + " _pitch_ctrl.get_desired_rate() %.4f" + " att_sp.roll_body %.4f", + (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), + (double)airspeed, (double)airspeed_scaling, + (double)roll_sp, (double)pitch_sp, + (double)_roll_ctrl.get_desired_rate(), + (double)_pitch_ctrl.get_desired_rate(), + (double)_att_sp.roll_body); } } - float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, - _att.pitch, _att.pitchspeed, _att.yawspeed, + float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, _pitch_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, - airspeed, airspeed_scaling, lock_integrator); - _actuators.control[2] = - (isfinite(yaw_u)) ? - yaw_u + _parameters.trim_yaw : - _parameters.trim_yaw; + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); + _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; if (!isfinite(yaw_u)) { _yaw_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { - warnx("yaw_u %.4f", (double) yaw_u); + warnx("yaw_u %.4f", (double)yaw_u); } } - /* throttle passed through if it is finite and if no engine failure was * detected */ _actuators.control[3] = (isfinite(throttle_sp) && @@ -905,15 +901,13 @@ void FixedwingAttitudeControl::task_main() { throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { if (_debug && loop_counter % 10 == 0) { - warnx("throttle_sp %.4f", (double) throttle_sp); + warnx("throttle_sp %.4f", (double)throttle_sp); } } } else { perf_count(_nonfinite_input_perf); if (_debug && loop_counter % 10 == 0) { - warnx( - "Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", - (double) roll_sp, (double) pitch_sp); + warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); } } @@ -930,13 +924,11 @@ void FixedwingAttitudeControl::task_main() { if (_rate_sp_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, - &rates_sp); + orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp); } else { /* advertise and publish */ - _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), - &rates_sp); + _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); } } else { @@ -958,19 +950,16 @@ void FixedwingAttitudeControl::task_main() { if (_actuators_0_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, - &_actuators); + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); } else { /* advertise and publish */ - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), - &_actuators); + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); } if (_actuators_1_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, - &_actuators_airframe); + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe); // warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f", // (double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2], // (double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5], @@ -978,8 +967,7 @@ void FixedwingAttitudeControl::task_main() { } else { /* advertise and publish */ - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), - &_actuators_airframe); + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe); } } @@ -995,14 +983,18 @@ void FixedwingAttitudeControl::task_main() { _exit(0); } -int FixedwingAttitudeControl::start() { +int +FixedwingAttitudeControl::start() +{ ASSERT(_control_task == -1); /* start the task */ _control_task = task_spawn_cmd("fw_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, 2048, - (main_t) &FixedwingAttitudeControl::task_main_trampoline, nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&FixedwingAttitudeControl::task_main_trampoline, + nullptr); if (_control_task < 0) { warn("task start failed"); @@ -1012,7 +1004,8 @@ int FixedwingAttitudeControl::start() { return OK; } -int fw_att_control_main(int argc, char *argv[]) { +int fw_att_control_main(int argc, char *argv[]) +{ if (argc < 1) errx(1, "usage: fw_att_control {start|stop|status}"); @@ -1033,8 +1026,7 @@ int fw_att_control_main(int argc, char *argv[]) { } /* avoid memory fragmentation by not exiting start handler until the task has fully started */ - while (att_control::g_control == nullptr - || !att_control::g_control->task_running()) { + while (att_control::g_control == nullptr || !att_control::g_control->task_running()) { usleep(50000); printf("."); fflush(stdout); -- cgit v1.2.3 From 4e0882121e975f19be130c68cacbda3bd817f772 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 13:07:35 +0100 Subject: removed ifdef because this file is specific to the nuttx platform --- src/drivers/drv_accel.h | 64 +++++++++++++++++++++++-------------------------- 1 file changed, 30 insertions(+), 34 deletions(-) diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 49b104d38..1f98d966b 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -42,13 +42,12 @@ #include #include -#ifdef CONFIG_ARCH_ARM + #include "drv_sensor.h" #include "drv_orb_dev.h" #define ACCEL_DEVICE_PATH "/dev/accel" -#endif /** * accel report structure. Reads from the device must be in multiples of this * structure. @@ -82,49 +81,46 @@ struct accel_scale { /* * ObjDev tag for raw accelerometer data. */ -#ifdef CONFIG_ARCH_ARM - ORB_DECLARE(sensor_accel0); - ORB_DECLARE(sensor_accel1); - ORB_DECLARE(sensor_accel2); - - /* - * ioctl() definitions - * - * Accelerometer drivers also implement the generic sensor driver - * interfaces from drv_sensor.h - */ +ORB_DECLARE(sensor_accel0); +ORB_DECLARE(sensor_accel1); +ORB_DECLARE(sensor_accel2); - #define _ACCELIOCBASE (0x2100) - #define _ACCELIOC(_n) (_IOC(_ACCELIOCBASE, _n)) +/* + * ioctl() definitions + * + * Accelerometer drivers also implement the generic sensor driver + * interfaces from drv_sensor.h + */ +#define _ACCELIOCBASE (0x2100) +#define _ACCELIOC(_n) (_IOC(_ACCELIOCBASE, _n)) - /** set the accel internal sample rate to at least (arg) Hz */ - #define ACCELIOCSSAMPLERATE _ACCELIOC(0) - /** return the accel internal sample rate in Hz */ - #define ACCELIOCGSAMPLERATE _ACCELIOC(1) +/** set the accel internal sample rate to at least (arg) Hz */ +#define ACCELIOCSSAMPLERATE _ACCELIOC(0) - /** set the accel internal lowpass filter to no lower than (arg) Hz */ - #define ACCELIOCSLOWPASS _ACCELIOC(2) +/** return the accel internal sample rate in Hz */ +#define ACCELIOCGSAMPLERATE _ACCELIOC(1) - /** return the accel internal lowpass filter in Hz */ - #define ACCELIOCGLOWPASS _ACCELIOC(3) +/** set the accel internal lowpass filter to no lower than (arg) Hz */ +#define ACCELIOCSLOWPASS _ACCELIOC(2) - /** set the accel scaling constants to the structure pointed to by (arg) */ - #define ACCELIOCSSCALE _ACCELIOC(5) +/** return the accel internal lowpass filter in Hz */ +#define ACCELIOCGLOWPASS _ACCELIOC(3) - /** get the accel scaling constants into the structure pointed to by (arg) */ - #define ACCELIOCGSCALE _ACCELIOC(6) +/** set the accel scaling constants to the structure pointed to by (arg) */ +#define ACCELIOCSSCALE _ACCELIOC(5) - /** set the accel measurement range to handle at least (arg) g */ - #define ACCELIOCSRANGE _ACCELIOC(7) +/** get the accel scaling constants into the structure pointed to by (arg) */ +#define ACCELIOCGSCALE _ACCELIOC(6) - /** get the current accel measurement range in g */ - #define ACCELIOCGRANGE _ACCELIOC(8) +/** set the accel measurement range to handle at least (arg) g */ +#define ACCELIOCSRANGE _ACCELIOC(7) - /** get the result of a sensor self-test */ - #define ACCELIOCSELFTEST _ACCELIOC(9) +/** get the current accel measurement range in g */ +#define ACCELIOCGRANGE _ACCELIOC(8) -#endif +/** get the result of a sensor self-test */ +#define ACCELIOCSELFTEST _ACCELIOC(9) #endif /* _DRV_ACCEL_H */ -- cgit v1.2.3 From 6b61e725916f90cabd9e8c07144dfcc5ac7b44f6 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 13:08:23 +0100 Subject: added header file with platformspecific definitions --- src/platforms/px4_defines.h | 56 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 56 insertions(+) create mode 100644 src/platforms/px4_defines.h diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h new file mode 100644 index 000000000..8d8fd5f3c --- /dev/null +++ b/src/platforms/px4_defines.h @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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 px4_defines.h + * + * Generally used magic defines + */ + +#pragma once + +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/* + * Building for running within the ROS environment + */ +#define __EXPORT +//#define PX4_MAIN_FUNCTION(_prefix) +#else +#include +//#define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##main(int argc, char **argv)() { return main(argc, argv); } + +#include "drv_sensor.h" +#include "drv_orb_dev.h" +#define ACCEL_DEVICE_PATH "/dev/accel" + +#endif -- cgit v1.2.3 From 3bfc4a5a52a7777088f21674dd8108026f5f057b Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 13:10:36 +0100 Subject: removed platform specific includes --- src/lib/mathlib/math/Limits.hpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp index 713cb51b5..79e127fdd 100644 --- a/src/lib/mathlib/math/Limits.hpp +++ b/src/lib/mathlib/math/Limits.hpp @@ -39,10 +39,7 @@ #pragma once -#ifdef CONFIG_ARCH_ARM -#include -#endif - +#include #include namespace math { -- cgit v1.2.3 From c8ad06ff9966884242a1ce80eddb4a45efc30c50 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 13:26:24 +0100 Subject: removed platform specificness --- src/modules/systemlib/err.h | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/modules/systemlib/err.h b/src/modules/systemlib/err.h index 2a201ee80..ca13d6265 100644 --- a/src/modules/systemlib/err.h +++ b/src/modules/systemlib/err.h @@ -67,7 +67,6 @@ #include -#ifdef CONFIG_ARCH_ARM __BEGIN_DECLS __EXPORT const char *getprogname(void); @@ -87,8 +86,4 @@ __EXPORT void vwarnx(const char *fmt, va_list) __attribute__((format(printf, 1, __END_DECLS -#else //we are using ROS (should make a variable!!!) -#include -#define warnx ROS_WARN -#endif #endif -- cgit v1.2.3 From ed409fd53757a43b49758d3ed5573e809a23b113 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 14:55:33 +0100 Subject: use px4_defines header to distinguish platform --- src/lib/geo_lookup/geo_mag_declination.h | 4 ---- src/lib/mathlib/math/Limits.hpp | 2 +- src/lib/mathlib/math/Vector.hpp | 2 +- src/modules/uORB/topics/actuator_armed.h | 8 +++----- src/modules/uORB/topics/actuator_controls.h | 6 +----- src/modules/uORB/topics/airspeed.h | 6 +----- src/modules/uORB/topics/manual_control_setpoint.h | 8 +++----- src/modules/uORB/topics/parameter_update.h | 8 ++------ src/modules/uORB/topics/vehicle_attitude.h | 6 +----- src/modules/uORB/topics/vehicle_attitude_setpoint.h | 6 +----- src/modules/uORB/topics/vehicle_control_mode.h | 6 +----- src/modules/uORB/topics/vehicle_global_position.h | 7 ++----- src/modules/uORB/topics/vehicle_rates_setpoint.h | 6 +----- src/modules/uORB/topics/vehicle_status.h | 6 +----- src/platforms/px4_defines.h | 4 +++- 15 files changed, 22 insertions(+), 63 deletions(-) diff --git a/src/lib/geo_lookup/geo_mag_declination.h b/src/lib/geo_lookup/geo_mag_declination.h index d79b78412..0ac062d6d 100644 --- a/src/lib/geo_lookup/geo_mag_declination.h +++ b/src/lib/geo_lookup/geo_mag_declination.h @@ -40,12 +40,8 @@ #pragma once -#ifdef CONFIG_ARCH_ARM __BEGIN_DECLS __EXPORT float get_mag_declination(float lat, float lon); __END_DECLS -#else -float get_mag_declination(float lat, float lon); -#endif diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp index 79e127fdd..fca4197b8 100644 --- a/src/lib/mathlib/math/Limits.hpp +++ b/src/lib/mathlib/math/Limits.hpp @@ -39,7 +39,7 @@ #pragma once -#include +#include #include namespace math { diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index b0b03980d..9accd0907 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -50,7 +50,7 @@ #include "../CMSIS/Include/arm_math.h" #else #include -#include +//#include #endif namespace math diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h index 9ec9d10ab..1e10e0ad1 100644 --- a/src/modules/uORB/topics/actuator_armed.h +++ b/src/modules/uORB/topics/actuator_armed.h @@ -42,9 +42,8 @@ #define TOPIC_ACTUATOR_ARMED_H #include -#ifdef CONFIG_ARCH_ARM -#include "../uORB.h" -#endif +#include + /** * @addtogroup topics * @{ @@ -65,7 +64,6 @@ struct actuator_armed_s { */ /* register this as object request broker structure */ -#ifdef CONFIG_ARCH_ARM ORB_DECLARE(actuator_armed); -#endif + #endif diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index 6c641dbce..43f7a59ee 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -47,9 +47,7 @@ #define TOPIC_ACTUATOR_CONTROLS_H #include -#ifdef CONFIG_ARCH_ARM -#include "../uORB.h" -#endif +#include #define NUM_ACTUATOR_CONTROLS 8 #define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ @@ -72,11 +70,9 @@ struct actuator_controls_s { */ /* actuator control sets; this list can be expanded as more controllers emerge */ -#ifdef CONFIG_ARCH_ARM ORB_DECLARE(actuator_controls_0); ORB_DECLARE(actuator_controls_1); ORB_DECLARE(actuator_controls_2); ORB_DECLARE(actuator_controls_3); -#endif #endif diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h index 4c115a811..676c37c77 100644 --- a/src/modules/uORB/topics/airspeed.h +++ b/src/modules/uORB/topics/airspeed.h @@ -40,9 +40,7 @@ #ifndef TOPIC_AIRSPEED_H_ #define TOPIC_AIRSPEED_H_ -#ifdef CONFIG_ARCH_ARM -#include "../uORB.h" -#endif +#include #include /** @@ -65,8 +63,6 @@ struct airspeed_s { */ /* register this as object request broker structure */ -#ifdef CONFIG_ARCH_ARM ORB_DECLARE(airspeed); -#endif #endif diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index af5df6979..15b55648d 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -41,9 +41,8 @@ #define TOPIC_MANUAL_CONTROL_SETPOINT_H_ #include -#ifdef CONFIG_ARCH_ARM -#include "../uORB.h" -#endif +#include + /** * Switch position */ @@ -107,7 +106,6 @@ struct manual_control_setpoint_s { */ /* register this as object request broker structure */ -#ifdef CONFIG_ARCH_ARM ORB_DECLARE(manual_control_setpoint); -#endif + #endif diff --git a/src/modules/uORB/topics/parameter_update.h b/src/modules/uORB/topics/parameter_update.h index 7afb78d49..fe9c9070f 100644 --- a/src/modules/uORB/topics/parameter_update.h +++ b/src/modules/uORB/topics/parameter_update.h @@ -40,9 +40,7 @@ #define TOPIC_PARAMETER_UPDATE_H #include -#ifdef CONFIG_ARCH_ARM -#include "../uORB.h" -#endif +#include /** * @addtogroup topics @@ -58,8 +56,6 @@ struct parameter_update_s { * @} */ -#ifdef CONFIG_ARCH_ARM ORB_DECLARE(parameter_update); -#endif -#endif +#endif \ No newline at end of file diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h index 7780988c8..1df1433ac 100755 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ b/src/modules/uORB/topics/vehicle_attitude.h @@ -44,9 +44,7 @@ #include #include -#ifdef CONFIG_ARCH_ARM -#include "../uORB.h" -#endif +#include /** * @addtogroup topics @@ -89,8 +87,6 @@ struct vehicle_attitude_s { */ /* register this as object request broker structure */ -#ifdef CONFIG_ARCH_ARM ORB_DECLARE(vehicle_attitude); -#endif #endif diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index 8b5a76143..a503aa0c6 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -42,9 +42,7 @@ #include #include -#ifdef CONFIG_ARCH_ARM -#include "../uORB.h" -#endif +#include /** * @addtogroup topics @@ -84,8 +82,6 @@ struct vehicle_attitude_setpoint_s { */ /* register this as object request broker structure */ -#ifdef CONFIG_ARCH_ARM ORB_DECLARE(vehicle_attitude_setpoint); -#endif #endif /* TOPIC_ARDRONE_CONTROL_H_ */ diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 78de55b7d..2dd8550bc 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -48,9 +48,7 @@ #include #include -#ifdef CONFIG_ARCH_ARM -#include "../uORB.h" -#endif +#include #include "vehicle_status.h" /** @@ -92,8 +90,6 @@ struct vehicle_control_mode_s { */ /* register this as object request broker structure */ -#ifdef CONFIG_ARCH_ARM ORB_DECLARE(vehicle_control_mode); -#endif #endif diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index e8f010924..f7a432495 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -45,9 +45,7 @@ #include #include -#ifdef CONFIG_ARCH_ARM -#include "../uORB.h" -#endif +#include /** * @addtogroup topics @@ -83,7 +81,6 @@ struct vehicle_global_position_s { */ /* register this as object request broker structure */ -#ifdef CONFIG_ARCH_ARM ORB_DECLARE(vehicle_global_position); -#endif + #endif diff --git a/src/modules/uORB/topics/vehicle_rates_setpoint.h b/src/modules/uORB/topics/vehicle_rates_setpoint.h index cbfab89d6..e5cecf02b 100644 --- a/src/modules/uORB/topics/vehicle_rates_setpoint.h +++ b/src/modules/uORB/topics/vehicle_rates_setpoint.h @@ -41,9 +41,7 @@ #define TOPIC_VEHICLE_RATES_SETPOINT_H_ #include -#ifdef CONFIG_ARCH_ARM -#include "../uORB.h" -#endif +#include /** * @addtogroup topics @@ -64,8 +62,6 @@ struct vehicle_rates_setpoint_s { */ /* register this as object request broker structure */ -#ifdef CONFIG_ARCH_ARM ORB_DECLARE(vehicle_rates_setpoint); -#endif #endif diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 6bd156ccd..8e85b4835 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -53,9 +53,7 @@ #include #include -#ifdef CONFIG_ARCH_ARM -#include "../uORB.h" -#endif +#include /** * @addtogroup topics @{ @@ -252,8 +250,6 @@ struct vehicle_status_s { */ /* register this as object request broker structure */ -#ifdef CONFIG_ARCH_ARM ORB_DECLARE(vehicle_status); -#endif #endif diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 8d8fd5f3c..327d0bea1 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -36,7 +36,6 @@ * * Generally used magic defines */ - #pragma once #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) @@ -45,6 +44,7 @@ */ #define __EXPORT //#define PX4_MAIN_FUNCTION(_prefix) +#define ORB_DECLARE(x) #else #include //#define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##main(int argc, char **argv)() { return main(argc, argv); } @@ -53,4 +53,6 @@ #include "drv_orb_dev.h" #define ACCEL_DEVICE_PATH "/dev/accel" +#include + #endif -- cgit v1.2.3 From a0e2e4e8b30ef64728ff57e56a28b95527f4a1b0 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 15:01:47 +0100 Subject: pixhawk specific files --- src/modules/fw_att_control/fw_att_control_base.cpp | 1 - src/modules/fw_att_control/fw_att_control_base.h | 2 -- src/platforms/px4_defines.h | 5 ----- 3 files changed, 8 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp index d8ba15969..99780bc7e 100644 --- a/src/modules/fw_att_control/fw_att_control_base.cpp +++ b/src/modules/fw_att_control/fw_att_control_base.cpp @@ -56,7 +56,6 @@ FixedwingAttitudeControlBase::FixedwingAttitudeControlBase() : _setpoint_valid(false), _debug(false) { /* safely initialize structs */ _att = {}; - _accel = {}; _att_sp = {}; _manual = {}; _airspeed = {}; diff --git a/src/modules/fw_att_control/fw_att_control_base.h b/src/modules/fw_att_control/fw_att_control_base.h index 6b2efc46b..1726c2e3e 100644 --- a/src/modules/fw_att_control/fw_att_control_base.h +++ b/src/modules/fw_att_control/fw_att_control_base.h @@ -54,7 +54,6 @@ #include #include -#include #include class FixedwingAttitudeControlBase @@ -78,7 +77,6 @@ protected: int _control_task; /**< task handle for sensor task */ struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct airspeed_s _airspeed; /**< airspeed */ diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 327d0bea1..1ff46b97c 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -48,11 +48,6 @@ #else #include //#define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##main(int argc, char **argv)() { return main(argc, argv); } - -#include "drv_sensor.h" -#include "drv_orb_dev.h" -#define ACCEL_DEVICE_PATH "/dev/accel" - #include #endif -- cgit v1.2.3 From 02175f4a678575430a6455e2e5d7f44f6cf64665 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 15:32:20 +0100 Subject: removed unnecessary diff content --- src/lib/ecl/attitude_fw/ecl_roll_controller.h | 1 - src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 1 - src/lib/ecl/ecl.h | 2 +- src/lib/geo/geo.h | 8 ++++---- 4 files changed, 5 insertions(+), 7 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index dbcabd847..0799dbe03 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -53,7 +53,6 @@ #include #include - class __EXPORT ECL_RollController //XXX: create controller superclass { public: diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index c9e80930f..a360c14b8 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -52,7 +52,6 @@ #include #include - class __EXPORT ECL_YawController //XXX: create controller superclass { public: diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h index 662e3a39f..aa3c5000a 100644 --- a/src/lib/ecl/ecl.h +++ b/src/lib/ecl/ecl.h @@ -38,6 +38,6 @@ */ #include + #define ecl_absolute_time hrt_absolute_time #define ecl_elapsed_time hrt_elapsed_time - diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index ff2d92389..fd754a1ae 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -45,12 +45,12 @@ #pragma once -#ifdef CONFIG_ARCH_ARM + #include "uORB/topics/fence.h" #include "uORB/topics/vehicle_global_position.h" __BEGIN_DECLS -#endif + #include "geo_lookup/geo_mag_declination.h" #include @@ -278,6 +278,6 @@ __EXPORT float _wrap_360(float bearing); __EXPORT float _wrap_pi(float bearing); __EXPORT float _wrap_2pi(float bearing); -#ifdef CONFIG_ARCH_ARM + __END_DECLS -#endif + -- cgit v1.2.3 From 8e205e0fef4f7a2d8fc832587ae51b685537991d Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 15:33:02 +0100 Subject: use px4_config header for multiple platform support --- src/modules/uORB/topics/fence.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/fence.h b/src/modules/uORB/topics/fence.h index 6f16c51cf..a61f078ba 100644 --- a/src/modules/uORB/topics/fence.h +++ b/src/modules/uORB/topics/fence.h @@ -42,7 +42,7 @@ #include #include -#include "../uORB.h" +#include /** * @addtogroup topics -- cgit v1.2.3 From 05fadc347e4137ecd409b1a6e79fe961f6070ae5 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 15:33:16 +0100 Subject: updated --- src/platforms/px4_defines.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 1ff46b97c..535cf67fe 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -45,6 +45,7 @@ #define __EXPORT //#define PX4_MAIN_FUNCTION(_prefix) #define ORB_DECLARE(x) + #else #include //#define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##main(int argc, char **argv)() { return main(argc, argv); } -- cgit v1.2.3 From 19bc137cf319a0380f2cab9224d3cb67b39995e8 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 15:34:31 +0100 Subject: removed unnecessary diff content --- src/lib/geo/geo.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index fd754a1ae..2311e0a7c 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -45,7 +45,6 @@ #pragma once - #include "uORB/topics/fence.h" #include "uORB/topics/vehicle_global_position.h" @@ -54,7 +53,6 @@ __BEGIN_DECLS #include "geo_lookup/geo_mag_declination.h" #include -#include #define CONSTANTS_ONE_G 9.80665f /* m/s^2 */ #define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f /* kg/m^3 */ @@ -278,6 +276,4 @@ __EXPORT float _wrap_360(float bearing); __EXPORT float _wrap_pi(float bearing); __EXPORT float _wrap_2pi(float bearing); - __END_DECLS - -- cgit v1.2.3 From 284787e02c7471c1f8d651dced0497e964f33454 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 15:41:07 +0100 Subject: removed platform specific included, they are not needed --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 8 +------- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 6 ------ src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 6 ------ 3 files changed, 1 insertion(+), 19 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index d1f79f0ea..926a8db2a 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -47,12 +47,6 @@ #include #include -#ifdef CONFIG_ARCH_ARM -#else -#include -using namespace std; -#endif - ECL_PitchController::ECL_PitchController() : _last_run(0), _tc(0.1f), @@ -81,7 +75,7 @@ float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, fl { /* Do not calculate control signal with bad inputs */ if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) { - //perf_count(_nonfinite_input_perf); + perf_count(_nonfinite_input_perf); warnx("not controlling pitch"); return _rate_setpoint; } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 30176f92f..94bd26f03 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -47,12 +47,6 @@ #include #include -#ifdef CONFIG_ARCH_ARM -#else -#include -using namespace std; -#endif - ECL_RollController::ECL_RollController() : _last_run(0), _tc(0.1f), diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 1b4d8486c..fe03b8065 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -46,12 +46,6 @@ #include #include -#ifdef CONFIG_ARCH_ARM -#else -#include -using namespace std; -#endif - ECL_YawController::ECL_YawController() : _last_run(0), _k_p(0.0f), -- cgit v1.2.3 From 076c1e6238127b7ae14ea9119c79bc286394be8f Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 15:43:47 +0100 Subject: removed unused include --- src/lib/mathlib/math/Quaternion.hpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index d8acc4443..b3cca30c6 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -45,10 +45,6 @@ #include -#ifdef CONFIG_ARCH_ARM -#include "../CMSIS/Include/arm_math.h" -#endif - #include "Vector.hpp" #include "Matrix.hpp" -- cgit v1.2.3 From 647ec65bd0a164b4bdb38cfc5073e01695676263 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 11 Nov 2014 16:01:01 +0100 Subject: added ros specific source file for performance_counter --- src/platforms/ros/perf_counter.cpp | 142 +++++++++++++++++++++++++++++++++++++ 1 file changed, 142 insertions(+) create mode 100755 src/platforms/ros/perf_counter.cpp diff --git a/src/platforms/ros/perf_counter.cpp b/src/platforms/ros/perf_counter.cpp new file mode 100755 index 000000000..aa8d85c60 --- /dev/null +++ b/src/platforms/ros/perf_counter.cpp @@ -0,0 +1,142 @@ +/* + * perf_counter.c + + * + * Created on: Sep 24, 2014 + * Author: roman + */ +#include +#include +#include + + + +perf_counter_t perf_alloc(enum perf_counter_type type, const char *name) +{ + return NULL; +} + +/** + * Free a counter. + * + * @param handle The performance counter's handle. + */ +void perf_free(perf_counter_t handle) +{ + +} + +/** + * Count a performance event. + * + * This call only affects counters that take single events; PC_COUNT, PC_INTERVAL etc. + * + * @param handle The handle returned from perf_alloc. + */ +void perf_count(perf_counter_t handle) +{ + +} + +/** + * Begin a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * + * @param handle The handle returned from perf_alloc. + */ +void perf_begin(perf_counter_t handle) +{ + +} + +/** + * End a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * If a call is made without a corresopnding perf_begin call, or if perf_cancel + * has been called subsequently, no change is made to the counter. + * + * @param handle The handle returned from perf_alloc. + */ +void perf_end(perf_counter_t handle) +{ + +} + +/** + * Cancel a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * It reverts the effect of a previous perf_begin. + * + * @param handle The handle returned from perf_alloc. + */ +void perf_cancel(perf_counter_t handle) +{ + +} + +/** + * Reset a performance counter. + * + * This call resets performance counter to initial state + * + * @param handle The handle returned from perf_alloc. + */ +void perf_reset(perf_counter_t handle) +{ + +} + +/** + * Print one performance counter to stdout + * + * @param handle The counter to print. + */ +void perf_print_counter(perf_counter_t handle) +{ + +} + +/** + * Print one performance counter to a fd. + * + * @param fd File descriptor to print to - e.g. 0 for stdout + * @param handle The counter to print. + */ +void perf_print_counter_fd(int fd, perf_counter_t handle) +{ + +} + +/** + * Print all of the performance counters. + * + * @param fd File descriptor to print to - e.g. 0 for stdout + */ +void perf_print_all(int fd) +{ + +} + +/** + * Reset all of the performance counters. + */ +void perf_reset_all(void) +{ + +} + +/** + * Return current event_count + * + * @param handle The counter returned from perf_alloc. + * @return event_count + */ +uint64_t perf_event_count(perf_counter_t handle) +{ + +} + + -- cgit v1.2.3 From 44127363b11a9280de818ff28047d170c487af1f Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 12 Nov 2014 16:01:36 +0100 Subject: added setter and getter functions for use with euroc-gazebo simulator --- src/modules/fw_att_control/fw_att_control_base.cpp | 57 ++++++++++++++++++++++ src/modules/fw_att_control/fw_att_control_base.h | 6 +++ 2 files changed, 63 insertions(+) diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp index 99780bc7e..46fef3e67 100644 --- a/src/modules/fw_att_control/fw_att_control_base.cpp +++ b/src/modules/fw_att_control/fw_att_control_base.cpp @@ -267,3 +267,60 @@ void FixedwingAttitudeControlBase::control_attitude() { } } + +void FixedwingAttitudeControlBase::set_attitude(const Eigen::Quaternion attitude) { + // watch out, still need to see where we modify attitude for the tailsitter case + math::Quaternion quat; + quat(0) = (float)attitude.w(); + quat(1) = (float)attitude.x(); + quat(2) = (float)attitude.y(); + quat(3) = (float)attitude.z(); + + _att.q[0] = quat(0); + _att.q[1] = quat(1); + _att.q[2] = quat(2); + _att.q[3] = quat(3); + + math::Matrix<3,3> Rot = quat.to_dcm(); + _att.R[0][0] = Rot(0,0); + _att.R[1][0] = Rot(1,0); + _att.R[2][0] = Rot(2,0); + _att.R[0][1] = Rot(0,1); + _att.R[1][1] = Rot(1,1); + _att.R[2][1] = Rot(2,1); + _att.R[0][2] = Rot(0,2); + _att.R[1][2] = Rot(1,2); + _att.R[2][2] = Rot(2,2); + + _att.R_valid = true; +} +void FixedwingAttitudeControlBase::set_attitude_rates(const Eigen::Vector3d& angular_rate) { + _att.rollspeed = angular_rate(0); + _att.pitchspeed = angular_rate(1); + _att.yawspeed = angular_rate(2); +} +void FixedwingAttitudeControlBase::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) { + _att_sp.roll_body = control_attitude_thrust_reference(0); + _att_sp.pitch_body = control_attitude_thrust_reference(1); + _att_sp.yaw_body = control_attitude_thrust_reference(2); + _att_sp.thrust = (control_attitude_thrust_reference(3) -30)*(-1)/30; + + // setup rotation matrix + math::Matrix<3,3> Rot_sp; + Rot_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body); + _att_sp.R_body[0][0] = Rot_sp(0,0); + _att_sp.R_body[1][0] = Rot_sp(1,0); + _att_sp.R_body[2][0] = Rot_sp(2,0); + _att_sp.R_body[0][1] = Rot_sp(0,1); + _att_sp.R_body[1][1] = Rot_sp(1,1); + _att_sp.R_body[2][1] = Rot_sp(2,1); + _att_sp.R_body[0][2] = Rot_sp(0,2); + _att_sp.R_body[1][2] = Rot_sp(1,2); + _att_sp.R_body[2][2] = Rot_sp(2,2); +} +void FixedwingAttitudeControlBase::get_mixer_input(Eigen::Vector4d& motor_inputs) { + motor_inputs(0) = _actuators.control[0]; + motor_inputs(1) = _actuators.control[1]; + motor_inputs(2) = _actuators.control[2]; + motor_inputs(3) = _actuators.control[3]; +} diff --git a/src/modules/fw_att_control/fw_att_control_base.h b/src/modules/fw_att_control/fw_att_control_base.h index 1726c2e3e..bde1b755f 100644 --- a/src/modules/fw_att_control/fw_att_control_base.h +++ b/src/modules/fw_att_control/fw_att_control_base.h @@ -140,6 +140,12 @@ protected: void control_attitude(); + // setters and getters for interface with euroc-gazebo simulator + void set_attitude(const Eigen::Quaternion attitude); + void set_attitude_rates(const Eigen::Vector3d& angular_rate); + void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); + void get_mixer_input(Eigen::Vector4d& motor_inputs); + }; #endif /* FW_ATT_CONTROL_BASE_H_ */ -- cgit v1.2.3 From 794a89e711a90a62510f6bdde8ee59511237f6ca Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 13 Nov 2014 10:51:54 +0100 Subject: added more digits to pi/2 --- src/lib/mathlib/math/Matrix.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 832be66db..9fc3c78c8 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -51,7 +51,7 @@ #else #include #include -#define M_PI_2_F 1.570769 +#define M_PI_2_F 1.5707963267948966192 #endif namespace math { -- cgit v1.2.3 From 003b326c7731b0f3b9b139ae008d6e4d07cefab8 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 13 Nov 2014 11:16:22 +0100 Subject: applied fix_code_style.sh --- src/lib/mathlib/math/Matrix.hpp | 113 +++++++++++++++++++++++----------------- 1 file changed, 64 insertions(+), 49 deletions(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 9fc3c78c8..806f5933a 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -51,10 +51,11 @@ #else #include #include -#define M_PI_2_F 1.5707963267948966192 +#define M_PI_2_F 1.5707963267948966192f #endif -namespace math { +namespace math +{ template class __EXPORT Matrix; @@ -83,9 +84,8 @@ public: * Initializes the elements to zero. */ MatrixBase() : - data {}, - arm_mat {M, N, &data[0][0]} - { + data {}, + arm_mat {M, N, &data[0][0]} { } virtual ~MatrixBase() {}; @@ -94,20 +94,17 @@ public: * copyt ctor */ MatrixBase(const MatrixBase &m) : - arm_mat {M, N, &data[0][0]} - { + arm_mat {M, N, &data[0][0]} { memcpy(data, m.data, sizeof(data)); } MatrixBase(const float *d) : - arm_mat {M, N, &data[0][0]} - { + arm_mat {M, N, &data[0][0]} { memcpy(data, d, sizeof(data)); } MatrixBase(const float d[M][N]) : - arm_mat {M, N, &data[0][0]} - { + arm_mat {M, N, &data[0][0]} { memcpy(data, d, sizeof(data)); } @@ -158,9 +155,10 @@ public: */ bool operator ==(const Matrix &m) const { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m.data[i][j]) - return false; + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m.data[i][j]) { + return false; + } return true; } @@ -170,9 +168,10 @@ public: */ bool operator !=(const Matrix &m) const { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m.data[i][j]) - return true; + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m.data[i][j]) { + return true; + } return false; } @@ -192,8 +191,9 @@ public: Matrix res; for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - res.data[i][j] = -data[i][j]; + for (unsigned int j = 0; j < M; j++) { + res.data[i][j] = -data[i][j]; + } return res; } @@ -205,16 +205,18 @@ public: Matrix res; for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - res.data[i][j] = data[i][j] + m.data[i][j]; + for (unsigned int j = 0; j < M; j++) { + res.data[i][j] = data[i][j] + m.data[i][j]; + } return res; } Matrix &operator +=(const Matrix &m) { for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - data[i][j] += m.data[i][j]; + for (unsigned int j = 0; j < M; j++) { + data[i][j] += m.data[i][j]; + } return *static_cast*>(this); } @@ -226,16 +228,18 @@ public: Matrix res; for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - res.data[i][j] = data[i][j] - m.data[i][j]; + for (unsigned int j = 0; j < N; j++) { + res.data[i][j] = data[i][j] - m.data[i][j]; + } return res; } Matrix &operator -=(const Matrix &m) { for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - data[i][j] -= m.data[i][j]; + for (unsigned int j = 0; j < M; j++) { + data[i][j] -= m.data[i][j]; + } return *static_cast*>(this); } @@ -247,8 +251,9 @@ public: Matrix res; for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - res.data[i][j] = data[i][j] * num; + for (unsigned int j = 0; j < N; j++) { + res.data[i][j] = data[i][j] * num; + } return res; @@ -256,8 +261,9 @@ public: Matrix &operator *=(const float num) { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - data[i][j] *= num; + for (unsigned int j = 0; j < N; j++) { + data[i][j] *= num; + } return *static_cast*>(this); } @@ -266,16 +272,18 @@ public: Matrix res; for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - res[i][j] = data[i][j] / num; + for (unsigned int j = 0; j < N; j++) { + res[i][j] = data[i][j] / num; + } return res; } Matrix &operator /=(const float num) { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - data[i][j] /= num; + for (unsigned int j = 0; j < N; j++) { + data[i][j] /= num; + } return *static_cast*>(this); } @@ -290,9 +298,11 @@ public: arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); return res; #else - Eigen::Matrix Me = Eigen::Map >(this->arm_mat.pData); - Eigen::Matrix Him = Eigen::Map >(m.arm_mat.pData); - Eigen::Matrix Product = Me * Him; + Eigen::Matrix Me = Eigen::Map > + (this->arm_mat.pData); + Eigen::Matrix Him = Eigen::Map > + (m.arm_mat.pData); + Eigen::Matrix Product = Me * Him; Matrix res(Product.data()); return res; #endif @@ -307,7 +317,8 @@ public: arm_mat_trans_f32(&this->arm_mat, &res.arm_mat); return res; #else - Eigen::Matrix Me = Eigen::Map >(this->arm_mat.pData); + Eigen::Matrix Me = Eigen::Map > + (this->arm_mat.pData); Me.transposeInPlace(); Matrix res(Me.data()); return res; @@ -323,8 +334,9 @@ public: arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); return res; #else - Eigen::Matrix Me = Eigen::Map >(this->arm_mat.pData); - Eigen::Matrix MyInverse = Me.inverse();//not sure if A = A.inverse() is a good idea + Eigen::Matrix Me = Eigen::Map > + (this->arm_mat.pData); + Eigen::Matrix MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea Matrix res(MyInverse.data()); return res; #endif @@ -344,16 +356,18 @@ public: memset(data, 0, sizeof(data)); unsigned int n = (M < N) ? M : N; - for (unsigned int i = 0; i < n; i++) - data[i][i] = 1; + for (unsigned int i = 0; i < n; i++) { + data[i][i] = 1; + } } void print(void) { for (unsigned int i = 0; i < M; i++) { printf("[ "); - for (unsigned int j = 0; j < N; j++) - printf("%.3f\t", data[i][j]); + for (unsigned int j = 0; j < N; j++) { + printf("%.3f\t", data[i][j]); + } printf(" ]\n"); } @@ -391,8 +405,9 @@ public: arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); #else //probably nicer if this could go into a function like "eigen_mat_mult" or so - Eigen::Matrix Me = Eigen::Map >(this->arm_mat.pData); - Eigen::VectorXf Vec = Eigen::Map(v.arm_col.pData,N); + Eigen::Matrix Me = Eigen::Map > + (this->arm_mat.pData); + Eigen::VectorXf Vec = Eigen::Map(v.arm_col.pData, N); Eigen::VectorXf Product = Me * Vec; Vector res(Product.data()); #endif @@ -427,8 +442,8 @@ public: */ Vector<3> operator *(const Vector<3> &v) const { Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2], - data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2], - data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]); + data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2], + data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]); return res; } -- cgit v1.2.3 From d9314653826d36b0a9723d69a291220245b3f699 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 13 Nov 2014 13:48:18 +0100 Subject: added ifdef guard --- src/lib/mathlib/math/Limits.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/lib/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp index c593936ce..e16f33bd6 100644 --- a/src/lib/mathlib/math/Limits.cpp +++ b/src/lib/mathlib/math/Limits.cpp @@ -45,7 +45,10 @@ namespace math { -#define M_PI_F 3.14159265358979323846 + +#ifndef CONFIG_ARCH_ARM +#define M_PI_F 3.14159265358979323846f +#endif float __EXPORT min(float val1, float val2) { -- cgit v1.2.3 From efe0938e9990225636e5fed78e945ded5f24220f Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 13 Nov 2014 13:48:40 +0100 Subject: removed comment --- src/lib/mathlib/math/Vector.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 9accd0907..57b45e3ab 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -50,7 +50,6 @@ #include "../CMSIS/Include/arm_math.h" #else #include -//#include #endif namespace math -- cgit v1.2.3 From 1e36de61579dd35fe46a069520b7c8970c3cb7cd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 24 Nov 2014 15:57:48 +0100 Subject: ignore build folder --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 8b09e4783..69112ee1f 100644 --- a/.gitignore +++ b/.gitignore @@ -17,6 +17,7 @@ .~lock.* Archives/* Build/* +build/* core cscope.out Firmware.sublime-workspace -- cgit v1.2.3 From 3f36d30a3413cd70096e953a2c9ea0ded65bf24e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 24 Nov 2014 15:58:06 +0100 Subject: wrapped subscriber --- CMakeLists.txt | 1 + src/examples/subscriber/subscriber.cpp | 4 ++-- src/include/px4.h | 2 ++ src/platforms/nuttx/px4_nodehandle.cpp | 39 ++++++++++++++++++++++++++++++ src/platforms/px4_nodehandle.h | 18 +++++++++++++- src/platforms/px4_publisher.h | 1 + src/platforms/px4_subscriber.h | 16 ++++++++++++- src/platforms/ros/px4_nodehandle.cpp | 44 ++++++++++++++++++++++++++++++++++ src/platforms/ros/px4_publisher.cpp | 1 + src/platforms/ros/px4_subscriber.cpp | 1 + 10 files changed, 123 insertions(+), 4 deletions(-) create mode 100644 src/platforms/nuttx/px4_nodehandle.cpp create mode 100644 src/platforms/ros/px4_nodehandle.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 36c2ffeff..8eb81c92c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -104,6 +104,7 @@ add_library(px4 src/platforms/ros/px4_ros_impl.cpp src/platforms/ros/px4_publisher.cpp src/platforms/ros/px4_subscriber.cpp + src/platforms/ros/px4_nodehandle.cpp ) target_link_libraries(px4 diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index bf16bf84e..39059daa3 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -57,7 +57,7 @@ int main(int argc, char **argv) * The first NodeHandle constructed will fully initialize this node, and the last * NodeHandle destructed will close down the node. */ - ros::NodeHandle n; + px4::NodeHandle n; /** * The subscribe() call is how you tell ROS that you want to receive messages @@ -74,7 +74,7 @@ int main(int argc, char **argv) * is the number of messages that will be buffered up before beginning to throw * away the oldest ones. */ - ros::Subscriber sub = n.subscribe("rc_channels", 1000, rc_channels_callback); + px4::Subscriber* sub = n.subscribe("rc_channels", rc_channels_callback); /** * px4::spin() will enter a loop, pumping callbacks. With this version, all diff --git a/src/include/px4.h b/src/include/px4.h index 0aba2ee77..bb97f2a8a 100644 --- a/src/include/px4.h +++ b/src/include/px4.h @@ -58,3 +58,5 @@ #include "../platforms/px4_defines.h" #include "../platforms/px4_middleware.h" +#include "../platforms/px4_nodehandle.h" +#include "../platforms/px4_subscriber.h" diff --git a/src/platforms/nuttx/px4_nodehandle.cpp b/src/platforms/nuttx/px4_nodehandle.cpp new file mode 100644 index 000000000..9d43daa49 --- /dev/null +++ b/src/platforms/nuttx/px4_nodehandle.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * 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 px4_nodehandle.cpp + * + * PX4 Middleware Wrapper Nodehandle + */ +#include diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index d278828b7..df198615c 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -36,11 +36,27 @@ * * PX4 Middleware Wrapper Node Handle */ +#pragma once +#include +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +#include "ros/ros.h" +#endif namespace px4 { +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +class NodeHandle : private ros::NodeHandle +{ +public: + template + Subscriber* subscribe(const char *topic, void(*fp)(M)) { + ros::NodeHandle::subscribe("rc_channels", 1000, fp); + return new Subscriber(); + } +}; +#else class NodeHandle { - }; +#endif } diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index 1b0952155..78d2a744b 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -36,6 +36,7 @@ * * PX4 Middleware Wrapper Node Handle */ +#pragma once namespace px4 { diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 8759f8b05..8f883154e 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -36,11 +36,25 @@ * * PX4 Middleware Wrapper Subscriber */ +#pragma once +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +#include "ros/ros.h" +#endif namespace px4 { + +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +class Subscriber +{ +public: + Subscriber() {}; + ~Subscriber() {}; +}; +#else class Subscriber { - }; +#endif + } diff --git a/src/platforms/ros/px4_nodehandle.cpp b/src/platforms/ros/px4_nodehandle.cpp new file mode 100644 index 000000000..6ac3c76d3 --- /dev/null +++ b/src/platforms/ros/px4_nodehandle.cpp @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * 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 px4_nodehandle.cpp + * + * PX4 Middleware Wrapper Nodehandle + */ +#include + +namespace px4 +{ +} + diff --git a/src/platforms/ros/px4_publisher.cpp b/src/platforms/ros/px4_publisher.cpp index ab6035b22..f02dbe4c9 100644 --- a/src/platforms/ros/px4_publisher.cpp +++ b/src/platforms/ros/px4_publisher.cpp @@ -36,5 +36,6 @@ * * PX4 Middleware Wrapper for Publisher */ +#include diff --git a/src/platforms/ros/px4_subscriber.cpp b/src/platforms/ros/px4_subscriber.cpp index 088c08fdb..d040b860d 100644 --- a/src/platforms/ros/px4_subscriber.cpp +++ b/src/platforms/ros/px4_subscriber.cpp @@ -37,4 +37,5 @@ * PX4 Middleware Wrapper Subscriber */ +#include -- cgit v1.2.3 From 1826fa5d39a5feb2a7c044102c899c23c1f12453 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 24 Nov 2014 17:38:27 +0100 Subject: improve subsciber api --- src/examples/subscriber/subscriber.cpp | 1 + src/platforms/px4_nodehandle.h | 5 +++-- src/platforms/px4_subscriber.h | 5 ++++- 3 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index 39059daa3..5d1d14d7f 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -75,6 +75,7 @@ int main(int argc, char **argv) * away the oldest ones. */ px4::Subscriber* sub = n.subscribe("rc_channels", rc_channels_callback); + PX4_INFO("subscribed"); /** * px4::spin() will enter a loop, pumping callbacks. With this version, all diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index df198615c..809d0eb15 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -50,8 +50,9 @@ class NodeHandle : private ros::NodeHandle public: template Subscriber* subscribe(const char *topic, void(*fp)(M)) { - ros::NodeHandle::subscribe("rc_channels", 1000, fp); - return new Subscriber(); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, 1000, fp); + //XXX create list here, for ros and nuttx + return new Subscriber(ros_sub); } }; #else diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 8f883154e..8933b2ab2 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -47,8 +47,11 @@ namespace px4 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) class Subscriber { +private: + ros::Subscriber _ros_sub; public: - Subscriber() {}; + Subscriber(ros::Subscriber ros_sub) : _ros_sub(ros_sub) + {} ~Subscriber() {}; }; #else -- cgit v1.2.3 From e2f846ee2fb20193ec73e1ea6ebbbd1fa967166e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 24 Nov 2014 18:53:08 +0100 Subject: create list of subscribers --- src/examples/subscriber/subscriber.cpp | 2 +- src/include/containers/List.hpp | 1 + src/platforms/px4_nodehandle.h | 14 ++++++++++++-- 3 files changed, 14 insertions(+), 3 deletions(-) diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index 5d1d14d7f..828b1f937 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -74,7 +74,7 @@ int main(int argc, char **argv) * is the number of messages that will be buffered up before beginning to throw * away the oldest ones. */ - px4::Subscriber* sub = n.subscribe("rc_channels", rc_channels_callback); + px4::Subscriber sub = n.subscribe("rc_channels", rc_channels_callback); PX4_INFO("subscribed"); /** diff --git a/src/include/containers/List.hpp b/src/include/containers/List.hpp index 13cbda938..5c0ba59fd 100644 --- a/src/include/containers/List.hpp +++ b/src/include/containers/List.hpp @@ -38,6 +38,7 @@ */ #pragma once +#include template class __EXPORT ListNode diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 809d0eb15..9bbf5c724 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -40,6 +40,7 @@ #include #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) #include "ros/ros.h" +#include #endif namespace px4 @@ -48,12 +49,21 @@ namespace px4 class NodeHandle : private ros::NodeHandle { public: + NodeHandle () : + ros::NodeHandle(), + _subs() + {} + template - Subscriber* subscribe(const char *topic, void(*fp)(M)) { + Subscriber subscribe(const char *topic, void(*fp)(M)) { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, 1000, fp); //XXX create list here, for ros and nuttx - return new Subscriber(ros_sub); + Subscriber sub(ros_sub); + _subs.push_back(sub); + return sub; } +private: + std::list _subs; }; #else class NodeHandle -- cgit v1.2.3 From 0a3492fc328280422df9472d3d8a586d92242feb Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 25 Nov 2014 09:20:57 +0100 Subject: define default queue size --- src/platforms/px4_nodehandle.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 9bbf5c724..6880d74f6 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -41,6 +41,7 @@ #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) #include "ros/ros.h" #include +#define QUEUE_SIZE_DEFAULT 1000 #endif namespace px4 @@ -56,7 +57,7 @@ public: template Subscriber subscribe(const char *topic, void(*fp)(M)) { - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, 1000, fp); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, QUEUE_SIZE_DEFAULT, fp); //XXX create list here, for ros and nuttx Subscriber sub(ros_sub); _subs.push_back(sub); -- cgit v1.2.3 From 978013bbb8d67e295d92a54e16f7728013722e92 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 25 Nov 2014 09:56:18 +0100 Subject: px4 wrapper for ros publisher --- src/examples/publisher/publisher.cpp | 5 +++-- src/platforms/px4_nodehandle.h | 16 +++++++++++++--- src/platforms/px4_publisher.h | 18 +++++++++++++++++- 3 files changed, 33 insertions(+), 6 deletions(-) diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp index 91e063162..6869e765b 100644 --- a/src/examples/publisher/publisher.cpp +++ b/src/examples/publisher/publisher.cpp @@ -38,7 +38,7 @@ int main(int argc, char **argv) px4::init(argc, argv, "px4_publisher"); - ros::NodeHandle n; + px4::NodeHandle n; /** * The advertise() function is how you tell ROS that you want to @@ -57,7 +57,8 @@ int main(int argc, char **argv) * than we can send them, the number here specifies how many messages to * buffer up before throwing some away. */ - ros::Publisher rc_channels_pub = n.advertise("rc_channels", 1000); + px4::Publisher rc_channels_pub = n.advertise("rc_channels"); + px4::Rate loop_rate(10); diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 6880d74f6..bfda636b0 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -38,6 +38,7 @@ */ #pragma once #include +#include #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) #include "ros/ros.h" #include @@ -52,19 +53,28 @@ class NodeHandle : private ros::NodeHandle public: NodeHandle () : ros::NodeHandle(), - _subs() + _subs(), + _pubs() {} - template + template Subscriber subscribe(const char *topic, void(*fp)(M)) { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, QUEUE_SIZE_DEFAULT, fp); - //XXX create list here, for ros and nuttx Subscriber sub(ros_sub); _subs.push_back(sub); return sub; } + + template + Publisher advertise(const char *topic) { + ros::Publisher ros_pub = ros::NodeHandle::advertise(topic, QUEUE_SIZE_DEFAULT); + Publisher pub(ros_pub); + _pubs.push_back(pub); + return pub; + } private: std::list _subs; + std::list _pubs; }; #else class NodeHandle diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index 78d2a744b..799525190 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -37,11 +37,27 @@ * PX4 Middleware Wrapper Node Handle */ #pragma once +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +#include "ros/ros.h" +#endif namespace px4 { +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +class Publisher +{ +private: + ros::Publisher _ros_pub; +public: + Publisher(ros::Publisher ros_pub) : _ros_pub(ros_pub) + {} + ~Publisher() {}; + template + int publish(const M &msg) { _ros_pub.publish(msg); return 0; } +}; +#else class Publisher { - }; +#endif } -- cgit v1.2.3 From c167df90380fdd99d1b56024c4de104a3f0a2f85 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 25 Nov 2014 10:19:18 +0100 Subject: ros wrapper: small reordering --- src/platforms/px4_nodehandle.h | 14 +++++++++++--- src/platforms/px4_publisher.h | 4 ++-- src/platforms/px4_subscriber.h | 4 ++-- 3 files changed, 15 insertions(+), 7 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index bfda636b0..d66884445 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -37,12 +37,19 @@ * PX4 Middleware Wrapper Node Handle */ #pragma once + +/* includes for all platforms */ #include #include + #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/* includes when building for ros */ #include "ros/ros.h" #include -#define QUEUE_SIZE_DEFAULT 1000 +#include +#else +/* includes when building for NuttX */ + #endif namespace px4 @@ -59,7 +66,7 @@ public: template Subscriber subscribe(const char *topic, void(*fp)(M)) { - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, QUEUE_SIZE_DEFAULT, fp); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp); Subscriber sub(ros_sub); _subs.push_back(sub); return sub; @@ -67,12 +74,13 @@ public: template Publisher advertise(const char *topic) { - ros::Publisher ros_pub = ros::NodeHandle::advertise(topic, QUEUE_SIZE_DEFAULT); + ros::Publisher ros_pub = ros::NodeHandle::advertise(topic, kQueueSizeDefault); Publisher pub(ros_pub); _pubs.push_back(pub); return pub; } private: + static const uint32_t kQueueSizeDefault = 1000; std::list _subs; std::list _pubs; }; diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index 799525190..72f69a5af 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -46,14 +46,14 @@ namespace px4 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) class Publisher { -private: - ros::Publisher _ros_pub; public: Publisher(ros::Publisher ros_pub) : _ros_pub(ros_pub) {} ~Publisher() {}; template int publish(const M &msg) { _ros_pub.publish(msg); return 0; } +private: + ros::Publisher _ros_pub; }; #else class Publisher diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 8933b2ab2..78be3ebff 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -47,12 +47,12 @@ namespace px4 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) class Subscriber { -private: - ros::Subscriber _ros_sub; public: Subscriber(ros::Subscriber ros_sub) : _ros_sub(ros_sub) {} ~Subscriber() {}; +private: + ros::Subscriber _ros_sub; }; #else class Subscriber -- cgit v1.2.3 From 55cf2fc61c7b90725cd960f9c7d72737024f1cfc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 25 Nov 2014 11:50:35 +0100 Subject: WIP, towards more px4 compatibility, first macros --- src/examples/publisher/publisher.cpp | 2 +- src/examples/subscriber/subscriber.cpp | 2 +- src/include/px4.h | 5 +++++ src/modules/uORB/Publication.cpp | 1 + src/modules/uORB/Subscription.cpp | 1 + src/platforms/px4_nodehandle.h | 35 +++++++++++++++++++++++++++++++--- src/platforms/px4_publisher.h | 14 +++++++++++++- src/platforms/px4_subscriber.h | 17 +++++++++++++++-- 8 files changed, 69 insertions(+), 8 deletions(-) diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp index 6869e765b..c09cca1fc 100644 --- a/src/examples/publisher/publisher.cpp +++ b/src/examples/publisher/publisher.cpp @@ -57,7 +57,7 @@ int main(int argc, char **argv) * than we can send them, the number here specifies how many messages to * buffer up before throwing some away. */ - px4::Publisher rc_channels_pub = n.advertise("rc_channels"); + px4::Publisher rc_channels_pub = n.advertise(PX4_TOPIC(rc_channels)); px4::Rate loop_rate(10); diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index 828b1f937..b91859027 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -74,7 +74,7 @@ int main(int argc, char **argv) * is the number of messages that will be buffered up before beginning to throw * away the oldest ones. */ - px4::Subscriber sub = n.subscribe("rc_channels", rc_channels_callback); + px4::Subscriber sub = n.subscribe(PX4_TOPIC(rc_channels), rc_channels_callback); PX4_INFO("subscribed"); /** diff --git a/src/include/px4.h b/src/include/px4.h index bb97f2a8a..391972b12 100644 --- a/src/include/px4.h +++ b/src/include/px4.h @@ -46,14 +46,19 @@ * Building for running within the ROS environment */ #include "ros/ros.h" + #define PX4_WARN ROS_WARN #define PX4_INFO ROS_INFO +#define PX4_TOPIC(name) #name #else /* * Building for NuttX */ +#include + #define PX4_WARN warnx #define PX4_INFO warnx +#define PX4_TOPIC(name) ORB_ID(name) #endif #include "../platforms/px4_defines.h" diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index cd0b30dd6..302b99c69 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -78,5 +78,6 @@ template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; +template class __EXPORT Publication; } diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 44b6debc7..3406a66d3 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -101,5 +101,6 @@ template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; +template class __EXPORT Subscription; } // namespace uORB diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index d66884445..fa2c8d6a4 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -49,21 +49,26 @@ #include #else /* includes when building for NuttX */ - +#include #endif namespace px4 { #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) -class NodeHandle : private ros::NodeHandle +class NodeHandle : + private ros::NodeHandle { public: - NodeHandle () : + NodeHandle() : ros::NodeHandle(), _subs(), _pubs() {} + ~NodeHandle() { + //XXX empty lists + }; + template Subscriber subscribe(const char *topic, void(*fp)(M)) { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp); @@ -87,6 +92,30 @@ private: #else class NodeHandle { +public: + NodeHandle() : + _subs(), + _pubs() + {} + + ~NodeHandle() {}; + + template + Subscriber subscribe(const char *topic, void(*fp)(M)) { + Subscriber sub(&_subs, , interval); + return sub; + } + + template + Publisher advertise(const char *topic) { + Publisher pub(ros_pub); + _pubs.push_back(pub); + return pub; + } +private: + List _subs; + List _pubs; + }; #endif } diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index 72f69a5af..53e63b695 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -38,7 +38,11 @@ */ #pragma once #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/* includes when building for ros */ #include "ros/ros.h" +#else +/* includes when building for NuttX */ +#include #endif namespace px4 @@ -56,7 +60,15 @@ private: ros::Publisher _ros_pub; }; #else -class Publisher +template +class Publisher : + public uORB::Publication +public: + Publisher(List * list, + const struct orb_metadata *meta, unsigned interval) : + uORB::Publication(list, meta) + {} + ~Publisher() {}; { }; #endif diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 78be3ebff..6312e0cbe 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -38,7 +38,11 @@ */ #pragma once #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/* includes when building for ros */ #include "ros/ros.h" +#else +/* includes when building for NuttX */ +#include #endif namespace px4 @@ -48,14 +52,23 @@ namespace px4 class Subscriber { public: - Subscriber(ros::Subscriber ros_sub) : _ros_sub(ros_sub) + Subscriber(ros::Subscriber ros_sub) : + _ros_sub(ros_sub) {} ~Subscriber() {}; private: ros::Subscriber _ros_sub; }; #else -class Subscriber +template +class Subscriber : + public uORB::Subscription +public: + Subscriber(List * list, + const struct orb_metadata *meta, unsigned interval) : + uORB::Subsciption(list, meta, interval) + {} + ~Subscriber() {}; { }; #endif -- cgit v1.2.3 From 579ec36c28d4cbf08d93531853b2b8fd3f3461ac Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 25 Nov 2014 15:56:05 +0100 Subject: fix order of arguments (merge fix) --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index bffeefc1f..ecdab2936 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -52,7 +52,7 @@ mTecs::mTecs() : _mTecsEnabled(this, "ENABLED"), _airspeedMin(this, "FW_AIRSPD_MIN", false), /* Publications */ - _status(&getPublications(), ORB_ID(tecs_status)), + _status(ORB_ID(tecs_status), &getPublications()), /* control blocks */ _controlTotalEnergy(this, "THR"), _controlEnergyDistribution(this, "PIT", true), -- cgit v1.2.3 From e7c1e5b1ff7b1bbdc11ab2cae6b99fe459487119 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 26 Nov 2014 11:36:23 +0100 Subject: wip, working on the nuttx wrapper --- src/examples/publisher/publisher.cpp | 13 +++++++------ src/examples/subscriber/subscriber.cpp | 11 ++++++----- src/include/px4.h | 10 ++++------ src/modules/uORB/Publication.cpp | 1 + src/modules/uORB/Publication.hpp | 10 +++++----- src/modules/uORB/Subscription.cpp | 1 + src/platforms/nuttx/module.mk | 3 ++- src/platforms/nuttx/px4_nodehandle.cpp | 2 +- src/platforms/nuttx/px4_nuttx_impl.cpp | 8 +++++--- src/platforms/nuttx/px4_publisher.cpp | 1 + src/platforms/nuttx/px4_subscriber.cpp | 2 +- src/platforms/px4_defines.h | 23 ++++++++++++++++++++--- src/platforms/px4_middleware.h | 11 ++++++----- src/platforms/px4_nodehandle.h | 23 ++++++++++++----------- src/platforms/px4_publisher.h | 21 +++++++++++++++------ src/platforms/px4_subscriber.h | 20 ++++++++++++++------ 16 files changed, 101 insertions(+), 59 deletions(-) diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp index c09cca1fc..68fd8a5c3 100644 --- a/src/examples/publisher/publisher.cpp +++ b/src/examples/publisher/publisher.cpp @@ -26,14 +26,15 @@ */ #include -#include -#include +using namespace px4; /** * This tutorial demonstrates simple sending of messages over the PX4 middleware system. */ -int main(int argc, char **argv) +// __EXPORT bool task_should_exit; + +PX4_MAIN_FUNCTION(publisher) { px4::init(argc, argv, "px4_publisher"); @@ -57,7 +58,7 @@ int main(int argc, char **argv) * than we can send them, the number here specifies how many messages to * buffer up before throwing some away. */ - px4::Publisher rc_channels_pub = n.advertise(PX4_TOPIC(rc_channels)); + px4::Publisher * rc_channels_pub = n.advertise(PX4_TOPIC(rc_channels)); px4::Rate loop_rate(10); @@ -72,7 +73,7 @@ int main(int argc, char **argv) /** * This is a message object. You stuff it with data, and then publish it. */ - px4::rc_channels msg; + PX4_TOPIC_T(rc_channels) msg; msg.timestamp_last_valid = px4::get_time_micros(); PX4_INFO("%lu", msg.timestamp_last_valid); @@ -83,7 +84,7 @@ int main(int argc, char **argv) * given as a template parameter to the advertise<>() call, as was done * in the constructor above. */ - rc_channels_pub.publish(msg); + rc_channels_pub->publish(msg); px4::spin_once(); diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index b91859027..cdadaf2bc 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -26,19 +26,20 @@ */ #include -#include "px4/rc_channels.h" + +using namespace px4; /** * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. */ -void rc_channels_callback(const px4::rc_channels::ConstPtr &msg) +void rc_channels_callback(const PX4_TOPIC_T(rc_channels) *msg) { PX4_INFO("I heard: [%lu]", msg->timestamp_last_valid); } -PX4_MAIN_FUNCTION(subscriber) +// __EXPORT bool task_should_exit; -int main(int argc, char **argv) +PX4_MAIN_FUNCTION(subscriber) { /** * The ros::init() function needs to see argc and argv so that it can perform @@ -74,7 +75,7 @@ int main(int argc, char **argv) * is the number of messages that will be buffered up before beginning to throw * away the oldest ones. */ - px4::Subscriber sub = n.subscribe(PX4_TOPIC(rc_channels), rc_channels_callback); + n.subscribe(PX4_TOPIC(rc_channels), rc_channels_callback); PX4_INFO("subscribed"); /** diff --git a/src/include/px4.h b/src/include/px4.h index 391972b12..22d661b17 100644 --- a/src/include/px4.h +++ b/src/include/px4.h @@ -46,19 +46,17 @@ * Building for running within the ROS environment */ #include "ros/ros.h" +#include "px4/rc_channels.h" -#define PX4_WARN ROS_WARN -#define PX4_INFO ROS_INFO -#define PX4_TOPIC(name) #name #else /* * Building for NuttX */ +#include #include +#include +#include -#define PX4_WARN warnx -#define PX4_INFO warnx -#define PX4_TOPIC(name) ORB_ID(name) #endif #include "../platforms/px4_defines.h" diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index 12ef83aa0..05605417d 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -48,6 +48,7 @@ #include "topics/actuator_outputs.h" #include "topics/encoders.h" #include "topics/tecs_status.h" +#include "topics/rc_channels.h" namespace uORB { diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index 1c48929e4..fd1ee4dec 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -59,7 +59,7 @@ public: * Constructor * * - * @param meta The uORB metadata (usually from the ORB_ID() + * @param meta The uORB metadata (usually from the ORB_ID() * macro) for the topic. */ PublicationBase(const struct orb_metadata *meta) : @@ -96,7 +96,7 @@ protected: orb_advert_t _handle; }; -/** +/** * alias class name so it is clear that the base class * can be used by itself if desired */ @@ -114,9 +114,9 @@ public: * Constructor * * - * @param meta The uORB metadata (usually from the ORB_ID() + * @param meta The uORB metadata (usually from the ORB_ID() * macro) for the topic. - * @param list A pointer to a list of subscriptions + * @param list A pointer to a list of subscriptions * that this should be appended to. */ PublicationNode(const struct orb_metadata *meta, @@ -144,7 +144,7 @@ public: /** * Constructor * - * @param meta The uORB metadata (usually from + * @param meta The uORB metadata (usually from * the ORB_ID() macro) for the topic. * @param list A list interface for adding to * list during construction diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index a681ccb30..61609d009 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -52,6 +52,7 @@ #include "topics/vehicle_local_position.h" #include "topics/vehicle_attitude_setpoint.h" #include "topics/vehicle_rates_setpoint.h" +#include "topics/rc_channels.h" namespace uORB { diff --git a/src/platforms/nuttx/module.mk b/src/platforms/nuttx/module.mk index 128f0e734..1c0ad7aa4 100644 --- a/src/platforms/nuttx/module.mk +++ b/src/platforms/nuttx/module.mk @@ -37,6 +37,7 @@ SRCS = px4_nuttx_impl.cpp \ px4_publisher.cpp \ - px4_subscriber.cpp + px4_subscriber.cpp \ + px4_nodehandle.cpp MAXOPTIMIZATION = -Os diff --git a/src/platforms/nuttx/px4_nodehandle.cpp b/src/platforms/nuttx/px4_nodehandle.cpp index 9d43daa49..473a5cf77 100644 --- a/src/platforms/nuttx/px4_nodehandle.cpp +++ b/src/platforms/nuttx/px4_nodehandle.cpp @@ -36,4 +36,4 @@ * * PX4 Middleware Wrapper Nodehandle */ -#include +#include diff --git a/src/platforms/nuttx/px4_nuttx_impl.cpp b/src/platforms/nuttx/px4_nuttx_impl.cpp index 3a6529716..4b87f68fe 100644 --- a/src/platforms/nuttx/px4_nuttx_impl.cpp +++ b/src/platforms/nuttx/px4_nuttx_impl.cpp @@ -38,6 +38,7 @@ */ #include +#include extern bool task_should_exit; @@ -46,8 +47,7 @@ namespace px4 void init(int argc, char *argv[], const char *process_name) { - px4_warn("process: %s", process_name); - return 0; + PX4_WARN("process: %s", process_name); } uint64_t get_time_micros() @@ -57,7 +57,9 @@ uint64_t get_time_micros() bool ok() { - return !task_should_exit; + // return !task_should_exit; + //XXX + return true; } void spin_once() diff --git a/src/platforms/nuttx/px4_publisher.cpp b/src/platforms/nuttx/px4_publisher.cpp index ab6035b22..3bd70272f 100644 --- a/src/platforms/nuttx/px4_publisher.cpp +++ b/src/platforms/nuttx/px4_publisher.cpp @@ -36,5 +36,6 @@ * * PX4 Middleware Wrapper for Publisher */ +#include diff --git a/src/platforms/nuttx/px4_subscriber.cpp b/src/platforms/nuttx/px4_subscriber.cpp index 088c08fdb..426e646c9 100644 --- a/src/platforms/nuttx/px4_subscriber.cpp +++ b/src/platforms/nuttx/px4_subscriber.cpp @@ -36,5 +36,5 @@ * * PX4 Middleware Wrapper Subscriber */ - +#include diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 48234766f..d4dc71453 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -44,8 +44,25 @@ * Building for running within the ROS environment */ #define __EXPORT -#define PX4_MAIN_FUNCTION(_prefix) +// #define PX4_MAIN_FUNCTION(_prefix) +#define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv) +#define PX4_WARN ROS_WARN +#define PX4_INFO ROS_INFO +#define PX4_TOPIC(name) #name +#define PX4_TOPIC_T(name) name + #else -#include -#define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##main(int argc, char **argv)() { return main(argc, argv); } +/* + * Building for NuttX + */ + +// #define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##_main(int argc, char **argv)() { return main(argc, argv); } +// #define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) { return main(argc, argv); } +#define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) +#define PX4_WARN warnx +#define PX4_WARN warnx +#define PX4_INFO warnx +#define PX4_TOPIC(name) ORB_ID(name) +#define PX4_TOPIC_T(name) name##_s + #endif diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index d1c0656af..eebfc9049 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -40,19 +40,20 @@ #pragma once #include +#include namespace px4 { -void init(int argc, char *argv[], const char *process_name); +__EXPORT void init(int argc, char *argv[], const char *process_name); -uint64_t get_time_micros(); +__EXPORT uint64_t get_time_micros(); -bool ok(); +__EXPORT bool ok(); -void spin_once(); +__EXPORT void spin_once(); -void spin(); +__EXPORT void spin(); class Rate { diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index fa2c8d6a4..34a605647 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -39,8 +39,8 @@ #pragma once /* includes for all platforms */ -#include -#include +#include "px4_subscriber.h" +#include "px4_publisher.h" #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) /* includes when building for ros */ @@ -49,7 +49,6 @@ #include #else /* includes when building for NuttX */ -#include #endif namespace px4 @@ -90,7 +89,7 @@ private: std::list _pubs; }; #else -class NodeHandle +class __EXPORT NodeHandle { public: NodeHandle() : @@ -101,20 +100,22 @@ public: ~NodeHandle() {}; template - Subscriber subscribe(const char *topic, void(*fp)(M)) { - Subscriber sub(&_subs, , interval); + Subscriber * subscribe(const struct orb_metadata *meta, void(*fp)(M)) { + unsigned interval = 0;//XXX decide how to wrap this, ros equivalent? + //XXX + Subscriber *sub = new Subscriber(meta, interval, fp, &_subs); return sub; } template - Publisher advertise(const char *topic) { - Publisher pub(ros_pub); - _pubs.push_back(pub); + Publisher * advertise(const struct orb_metadata *meta) { + //XXX + Publisher * pub = new Publisher(meta, &_pubs); return pub; } private: - List _subs; - List _pubs; + List _subs; + List _pubs; }; #endif diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index 53e63b695..9ce211d25 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -43,6 +43,7 @@ #else /* includes when building for NuttX */ #include +#include #endif namespace px4 @@ -60,16 +61,24 @@ private: ros::Publisher _ros_pub; }; #else -template class Publisher : - public uORB::Publication + public uORB::PublicationNode +{ public: - Publisher(List * list, - const struct orb_metadata *meta, unsigned interval) : - uORB::Publication(list, meta) + Publisher(const struct orb_metadata *meta, + List * list) : + uORB::PublicationNode(meta, list) {} ~Publisher() {}; -{ + template + int publish(const M &msg) { + uORB::PublicationBase::update((void*)&msg); + return 0; + } + + void update() { + //XXX list traversal callback, needed? + } ; }; #endif } diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 6312e0cbe..12d422bb3 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -43,6 +43,7 @@ #else /* includes when building for NuttX */ #include +#include #endif namespace px4 @@ -60,16 +61,23 @@ private: ros::Subscriber _ros_sub; }; #else -template class Subscriber : - public uORB::Subscription + public uORB::SubscriptionNode +{ public: - Subscriber(List * list, - const struct orb_metadata *meta, unsigned interval) : - uORB::Subsciption(list, meta, interval) + template + Subscriber(const struct orb_metadata *meta, + unsigned interval, + void(*fp)(M), + List * list) : + uORB::SubscriptionNode(meta, interval, list) + //XXX store callback {} ~Subscriber() {}; -{ + + void update() { + //XXX list traversal callback, needed? + } ; }; #endif -- cgit v1.2.3 From 0474908e1c600d312934fcdd5790813813e799b1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 26 Nov 2014 11:55:41 +0100 Subject: reenable task flag --- src/examples/publisher/publisher.cpp | 6 +++++- src/examples/subscriber/subscriber.cpp | 6 ++++-- src/platforms/nuttx/px4_nodehandle.cpp | 5 +++++ src/platforms/nuttx/px4_nuttx_impl.cpp | 5 +---- src/platforms/px4_middleware.h | 2 ++ 5 files changed, 17 insertions(+), 7 deletions(-) diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp index 68fd8a5c3..5ee8b459b 100644 --- a/src/examples/publisher/publisher.cpp +++ b/src/examples/publisher/publisher.cpp @@ -32,7 +32,11 @@ using namespace px4; /** * This tutorial demonstrates simple sending of messages over the PX4 middleware system. */ -// __EXPORT bool task_should_exit; + +namespace px4 +{ +bool task_should_exit = false; +} PX4_MAIN_FUNCTION(publisher) { diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index cdadaf2bc..b87231880 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -36,8 +36,10 @@ void rc_channels_callback(const PX4_TOPIC_T(rc_channels) *msg) { PX4_INFO("I heard: [%lu]", msg->timestamp_last_valid); } - -// __EXPORT bool task_should_exit; +namespace px4 +{ +bool task_should_exit = false; +} PX4_MAIN_FUNCTION(subscriber) { diff --git a/src/platforms/nuttx/px4_nodehandle.cpp b/src/platforms/nuttx/px4_nodehandle.cpp index 473a5cf77..091b5c6c6 100644 --- a/src/platforms/nuttx/px4_nodehandle.cpp +++ b/src/platforms/nuttx/px4_nodehandle.cpp @@ -37,3 +37,8 @@ * PX4 Middleware Wrapper Nodehandle */ #include + +namespace px4 +{ +bool task_should_exit = false; +} diff --git a/src/platforms/nuttx/px4_nuttx_impl.cpp b/src/platforms/nuttx/px4_nuttx_impl.cpp index 4b87f68fe..6471e3e38 100644 --- a/src/platforms/nuttx/px4_nuttx_impl.cpp +++ b/src/platforms/nuttx/px4_nuttx_impl.cpp @@ -40,7 +40,6 @@ #include #include -extern bool task_should_exit; namespace px4 { @@ -57,9 +56,7 @@ uint64_t get_time_micros() bool ok() { - // return !task_should_exit; - //XXX - return true; + return !task_should_exit; } void spin_once() diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index eebfc9049..a09d9ac58 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -68,6 +68,8 @@ private: }; +extern bool task_should_exit; + // /** // * A limiter/ saturation. // * The output of update is the input, bounded -- cgit v1.2.3 From 818a49b5a81a5b821fa9a1a13db4591da5c33751 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 26 Nov 2014 12:45:03 +0100 Subject: fix ros compile errors --- src/examples/subscriber/subscriber.cpp | 4 ++-- src/platforms/px4_nodehandle.h | 12 ++++++------ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index b87231880..093697d8e 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -32,9 +32,9 @@ using namespace px4; /** * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. */ -void rc_channels_callback(const PX4_TOPIC_T(rc_channels) *msg) +void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("I heard: [%lu]", msg->timestamp_last_valid); + PX4_INFO("I heard: [%lu]", msg.timestamp_last_valid); } namespace px4 { diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 34a605647..972792d53 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -69,24 +69,24 @@ public: }; template - Subscriber subscribe(const char *topic, void(*fp)(M)) { + Subscriber * subscribe(const char *topic, void(*fp)(M)) { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp); - Subscriber sub(ros_sub); + Subscriber * sub = new Subscriber(ros_sub); _subs.push_back(sub); return sub; } template - Publisher advertise(const char *topic) { + Publisher * advertise(const char *topic) { ros::Publisher ros_pub = ros::NodeHandle::advertise(topic, kQueueSizeDefault); - Publisher pub(ros_pub); + Publisher *pub = new Publisher(ros_pub); _pubs.push_back(pub); return pub; } private: static const uint32_t kQueueSizeDefault = 1000; - std::list _subs; - std::list _pubs; + std::list _subs; + std::list _pubs; }; #else class __EXPORT NodeHandle -- cgit v1.2.3 From ee534b827a51a7356e565b665e280f765bd8d302 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 26 Nov 2014 13:18:28 +0100 Subject: move spin functions to nodehandle --- src/examples/publisher/publisher.cpp | 2 +- src/examples/subscriber/subscriber.cpp | 2 +- src/platforms/px4_middleware.h | 4 ---- src/platforms/px4_nodehandle.h | 5 +++++ src/platforms/ros/px4_ros_impl.cpp | 10 ---------- 5 files changed, 7 insertions(+), 16 deletions(-) diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp index 5ee8b459b..521c7c838 100644 --- a/src/examples/publisher/publisher.cpp +++ b/src/examples/publisher/publisher.cpp @@ -90,7 +90,7 @@ PX4_MAIN_FUNCTION(publisher) */ rc_channels_pub->publish(msg); - px4::spin_once(); + n.spinOnce(); loop_rate.sleep(); ++count; diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index 093697d8e..0d0a81d7e 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -85,7 +85,7 @@ PX4_MAIN_FUNCTION(subscriber) * 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. */ - px4::spin(); + n.spin(); return 0; } diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index a09d9ac58..8a83f1af4 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -51,10 +51,6 @@ __EXPORT uint64_t get_time_micros(); __EXPORT bool ok(); -__EXPORT void spin_once(); - -__EXPORT void spin(); - class Rate { diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 972792d53..b665f3fe7 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -53,6 +53,7 @@ namespace px4 { +//XXX create abstract base class #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) class NodeHandle : private ros::NodeHandle @@ -83,6 +84,10 @@ public: _pubs.push_back(pub); return pub; } + + void spin() { ros::spin(); } + + void spinOnce() { ros::spinOnce(); } private: static const uint32_t kQueueSizeDefault = 1000; std::list _subs; diff --git a/src/platforms/ros/px4_ros_impl.cpp b/src/platforms/ros/px4_ros_impl.cpp index eda17e5a9..3aa976138 100644 --- a/src/platforms/ros/px4_ros_impl.cpp +++ b/src/platforms/ros/px4_ros_impl.cpp @@ -58,14 +58,4 @@ bool ok() return ros::ok(); } -void spin_once() -{ - ros::spinOnce(); -} - -void spin() -{ - ros::spin(); -} - } -- cgit v1.2.3 From 486d81cb95bb7dc912ee0a7636b0a6aeb87009d9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 28 Nov 2014 08:12:46 +0100 Subject: NuttX submodule: header changes --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index ec6b670f6..e3da98e37 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit ec6b670f6d1e964700c0b0a50d14db12761e3097 +Subproject commit e3da98e379452a1736c1098a44c000d2841956c0 -- cgit v1.2.3 From 36bf0c04c8cb052c67e613eb051b0deb650eb216 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 28 Nov 2014 08:58:44 +0100 Subject: WIP, c++11 style callbacks for px4 --- nuttx-configs/px4fmu-v2/nsh/Make.defs | 4 ++-- src/examples/subscriber/subscriber.cpp | 10 +++++++- src/platforms/nuttx/px4_nodehandle.cpp | 19 +++++++++++++++ src/platforms/nuttx/px4_nuttx_impl.cpp | 12 ---------- src/platforms/px4_nodehandle.h | 13 +++++++++-- src/platforms/px4_subscriber.h | 42 ++++++++++++++++++++++++++-------- 6 files changed, 74 insertions(+), 26 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs index f3ce53b4a..5ae27aa00 100644 --- a/nuttx-configs/px4fmu-v2/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -90,7 +90,7 @@ else ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) else - # Linux/Cygwin-native toolchain + # Linux/Cygwin-native toolchain MKDEP = $(TOPDIR)/tools/mkdeps.sh ARCHINCLUDES = -I. -isystem $(TOPDIR)/include ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx @@ -117,7 +117,7 @@ ARCHOPTIMIZATION += -g endif ARCHCFLAGS = -std=gnu99 -ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=c++11 ARCHWARNINGS = -Wall \ -Wextra \ -Wdouble-promotion \ diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index 0d0a81d7e..f8b138844 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -36,6 +36,10 @@ void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { PX4_INFO("I heard: [%lu]", msg.timestamp_last_valid); } +// void rc_channels_callback(int i) +// { + // PX4_INFO("I heard: [%d]", i); +// } namespace px4 { bool task_should_exit = false; @@ -77,7 +81,11 @@ PX4_MAIN_FUNCTION(subscriber) * is the number of messages that will be buffered up before beginning to throw * away the oldest ones. */ - n.subscribe(PX4_TOPIC(rc_channels), rc_channels_callback); + // n.subscribe(PX4_TOPIC(rc_channels), [&](const PX4_TOPIC_T(rc_channels) &msg){rc_channels_callback(msg);}); + // n.subscribe(PX4_TOPIC(rc_channels), [&](int i){ return rc_channels_callback(i);}); + // CallbackFunction cbf = [](int i){ return rc_channels_callback(i);}; + std::function cbf = [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);}; + n.subscribe(PX4_TOPIC(rc_channels), cbf); PX4_INFO("subscribed"); /** diff --git a/src/platforms/nuttx/px4_nodehandle.cpp b/src/platforms/nuttx/px4_nodehandle.cpp index 091b5c6c6..7930a0680 100644 --- a/src/platforms/nuttx/px4_nodehandle.cpp +++ b/src/platforms/nuttx/px4_nodehandle.cpp @@ -36,9 +36,28 @@ * * PX4 Middleware Wrapper Nodehandle */ +#include #include namespace px4 { bool task_should_exit = false; + + +void NodeHandle::spinOnce() { + /* Loop through subscriptions, call callback for updated subscriptions */ + uORB::SubscriptionNode *sub = _subs.getHead(); + int count = 0; + + while (sub != nullptr) { + if (count++ > kMaxSubscriptions) { + PX4_WARN("exceeded max subscriptions"); + break; + } + + sub->update(); + sub = sub->getSibling(); + } +} + } diff --git a/src/platforms/nuttx/px4_nuttx_impl.cpp b/src/platforms/nuttx/px4_nuttx_impl.cpp index 6471e3e38..22daee823 100644 --- a/src/platforms/nuttx/px4_nuttx_impl.cpp +++ b/src/platforms/nuttx/px4_nuttx_impl.cpp @@ -59,16 +59,4 @@ bool ok() return !task_should_exit; } -void spin_once() -{ - // XXX check linked list of topics with orb_check() here - -} - -void spin() -{ - // XXX block waiting for updated topics here - -} - } diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index b665f3fe7..f473bf629 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -105,10 +105,12 @@ public: ~NodeHandle() {}; template - Subscriber * subscribe(const struct orb_metadata *meta, void(*fp)(M)) { + Subscriber * subscribe(const struct orb_metadata *meta, std::function callback) { + // Subscriber * subscribe(const struct orb_metadata *meta, std::function callback) { + // Subscriber * subscribe(const struct orb_metadata *meta, CallbackFunction callback) { unsigned interval = 0;//XXX decide how to wrap this, ros equivalent? //XXX - Subscriber *sub = new Subscriber(meta, interval, fp, &_subs); + Subscriber *sub = new SubscriberPX4(meta, interval, callback, &_subs); return sub; } @@ -118,7 +120,14 @@ public: Publisher * pub = new Publisher(meta, &_pubs); return pub; } + + void spinOnce(); + + void spin() { + //XXX: call callbacks and do not return until task is terminated + } private: + static const uint16_t kMaxSubscriptions = 100; List _subs; List _pubs; diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 12d422bb3..71c3a766d 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -44,6 +44,7 @@ /* includes when building for NuttX */ #include #include +#include #endif namespace px4 @@ -61,23 +62,46 @@ private: ros::Subscriber _ros_sub; }; #else -class Subscriber : - public uORB::SubscriptionNode +// typedef std::function CallbackFunction; +class Subscriber +{ +public: + Subscriber() {}; + ~Subscriber() {}; +private: +}; + +template +class SubscriberPX4 : + public Subscriber, + public uORB::Subscription { public: - template - Subscriber(const struct orb_metadata *meta, + SubscriberPX4(const struct orb_metadata *meta, unsigned interval, - void(*fp)(M), + std::function callback, + // std::function callback, + // CallbackFunction callback, List * list) : - uORB::SubscriptionNode(meta, interval, list) + Subscriber(), + uORB::Subscription(meta, interval, list) //XXX store callback {} - ~Subscriber() {}; + ~SubscriberPX4() {}; void update() { - //XXX list traversal callback, needed? - } ; + /* get latest data */ + uORB::Subscription::update(); + + /* Call callback which performs actions based on this data */ + // _callback(); + + }; +private: + // std::function _callback; + // CallbackFunction _callback; + std::function _callback; + }; #endif -- cgit v1.2.3 From 946d1203cfbbe522c61d6ed5dc1205c9f809f6f1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 28 Nov 2014 09:45:39 +0100 Subject: remove unnecessary variable --- src/examples/subscriber/subscriber.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index f8b138844..178fca4b5 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -84,8 +84,10 @@ PX4_MAIN_FUNCTION(subscriber) // n.subscribe(PX4_TOPIC(rc_channels), [&](const PX4_TOPIC_T(rc_channels) &msg){rc_channels_callback(msg);}); // n.subscribe(PX4_TOPIC(rc_channels), [&](int i){ return rc_channels_callback(i);}); // CallbackFunction cbf = [](int i){ return rc_channels_callback(i);}; - std::function cbf = [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);}; - n.subscribe(PX4_TOPIC(rc_channels), cbf); + // std::function cbf = [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);}; + // n.subscribe(PX4_TOPIC(rc_channels), cbf); + n.subscribe(PX4_TOPIC(rc_channels), + [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);}); PX4_INFO("subscribed"); /** -- cgit v1.2.3 From bfc0a52ea23d18c9ef72690aa511e0feeb5a4176 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 28 Nov 2014 09:48:12 +0100 Subject: update nuttx --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index e3da98e37..ae4b05e2c 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit e3da98e379452a1736c1098a44c000d2841956c0 +Subproject commit ae4b05e2c51d07369b5d131052099ac346b0841c -- cgit v1.2.3 From 59a9648bb60f8c33ca26b5f248d4b28dfc1de139 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 28 Nov 2014 10:18:28 +0100 Subject: macro for topic subscription --- src/examples/subscriber/subscriber.cpp | 7 +++++-- src/platforms/px4_defines.h | 10 ++++++---- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index 178fca4b5..d6bc0bf37 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -86,8 +86,11 @@ PX4_MAIN_FUNCTION(subscriber) // CallbackFunction cbf = [](int i){ return rc_channels_callback(i);}; // std::function cbf = [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);}; // n.subscribe(PX4_TOPIC(rc_channels), cbf); - n.subscribe(PX4_TOPIC(rc_channels), - [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);}); + // n.subscribe(PX4_TOPIC(rc_channels), + // [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);}); + // n.subscribe(PX4_TOPIC(rc_channels), + // [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);}); + PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback); PX4_INFO("subscribed"); /** diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index d4dc71453..79f305140 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -48,8 +48,9 @@ #define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv) #define PX4_WARN ROS_WARN #define PX4_INFO ROS_INFO -#define PX4_TOPIC(name) #name -#define PX4_TOPIC_T(name) name +#define PX4_TOPIC(_name) #_name +#define PX4_TOPIC_T(_name) _name +#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); #else /* @@ -62,7 +63,8 @@ #define PX4_WARN warnx #define PX4_WARN warnx #define PX4_INFO warnx -#define PX4_TOPIC(name) ORB_ID(name) -#define PX4_TOPIC_T(name) name##_s +#define PX4_TOPIC(_name) ORB_ID(_name) +#define PX4_TOPIC_T(_name) _name##_s +#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf) _nodehandle.subscribe(PX4_TOPIC(_name), [](const PX4_TOPIC_T(_name)& msg){ return _cbf(msg);}) #endif -- cgit v1.2.3 From 244c1cb58387c5e8825abd4b27b4ff0b893f5383 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 28 Nov 2014 11:34:52 +0100 Subject: enable systemcmds in test build --- makefiles/config_px4fmu-v2_test.mk | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 8aa8badc3..844f1236a 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -33,6 +33,25 @@ MODULES += systemcmds/nshterm MODULES += systemcmds/mtd MODULES += systemcmds/ver +# +# System commands +# +MODULES += systemcmds/bl_update +MODULES += systemcmds/boardinfo +MODULES += systemcmds/mixer +MODULES += systemcmds/param +MODULES += systemcmds/perf +MODULES += systemcmds/preflight_check +MODULES += systemcmds/pwm +MODULES += systemcmds/esc_calib +MODULES += systemcmds/reboot +MODULES += systemcmds/top +MODULES += systemcmds/config +MODULES += systemcmds/nshterm +MODULES += systemcmds/mtd +MODULES += systemcmds/dumpfile +MODULES += systemcmds/ver + # # Example modules # -- cgit v1.2.3 From b351d67175e328ce3ab16642cdc69f6b8a239366 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 28 Nov 2014 11:37:30 +0100 Subject: fix print for px4 --- src/examples/publisher/publisher.cpp | 3 +-- src/examples/subscriber/subscriber.cpp | 3 ++- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp index 521c7c838..5c7156c08 100644 --- a/src/examples/publisher/publisher.cpp +++ b/src/examples/publisher/publisher.cpp @@ -80,7 +80,7 @@ PX4_MAIN_FUNCTION(publisher) PX4_TOPIC_T(rc_channels) msg; msg.timestamp_last_valid = px4::get_time_micros(); - PX4_INFO("%lu", msg.timestamp_last_valid); + PX4_INFO("%llu", msg.timestamp_last_valid); /** * The publish() function is how you send messages. The parameter @@ -96,6 +96,5 @@ PX4_MAIN_FUNCTION(publisher) ++count; } - return 0; } diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index d6bc0bf37..fa863463b 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -34,7 +34,7 @@ using namespace px4; */ void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("I heard: [%lu]", msg.timestamp_last_valid); + PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid); } // void rc_channels_callback(int i) // { @@ -99,6 +99,7 @@ PX4_MAIN_FUNCTION(subscriber) * will exit when Ctrl-C is pressed, or the node is shutdown by the master. */ n.spin(); + PX4_INFO("finished, returning"); return 0; } -- cgit v1.2.3 From 16f21d36dce17868f455318273140013febb6770 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 28 Nov 2014 11:38:22 +0100 Subject: actually call callback --- src/platforms/px4_nodehandle.h | 6 +++--- src/platforms/px4_subscriber.h | 11 ++++------- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index f473bf629..e228dfca5 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -106,8 +106,6 @@ public: template Subscriber * subscribe(const struct orb_metadata *meta, std::function callback) { - // Subscriber * subscribe(const struct orb_metadata *meta, std::function callback) { - // Subscriber * subscribe(const struct orb_metadata *meta, CallbackFunction callback) { unsigned interval = 0;//XXX decide how to wrap this, ros equivalent? //XXX Subscriber *sub = new SubscriberPX4(meta, interval, callback, &_subs); @@ -124,7 +122,9 @@ public: void spinOnce(); void spin() { - //XXX: call callbacks and do not return until task is terminated + while (true) { //XXX + spinOnce(); + } } private: static const uint16_t kMaxSubscriptions = 100; diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 71c3a766d..efec8f2a3 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -80,11 +80,10 @@ public: SubscriberPX4(const struct orb_metadata *meta, unsigned interval, std::function callback, - // std::function callback, - // CallbackFunction callback, List * list) : Subscriber(), - uORB::Subscription(meta, interval, list) + uORB::Subscription(meta, interval, list), + _callback(callback) //XXX store callback {} ~SubscriberPX4() {}; @@ -94,13 +93,11 @@ public: uORB::Subscription::update(); /* Call callback which performs actions based on this data */ - // _callback(); + _callback(uORB::Subscription::getData()); }; private: - // std::function _callback; - // CallbackFunction _callback; - std::function _callback; + std::function _callback; }; #endif -- cgit v1.2.3 From b0cfc2d1226c2b62a6bf3aac1809458b04902728 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 28 Nov 2014 13:59:32 +0100 Subject: uORB::SubscriptionNode stores interval --- src/modules/uORB/Subscription.hpp | 27 +++++++++++++++++---------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index bab1ef2ea..f82586285 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -58,10 +58,10 @@ public: /** * Constructor * - * @param meta The uORB metadata (usually from the ORB_ID() + * @param meta The uORB metadata (usually from the ORB_ID() * macro) for the topic. * - * @param interval The minimum interval in milliseconds + * @param interval The minimum interval in milliseconds * between updates */ SubscriptionBase(const struct orb_metadata *meta, @@ -126,17 +126,18 @@ public: * Constructor * * - * @param meta The uORB metadata (usually from the ORB_ID() + * @param meta The uORB metadata (usually from the ORB_ID() * macro) for the topic. - * @param interval The minimum interval in milliseconds + * @param interval The minimum interval in milliseconds * between updates - * @param list A pointer to a list of subscriptions + * @param list A pointer to a list of subscriptions * that this should be appended to. */ SubscriptionNode(const struct orb_metadata *meta, unsigned interval=0, List * list=nullptr) : - SubscriptionBase(meta, interval) { + SubscriptionBase(meta, interval), + _interval(interval) { if (list != nullptr) list->add(this); } @@ -145,6 +146,12 @@ public: * updates, a child class must implement it. */ virtual void update() = 0; +// accessors + unsigned getInterval() { return _interval; } +protected: +// attributes + unsigned _interval; + }; /** @@ -159,14 +166,14 @@ public: /** * Constructor * - * @param meta The uORB metadata (usually from + * @param meta The uORB metadata (usually from * the ORB_ID() macro) for the topic. - * @param interval The minimum interval in milliseconds + * @param interval The minimum interval in milliseconds * between updates - * @param list A list interface for adding to + * @param list A list interface for adding to * list during construction */ - Subscription(const struct orb_metadata *meta, + Subscription(const struct orb_metadata *meta, unsigned interval=0, List * list=nullptr); /** -- cgit v1.2.3 From 1b416a8e1f2b54e183d61a0022bb48de47576a6c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 28 Nov 2014 14:00:02 +0100 Subject: use interval setting correctly, improve px4::spin --- src/examples/subscriber/subscriber.cpp | 11 +--------- src/platforms/px4_defines.h | 4 ++-- src/platforms/px4_nodehandle.h | 40 +++++++++++++++++++++++++++------- 3 files changed, 35 insertions(+), 20 deletions(-) diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index fa863463b..ed0635f18 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -81,16 +81,7 @@ PX4_MAIN_FUNCTION(subscriber) * is the number of messages that will be buffered up before beginning to throw * away the oldest ones. */ - // n.subscribe(PX4_TOPIC(rc_channels), [&](const PX4_TOPIC_T(rc_channels) &msg){rc_channels_callback(msg);}); - // n.subscribe(PX4_TOPIC(rc_channels), [&](int i){ return rc_channels_callback(i);}); - // CallbackFunction cbf = [](int i){ return rc_channels_callback(i);}; - // std::function cbf = [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);}; - // n.subscribe(PX4_TOPIC(rc_channels), cbf); - // n.subscribe(PX4_TOPIC(rc_channels), - // [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);}); - // n.subscribe(PX4_TOPIC(rc_channels), - // [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);}); - PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback); + PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, 1000); PX4_INFO("subscribed"); /** diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 79f305140..7efe13bae 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -50,7 +50,7 @@ #define PX4_INFO ROS_INFO #define PX4_TOPIC(_name) #_name #define PX4_TOPIC_T(_name) _name -#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); +#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); #else /* @@ -65,6 +65,6 @@ #define PX4_INFO warnx #define PX4_TOPIC(_name) ORB_ID(_name) #define PX4_TOPIC_T(_name) _name##_s -#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf) _nodehandle.subscribe(PX4_TOPIC(_name), [](const PX4_TOPIC_T(_name)& msg){ return _cbf(msg);}) +#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), [](const PX4_TOPIC_T(_name)& msg){ return _cbf(msg);}, _interval) #endif diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index e228dfca5..ced844fd5 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -49,6 +49,7 @@ #include #else /* includes when building for NuttX */ +#include #endif namespace px4 @@ -99,17 +100,23 @@ class __EXPORT NodeHandle public: NodeHandle() : _subs(), - _pubs() + _pubs(), + _sub_min_interval(nullptr) {} ~NodeHandle() {}; template - Subscriber * subscribe(const struct orb_metadata *meta, std::function callback) { - unsigned interval = 0;//XXX decide how to wrap this, ros equivalent? - //XXX - Subscriber *sub = new SubscriberPX4(meta, interval, callback, &_subs); - return sub; + Subscriber * subscribe(const struct orb_metadata *meta, + std::function callback, + unsigned interval) { + SubscriberPX4 *sub_px4 = new SubscriberPX4(meta, interval, callback, &_subs); + + /* Check if this is the smallest interval so far and update _sub_min_interval */ + if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() < sub_px4->getInterval()) { + _sub_min_interval = sub_px4; + } + return (Subscriber*)sub_px4; } template @@ -122,7 +129,23 @@ public: void spinOnce(); void spin() { - while (true) { //XXX + while (true) { //XXX check for termination + const int timeout_ms = 100; + /* Only continue in the loop if the nodehandle has subscriptions */ + if (_sub_min_interval == nullptr) { + usleep(timeout_ms * 1000); + continue; + } + + /* Poll fd with smallest interval */ + struct pollfd pfd; + pfd.fd = _sub_min_interval->getHandle(); + pfd.events = POLLIN; + if (poll(&pfd, 1, timeout_ms) <= 0) { + /* timed out */ + continue; + } + spinOnce(); } } @@ -130,7 +153,8 @@ private: static const uint16_t kMaxSubscriptions = 100; List _subs; List _pubs; - + uORB::SubscriptionNode* _sub_min_interval; /**< Points to the sub wtih the smallest interval + of all Subscriptions in _subs*/ }; #endif } -- cgit v1.2.3 From bc4209681c39fc934defcff318221e2c17c1ddb8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 28 Nov 2014 14:05:24 +0100 Subject: remove comment --- src/platforms/px4_middleware.h | 27 --------------------------- 1 file changed, 27 deletions(-) diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index 8a83f1af4..fa07b3766 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -65,30 +65,3 @@ private: }; extern bool task_should_exit; - -// /** -// * A limiter/ saturation. -// * The output of update is the input, bounded -// * by min/max. -// */ -// class __EXPORT BlockLimit : public Block -// { -// public: -// // methods -// BlockLimit(SuperBlock *parent, const char *name) : -// Block(parent, name), -// _min(this, "MIN"), -// _max(this, "MAX") -// {}; -// virtual ~BlockLimit() {}; -// float update(float input); -// // accessors -// float getMin() { return _min.get(); } -// float getMax() { return _max.get(); } -// protected: -// // attributes -// control::BlockParamFloat _min; -// control::BlockParamFloat _max; -// }; - -} // namespace px4 -- cgit v1.2.3 From cefccc0037f18275b8d8c7e49e15d13801ef28a1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 28 Nov 2014 14:18:30 +0100 Subject: re-add accidentally deleted line from commit bc4209681c39fc934defcff318221e2c17c1ddb8 --- src/platforms/px4_middleware.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index fa07b3766..462a06e97 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -65,3 +65,5 @@ private: }; extern bool task_should_exit; + +} // namespace px4 -- cgit v1.2.3 From a9c1e4ad6145485805366fad5c08ae7351886ff3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 28 Nov 2014 16:08:51 +0100 Subject: make px4::ok work, use it in px4::spin --- CMakeLists.txt | 3 --- src/platforms/nuttx/module.mk | 5 +---- src/platforms/nuttx/px4_nodehandle.cpp | 19 ------------------- src/platforms/nuttx/px4_nuttx_impl.cpp | 5 ----- src/platforms/px4_middleware.h | 9 ++++++--- src/platforms/px4_nodehandle.h | 19 +++++++++++++++++-- src/platforms/ros/px4_ros_impl.cpp | 5 ----- 7 files changed, 24 insertions(+), 41 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8eb81c92c..610284abe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -102,9 +102,6 @@ include_directories( ## Declare a cpp library add_library(px4 src/platforms/ros/px4_ros_impl.cpp - src/platforms/ros/px4_publisher.cpp - src/platforms/ros/px4_subscriber.cpp - src/platforms/ros/px4_nodehandle.cpp ) target_link_libraries(px4 diff --git a/src/platforms/nuttx/module.mk b/src/platforms/nuttx/module.mk index 1c0ad7aa4..4a2aff824 100644 --- a/src/platforms/nuttx/module.mk +++ b/src/platforms/nuttx/module.mk @@ -35,9 +35,6 @@ # NuttX / uORB adapter library # -SRCS = px4_nuttx_impl.cpp \ - px4_publisher.cpp \ - px4_subscriber.cpp \ - px4_nodehandle.cpp +SRCS = px4_nuttx_impl.cpp MAXOPTIMIZATION = -Os diff --git a/src/platforms/nuttx/px4_nodehandle.cpp b/src/platforms/nuttx/px4_nodehandle.cpp index 7930a0680..ec557e8aa 100644 --- a/src/platforms/nuttx/px4_nodehandle.cpp +++ b/src/platforms/nuttx/px4_nodehandle.cpp @@ -37,27 +37,8 @@ * PX4 Middleware Wrapper Nodehandle */ #include -#include namespace px4 { -bool task_should_exit = false; - - -void NodeHandle::spinOnce() { - /* Loop through subscriptions, call callback for updated subscriptions */ - uORB::SubscriptionNode *sub = _subs.getHead(); - int count = 0; - - while (sub != nullptr) { - if (count++ > kMaxSubscriptions) { - PX4_WARN("exceeded max subscriptions"); - break; - } - - sub->update(); - sub = sub->getSibling(); - } -} } diff --git a/src/platforms/nuttx/px4_nuttx_impl.cpp b/src/platforms/nuttx/px4_nuttx_impl.cpp index 22daee823..70e292320 100644 --- a/src/platforms/nuttx/px4_nuttx_impl.cpp +++ b/src/platforms/nuttx/px4_nuttx_impl.cpp @@ -54,9 +54,4 @@ uint64_t get_time_micros() return hrt_absolute_time(); } -bool ok() -{ - return !task_should_exit; -} - } diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index 462a06e97..dece72907 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -49,7 +49,12 @@ __EXPORT void init(int argc, char *argv[], const char *process_name); __EXPORT uint64_t get_time_micros(); -__EXPORT bool ok(); +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +bool ok() { return ros::ok(); } +#else +extern bool task_should_exit; +bool ok() { return !task_should_exit; } +#endif class Rate { @@ -64,6 +69,4 @@ private: }; -extern bool task_should_exit; - } // namespace px4 diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index ced844fd5..eb90590e4 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -41,6 +41,7 @@ /* includes for all platforms */ #include "px4_subscriber.h" #include "px4_publisher.h" +#include "px4_middleware.h" #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) /* includes when building for ros */ @@ -126,10 +127,24 @@ public: return pub; } - void spinOnce(); + void spinOnce() { + /* Loop through subscriptions, call callback for updated subscriptions */ + uORB::SubscriptionNode *sub = _subs.getHead(); + int count = 0; + + while (sub != nullptr) { + if (count++ > kMaxSubscriptions) { + PX4_WARN("exceeded max subscriptions"); + break; + } + + sub->update(); + sub = sub->getSibling(); + } + } void spin() { - while (true) { //XXX check for termination + while (ok()) { const int timeout_ms = 100; /* Only continue in the loop if the nodehandle has subscriptions */ if (_sub_min_interval == nullptr) { diff --git a/src/platforms/ros/px4_ros_impl.cpp b/src/platforms/ros/px4_ros_impl.cpp index 3aa976138..854986a7f 100644 --- a/src/platforms/ros/px4_ros_impl.cpp +++ b/src/platforms/ros/px4_ros_impl.cpp @@ -53,9 +53,4 @@ uint64_t get_time_micros() return time.sec * 1e6 + time.nsec / 1000; } -bool ok() -{ - return ros::ok(); -} - } -- cgit v1.2.3 From 9abc8e26b789af0ef132c5c38e3d8ada821c3657 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 28 Nov 2014 16:30:12 +0100 Subject: correctly handle interval, call callback only when topic updated, add example for 2 topics --- src/examples/subscriber/subscriber.cpp | 11 ++++++----- src/platforms/px4_nodehandle.h | 2 +- src/platforms/px4_subscriber.h | 5 +++++ 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index ed0635f18..a186ba94a 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -36,10 +36,10 @@ void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid); } -// void rc_channels_callback(int i) -// { - // PX4_INFO("I heard: [%d]", i); -// } +void rc_channels_callback2(const PX4_TOPIC_T(rc_channels) &msg) +{ + PX4_INFO("I heard2: [%llu]", msg.timestamp_last_valid); +} namespace px4 { bool task_should_exit = false; @@ -81,7 +81,8 @@ PX4_MAIN_FUNCTION(subscriber) * is the number of messages that will be buffered up before beginning to throw * away the oldest ones. */ - PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, 1000); + PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, 100); + PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback2, 1000); PX4_INFO("subscribed"); /** diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index eb90590e4..45ef225fa 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -114,7 +114,7 @@ public: SubscriberPX4 *sub_px4 = new SubscriberPX4(meta, interval, callback, &_subs); /* Check if this is the smallest interval so far and update _sub_min_interval */ - if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() < sub_px4->getInterval()) { + if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { _sub_min_interval = sub_px4; } return (Subscriber*)sub_px4; diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index efec8f2a3..6e0ef8aed 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -89,6 +89,11 @@ public: ~SubscriberPX4() {}; void update() { + if (!uORB::Subscription::updated()) { + /* Topic not updated, do not call callback */ + return; + } + /* get latest data */ uORB::Subscription::update(); -- cgit v1.2.3 From caa61a4fdc7898987da7a03e1924ced8962bb92c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 28 Nov 2014 23:09:45 +0100 Subject: add support for subcription method callbacks for ros and nuttx --- src/examples/subscriber/subscriber.cpp | 30 +++++++++++++++++++++++------- src/platforms/px4_defines.h | 15 ++++++++------- src/platforms/px4_nodehandle.h | 9 +++++++++ 3 files changed, 40 insertions(+), 14 deletions(-) diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index a186ba94a..3d54a1a2f 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -32,21 +32,29 @@ using namespace px4; /** * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. */ -void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) -{ +void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid); } -void rc_channels_callback2(const PX4_TOPIC_T(rc_channels) &msg) -{ - PX4_INFO("I heard2: [%llu]", msg.timestamp_last_valid); +void rc_channels_callback2(const PX4_TOPIC_T(rc_channels) &msg) { + PX4_INFO("I heard(2): [%llu]", msg.timestamp_last_valid); } + +class RCHandler { +public: + void callback(const PX4_TOPIC_T(rc_channels) &msg) { + PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp_last_valid); + } +}; + +RCHandler rchandler; + + namespace px4 { bool task_should_exit = false; } -PX4_MAIN_FUNCTION(subscriber) -{ +PX4_MAIN_FUNCTION(subscriber) { /** * The ros::init() function needs to see argc and argv so that it can perform * any ROS arguments and name remapping that were provided at the command line. For programmatic @@ -83,6 +91,14 @@ PX4_MAIN_FUNCTION(subscriber) */ PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, 100); PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback2, 1000); + //1 + // PX4_SUBSCRIBE(n, rc_channels, callee.rc_channels_callback, , 1000); + //2 + // PX4_SUBSCRIBE(n, rc_channels, rchandler.callback, &rchandler, 1000); + //3 for bind + PX4_SUBSCRIBE(n, rc_channels, RCHandler::callback, rchandler, 1000); + // ros::NodeHandle n2; + // n2.subscribe("chatter", 1000, &RCHandler::callback, &rchandler); PX4_INFO("subscribed"); /** diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 7efe13bae..f6679d01b 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -44,27 +44,28 @@ * Building for running within the ROS environment */ #define __EXPORT -// #define PX4_MAIN_FUNCTION(_prefix) #define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv) #define PX4_WARN ROS_WARN #define PX4_INFO ROS_INFO #define PX4_TOPIC(_name) #_name #define PX4_TOPIC_T(_name) _name -#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); +#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj); +#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); #else /* * Building for NuttX */ - -// #define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##_main(int argc, char **argv)() { return main(argc, argv); } -// #define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) { return main(argc, argv); } #define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) #define PX4_WARN warnx #define PX4_WARN warnx #define PX4_INFO warnx #define PX4_TOPIC(_name) ORB_ID(_name) #define PX4_TOPIC_T(_name) _name##_s -#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), [](const PX4_TOPIC_T(_name)& msg){ return _cbf(msg);}, _interval) - +#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval) +#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) #endif + +/* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */ +#define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME +#define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC)(__VA_ARGS__) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 45ef225fa..415351756 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -71,6 +71,7 @@ public: //XXX empty lists }; + /* Constructor with callback to function */ template Subscriber * subscribe(const char *topic, void(*fp)(M)) { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp); @@ -78,6 +79,14 @@ public: _subs.push_back(sub); return sub; } + /* Constructor with callback to class method */ + template + Subscriber * subscribe(const char *topic, void(T::*fp)(M), T *obj) { + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj); + Subscriber * sub = new Subscriber(ros_sub); + _subs.push_back(sub); + return sub; + } template Publisher * advertise(const char *topic) { -- cgit v1.2.3 From 6b695ac9e8be9e7fe480238c967316366cba444c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 28 Nov 2014 23:14:50 +0100 Subject: add PX4 advertise macro --- src/examples/publisher/publisher.cpp | 2 +- src/platforms/px4_defines.h | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp index 5c7156c08..8ef147493 100644 --- a/src/examples/publisher/publisher.cpp +++ b/src/examples/publisher/publisher.cpp @@ -62,7 +62,7 @@ PX4_MAIN_FUNCTION(publisher) * than we can send them, the number here specifies how many messages to * buffer up before throwing some away. */ - px4::Publisher * rc_channels_pub = n.advertise(PX4_TOPIC(rc_channels)); + px4::Publisher * rc_channels_pub = PX4_ADVERTISE(n, rc_channels); px4::Rate loop_rate(10); diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index f6679d01b..84e5c8da0 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -69,3 +69,4 @@ /* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */ #define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME #define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC)(__VA_ARGS__) +#define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise(PX4_TOPIC(_name)) -- cgit v1.2.3 From 8b5bc703a11805cada41e06b5e2327d0796ec0e5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 1 Dec 2014 16:39:27 +0100 Subject: initial version of msg to uorb script Standard and embedded types work, may need small refinements for some types --- .gitignore | 1 + Makefile | 20 +++-- Tools/px_generate_uorb_topic_headers.py | 98 ++++++++++++++++++++++ msg/px4_msgs/actuator_armed.msg | 6 ++ msg/px4_msgs/rc_channels.msg | 24 ++++++ msg/rc_channels.msg | 8 -- msg/std_msgs/Bool.msg | 1 + msg/std_msgs/Byte.msg | 1 + msg/std_msgs/ByteMultiArray.msg | 6 ++ msg/std_msgs/Char.msg | 1 + msg/std_msgs/ColorRGBA.msg | 4 + msg/std_msgs/Duration.msg | 1 + msg/std_msgs/Empty.msg | 0 msg/std_msgs/Float32.msg | 1 + msg/std_msgs/Float32MultiArray.msg | 6 ++ msg/std_msgs/Float64.msg | 1 + msg/std_msgs/Float64MultiArray.msg | 6 ++ msg/std_msgs/Header.msg | 15 ++++ msg/std_msgs/Int16.msg | 1 + msg/std_msgs/Int16MultiArray.msg | 6 ++ msg/std_msgs/Int32.msg | 1 + msg/std_msgs/Int32MultiArray.msg | 6 ++ msg/std_msgs/Int64.msg | 1 + msg/std_msgs/Int64MultiArray.msg | 6 ++ msg/std_msgs/Int8.msg | 1 + msg/std_msgs/Int8MultiArray.msg | 6 ++ msg/std_msgs/MultiArrayDimension.msg | 3 + msg/std_msgs/MultiArrayLayout.msg | 26 ++++++ msg/std_msgs/String.msg | 1 + msg/std_msgs/Time.msg | 1 + msg/std_msgs/UInt16.msg | 1 + msg/std_msgs/UInt16MultiArray.msg | 6 ++ msg/std_msgs/UInt32.msg | 1 + msg/std_msgs/UInt32MultiArray.msg | 6 ++ msg/std_msgs/UInt64.msg | 1 + msg/std_msgs/UInt64MultiArray.msg | 6 ++ msg/std_msgs/UInt8.msg | 1 + msg/std_msgs/UInt8MultiArray.msg | 6 ++ msg/templates/msg.h.template | 141 ++++++++++++++++++++++++++++++++ 39 files changed, 415 insertions(+), 13 deletions(-) create mode 100755 Tools/px_generate_uorb_topic_headers.py create mode 100644 msg/px4_msgs/actuator_armed.msg create mode 100644 msg/px4_msgs/rc_channels.msg delete mode 100644 msg/rc_channels.msg create mode 100644 msg/std_msgs/Bool.msg create mode 100644 msg/std_msgs/Byte.msg create mode 100644 msg/std_msgs/ByteMultiArray.msg create mode 100644 msg/std_msgs/Char.msg create mode 100644 msg/std_msgs/ColorRGBA.msg create mode 100644 msg/std_msgs/Duration.msg create mode 100644 msg/std_msgs/Empty.msg create mode 100644 msg/std_msgs/Float32.msg create mode 100644 msg/std_msgs/Float32MultiArray.msg create mode 100644 msg/std_msgs/Float64.msg create mode 100644 msg/std_msgs/Float64MultiArray.msg create mode 100644 msg/std_msgs/Header.msg create mode 100644 msg/std_msgs/Int16.msg create mode 100644 msg/std_msgs/Int16MultiArray.msg create mode 100644 msg/std_msgs/Int32.msg create mode 100644 msg/std_msgs/Int32MultiArray.msg create mode 100644 msg/std_msgs/Int64.msg create mode 100644 msg/std_msgs/Int64MultiArray.msg create mode 100644 msg/std_msgs/Int8.msg create mode 100644 msg/std_msgs/Int8MultiArray.msg create mode 100644 msg/std_msgs/MultiArrayDimension.msg create mode 100644 msg/std_msgs/MultiArrayLayout.msg create mode 100644 msg/std_msgs/String.msg create mode 100644 msg/std_msgs/Time.msg create mode 100644 msg/std_msgs/UInt16.msg create mode 100644 msg/std_msgs/UInt16MultiArray.msg create mode 100644 msg/std_msgs/UInt32.msg create mode 100644 msg/std_msgs/UInt32MultiArray.msg create mode 100644 msg/std_msgs/UInt64.msg create mode 100644 msg/std_msgs/UInt64MultiArray.msg create mode 100644 msg/std_msgs/UInt8.msg create mode 100644 msg/std_msgs/UInt8MultiArray.msg create mode 100644 msg/templates/msg.h.template diff --git a/.gitignore b/.gitignore index 69112ee1f..9d9c3d383 100644 --- a/.gitignore +++ b/.gitignore @@ -39,3 +39,4 @@ tags .pydevproject .ropeproject *.orig +src/modules/uORB/topics/* diff --git a/Makefile b/Makefile index 809f54dd3..7df8004a4 100644 --- a/Makefile +++ b/Makefile @@ -104,13 +104,13 @@ DESIRED_FIRMWARES = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4) STAGED_FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(IMAGE_DIR)$(config).px4) FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4) -all: checksubmodules $(DESIRED_FIRMWARES) +all: checksubmodules generateuorbtopicheaders $(DESIRED_FIRMWARES) # # Copy FIRMWARES into the image directory. # -# XXX copying the .bin files is a hack to work around the PX4IO uploader -# not supporting .px4 files, and it should be deprecated onced that +# XXX copying the .bin files is a hack to work around the PX4IO uploader +# not supporting .px4 files, and it should be deprecated onced that # is taken care of. # $(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4 @@ -152,7 +152,7 @@ $(foreach config,$(FMU_CONFIGS),$(eval $(call FMU_DEP,$(config)))) # Build the NuttX export archives. # # Note that there are no explicit dependencies extended from these -# archives. If NuttX is updated, the user is expected to rebuild the +# archives. If NuttX is updated, the user is expected to rebuild the # archives/build area manually. Likewise, when the 'archives' target is # invoked, all archives are always rebuilt. # @@ -224,6 +224,16 @@ updatesubmodules: $(Q) (git submodule init) $(Q) (git submodule update) +MSG_DIR = $(PX4_BASE)msg/px4_msgs +MSG_TEMPLATE_DIR = $(PX4_BASE)msg/templates +TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics + +.PHONY: generateuorbtopicheaders +generateuorbtopicheaders: + @$(ECHO) "Generating uORB topic headers" + $(Q) ($(PX4_BASE)/Tools/px_generate_uorb_topic_headers.py -d $(MSG_DIR) \ + -o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR)) + # # Testing targets # @@ -232,7 +242,7 @@ testbuild: # # Cleanup targets. 'clean' should remove all built products and force -# a complete re-compilation, 'distclean' should remove everything +# a complete re-compilation, 'distclean' should remove everything # that's generated leaving only files that are in source control. # .PHONY: clean diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py new file mode 100755 index 000000000..a738dcb7e --- /dev/null +++ b/Tools/px_generate_uorb_topic_headers.py @@ -0,0 +1,98 @@ +#!/usr/bin/env python +############################################################################# +# +# 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. +# +############################################################################# + +""" +px_generate_uorb_topic_headers.py +Generates c/cpp header files for uorb topics from .msg (ROS syntax) +message files +""" +from __future__ import print_function +import os +import argparse +import genmsg.template_tools + +__author__ = "Thomas Gubler" +__copyright__ = "Copyright (C) 2013-2014 PX4 Development Team." +__license__ = "BSD" +__email__ = "thomasgubler@gmail.com" + + +msg_template_map = {'msg.h.template': '@NAME@.h'} +srv_template_map = {} +incl_default = ['std_msgs:./msg/std_msgs'] +package = 'px4' + + +def convert_file(filename, outputdir, templatedir, includepath): + """ + Converts a single .msg file to a uorb header + """ + print("Generating uORB headers from {0}".format(filename)) + genmsg.template_tools.generate_from_file(filename, + package, + outputdir, + templatedir, + includepath, + msg_template_map, + srv_template_map) + + +def convert_dir(inputdir, outputdir, templatedir): + """ + Converts all .msg files in inputdir to uORB header files + """ + includepath = incl_default + [':'.join([package, inputdir])] + for f in os.listdir(inputdir): + fn = os.path.join(inputdir, f) + if os.path.isfile(fn): + convert_file(fn, outputdir, templatedir, includepath) + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description='Convert msg files to uorb headers') + parser.add_argument('-d', dest='dir', help='directory with msg files') + parser.add_argument('-f', dest='file', + help="files to convert (use only without -d)", + nargs="+") + parser.add_argument('-e', dest='templatedir', + help='directory with template files',) + parser.add_argument('-o', dest='outputdir', + help='output directory for header files') + args = parser.parse_args() + + if args.file is not None: + for f in args.file: + convert_file(f, args.outputdir, args.templatedir, incl_default) + elif args.dir is not None: + convert_dir(args.dir, args.outputdir, args.templatedir) diff --git a/msg/px4_msgs/actuator_armed.msg b/msg/px4_msgs/actuator_armed.msg new file mode 100644 index 000000000..f6bf58307 --- /dev/null +++ b/msg/px4_msgs/actuator_armed.msg @@ -0,0 +1,6 @@ + +uint64 timestamp # Microseconds since system boot +bool armed # Set to true if system is armed +bool ready_to_arm # Set to true if system is ready to be armed +bool lockdown # Set to true if actuators are forced to being disabled (due to emergency or HIL) +bool force_failsafe # Set to true if the actuators are forced to the failsafe position diff --git a/msg/px4_msgs/rc_channels.msg b/msg/px4_msgs/rc_channels.msg new file mode 100644 index 000000000..aaa6696f9 --- /dev/null +++ b/msg/px4_msgs/rc_channels.msg @@ -0,0 +1,24 @@ +int32 RC_CHANNELS_FUNCTION_MAX=18 +uint8 RC_CHANNELS_FUNCTION_THROTTLE=0 +uint8 RC_CHANNELS_FUNCTION_ROLL=1 +uint8 RC_CHANNELS_FUNCTION_PITCH=2 +uint8 RC_CHANNELS_FUNCTION_YAW=3 +uint8 RC_CHANNELS_FUNCTION_MODE=4 +uint8 RC_CHANNELS_FUNCTION_RETURN=5 +uint8 RC_CHANNELS_FUNCTION_POSCTL=6 +uint8 RC_CHANNELS_FUNCTION_LOITER=7 +uint8 RC_CHANNELS_FUNCTION_OFFBOARD=8 +uint8 RC_CHANNELS_FUNCTION_ACRO=9 +uint8 RC_CHANNELS_FUNCTION_FLAPS=10 +uint8 RC_CHANNELS_FUNCTION_AUX_1=11 +uint8 RC_CHANNELS_FUNCTION_AUX_2=12 +uint8 RC_CHANNELS_FUNCTION_AUX_3=13 +uint8 RC_CHANNELS_FUNCTION_AUX_4=14 +uint8 RC_CHANNELS_FUNCTION_AUX_5=15 +uint64 timestamp_last_valid # Timestamp of last valid RC signal +float32[18] channels # Scaled to -1..1 (throttle: 0..1) +uint8 channel_count # Number of valid channels +int8[18] function # Functions mapping +uint8 rssi # Receive signal strength index +bool signal_lost # Control signal lost, should be checked together with topic timeout +actuator_armed actuator diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg deleted file mode 100644 index 11dc57c42..000000000 --- a/msg/rc_channels.msg +++ /dev/null @@ -1,8 +0,0 @@ -Header header -int32 RC_CHANNELS_FUNCTION_MAX=18 -uint64 timestamp_last_valid # Timestamp of last valid RC signal -float32[18] channels # Scaled to -1..1 (throttle: 0..1) -uint8 channel_count # Number of valid channels -int8[18] function # Functions mapping -uint8 rssi # Receive signal strength index -bool signal_lost # Control signal lost, should be checked together with topic timeout diff --git a/msg/std_msgs/Bool.msg b/msg/std_msgs/Bool.msg new file mode 100644 index 000000000..f7cabb94f --- /dev/null +++ b/msg/std_msgs/Bool.msg @@ -0,0 +1 @@ +bool data \ No newline at end of file diff --git a/msg/std_msgs/Byte.msg b/msg/std_msgs/Byte.msg new file mode 100644 index 000000000..d993b3455 --- /dev/null +++ b/msg/std_msgs/Byte.msg @@ -0,0 +1 @@ +byte data diff --git a/msg/std_msgs/ByteMultiArray.msg b/msg/std_msgs/ByteMultiArray.msg new file mode 100644 index 000000000..bb00bd348 --- /dev/null +++ b/msg/std_msgs/ByteMultiArray.msg @@ -0,0 +1,6 @@ +# Please look at the MultiArrayLayout message definition for +# documentation on all multiarrays. + +MultiArrayLayout layout # specification of data layout +byte[] data # array of data + diff --git a/msg/std_msgs/Char.msg b/msg/std_msgs/Char.msg new file mode 100644 index 000000000..39a1d46a9 --- /dev/null +++ b/msg/std_msgs/Char.msg @@ -0,0 +1 @@ +char data \ No newline at end of file diff --git a/msg/std_msgs/ColorRGBA.msg b/msg/std_msgs/ColorRGBA.msg new file mode 100644 index 000000000..182dbc834 --- /dev/null +++ b/msg/std_msgs/ColorRGBA.msg @@ -0,0 +1,4 @@ +float32 r +float32 g +float32 b +float32 a diff --git a/msg/std_msgs/Duration.msg b/msg/std_msgs/Duration.msg new file mode 100644 index 000000000..f13931ec8 --- /dev/null +++ b/msg/std_msgs/Duration.msg @@ -0,0 +1 @@ +duration data diff --git a/msg/std_msgs/Empty.msg b/msg/std_msgs/Empty.msg new file mode 100644 index 000000000..e69de29bb diff --git a/msg/std_msgs/Float32.msg b/msg/std_msgs/Float32.msg new file mode 100644 index 000000000..e89740534 --- /dev/null +++ b/msg/std_msgs/Float32.msg @@ -0,0 +1 @@ +float32 data \ No newline at end of file diff --git a/msg/std_msgs/Float32MultiArray.msg b/msg/std_msgs/Float32MultiArray.msg new file mode 100644 index 000000000..915830846 --- /dev/null +++ b/msg/std_msgs/Float32MultiArray.msg @@ -0,0 +1,6 @@ +# Please look at the MultiArrayLayout message definition for +# documentation on all multiarrays. + +MultiArrayLayout layout # specification of data layout +float32[] data # array of data + diff --git a/msg/std_msgs/Float64.msg b/msg/std_msgs/Float64.msg new file mode 100644 index 000000000..cd09d39b8 --- /dev/null +++ b/msg/std_msgs/Float64.msg @@ -0,0 +1 @@ +float64 data \ No newline at end of file diff --git a/msg/std_msgs/Float64MultiArray.msg b/msg/std_msgs/Float64MultiArray.msg new file mode 100644 index 000000000..0a13b928f --- /dev/null +++ b/msg/std_msgs/Float64MultiArray.msg @@ -0,0 +1,6 @@ +# Please look at the MultiArrayLayout message definition for +# documentation on all multiarrays. + +MultiArrayLayout layout # specification of data layout +float64[] data # array of data + diff --git a/msg/std_msgs/Header.msg b/msg/std_msgs/Header.msg new file mode 100644 index 000000000..f90d622ea --- /dev/null +++ b/msg/std_msgs/Header.msg @@ -0,0 +1,15 @@ +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +# 0: no frame +# 1: global frame +string frame_id diff --git a/msg/std_msgs/Int16.msg b/msg/std_msgs/Int16.msg new file mode 100644 index 000000000..c4389faf7 --- /dev/null +++ b/msg/std_msgs/Int16.msg @@ -0,0 +1 @@ +int16 data diff --git a/msg/std_msgs/Int16MultiArray.msg b/msg/std_msgs/Int16MultiArray.msg new file mode 100644 index 000000000..d2ddea1d1 --- /dev/null +++ b/msg/std_msgs/Int16MultiArray.msg @@ -0,0 +1,6 @@ +# Please look at the MultiArrayLayout message definition for +# documentation on all multiarrays. + +MultiArrayLayout layout # specification of data layout +int16[] data # array of data + diff --git a/msg/std_msgs/Int32.msg b/msg/std_msgs/Int32.msg new file mode 100644 index 000000000..0ecfe35f5 --- /dev/null +++ b/msg/std_msgs/Int32.msg @@ -0,0 +1 @@ +int32 data \ No newline at end of file diff --git a/msg/std_msgs/Int32MultiArray.msg b/msg/std_msgs/Int32MultiArray.msg new file mode 100644 index 000000000..af60abda3 --- /dev/null +++ b/msg/std_msgs/Int32MultiArray.msg @@ -0,0 +1,6 @@ +# Please look at the MultiArrayLayout message definition for +# documentation on all multiarrays. + +MultiArrayLayout layout # specification of data layout +int32[] data # array of data + diff --git a/msg/std_msgs/Int64.msg b/msg/std_msgs/Int64.msg new file mode 100644 index 000000000..6961e00f5 --- /dev/null +++ b/msg/std_msgs/Int64.msg @@ -0,0 +1 @@ +int64 data \ No newline at end of file diff --git a/msg/std_msgs/Int64MultiArray.msg b/msg/std_msgs/Int64MultiArray.msg new file mode 100644 index 000000000..f4f35e171 --- /dev/null +++ b/msg/std_msgs/Int64MultiArray.msg @@ -0,0 +1,6 @@ +# Please look at the MultiArrayLayout message definition for +# documentation on all multiarrays. + +MultiArrayLayout layout # specification of data layout +int64[] data # array of data + diff --git a/msg/std_msgs/Int8.msg b/msg/std_msgs/Int8.msg new file mode 100644 index 000000000..1e42e554f --- /dev/null +++ b/msg/std_msgs/Int8.msg @@ -0,0 +1 @@ +int8 data diff --git a/msg/std_msgs/Int8MultiArray.msg b/msg/std_msgs/Int8MultiArray.msg new file mode 100644 index 000000000..a59a37259 --- /dev/null +++ b/msg/std_msgs/Int8MultiArray.msg @@ -0,0 +1,6 @@ +# Please look at the MultiArrayLayout message definition for +# documentation on all multiarrays. + +MultiArrayLayout layout # specification of data layout +int8[] data # array of data + diff --git a/msg/std_msgs/MultiArrayDimension.msg b/msg/std_msgs/MultiArrayDimension.msg new file mode 100644 index 000000000..08240462c --- /dev/null +++ b/msg/std_msgs/MultiArrayDimension.msg @@ -0,0 +1,3 @@ +string label # label of given dimension +uint32 size # size of given dimension (in type units) +uint32 stride # stride of given dimension \ No newline at end of file diff --git a/msg/std_msgs/MultiArrayLayout.msg b/msg/std_msgs/MultiArrayLayout.msg new file mode 100644 index 000000000..5437f8542 --- /dev/null +++ b/msg/std_msgs/MultiArrayLayout.msg @@ -0,0 +1,26 @@ +# The multiarray declares a generic multi-dimensional array of a +# particular data type. Dimensions are ordered from outer most +# to inner most. + +MultiArrayDimension[] dim # Array of dimension properties +uint32 data_offset # padding bytes at front of data + +# Accessors should ALWAYS be written in terms of dimension stride +# and specified outer-most dimension first. +# +# multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k] +# +# A standard, 3-channel 640x480 image with interleaved color channels +# would be specified as: +# +# dim[0].label = "height" +# dim[0].size = 480 +# dim[0].stride = 3*640*480 = 921600 (note dim[0] stride is just size of image) +# dim[1].label = "width" +# dim[1].size = 640 +# dim[1].stride = 3*640 = 1920 +# dim[2].label = "channel" +# dim[2].size = 3 +# dim[2].stride = 3 +# +# multiarray(i,j,k) refers to the ith row, jth column, and kth channel. \ No newline at end of file diff --git a/msg/std_msgs/String.msg b/msg/std_msgs/String.msg new file mode 100644 index 000000000..ae721739e --- /dev/null +++ b/msg/std_msgs/String.msg @@ -0,0 +1 @@ +string data diff --git a/msg/std_msgs/Time.msg b/msg/std_msgs/Time.msg new file mode 100644 index 000000000..7f8f72171 --- /dev/null +++ b/msg/std_msgs/Time.msg @@ -0,0 +1 @@ +time data diff --git a/msg/std_msgs/UInt16.msg b/msg/std_msgs/UInt16.msg new file mode 100644 index 000000000..87d0c44eb --- /dev/null +++ b/msg/std_msgs/UInt16.msg @@ -0,0 +1 @@ +uint16 data diff --git a/msg/std_msgs/UInt16MultiArray.msg b/msg/std_msgs/UInt16MultiArray.msg new file mode 100644 index 000000000..f38970b65 --- /dev/null +++ b/msg/std_msgs/UInt16MultiArray.msg @@ -0,0 +1,6 @@ +# Please look at the MultiArrayLayout message definition for +# documentation on all multiarrays. + +MultiArrayLayout layout # specification of data layout +uint16[] data # array of data + diff --git a/msg/std_msgs/UInt32.msg b/msg/std_msgs/UInt32.msg new file mode 100644 index 000000000..b6c696b42 --- /dev/null +++ b/msg/std_msgs/UInt32.msg @@ -0,0 +1 @@ +uint32 data \ No newline at end of file diff --git a/msg/std_msgs/UInt32MultiArray.msg b/msg/std_msgs/UInt32MultiArray.msg new file mode 100644 index 000000000..b2bb0771f --- /dev/null +++ b/msg/std_msgs/UInt32MultiArray.msg @@ -0,0 +1,6 @@ +# Please look at the MultiArrayLayout message definition for +# documentation on all multiarrays. + +MultiArrayLayout layout # specification of data layout +uint32[] data # array of data + diff --git a/msg/std_msgs/UInt64.msg b/msg/std_msgs/UInt64.msg new file mode 100644 index 000000000..2eb1afad3 --- /dev/null +++ b/msg/std_msgs/UInt64.msg @@ -0,0 +1 @@ +uint64 data \ No newline at end of file diff --git a/msg/std_msgs/UInt64MultiArray.msg b/msg/std_msgs/UInt64MultiArray.msg new file mode 100644 index 000000000..30d0cd928 --- /dev/null +++ b/msg/std_msgs/UInt64MultiArray.msg @@ -0,0 +1,6 @@ +# Please look at the MultiArrayLayout message definition for +# documentation on all multiarrays. + +MultiArrayLayout layout # specification of data layout +uint64[] data # array of data + diff --git a/msg/std_msgs/UInt8.msg b/msg/std_msgs/UInt8.msg new file mode 100644 index 000000000..5eefd870d --- /dev/null +++ b/msg/std_msgs/UInt8.msg @@ -0,0 +1 @@ +uint8 data diff --git a/msg/std_msgs/UInt8MultiArray.msg b/msg/std_msgs/UInt8MultiArray.msg new file mode 100644 index 000000000..31f7d6a21 --- /dev/null +++ b/msg/std_msgs/UInt8MultiArray.msg @@ -0,0 +1,6 @@ +# Please look at the MultiArrayLayout message definition for +# documentation on all multiarrays. + +MultiArrayLayout layout # specification of data layout +uint8[] data # array of data + diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template new file mode 100644 index 000000000..d05282c88 --- /dev/null +++ b/msg/templates/msg.h.template @@ -0,0 +1,141 @@ +@############################################### +@# +@# PX4 ROS compatible message source code +@# generation for C++ +@# +@# EmPy template for generating .h files +@# Based on the original template for ROS +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - file_name_in (String) Source file +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@# - md5sum (String) MD5Sum of the .msg specification +@############################################### +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + + /* Auto-generated by genmsg_cpp from file @file_name_in */ + +@{ +import genmsg.msgs +import gencpp + +cpp_namespace = '::%s::'%(spec.package) # TODO handle nested namespace +cpp_class = '%s_'%spec.short_name +cpp_full_name = '%s%s'%(cpp_namespace,cpp_class) +cpp_full_name_with_alloc = '%s'%(cpp_full_name) +cpp_msg_definition = gencpp.escape_message_definition(msg_definition) +}@ + +#pragma once + +@############################## +@# Generic Includes +@############################## +#include +#include "../uORB.h" + +@############################## +@# Includes for dependencies +@############################## +@{ +for field in spec.parsed_fields(): + if (not field.is_builtin): + if (not field.is_header): + (package, name) = genmsg.names.package_resource_name(field.base_type) + package = package or spec.package # convert '' to package + print('#include '%(name)) + +}@ +@# Constants +@[for constant in spec.constants]@ +#define @(constant.name) = @(int(constant.val)) +@[end for] + +/** + * @@addtogroup topics + * @@{ + */ + +@############################## +@# Main struct of message +@############################## +@{ + +inttypes = ['int8', 'int16', 'int32', 'int64', 'uint8', 'uint16', 'uint32', 'uint64'] + +# Function to print a standard ros type +def print_field_def(field): + type = field.type + # detect embedded types + sl_pos = type.find('/') + type_appendix = '' + type_prefix = '' + if (sl_pos >= 0): + type = type[sl_pos + 1:] + type_prefix = 'struct ' + type_appendix = '_s' + + # detect arrays + a_pos = type.find('[') + array_size = '' + if (a_pos >= 0): + # field is array + array_size = type[a_pos:] + type = type[:a_pos] + + if type in inttypes: + # need to add _t: int8 --> int8_t + type_appendix = '_t' + + print('\t%s%s%s%s %s;'%(type_prefix, type, type_appendix, array_size, field.name)) + +} +struct @(spec.short_name)_s +@{ +# loop over all fields and print the type and name +for field in spec.parsed_fields(): + if (not field.is_header): + print_field_def(field) +}@ +} + +/** + * @@} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(@(spec.short_name)); -- cgit v1.2.3 From 6a2fcb8874ba996fb7f5914376197a38b7e73459 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 1 Dec 2014 17:44:15 +0100 Subject: move (for now) unused msg file --- msg/px4_msgs/actuator_armed.msg | 6 ------ msg/px4_msgs/unused/actuator_armed.msg | 6 ++++++ 2 files changed, 6 insertions(+), 6 deletions(-) delete mode 100644 msg/px4_msgs/actuator_armed.msg create mode 100644 msg/px4_msgs/unused/actuator_armed.msg diff --git a/msg/px4_msgs/actuator_armed.msg b/msg/px4_msgs/actuator_armed.msg deleted file mode 100644 index f6bf58307..000000000 --- a/msg/px4_msgs/actuator_armed.msg +++ /dev/null @@ -1,6 +0,0 @@ - -uint64 timestamp # Microseconds since system boot -bool armed # Set to true if system is armed -bool ready_to_arm # Set to true if system is ready to be armed -bool lockdown # Set to true if actuators are forced to being disabled (due to emergency or HIL) -bool force_failsafe # Set to true if the actuators are forced to the failsafe position diff --git a/msg/px4_msgs/unused/actuator_armed.msg b/msg/px4_msgs/unused/actuator_armed.msg new file mode 100644 index 000000000..f6bf58307 --- /dev/null +++ b/msg/px4_msgs/unused/actuator_armed.msg @@ -0,0 +1,6 @@ + +uint64 timestamp # Microseconds since system boot +bool armed # Set to true if system is armed +bool ready_to_arm # Set to true if system is ready to be armed +bool lockdown # Set to true if actuators are forced to being disabled (due to emergency or HIL) +bool force_failsafe # Set to true if the actuators are forced to the failsafe position -- cgit v1.2.3 From 2eeeab8ecd04cdbf4ef80161a1708c140a9d8ba6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 1 Dec 2014 17:45:10 +0100 Subject: improve msg template file --- msg/templates/msg.h.template | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template index d05282c88..f60001b55 100644 --- a/msg/templates/msg.h.template +++ b/msg/templates/msg.h.template @@ -95,7 +95,16 @@ for field in spec.parsed_fields(): @############################## @{ -inttypes = ['int8', 'int16', 'int32', 'int64', 'uint8', 'uint16', 'uint32', 'uint64'] +type_map = {'int8': 'int8_t', + 'int16': 'int16_t', + 'int32': 'int32_t', + 'int64': 'int64_t', + 'uint8': 'uint8_t', + 'uint16': 'uint16_t', + 'uint32': 'uint32_t', + 'uint64': 'uint64_t', + 'float32': 'float', + 'bool': 'bool'} # Function to print a standard ros type def print_field_def(field): @@ -117,21 +126,23 @@ def print_field_def(field): array_size = type[a_pos:] type = type[:a_pos] - if type in inttypes: + if type in type_map: # need to add _t: int8 --> int8_t - type_appendix = '_t' + type_px4 = type_map[type] + else: + raise Exception("Type {0} not supported, add to to template file!".format(type)) - print('\t%s%s%s%s %s;'%(type_prefix, type, type_appendix, array_size, field.name)) + print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size)) } -struct @(spec.short_name)_s +struct @(spec.short_name)_s { @{ # loop over all fields and print the type and name for field in spec.parsed_fields(): if (not field.is_header): print_field_def(field) }@ -} +}; /** * @@} -- cgit v1.2.3 From 3856271abb0b90aba62f35668513463c6555734b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 1 Dec 2014 17:46:25 +0100 Subject: remove embedded message test from rc_channels.msg --- msg/px4_msgs/rc_channels.msg | 1 - 1 file changed, 1 deletion(-) diff --git a/msg/px4_msgs/rc_channels.msg b/msg/px4_msgs/rc_channels.msg index aaa6696f9..19fd14c7c 100644 --- a/msg/px4_msgs/rc_channels.msg +++ b/msg/px4_msgs/rc_channels.msg @@ -21,4 +21,3 @@ uint8 channel_count # Number of valid channels int8[18] function # Functions mapping uint8 rssi # Receive signal strength index bool signal_lost # Control signal lost, should be checked together with topic timeout -actuator_armed actuator -- cgit v1.2.3 From dfb266565a2f1849ddd67460950b674a44eb6530 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 2 Dec 2014 10:45:42 +0100 Subject: update ros configuration for new file locations --- CMakeLists.txt | 4 +++- msg/px4_msgs/vehicle_attitude.msg | 18 ++++++++++++++++++ 2 files changed, 21 insertions(+), 1 deletion(-) create mode 100644 msg/px4_msgs/vehicle_attitude.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 610284abe..636cc07e3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -47,7 +47,9 @@ find_package(catkin REQUIRED COMPONENTS ## Generate messages in the 'msg' folder add_message_files( FILES - rc_channels.msg + px4_msgs/rc_channels.msg + px4_msgs/vehicle_attitude.msg + px4_msgs/rc_channels.msg ) ## Generate services in the 'srv' folder diff --git a/msg/px4_msgs/vehicle_attitude.msg b/msg/px4_msgs/vehicle_attitude.msg new file mode 100644 index 000000000..98018a1df --- /dev/null +++ b/msg/px4_msgs/vehicle_attitude.msg @@ -0,0 +1,18 @@ +# This is similar to the mavlink message ATTITUDE, but for onboard use */ +uint64 timestamp # in microseconds since system start +# @warning roll, pitch and yaw have always to be valid, the rotation matrix and quaternion are optional +float32 roll # Roll angle (rad, Tait-Bryan, NED) +float32 pitch # Pitch angle (rad, Tait-Bryan, NED) +float32 yaw # Yaw angle (rad, Tait-Bryan, NED) +float32 rollspeed # Roll angular speed (rad/s, Tait-Bryan, NED) +float32 pitchspeed # Pitch angular speed (rad/s, Tait-Bryan, NED) +float32 yawspeed # Yaw angular speed (rad/s, Tait-Bryan, NED) +float32 rollacc # Roll angular accelration (rad/s, Tait-Bryan, NED) +float32 pitchacc # Pitch angular acceleration (rad/s, Tait-Bryan, NED) +float32 yawacc # Yaw angular acceleration (rad/s, Tait-Bryan, NED) +float32[3] rate_offsets # Offsets of the body angular rates from zero +float32[9] R # Rotation matrix, body to world, (Tait-Bryan, NED) +float32[4] q # Quaternion (NED) +float32[3] g_comp # Compensated gravity vector +bool R_valid # Rotation matrix valid +bool q_valid # Quaternion valid -- cgit v1.2.3 From 4d91c61f8f5aaebd8bcf0add27591fafa9c41dbe Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 2 Dec 2014 10:46:29 +0100 Subject: add macros for easy 2d array support, builds on px4 test build --- .../flow_position_estimator_main.c | 3 +- src/include/px4.h | 2 +- src/include/px4_defines.h | 77 ++++++++++++++++++++++ src/platforms/px4_defines.h | 72 -------------------- 4 files changed, 80 insertions(+), 74 deletions(-) create mode 100644 src/include/px4_defines.h delete mode 100644 src/platforms/px4_defines.h diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index 0b8c01f79..8cc9ed686 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -67,6 +67,7 @@ #include #include #include +#include #include "flow_position_estimator_params.h" @@ -337,7 +338,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) { float sum = 0.0f; for(uint8_t j = 0; j < 3; j++) - sum = sum + speed[j] * att.R[i][j]; + sum = sum + speed[j] * PX4_R(att.R, i, j); global_speed[i] = sum; } diff --git a/src/include/px4.h b/src/include/px4.h index 22d661b17..45068a6f7 100644 --- a/src/include/px4.h +++ b/src/include/px4.h @@ -59,7 +59,7 @@ #endif -#include "../platforms/px4_defines.h" +#include #include "../platforms/px4_middleware.h" #include "../platforms/px4_nodehandle.h" #include "../platforms/px4_subscriber.h" diff --git a/src/include/px4_defines.h b/src/include/px4_defines.h new file mode 100644 index 000000000..2dd57940d --- /dev/null +++ b/src/include/px4_defines.h @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * 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 px4_defines.h + * + * Generally used magic defines + */ + +#pragma once + +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/* + * Building for running within the ROS environment + */ +#define __EXPORT +#define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv) +#define PX4_WARN ROS_WARN +#define PX4_INFO ROS_INFO +#define PX4_TOPIC(_name) #_name +#define PX4_TOPIC_T(_name) _name +#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj); +#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); +#else +/* + * Building for NuttX + */ +#define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) +#define PX4_WARN warnx +#define PX4_WARN warnx +#define PX4_INFO warnx +#define PX4_TOPIC(_name) ORB_ID(_name) +#define PX4_TOPIC_T(_name) _name##_s +#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval) +#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) +#endif + +/* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */ +#define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME +#define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC)(__VA_ARGS__) +#define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise(PX4_TOPIC(_name)) + +/* wrapper for 2d matrices */ +#define PX4_ARRAY2D(_array, _ncols, _x, _y) (_array[_x * _ncols + _y]) + +/* wrapper for rotation matrices stored in arrays */ +#define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h deleted file mode 100644 index 84e5c8da0..000000000 --- a/src/platforms/px4_defines.h +++ /dev/null @@ -1,72 +0,0 @@ -/**************************************************************************** - * - * 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 px4_defines.h - * - * Generally used magic defines - */ - -#pragma once - -#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) -/* - * Building for running within the ROS environment - */ -#define __EXPORT -#define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv) -#define PX4_WARN ROS_WARN -#define PX4_INFO ROS_INFO -#define PX4_TOPIC(_name) #_name -#define PX4_TOPIC_T(_name) _name -#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj); -#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); - -#else -/* - * Building for NuttX - */ -#define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) -#define PX4_WARN warnx -#define PX4_WARN warnx -#define PX4_INFO warnx -#define PX4_TOPIC(_name) ORB_ID(_name) -#define PX4_TOPIC_T(_name) _name##_s -#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval) -#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) -#endif - -/* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */ -#define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME -#define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC)(__VA_ARGS__) -#define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise(PX4_TOPIC(_name)) -- cgit v1.2.3 From fbc497452517a748232a0b88b0a8f8f6ca8a9b92 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 2 Dec 2014 10:50:31 +0100 Subject: remove rc_channels, vehicle_attitude topic header files --- src/modules/uORB/topics/rc_channels.h | 96 ------------------------------ src/modules/uORB/topics/vehicle_attitude.h | 92 ---------------------------- 2 files changed, 188 deletions(-) delete mode 100644 src/modules/uORB/topics/rc_channels.h delete mode 100755 src/modules/uORB/topics/vehicle_attitude.h diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h deleted file mode 100644 index 16916cc4d..000000000 --- a/src/modules/uORB/topics/rc_channels.h +++ /dev/null @@ -1,96 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-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 rc_channels.h - * Definition of the rc_channels uORB topic. - * - * @deprecated DO NOT USE FOR NEW CODE - */ - -#ifndef RC_CHANNELS_H_ -#define RC_CHANNELS_H_ - -#include -#include "../uORB.h" - -/** - * This defines the mapping of the RC functions. - * The value assigned to the specific function corresponds to the entry of - * the channel array channels[]. - */ -enum RC_CHANNELS_FUNCTION { - THROTTLE = 0, - ROLL, - PITCH, - YAW, - MODE, - RETURN, - POSCTL, - LOITER, - OFFBOARD, - ACRO, - FLAPS, - AUX_1, - AUX_2, - AUX_3, - AUX_4, - AUX_5 -}; - -// MAXIMUM FUNCTIONS IS != MAXIMUM RC INPUT CHANNELS - -#define RC_CHANNELS_FUNCTION_MAX 18 - -/** - * @addtogroup topics - * @{ - */ -struct rc_channels_s { - uint64_t timestamp; /**< Timestamp in microseconds since boot time */ - uint64_t timestamp_last_valid; /**< Timestamp of last valid RC signal */ - float channels[RC_CHANNELS_FUNCTION_MAX]; /**< Scaled to -1..1 (throttle: 0..1) */ - uint8_t channel_count; /**< Number of valid channels */ - int8_t function[RC_CHANNELS_FUNCTION_MAX]; /**< Functions mapping */ - uint8_t rssi; /**< Receive signal strength index */ - bool signal_lost; /**< Control signal lost, should be checked together with topic timeout */ -}; /**< radio control channels. */ - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(rc_channels); - -#endif diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h deleted file mode 100755 index 40328af14..000000000 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ /dev/null @@ -1,92 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier - * - * 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 vehicle_attitude.h - * Definition of the attitude uORB topic. - */ - -#ifndef VEHICLE_ATTITUDE_H_ -#define VEHICLE_ATTITUDE_H_ - -#include -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * Attitude in NED body frame in SI units. - * - * @see http://en.wikipedia.org/wiki/International_System_of_Units - */ -struct vehicle_attitude_s { - - uint64_t timestamp; /**< in microseconds since system start */ - - /* This is similar to the mavlink message ATTITUDE, but for onboard use */ - - /** @warning roll, pitch and yaw have always to be valid, the rotation matrix and quaternion are optional */ - - float roll; /**< Roll angle (rad, Tait-Bryan, NED) */ - float pitch; /**< Pitch angle (rad, Tait-Bryan, NED) */ - float yaw; /**< Yaw angle (rad, Tait-Bryan, NED) */ - float rollspeed; /**< Roll angular speed (rad/s, Tait-Bryan, NED) */ - float pitchspeed; /**< Pitch angular speed (rad/s, Tait-Bryan, NED) */ - float yawspeed; /**< Yaw angular speed (rad/s, Tait-Bryan, NED) */ - float rollacc; /**< Roll angular accelration (rad/s, Tait-Bryan, NED) */ - float pitchacc; /**< Pitch angular acceleration (rad/s, Tait-Bryan, NED) */ - float yawacc; /**< Yaw angular acceleration (rad/s, Tait-Bryan, NED) */ - float rate_offsets[3]; /**< Offsets of the body angular rates from zero */ - float R[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */ - float q[4]; /**< Quaternion (NED) */ - float g_comp[3]; /**< Compensated gravity vector */ - bool R_valid; /**< Rotation matrix valid */ - bool q_valid; /**< Quaternion valid */ - -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_attitude); - -#endif -- cgit v1.2.3 From eba62a75a445bf1f6302af7ea0cb24b458d65d25 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 2 Dec 2014 12:17:06 +0100 Subject: add missing timestamp field --- msg/px4_msgs/rc_channels.msg | 1 + 1 file changed, 1 insertion(+) diff --git a/msg/px4_msgs/rc_channels.msg b/msg/px4_msgs/rc_channels.msg index 19fd14c7c..4e0e5b494 100644 --- a/msg/px4_msgs/rc_channels.msg +++ b/msg/px4_msgs/rc_channels.msg @@ -15,6 +15,7 @@ uint8 RC_CHANNELS_FUNCTION_AUX_2=12 uint8 RC_CHANNELS_FUNCTION_AUX_3=13 uint8 RC_CHANNELS_FUNCTION_AUX_4=14 uint8 RC_CHANNELS_FUNCTION_AUX_5=15 +uint64 timestamp # Timestamp in microseconds since boot time uint64 timestamp_last_valid # Timestamp of last valid RC signal float32[18] channels # Scaled to -1..1 (throttle: 0..1) uint8 channel_count # Number of valid channels -- cgit v1.2.3 From 141dda209222a7dd5afad6b844735fd9a44756a6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 2 Dec 2014 12:17:34 +0100 Subject: fix uorb header template for constants --- msg/templates/msg.h.template | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template index f60001b55..5fce506b6 100644 --- a/msg/templates/msg.h.template +++ b/msg/templates/msg.h.template @@ -82,7 +82,7 @@ for field in spec.parsed_fields(): }@ @# Constants @[for constant in spec.constants]@ -#define @(constant.name) = @(int(constant.val)) +#define @(constant.name) @(int(constant.val)) @[end for] /** -- cgit v1.2.3 From e28e8c11bba0779386fc16ee47deac4db39b51c0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 2 Dec 2014 12:19:24 +0100 Subject: make default apps compatible with autogenerated attitude and rc_channels message --- .../attitude_estimator_ekf_main.cpp | 2 +- .../ekf_att_pos_estimator_main.cpp | 3 +- src/modules/fw_att_control/fw_att_control_main.cpp | 7 +- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 3 +- src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 +-- .../position_estimator_inav_main.c | 27 +++---- src/modules/sensors/sensors.cpp | 82 +++++++++++----------- 7 files changed, 69 insertions(+), 64 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index e2dbc30dd..8d537d676 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -549,7 +549,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds R = R_decl * R_body; /* copy rotation matrix */ - memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R)); + memcpy(&att.R[0], &R.data[0][0], sizeof(att.R)); att.R_valid = true; if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index e7805daa9..db6773b8a 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -83,6 +83,7 @@ #include #include #include +#include #include "estimator_23states.h" @@ -1384,7 +1385,7 @@ FixedwingEstimator::task_main() math::Vector<3> euler = R.to_euler(); for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - _att.R[i][j] = R(i, j); + PX4_R(_att.R, i, j) = R(i, j); _att.timestamp = _last_sensor_timestamp; _att.q[0] = _ekf->states[0]; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index e770c11a2..83fe25571 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -75,6 +75,7 @@ #include #include #include +#include /** * Fixedwing attitude control app start / stop handling function @@ -824,9 +825,9 @@ FixedwingAttitudeControl::task_main() float speed_body_v = 0.0f; float speed_body_w = 0.0f; if(_att.R_valid) { - speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d; - speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; - speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; + speed_body_u = PX4_R(_att.R, 0, 0) * _global_pos.vel_n + PX4_R(_att.R, 1, 0) * _global_pos.vel_e + PX4_R(_att.R, 2, 0) * _global_pos.vel_d; + speed_body_v = PX4_R(_att.R, 0, 1) * _global_pos.vel_n + PX4_R(_att.R, 1, 1) * _global_pos.vel_e + PX4_R(_att.R, 2, 1) * _global_pos.vel_d; + speed_body_w = PX4_R(_att.R, 0, 2) * _global_pos.vel_n + PX4_R(_att.R, 1, 2) * _global_pos.vel_e + PX4_R(_att.R, 2, 2) * _global_pos.vel_d; } else { if (_debug && loop_counter % 10 == 0) { warnx("Did not get a valid R\n"); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 6017369aa..f441c4a91 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -90,6 +90,7 @@ #include #include "landingslope.h" #include "mtecs/mTecs.h" +#include static int _control_task = -1; /**< task handle for sensor task */ @@ -704,7 +705,7 @@ FixedwingPositionControl::vehicle_attitude_poll() /* set rotation matrix */ for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - _R_nb(i, j) = _att.R[i][j]; + _R_nb(i, j) = PX4_R(_att.R, i, j); } } 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 5918d6bc5..6682a9c89 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -73,6 +73,7 @@ #include #include #include +#include #define TILT_COS_MAX 0.7f #define SIGMA 0.000001f @@ -1147,11 +1148,11 @@ MulticopterPositionControl::task_main() /* thrust compensation for altitude only control mode */ float att_comp; - if (_att.R[2][2] > TILT_COS_MAX) { - att_comp = 1.0f / _att.R[2][2]; + if (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) { + att_comp = 1.0f / PX4_R(_att.R, 2, 2); - } else if (_att.R[2][2] > 0.0f) { - att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f; + } else if (PX4_R(_att.R, 2, 2) > 0.0f) { + att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f; saturation_z = true; } else { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 296919c04..2a601b630 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -68,6 +68,7 @@ #include #include #include +#include #include "position_estimator_inav_params.h" #include "inertial_filter.h" @@ -282,13 +283,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) }; float w_gps_xy = 1.0f; float w_gps_z = 1.0f; - + float corr_vision[3][2] = { { 0.0f, 0.0f }, // N (pos, vel) { 0.0f, 0.0f }, // E (pos, vel) { 0.0f, 0.0f }, // D (pos, vel) }; - + float corr_sonar = 0.0f; float corr_sonar_filtered = 0.0f; @@ -461,7 +462,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) acc[i] = 0.0f; for (int j = 0; j < 3; j++) { - acc[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; + acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j]; } } @@ -494,7 +495,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if ((flow.ground_distance_m > 0.31f) && (flow.ground_distance_m < 4.0f) && - (att.R[2][2] > 0.7f) && + (PX4_R(att.R, 2, 2) > 0.7f) && (fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) { sonar_time = t; @@ -531,15 +532,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float flow_q = flow.quality / 255.0f; float dist_bottom = - z_est[0] - surface_offset; - if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7f) { + if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && PX4_R(att.R, 2, 2) > 0.7f) { /* distance to surface */ - float flow_dist = dist_bottom / att.R[2][2]; + float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); /* check if flow if too large for accurate measurements */ /* calculate estimated velocity in body frame */ float body_v_est[2] = { 0.0f, 0.0f }; for (int i = 0; i < 2; i++) { - body_v_est[i] = att.R[0][i] * x_est[1] + att.R[1][i] * y_est[1] + att.R[2][i] * z_est[1]; + body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1]; } /* set this flag if flow should be accurate according to current velocity and attitude rate estimate */ @@ -562,7 +563,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* project measurements vector to NED basis, skip Z component */ for (int i = 0; i < 2; i++) { for (int j = 0; j < 3; j++) { - flow_v[i] += att.R[i][j] * flow_m[j]; + flow_v[i] += PX4_R(att.R, i, j) * flow_m[j]; } } @@ -571,7 +572,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_flow[1] = flow_v[1] - y_est[1]; /* adjust correction weight */ float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); - w_flow = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist); + w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist); /* if flow is not accurate, reduce weight for it */ // TODO make this more fuzzy @@ -646,13 +647,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) x_est[1] = vision.vx; y_est[0] = vision.y; y_est[1] = vision.vy; - /* only reset the z estimate if the z weight parameter is not zero */ + /* only reset the z estimate if the z weight parameter is not zero */ if (params.w_z_vision_p > MIN_VALID_W) { z_est[0] = vision.z; z_est[1] = vision.vz; } - + vision_valid = true; warnx("VISION estimate valid"); mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid"); @@ -912,7 +913,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float c = 0.0f; for (int j = 0; j < 3; j++) { - c += att.R[j][i] * accel_bias_corr[j]; + c += PX4_R(att.R, j, i) * accel_bias_corr[j]; } if (isfinite(c)) { @@ -937,7 +938,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float c = 0.0f; for (int j = 0; j < 3; j++) { - c += att.R[j][i] * accel_bias_corr[j]; + c += PX4_R(att.R, j, i) * accel_bias_corr[j]; } if (isfinite(c)) { diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index cdcb428dd..672dc52c3 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -182,13 +182,13 @@ private: /** * Get and limit value for specified RC function. Returns NAN if not mapped. */ - float get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value); + float get_rc_value(uint8_t func, float min_value, float max_value); /** * Get switch position for specified function. */ - switch_pos_t get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv); - switch_pos_t get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv); + switch_pos_t get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv); + switch_pos_t get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv); /** * Gather and publish RC input data. @@ -771,25 +771,25 @@ Sensors::parameters_update() _parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th); /* update RC function mappings */ - _rc.function[THROTTLE] = _parameters.rc_map_throttle - 1; - _rc.function[ROLL] = _parameters.rc_map_roll - 1; - _rc.function[PITCH] = _parameters.rc_map_pitch - 1; - _rc.function[YAW] = _parameters.rc_map_yaw - 1; - - _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; - _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; - _rc.function[POSCTL] = _parameters.rc_map_posctl_sw - 1; - _rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1; - _rc.function[ACRO] = _parameters.rc_map_acro_sw - 1; - _rc.function[OFFBOARD] = _parameters.rc_map_offboard_sw - 1; - - _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; - - _rc.function[AUX_1] = _parameters.rc_map_aux1 - 1; - _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1; - _rc.function[AUX_3] = _parameters.rc_map_aux3 - 1; - _rc.function[AUX_4] = _parameters.rc_map_aux4 - 1; - _rc.function[AUX_5] = _parameters.rc_map_aux5 - 1; + _rc.function[RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1; + _rc.function[RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1; + _rc.function[RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1; + _rc.function[RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1; + + _rc.function[RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1; + _rc.function[RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1; + _rc.function[RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1; + _rc.function[RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1; + _rc.function[RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1; + _rc.function[RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1; + + _rc.function[RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1; + + _rc.function[RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1; + _rc.function[RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1; + _rc.function[RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1; + _rc.function[RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1; + _rc.function[RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1; /* gyro offsets */ param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0])); @@ -1492,7 +1492,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } float -Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value) +Sensors::get_rc_value(uint8_t func, float min_value, float max_value) { if (_rc.function[func] >= 0) { float value = _rc.channels[_rc.function[func]]; @@ -1513,7 +1513,7 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max } switch_pos_t -Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv) +Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv) { if (_rc.function[func] >= 0) { float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; @@ -1534,7 +1534,7 @@ Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, boo } switch_pos_t -Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv) +Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv) { if (_rc.function[func] >= 0) { float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; @@ -1668,24 +1668,24 @@ Sensors::rc_poll() manual.timestamp = rc_input.timestamp_last_signal; /* limit controls */ - manual.y = get_rc_value(ROLL, -1.0, 1.0); - manual.x = get_rc_value(PITCH, -1.0, 1.0); - manual.r = get_rc_value(YAW, -1.0, 1.0); - manual.z = get_rc_value(THROTTLE, 0.0, 1.0); - manual.flaps = get_rc_value(FLAPS, -1.0, 1.0); - manual.aux1 = get_rc_value(AUX_1, -1.0, 1.0); - manual.aux2 = get_rc_value(AUX_2, -1.0, 1.0); - manual.aux3 = get_rc_value(AUX_3, -1.0, 1.0); - manual.aux4 = get_rc_value(AUX_4, -1.0, 1.0); - manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0); + manual.y = get_rc_value(RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); + manual.x = get_rc_value(RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); + manual.r = get_rc_value(RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); + manual.z = get_rc_value(RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); + manual.flaps = get_rc_value(RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); + manual.aux1 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); + manual.aux2 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); + manual.aux3 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); + manual.aux4 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); + manual.aux5 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0); /* mode switches */ - manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); - manual.posctl_switch = get_rc_sw2pos_position(POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); - manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); - manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); - manual.acro_switch = get_rc_sw2pos_position(ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); - manual.offboard_switch = get_rc_sw2pos_position(OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv); + manual.mode_switch = get_rc_sw3pos_position(RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); + manual.posctl_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); + manual.return_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); + manual.loiter_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); + manual.acro_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); + manual.offboard_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv); /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { -- cgit v1.2.3 From 5bb03f1c2db3eb5620caf806b053f8194490969a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 2 Dec 2014 15:21:58 +0100 Subject: subscriber example increase stack size --- src/examples/subscriber/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/examples/subscriber/module.mk b/src/examples/subscriber/module.mk index 44d3d03a9..481c36ea7 100644 --- a/src/examples/subscriber/module.mk +++ b/src/examples/subscriber/module.mk @@ -39,4 +39,4 @@ MODULE_COMMAND = subscriber SRCS = subscriber.cpp -MODULE_STACKSIZE = 1200 +MODULE_STACKSIZE = 2400 -- cgit v1.2.3 From 8d3d8a3358c4c44d6de13a0c8e6a88ea98288bf2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 2 Dec 2014 15:22:21 +0100 Subject: subscriber example clean up --- src/examples/subscriber/subscriber.cpp | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index 3d54a1a2f..a29f9ab93 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -36,7 +36,7 @@ void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid); } void rc_channels_callback2(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("I heard(2): [%llu]", msg.timestamp_last_valid); + PX4_INFO("I heard (2): [%llu]", msg.timestamp_last_valid); } class RCHandler { @@ -91,14 +91,7 @@ PX4_MAIN_FUNCTION(subscriber) { */ PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, 100); PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback2, 1000); - //1 - // PX4_SUBSCRIBE(n, rc_channels, callee.rc_channels_callback, , 1000); - //2 - // PX4_SUBSCRIBE(n, rc_channels, rchandler.callback, &rchandler, 1000); - //3 for bind PX4_SUBSCRIBE(n, rc_channels, RCHandler::callback, rchandler, 1000); - // ros::NodeHandle n2; - // n2.subscribe("chatter", 1000, &RCHandler::callback, &rchandler); PX4_INFO("subscribed"); /** -- cgit v1.2.3 From f2fdd0d69bd485840c5ae491040322671e202fa2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 2 Dec 2014 15:44:57 +0100 Subject: add mc_att_control to CMakeList --- CMakeLists.txt | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 636cc07e3..8b3ca17e3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -136,6 +136,14 @@ target_link_libraries(subscriber px4 ) +## mc_att_control +add_executable(mc_att_control src/module/mc_att_control_main.cpp) +add_dependencies(mc_att_control px4_generate_messages_cpp) +target_link_libraries(mc_att_control + ${catkin_LIBRARIES} + px4 +) + ############# ## Install ## ############# -- cgit v1.2.3 From 05a87a706a122b8a83becaaa94c70161fb69c82a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 2 Dec 2014 16:17:17 +0100 Subject: move px4_defines file --- .../flow_position_estimator_main.c | 2 +- src/include/px4.h | 2 +- src/include/px4_defines.h | 77 ---------------------- .../ekf_att_pos_estimator_main.cpp | 2 +- src/modules/fw_att_control/fw_att_control_main.cpp | 2 +- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- .../position_estimator_inav_main.c | 2 +- src/platforms/px4_defines.h | 77 ++++++++++++++++++++++ 9 files changed, 84 insertions(+), 84 deletions(-) delete mode 100644 src/include/px4_defines.h create mode 100644 src/platforms/px4_defines.h diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index 8cc9ed686..5a23932d3 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -67,7 +67,7 @@ #include #include #include -#include +#include #include "flow_position_estimator_params.h" diff --git a/src/include/px4.h b/src/include/px4.h index 45068a6f7..22d661b17 100644 --- a/src/include/px4.h +++ b/src/include/px4.h @@ -59,7 +59,7 @@ #endif -#include +#include "../platforms/px4_defines.h" #include "../platforms/px4_middleware.h" #include "../platforms/px4_nodehandle.h" #include "../platforms/px4_subscriber.h" diff --git a/src/include/px4_defines.h b/src/include/px4_defines.h deleted file mode 100644 index 2dd57940d..000000000 --- a/src/include/px4_defines.h +++ /dev/null @@ -1,77 +0,0 @@ -/**************************************************************************** - * - * 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 px4_defines.h - * - * Generally used magic defines - */ - -#pragma once - -#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) -/* - * Building for running within the ROS environment - */ -#define __EXPORT -#define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv) -#define PX4_WARN ROS_WARN -#define PX4_INFO ROS_INFO -#define PX4_TOPIC(_name) #_name -#define PX4_TOPIC_T(_name) _name -#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj); -#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); -#else -/* - * Building for NuttX - */ -#define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) -#define PX4_WARN warnx -#define PX4_WARN warnx -#define PX4_INFO warnx -#define PX4_TOPIC(_name) ORB_ID(_name) -#define PX4_TOPIC_T(_name) _name##_s -#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval) -#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) -#endif - -/* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */ -#define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME -#define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC)(__VA_ARGS__) -#define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise(PX4_TOPIC(_name)) - -/* wrapper for 2d matrices */ -#define PX4_ARRAY2D(_array, _ncols, _x, _y) (_array[_x * _ncols + _y]) - -/* wrapper for rotation matrices stored in arrays */ -#define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index db6773b8a..c0b1bb404 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -83,7 +83,7 @@ #include #include #include -#include +#include #include "estimator_23states.h" diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 83fe25571..57c1e72f3 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -75,7 +75,7 @@ #include #include #include -#include +#include /** * Fixedwing attitude control app start / stop handling function diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index f441c4a91..e07bcc225 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -90,7 +90,7 @@ #include #include "landingslope.h" #include "mtecs/mTecs.h" -#include +#include static int _control_task = -1; /**< task handle for sensor task */ 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 6682a9c89..cea847603 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -73,7 +73,7 @@ #include #include #include -#include +#include #define TILT_COS_MAX 0.7f #define SIGMA 0.000001f diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 2a601b630..de6357d31 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -68,7 +68,7 @@ #include #include #include -#include +#include #include "position_estimator_inav_params.h" #include "inertial_filter.h" diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h new file mode 100644 index 000000000..2dd57940d --- /dev/null +++ b/src/platforms/px4_defines.h @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * 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 px4_defines.h + * + * Generally used magic defines + */ + +#pragma once + +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/* + * Building for running within the ROS environment + */ +#define __EXPORT +#define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv) +#define PX4_WARN ROS_WARN +#define PX4_INFO ROS_INFO +#define PX4_TOPIC(_name) #_name +#define PX4_TOPIC_T(_name) _name +#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj); +#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); +#else +/* + * Building for NuttX + */ +#define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) +#define PX4_WARN warnx +#define PX4_WARN warnx +#define PX4_INFO warnx +#define PX4_TOPIC(_name) ORB_ID(_name) +#define PX4_TOPIC_T(_name) _name##_s +#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval) +#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) +#endif + +/* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */ +#define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME +#define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC)(__VA_ARGS__) +#define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise(PX4_TOPIC(_name)) + +/* wrapper for 2d matrices */ +#define PX4_ARRAY2D(_array, _ncols, _x, _y) (_array[_x * _ncols + _y]) + +/* wrapper for rotation matrices stored in arrays */ +#define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y) -- cgit v1.2.3 From 905913986a03cdb31d056b278cadc2d6f6421e38 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Dec 2014 07:43:08 +0100 Subject: update comments --- src/platforms/px4_nodehandle.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 415351756..cf47fec59 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -55,7 +55,6 @@ namespace px4 { -//XXX create abstract base class #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) class NodeHandle : private ros::NodeHandle @@ -71,7 +70,7 @@ public: //XXX empty lists }; - /* Constructor with callback to function */ + /* Subscribe with callback to function */ template Subscriber * subscribe(const char *topic, void(*fp)(M)) { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp); @@ -79,7 +78,8 @@ public: _subs.push_back(sub); return sub; } - /* Constructor with callback to class method */ + + /* Subscribe with callback to class method */ template Subscriber * subscribe(const char *topic, void(T::*fp)(M), T *obj) { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj); -- cgit v1.2.3 From 6a99b04fb74f6da3faea54c93d234a9b57d7bd0e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Dec 2014 12:31:37 +0100 Subject: add parameter wrapper macros for ros --- src/examples/subscriber/subscriber.cpp | 8 ++++++++ src/platforms/px4_defines.h | 2 ++ 2 files changed, 10 insertions(+) diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index a29f9ab93..a8ecd4a7d 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -74,6 +74,14 @@ PX4_MAIN_FUNCTION(subscriber) { */ px4::NodeHandle n; + /* Define a parameter */ + PX4_PARAM_INIT("SUB_INTERV", 100); + + /* Read the parameter back for testing */ + int32_t sub_interval; + PX4_PARAM_GET("SUB_INTERV", &sub_interval); + PX4_INFO("Param SUB_INTERV = %d", sub_interval); + /** * The subscribe() call is how you tell ROS that you want to receive messages * on a given topic. This invokes a call to the ROS diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 2dd57940d..2d9051aae 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -51,6 +51,8 @@ #define PX4_TOPIC_T(_name) _name #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj); #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); +#define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default); +#define PX4_PARAM_GET(_name, _destpt) ros::param::get(_name, *_destpt) #else /* * Building for NuttX -- cgit v1.2.3 From 1c79f0cef1f21cff7935ddd7caf048fd96991eea Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Dec 2014 14:43:03 +0100 Subject: improve param wrapper for ros, prepare for px4 --- src/examples/subscriber/subscriber.cpp | 12 ++++++++---- src/include/px4.h | 1 + src/platforms/px4_defines.h | 17 +++++++++++++++-- src/platforms/px4_nodehandle.h | 1 + 4 files changed, 25 insertions(+), 6 deletions(-) diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index a8ecd4a7d..e52d661a9 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -74,13 +74,17 @@ PX4_MAIN_FUNCTION(subscriber) { */ px4::NodeHandle n; - /* Define a parameter */ - PX4_PARAM_INIT("SUB_INTERV", 100); + /* Define parameters */ + px4_param_t p_sub_interv = PX4_PARAM_INIT("SUB_INTERV", 100); + px4_param_t p_test_float = PX4_PARAM_INIT("SUB_TESTF", 3.14f); /* Read the parameter back for testing */ int32_t sub_interval; - PX4_PARAM_GET("SUB_INTERV", &sub_interval); + float test_float; + PX4_PARAM_GET(p_sub_interv, &sub_interval); PX4_INFO("Param SUB_INTERV = %d", sub_interval); + PX4_PARAM_GET(p_test_float, &test_float); + PX4_INFO("Param SUB_TESTF = %f", (double)test_float); /** * The subscribe() call is how you tell ROS that you want to receive messages @@ -97,7 +101,7 @@ PX4_MAIN_FUNCTION(subscriber) { * is the number of messages that will be buffered up before beginning to throw * away the oldest ones. */ - PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, 100); + PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, sub_interval); PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback2, 1000); PX4_SUBSCRIBE(n, rc_channels, RCHandler::callback, rchandler, 1000); PX4_INFO("subscribed"); diff --git a/src/include/px4.h b/src/include/px4.h index 22d661b17..2d5d25964 100644 --- a/src/include/px4.h +++ b/src/include/px4.h @@ -56,6 +56,7 @@ #include #include #include +#include #endif diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 2d9051aae..a10df858a 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -51,8 +51,18 @@ #define PX4_TOPIC_T(_name) _name #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj); #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); -#define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default); -#define PX4_PARAM_GET(_name, _destpt) ros::param::get(_name, *_destpt) +typedef const std::string px4_param_t; +static inline px4_param_t ROS_PARAM_SET(const std::string &name, int32_t value) { + ros::param::set(name, value); + return (px4_param_t)name; +}; +static inline px4_param_t ROS_PARAM_SET(const std::string &name, float value) { + ros::param::set(name, value); + return (px4_param_t)name; +}; +#define PX4_PARAM_INIT(_name, _default) ROS_PARAM_SET(_name, _default) +// #define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default) +#define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) #else /* * Building for NuttX @@ -65,6 +75,9 @@ #define PX4_TOPIC_T(_name) _name##_s #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval) #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) +typedef param_t px4_param_t +#define PX4_PARAM_INIT(_name, _default) param_find(_name) +#define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt) #endif /* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */ diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index cf47fec59..a2775d69a 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -99,6 +99,7 @@ public: void spin() { ros::spin(); } void spinOnce() { ros::spinOnce(); } + private: static const uint32_t kQueueSizeDefault = 1000; std::list _subs; -- cgit v1.2.3 From 924350a5de1a609e470618ef78212bf7b0044c33 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Dec 2014 15:11:27 +0100 Subject: bring up param wrapper for px4, moved global include file --- src/examples/subscriber/module.mk | 3 +- src/examples/subscriber/subscriber.cpp | 2 +- src/examples/subscriber/subscriber_params.c | 55 ++++++++++++++++++++++++++ src/include/px4.h | 22 +---------- src/platforms/px4_defines.h | 4 +- src/platforms/px4_includes.h | 61 +++++++++++++++++++++++++++++ 6 files changed, 123 insertions(+), 24 deletions(-) create mode 100644 src/examples/subscriber/subscriber_params.c create mode 100644 src/platforms/px4_includes.h diff --git a/src/examples/subscriber/module.mk b/src/examples/subscriber/module.mk index 481c36ea7..90b4d8ffc 100644 --- a/src/examples/subscriber/module.mk +++ b/src/examples/subscriber/module.mk @@ -37,6 +37,7 @@ MODULE_COMMAND = subscriber -SRCS = subscriber.cpp +SRCS = subscriber.cpp \ + subscriber_params.c MODULE_STACKSIZE = 2400 diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index e52d661a9..cc9b7a794 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -84,7 +84,7 @@ PX4_MAIN_FUNCTION(subscriber) { PX4_PARAM_GET(p_sub_interv, &sub_interval); PX4_INFO("Param SUB_INTERV = %d", sub_interval); PX4_PARAM_GET(p_test_float, &test_float); - PX4_INFO("Param SUB_TESTF = %f", (double)test_float); + PX4_INFO("Param SUB_TESTF = %.3f", (double)test_float); /** * The subscribe() call is how you tell ROS that you want to receive messages diff --git a/src/examples/subscriber/subscriber_params.c b/src/examples/subscriber/subscriber_params.c new file mode 100644 index 000000000..e28bfbc36 --- /dev/null +++ b/src/examples/subscriber/subscriber_params.c @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * 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 subscriber_params.c + * Parameters for the subscriber example + * + * @author Thomas Gubler + */ + +#include + +/** + * Interval of one subscriber in the example in ms + * + * @group Subscriber Example + */ +PARAM_DEFINE_INT32(SUB_INTERV, 100); + +/** + * Float Demonstration Parameter in the Example + * + * @group Subscriber Example + */ +PARAM_DEFINE_FLOAT(SUB_TESTF, 3.14f); diff --git a/src/include/px4.h b/src/include/px4.h index 2d5d25964..ca702d63c 100644 --- a/src/include/px4.h +++ b/src/include/px4.h @@ -39,27 +39,7 @@ #pragma once -#include - -#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) -/* - * Building for running within the ROS environment - */ -#include "ros/ros.h" -#include "px4/rc_channels.h" - -#else -/* - * Building for NuttX - */ -#include -#include -#include -#include -#include - -#endif - +#include "../platforms/px4_includes.h" #include "../platforms/px4_defines.h" #include "../platforms/px4_middleware.h" #include "../platforms/px4_nodehandle.h" diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index a10df858a..a71507510 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -63,10 +63,12 @@ static inline px4_param_t ROS_PARAM_SET(const std::string &name, float value) { #define PX4_PARAM_INIT(_name, _default) ROS_PARAM_SET(_name, _default) // #define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default) #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) + #else /* * Building for NuttX */ +#include #define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) #define PX4_WARN warnx #define PX4_WARN warnx @@ -75,7 +77,7 @@ static inline px4_param_t ROS_PARAM_SET(const std::string &name, float value) { #define PX4_TOPIC_T(_name) _name##_s #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval) #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) -typedef param_t px4_param_t +typedef param_t px4_param_t; #define PX4_PARAM_INIT(_name, _default) param_find(_name) #define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt) #endif diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h new file mode 100644 index 000000000..a3b59b766 --- /dev/null +++ b/src/platforms/px4_includes.h @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * 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 px4_includes.h + * + * Includes headers depending on the build target + */ + +#pragma once + +#include + +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/* + * Building for running within the ROS environment + */ +#include "ros/ros.h" +#include "px4/rc_channels.h" + +#else +/* + * Building for NuttX + */ +#include +#include +#include +#include +#include + +#endif -- cgit v1.2.3 From c2e2b3d52fa7bcfbbec110b5555d2c437657f118 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Dec 2014 17:04:15 +0100 Subject: make param wrapper macros compatible for px4 and ros, needs cleanup --- makefiles/setup.mk | 6 ++-- src/examples/subscriber/subscriber.cpp | 5 ++-- src/examples/subscriber/subscriber_params.c | 7 +++-- src/examples/subscriber/subscriber_params.h | 43 +++++++++++++++++++++++++++++ src/platforms/px4_defines.h | 21 ++++++++++---- 5 files changed, 70 insertions(+), 12 deletions(-) create mode 100644 src/examples/subscriber/subscriber_params.h diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 4bfa7a087..435c240bf 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -37,13 +37,14 @@ # Some useful paths. # # Note that in general we always keep directory paths with the separator -# at the end, and join paths without explicit separators. This reduces +# at the end, and join paths without explicit separators. This reduces # the number of duplicate slashes we have lying around in paths, # and is consistent with joining the results of $(dir) and $(notdir). # export PX4_INCLUDE_DIR = $(abspath $(PX4_BASE)/src/include)/ export PX4_MODULE_SRC = $(abspath $(PX4_BASE)/src)/ export PX4_LIB_DIR = $(abspath $(PX4_BASE)/src/lib)/ +export PX4_PLATFORMS_DIR = $(abspath $(PX4_BASE)/src/platforms)/ export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/ export NUTTX_SRC = $(abspath $(PX4_BASE)/NuttX/nuttx)/ export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink/include/mavlink/v1.0)/ @@ -61,7 +62,8 @@ export ARCHIVE_DIR = $(abspath $(PX4_BASE)/Archives)/ export INCLUDE_DIRS := $(PX4_MODULE_SRC) \ $(PX4_MODULE_SRC)/modules/ \ $(PX4_INCLUDE_DIR) \ - $(PX4_LIB_DIR) + $(PX4_LIB_DIR) \ + $(PX4_PLATFORMS_DIR) # # Tools diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index cc9b7a794..68003b6df 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -26,6 +26,7 @@ */ #include +#include "subscriber_params.h" using namespace px4; @@ -75,8 +76,8 @@ PX4_MAIN_FUNCTION(subscriber) { px4::NodeHandle n; /* Define parameters */ - px4_param_t p_sub_interv = PX4_PARAM_INIT("SUB_INTERV", 100); - px4_param_t p_test_float = PX4_PARAM_INIT("SUB_TESTF", 3.14f); + px4_param_t p_sub_interv = PX4_PARAM_INIT(SUB_INTERV); + px4_param_t p_test_float = PX4_PARAM_INIT(SUB_TESTF); /* Read the parameter back for testing */ int32_t sub_interval; diff --git a/src/examples/subscriber/subscriber_params.c b/src/examples/subscriber/subscriber_params.c index e28bfbc36..0d36f5021 100644 --- a/src/examples/subscriber/subscriber_params.c +++ b/src/examples/subscriber/subscriber_params.c @@ -38,18 +38,19 @@ * @author Thomas Gubler */ -#include +#include +#include "subscriber_params.h" /** * Interval of one subscriber in the example in ms * * @group Subscriber Example */ -PARAM_DEFINE_INT32(SUB_INTERV, 100); +PARAM_DEFINE_INT32(SUB_INTERV, PX4_PARAM_DEFAULT_VALUE_NAME(SUB_INTERV)); /** * Float Demonstration Parameter in the Example * * @group Subscriber Example */ -PARAM_DEFINE_FLOAT(SUB_TESTF, 3.14f); +PARAM_DEFINE_FLOAT(SUB_TESTF, PX4_PARAM_DEFAULT_VALUE_NAME(SUB_TESTF)); diff --git a/src/examples/subscriber/subscriber_params.h b/src/examples/subscriber/subscriber_params.h new file mode 100644 index 000000000..3f3ff5bce --- /dev/null +++ b/src/examples/subscriber/subscriber_params.h @@ -0,0 +1,43 @@ +/**************************************************************************** + * + * 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 subscriber_params.h + * Default parameters for the subscriber example + * + * @author Thomas Gubler + */ +#pragma once + +#define SUB_INTERV_DEFAULT_VALUE 100 +#define SUB_TESTF_DEFAULT_VALUE 3.14f diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index a71507510..6760f57ea 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -39,11 +39,15 @@ #pragma once +#define PX4_PARAM_DEFAULT_VALUE_NAME(_name) _name##_DEFAULT_VALUE + + #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) /* * Building for running within the ROS environment */ #define __EXPORT +#include "ros/ros.h" #define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv) #define PX4_WARN ROS_WARN #define PX4_INFO ROS_INFO @@ -51,18 +55,21 @@ #define PX4_TOPIC_T(_name) _name #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj); #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); -typedef const std::string px4_param_t; -static inline px4_param_t ROS_PARAM_SET(const std::string &name, int32_t value) { +typedef const char* px4_param_t; +static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value) { ros::param::set(name, value); return (px4_param_t)name; }; -static inline px4_param_t ROS_PARAM_SET(const std::string &name, float value) { +static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) { ros::param::set(name, value); return (px4_param_t)name; }; -#define PX4_PARAM_INIT(_name, _default) ROS_PARAM_SET(_name, _default) +// #define PARAM_DEFINE_INT32(_name, _default) static const int PX4_ROS_PARAM_DEFAULT_VALUE_NAME(_name) = _default; +// #define PARAM_DEFINE_FLOAT(_name, _default) static const float PX4_ROS_PARAM_DEFAULT_VALUE_NAME(_name) = _default; +#define PX4_PARAM_INIT(_name) PX4_ROS_PARAM_SET(#_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) // #define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default) #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) +#define PX4_PARAM_INT32_T int //XXX #else /* @@ -78,8 +85,9 @@ static inline px4_param_t ROS_PARAM_SET(const std::string &name, float value) { #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval) #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) typedef param_t px4_param_t; -#define PX4_PARAM_INIT(_name, _default) param_find(_name) +#define PX4_PARAM_INIT(_name) param_find(#_name) #define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt) +#define PX4_PARAM_INT32_T int32_t #endif /* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */ @@ -92,3 +100,6 @@ typedef param_t px4_param_t; /* wrapper for rotation matrices stored in arrays */ #define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y) + +// #define PX4_PARAM_DEFAULT_INT32(_name, _value) static const PX4_PARAM_INT32_T PX4_PARAM_DEFAULT_VALUE_NAME(_name) = _value; +// #define PX4_PARAM_DEFAULT_FLOAT(_name, _value) static const float PX4_PARAM_DEFAULT_VALUE_NAME(_name) = _value; -- cgit v1.2.3 From 83d97fd1c27728145e4869f957afcc7cae57ff8a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Dec 2014 09:53:37 +0100 Subject: better support for param default values on ros and px4 --- src/examples/subscriber/subscriber_params.c | 4 ++-- src/examples/subscriber/subscriber_params.h | 4 ++-- src/platforms/px4_defines.h | 4 +++- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/examples/subscriber/subscriber_params.c b/src/examples/subscriber/subscriber_params.c index 0d36f5021..a43817b5b 100644 --- a/src/examples/subscriber/subscriber_params.c +++ b/src/examples/subscriber/subscriber_params.c @@ -46,11 +46,11 @@ * * @group Subscriber Example */ -PARAM_DEFINE_INT32(SUB_INTERV, PX4_PARAM_DEFAULT_VALUE_NAME(SUB_INTERV)); +PX4_PARAM_DEFINE_INT32(SUB_INTERV); /** * Float Demonstration Parameter in the Example * * @group Subscriber Example */ -PARAM_DEFINE_FLOAT(SUB_TESTF, PX4_PARAM_DEFAULT_VALUE_NAME(SUB_TESTF)); +PX4_PARAM_DEFINE_FLOAT(SUB_TESTF); diff --git a/src/examples/subscriber/subscriber_params.h b/src/examples/subscriber/subscriber_params.h index 3f3ff5bce..f6f1d6ba0 100644 --- a/src/examples/subscriber/subscriber_params.h +++ b/src/examples/subscriber/subscriber_params.h @@ -39,5 +39,5 @@ */ #pragma once -#define SUB_INTERV_DEFAULT_VALUE 100 -#define SUB_TESTF_DEFAULT_VALUE 3.14f +#define PARAM_SUB_INTERV_DEFAULT 100 +#define PARAM_SUB_TESTF_DEFAULT 3.14f diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 6760f57ea..09338acab 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -39,7 +39,9 @@ #pragma once -#define PX4_PARAM_DEFAULT_VALUE_NAME(_name) _name##_DEFAULT_VALUE +#define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT +#define PX4_PARAM_DEFINE_INT32(_name) PARAM_DEFINE_INT32(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) +#define PX4_PARAM_DEFINE_FLOAT(_name) PARAM_DEFINE_FLOAT(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) -- cgit v1.2.3 From 8e8f84bde0d2e15734d931ea38a7b294a06d7314 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Dec 2014 10:39:24 +0100 Subject: uorb topic header generator: only create new files if the file content really changed --- Makefile | 5 +- Tools/px_generate_uorb_topic_headers.py | 121 +++++++++++++++++++++++--------- 2 files changed, 90 insertions(+), 36 deletions(-) diff --git a/Makefile b/Makefile index 7df8004a4..bdbc18be5 100644 --- a/Makefile +++ b/Makefile @@ -227,12 +227,15 @@ updatesubmodules: MSG_DIR = $(PX4_BASE)msg/px4_msgs MSG_TEMPLATE_DIR = $(PX4_BASE)msg/templates TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics +TOPICS_TEMPORARY_DIR = $(BUILD_DIR)topics_temporary .PHONY: generateuorbtopicheaders generateuorbtopicheaders: @$(ECHO) "Generating uORB topic headers" $(Q) ($(PX4_BASE)/Tools/px_generate_uorb_topic_headers.py -d $(MSG_DIR) \ - -o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR)) + -o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR) -t $(TOPICS_TEMPORARY_DIR)) +# clean up temporary files + $(Q) (rm -r $(TOPICS_TEMPORARY_DIR)) # # Testing targets diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py index a738dcb7e..2ddbd6984 100755 --- a/Tools/px_generate_uorb_topic_headers.py +++ b/Tools/px_generate_uorb_topic_headers.py @@ -39,6 +39,8 @@ message files """ from __future__ import print_function import os +import shutil +import filecmp import argparse import genmsg.template_tools @@ -55,44 +57,93 @@ package = 'px4' def convert_file(filename, outputdir, templatedir, includepath): - """ - Converts a single .msg file to a uorb header - """ - print("Generating uORB headers from {0}".format(filename)) - genmsg.template_tools.generate_from_file(filename, - package, - outputdir, - templatedir, - includepath, - msg_template_map, - srv_template_map) + """ + Converts a single .msg file to a uorb header + """ + print("Generating uORB headers from {0}".format(filename)) + genmsg.template_tools.generate_from_file(filename, + package, + outputdir, + templatedir, + includepath, + msg_template_map, + srv_template_map) def convert_dir(inputdir, outputdir, templatedir): - """ - Converts all .msg files in inputdir to uORB header files - """ - includepath = incl_default + [':'.join([package, inputdir])] - for f in os.listdir(inputdir): - fn = os.path.join(inputdir, f) - if os.path.isfile(fn): - convert_file(fn, outputdir, templatedir, includepath) + """ + Converts all .msg files in inputdir to uORB header files + """ + includepath = incl_default + [':'.join([package, inputdir])] + for f in os.listdir(inputdir): + fn = os.path.join(inputdir, f) + if os.path.isfile(fn): + convert_file( + fn, + outputdir, + templatedir, + includepath) + + +def copy_changed(inputdir, outputdir): + """ + Copies files from inputdir to outputdir if they don't exist in + ouputdir or if their content changed + """ + for f in os.listdir(inputdir): + fni = os.path.join(inputdir, f) + if os.path.isfile(fni): + # Check if f exists in outpoutdir, copy the file if not + fno = os.path.join(outputdir, f) + if not os.path.isfile(fno): + shutil.copy(fni, fno) + print("{0}: new header file".format(f)) + continue + # The file exists in inputdir and outputdir + # only copy if contents do not match + if not filecmp.cmp(fni, fno): + shutil.copy(fni, fno) + print("{0}: updated".format(f)) + continue + + print("{0}: unchanged".format(f)) + +def convert_dir_save(inputdir, outputdir, templatedir, temporarydir): + """ + Converts all .msg files in inputdir to uORB header files + Unchanged existing files are not overwritten. + """ + # Create new headers in temporary output directory + convert_dir(inputdir, temporarydir, templatedir) + + # Copy changed headers from temporary dir to output dir + copy_changed(temporarydir, outputdir) if __name__ == "__main__": - parser = argparse.ArgumentParser( - description='Convert msg files to uorb headers') - parser.add_argument('-d', dest='dir', help='directory with msg files') - parser.add_argument('-f', dest='file', - help="files to convert (use only without -d)", - nargs="+") - parser.add_argument('-e', dest='templatedir', - help='directory with template files',) - parser.add_argument('-o', dest='outputdir', - help='output directory for header files') - args = parser.parse_args() + parser = argparse.ArgumentParser( + description='Convert msg files to uorb headers') + parser.add_argument('-d', dest='dir', help='directory with msg files') + parser.add_argument('-f', dest='file', + help="files to convert (use only without -d)", + nargs="+") + parser.add_argument('-e', dest='templatedir', + help='directory with template files',) + parser.add_argument('-o', dest='outputdir', + help='output directory for header files') + parser.add_argument('-t', dest='temporarydir', + help='temporary directory') + args = parser.parse_args() - if args.file is not None: - for f in args.file: - convert_file(f, args.outputdir, args.templatedir, incl_default) - elif args.dir is not None: - convert_dir(args.dir, args.outputdir, args.templatedir) + if args.file is not None: + for f in args.file: + convert_file( + f, + args.outputdir, + args.templatedir, + incl_default) + elif args.dir is not None: + convert_dir_save( + args.dir, + args.outputdir, + args.templatedir, + args.temporarydir) -- cgit v1.2.3 From befe4c399e1f0f2864f91ddde4db585dabf3db99 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Dec 2014 11:50:59 +0100 Subject: comments for px4 defines --- src/platforms/px4_defines.h | 53 +++++++++++++++++++++++++++++++++++---------- 1 file changed, 42 insertions(+), 11 deletions(-) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 09338acab..5858a69d8 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -38,8 +38,10 @@ */ #pragma once - +/* Get the name of the default value fiven the param name */ #define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT + +/* Shortcuts to define parameters when the default value is defined according to PX4_PARAM_DEFAULT_VALUE_NAME */ #define PX4_PARAM_DEFINE_INT32(_name) PARAM_DEFINE_INT32(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) #define PX4_PARAM_DEFINE_FLOAT(_name) PARAM_DEFINE_FLOAT(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) @@ -50,14 +52,28 @@ */ #define __EXPORT #include "ros/ros.h" +/* Main entry point */ #define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv) + +/* Print/output wrappers */ #define PX4_WARN ROS_WARN #define PX4_INFO ROS_INFO + +/* Topic Handle */ #define PX4_TOPIC(_name) #_name + +/* Topic type */ #define PX4_TOPIC_T(_name) _name + +/* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj); +/* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); + +/* Parameter handle datatype */ typedef const char* px4_param_t; + +/* Helper fucntions to set ROS params, only int and float supported */ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value) { ros::param::set(name, value); return (px4_param_t)name; @@ -66,35 +82,53 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) { ros::param::set(name, value); return (px4_param_t)name; }; -// #define PARAM_DEFINE_INT32(_name, _default) static const int PX4_ROS_PARAM_DEFAULT_VALUE_NAME(_name) = _default; -// #define PARAM_DEFINE_FLOAT(_name, _default) static const float PX4_ROS_PARAM_DEFAULT_VALUE_NAME(_name) = _default; + +/* Initialize a param, in case of ROS the parameter is sent to the parameter server here */ #define PX4_PARAM_INIT(_name) PX4_ROS_PARAM_SET(#_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) -// #define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default) + +/* Get value of parameter */ #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) -#define PX4_PARAM_INT32_T int //XXX #else /* * Building for NuttX */ #include +/* Main entry point */ #define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) -#define PX4_WARN warnx + +/* Print/output wrappers */ #define PX4_WARN warnx #define PX4_INFO warnx + +/* Topic Handle */ #define PX4_TOPIC(_name) ORB_ID(_name) + +/* Topic type */ #define PX4_TOPIC_T(_name) _name##_s + +/* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval) +/* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) + +/* Parameter handle datatype */ typedef param_t px4_param_t; + +/* Initialize a param, get param handle */ #define PX4_PARAM_INIT(_name) param_find(#_name) + +/* Get value of parameter */ #define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt) -#define PX4_PARAM_INT32_T int32_t #endif -/* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */ +/* Shortcut for subscribing to topics + * Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback + */ #define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME #define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC)(__VA_ARGS__) + +/* shortcut for advertising topics */ #define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise(PX4_TOPIC(_name)) /* wrapper for 2d matrices */ @@ -102,6 +136,3 @@ typedef param_t px4_param_t; /* wrapper for rotation matrices stored in arrays */ #define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y) - -// #define PX4_PARAM_DEFAULT_INT32(_name, _value) static const PX4_PARAM_INT32_T PX4_PARAM_DEFAULT_VALUE_NAME(_name) = _value; -// #define PX4_PARAM_DEFAULT_FLOAT(_name, _value) static const float PX4_PARAM_DEFAULT_VALUE_NAME(_name) = _value; -- cgit v1.2.3 From 0f094d35d5dd183e44148eb1acbd7b7d76fde669 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Dec 2014 12:22:21 +0100 Subject: nodehandle: add documentation comments --- src/platforms/px4_nodehandle.h | 52 +++++++++++++++++++++++++++++++++++------- 1 file changed, 44 insertions(+), 8 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index a2775d69a..1762e1656 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -70,7 +70,11 @@ public: //XXX empty lists }; - /* Subscribe with callback to function */ + /** + * Subscribe with callback to function + * @param topic Name of the topic + * @param fb Callback, executed on receiving a new message + */ template Subscriber * subscribe(const char *topic, void(*fp)(M)) { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp); @@ -79,7 +83,11 @@ public: return sub; } - /* Subscribe with callback to class method */ + /** + * Subscribe with callback to class method + * @param topic Name of the topic + * @param fb Callback, executed on receiving a new message + */ template Subscriber * subscribe(const char *topic, void(T::*fp)(M), T *obj) { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj); @@ -88,6 +96,10 @@ public: return sub; } + /** + * Advertise topic + * @param topic Name of the topic + */ template Publisher * advertise(const char *topic) { ros::Publisher ros_pub = ros::NodeHandle::advertise(topic, kQueueSizeDefault); @@ -96,14 +108,21 @@ public: return pub; } + /** + * Calls all callback waiting to be called + */ + void spinOnce() { ros::spinOnce(); } + + /** + * Keeps calling callbacks for incomming messages, returns when module is terminated + */ void spin() { ros::spin(); } - void spinOnce() { ros::spinOnce(); } private: - static const uint32_t kQueueSizeDefault = 1000; - std::list _subs; - std::list _pubs; + static const uint32_t kQueueSizeDefault = 1000; /**< Size of queue for ROS */ + std::list _subs; /**< Subcriptions of node */ + std::list _pubs; /**< Publications of node */ }; #else class __EXPORT NodeHandle @@ -117,6 +136,13 @@ public: ~NodeHandle() {}; + /** + * Subscribe with callback to function + * @param meta Describes the topic which nodehande should subscribe to + * @param callback Callback, executed on receiving a new message + * @param interval Minimal interval between calls to callback + */ + template Subscriber * subscribe(const struct orb_metadata *meta, std::function callback, @@ -130,6 +156,10 @@ public: return (Subscriber*)sub_px4; } + /** + * Advertise topic + * @param meta Describes the topic which is advertised + */ template Publisher * advertise(const struct orb_metadata *meta) { //XXX @@ -137,6 +167,9 @@ public: return pub; } + /** + * Calls all callback waiting to be called + */ void spinOnce() { /* Loop through subscriptions, call callback for updated subscriptions */ uORB::SubscriptionNode *sub = _subs.getHead(); @@ -153,6 +186,9 @@ public: } } + /** + * Keeps calling callbacks for incomming messages, returns when module is terminated + */ void spin() { while (ok()) { const int timeout_ms = 100; @@ -176,8 +212,8 @@ public: } private: static const uint16_t kMaxSubscriptions = 100; - List _subs; - List _pubs; + List _subs; /**< Subcriptions of node */ + List _pubs; /**< Publications of node */ uORB::SubscriptionNode* _sub_min_interval; /**< Points to the sub wtih the smallest interval of all Subscriptions in _subs*/ }; -- cgit v1.2.3 From e0bb713bb03c0aa79e69496c787c012a8e1b78d1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Dec 2014 13:06:38 +0100 Subject: more documentation comments --- src/platforms/px4_middleware.h | 13 +++++++++++++ src/platforms/px4_publisher.h | 28 ++++++++++++++++++++++++---- src/platforms/px4_subscriber.h | 32 +++++++++++++++++++++++++++++--- 3 files changed, 66 insertions(+), 7 deletions(-) diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index dece72907..c1465b4b1 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -50,9 +50,15 @@ __EXPORT void init(int argc, char *argv[], const char *process_name); __EXPORT uint64_t get_time_micros(); #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/** + * Returns true if the app/task should continue to run + */ bool ok() { return ros::ok(); } #else extern bool task_should_exit; +/** + * Returns true if the app/task should continue to run + */ bool ok() { return !task_should_exit; } #endif @@ -60,8 +66,15 @@ class Rate { public: + /** + * Construct the Rate object and set rate + * @param rate_hz rate from which sleep time is calculated in Hz + */ explicit Rate(unsigned rate_hz) { sleep_interval = 1e6 / rate_hz; } + /** + * Sleep for 1/rate_hz s + */ void sleep() { usleep(sleep_interval); } private: diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index 9ce211d25..b67421066 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -52,33 +52,53 @@ namespace px4 class Publisher { public: + /** + * Construct Publisher by providing a ros::Publisher + * @param ros_pub the ros publisher which will be used to perform the publications + */ Publisher(ros::Publisher ros_pub) : _ros_pub(ros_pub) {} + ~Publisher() {}; + + /** Publishes msg + * @param msg the message which is published to the topic + */ template int publish(const M &msg) { _ros_pub.publish(msg); return 0; } private: - ros::Publisher _ros_pub; + ros::Publisher _ros_pub; /**< Handle to the ros publisher */ }; #else class Publisher : public uORB::PublicationNode { public: + /** + * Construct Publisher by providing orb meta data + * @param meta orb metadata for the topic which is used + * @param list publisher is added to this list + */ Publisher(const struct orb_metadata *meta, List * list) : uORB::PublicationNode(meta, list) {} + ~Publisher() {}; + + /** Publishes msg + * @param msg the message which is published to the topic + */ template int publish(const M &msg) { uORB::PublicationBase::update((void*)&msg); return 0; } - void update() { - //XXX list traversal callback, needed? - } ; + /** + * Empty callback for list traversal + */ + void update() {} ; }; #endif } diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 6e0ef8aed..fdd0367d5 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -54,15 +54,24 @@ namespace px4 class Subscriber { public: + /** + * Construct Subscriber by providing a ros::Subscriber + * @param ros_sub the ros subscriber which will be used to perform the publications + */ Subscriber(ros::Subscriber ros_sub) : _ros_sub(ros_sub) {} + ~Subscriber() {}; private: - ros::Subscriber _ros_sub; + ros::Subscriber _ros_sub; /**< Handle to ros subscriber */ }; + #else -// typedef std::function CallbackFunction; + +/** + * Subscriber class which is used by nodehandle when building for NuttX + */ class Subscriber { public: @@ -71,12 +80,22 @@ public: private: }; +/** + * Subscriber class that is templated with the uorb subscription message type + */ template class SubscriberPX4 : public Subscriber, public uORB::Subscription { public: + /** + * Construct SubscriberPX4 by providing orb meta data + * @param meta orb metadata for the topic which is used + * @param callback Callback, executed on receiving a new message + * @param interval Minimal interval between calls to callback + * @param list subscriber is added to this list + */ SubscriberPX4(const struct orb_metadata *meta, unsigned interval, std::function callback, @@ -86,8 +105,14 @@ public: _callback(callback) //XXX store callback {} + ~SubscriberPX4() {}; + /** + * Update Subscription + * Invoked by the list traversal in NodeHandle::spinOnce + * If new data is available the callback is called + */ void update() { if (!uORB::Subscription::updated()) { /* Topic not updated, do not call callback */ @@ -102,7 +127,8 @@ public: }; private: - std::function _callback; + std::function _callback; /**< Callback handle, + called when new data is available */ }; #endif -- cgit v1.2.3 From f4a326c2bf7af6eac86983cd65e66ff76e623e22 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Dec 2014 13:20:12 +0100 Subject: platform headers: fix code style --- src/platforms/px4_defines.h | 8 ++++--- src/platforms/px4_nodehandle.h | 49 ++++++++++++++++++++++++++---------------- src/platforms/px4_publisher.h | 7 +++--- src/platforms/px4_subscriber.h | 11 +++++----- 4 files changed, 45 insertions(+), 30 deletions(-) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 5858a69d8..af7188f32 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -71,14 +71,16 @@ #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); /* Parameter handle datatype */ -typedef const char* px4_param_t; +typedef const char *px4_param_t; /* Helper fucntions to set ROS params, only int and float supported */ -static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value) { +static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value) +{ ros::param::set(name, value); return (px4_param_t)name; }; -static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) { +static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) +{ ros::param::set(name, value); return (px4_param_t)name; }; diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 1762e1656..5b7247b20 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -66,7 +66,8 @@ public: _pubs() {} - ~NodeHandle() { + ~NodeHandle() + { //XXX empty lists }; @@ -76,9 +77,10 @@ public: * @param fb Callback, executed on receiving a new message */ template - Subscriber * subscribe(const char *topic, void(*fp)(M)) { + Subscriber *subscribe(const char *topic, void(*fp)(M)) + { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp); - Subscriber * sub = new Subscriber(ros_sub); + Subscriber *sub = new Subscriber(ros_sub); _subs.push_back(sub); return sub; } @@ -89,9 +91,10 @@ public: * @param fb Callback, executed on receiving a new message */ template - Subscriber * subscribe(const char *topic, void(T::*fp)(M), T *obj) { + Subscriber *subscribe(const char *topic, void(T::*fp)(M), T *obj) + { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj); - Subscriber * sub = new Subscriber(ros_sub); + Subscriber *sub = new Subscriber(ros_sub); _subs.push_back(sub); return sub; } @@ -101,7 +104,8 @@ public: * @param topic Name of the topic */ template - Publisher * advertise(const char *topic) { + Publisher *advertise(const char *topic) + { ros::Publisher ros_pub = ros::NodeHandle::advertise(topic, kQueueSizeDefault); Publisher *pub = new Publisher(ros_pub); _pubs.push_back(pub); @@ -121,8 +125,8 @@ public: private: static const uint32_t kQueueSizeDefault = 1000; /**< Size of queue for ROS */ - std::list _subs; /**< Subcriptions of node */ - std::list _pubs; /**< Publications of node */ + std::list _subs; /**< Subcriptions of node */ + std::list _pubs; /**< Publications of node */ }; #else class __EXPORT NodeHandle @@ -144,16 +148,18 @@ public: */ template - Subscriber * subscribe(const struct orb_metadata *meta, - std::function callback, - unsigned interval) { + Subscriber *subscribe(const struct orb_metadata *meta, + std::function callback, + unsigned interval) + { SubscriberPX4 *sub_px4 = new SubscriberPX4(meta, interval, callback, &_subs); /* Check if this is the smallest interval so far and update _sub_min_interval */ if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { _sub_min_interval = sub_px4; } - return (Subscriber*)sub_px4; + + return (Subscriber *)sub_px4; } /** @@ -161,16 +167,18 @@ public: * @param meta Describes the topic which is advertised */ template - Publisher * advertise(const struct orb_metadata *meta) { + Publisher *advertise(const struct orb_metadata *meta) + { //XXX - Publisher * pub = new Publisher(meta, &_pubs); + Publisher *pub = new Publisher(meta, &_pubs); return pub; } /** * Calls all callback waiting to be called */ - void spinOnce() { + void spinOnce() + { /* Loop through subscriptions, call callback for updated subscriptions */ uORB::SubscriptionNode *sub = _subs.getHead(); int count = 0; @@ -189,9 +197,11 @@ public: /** * Keeps calling callbacks for incomming messages, returns when module is terminated */ - void spin() { + void spin() + { while (ok()) { const int timeout_ms = 100; + /* Only continue in the loop if the nodehandle has subscriptions */ if (_sub_min_interval == nullptr) { usleep(timeout_ms * 1000); @@ -202,6 +212,7 @@ public: struct pollfd pfd; pfd.fd = _sub_min_interval->getHandle(); pfd.events = POLLIN; + if (poll(&pfd, 1, timeout_ms) <= 0) { /* timed out */ continue; @@ -212,9 +223,9 @@ public: } private: static const uint16_t kMaxSubscriptions = 100; - List _subs; /**< Subcriptions of node */ - List _pubs; /**< Publications of node */ - uORB::SubscriptionNode* _sub_min_interval; /**< Points to the sub wtih the smallest interval + List _subs; /**< Subcriptions of node */ + List _pubs; /**< Publications of node */ + uORB::SubscriptionNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval of all Subscriptions in _subs*/ }; #endif diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index b67421066..cb15eeb58 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -80,7 +80,7 @@ public: * @param list publisher is added to this list */ Publisher(const struct orb_metadata *meta, - List * list) : + List *list) : uORB::PublicationNode(meta, list) {} @@ -90,8 +90,9 @@ public: * @param msg the message which is published to the topic */ template - int publish(const M &msg) { - uORB::PublicationBase::update((void*)&msg); + int publish(const M &msg) + { + uORB::PublicationBase::update((void *)&msg); return 0; } diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index fdd0367d5..e995c6e49 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -97,9 +97,9 @@ public: * @param list subscriber is added to this list */ SubscriberPX4(const struct orb_metadata *meta, - unsigned interval, - std::function callback, - List * list) : + unsigned interval, + std::function callback, + List *list) : Subscriber(), uORB::Subscription(meta, interval, list), _callback(callback) @@ -113,7 +113,8 @@ public: * Invoked by the list traversal in NodeHandle::spinOnce * If new data is available the callback is called */ - void update() { + void update() + { if (!uORB::Subscription::updated()) { /* Topic not updated, do not call callback */ return; @@ -127,7 +128,7 @@ public: }; private: - std::function _callback; /**< Callback handle, + std::function _callback; /**< Callback handle, called when new data is available */ }; -- cgit v1.2.3 From 6f9cbd97510dce4d0bfe715fa77af3b99d282659 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Dec 2014 16:40:46 +0100 Subject: add genmsg and gencpp python modules, ros not required anymore for message generation --- .gitmodules | 6 ++++++ Makefile | 7 +++++-- Tools/gencpp | 1 + Tools/genmsg | 1 + 4 files changed, 13 insertions(+), 2 deletions(-) create mode 160000 Tools/gencpp create mode 160000 Tools/genmsg diff --git a/.gitmodules b/.gitmodules index 4b84afac2..4996b274b 100644 --- a/.gitmodules +++ b/.gitmodules @@ -7,3 +7,9 @@ [submodule "uavcan"] path = uavcan url = git://github.com/pavel-kirienko/uavcan.git +[submodule "Tools/genmsg"] + path = Tools/genmsg + url = https://github.com/ros/genmsg.git +[submodule "Tools/gencpp"] + path = Tools/gencpp + url = https://github.com/ros/gencpp.git diff --git a/Makefile b/Makefile index bdbc18be5..f2e467e5a 100644 --- a/Makefile +++ b/Makefile @@ -228,12 +228,15 @@ MSG_DIR = $(PX4_BASE)msg/px4_msgs MSG_TEMPLATE_DIR = $(PX4_BASE)msg/templates TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics TOPICS_TEMPORARY_DIR = $(BUILD_DIR)topics_temporary +GENMSG_PYTHONPATH = $(PX4_BASE)/Tools/genmsg/src +GENCPP_PYTHONPATH = $(PX4_BASE)/Tools/gencpp/src .PHONY: generateuorbtopicheaders generateuorbtopicheaders: @$(ECHO) "Generating uORB topic headers" - $(Q) ($(PX4_BASE)/Tools/px_generate_uorb_topic_headers.py -d $(MSG_DIR) \ - -o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR) -t $(TOPICS_TEMPORARY_DIR)) + $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH) $(PYTHON) \ + $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ + -d $(MSG_DIR) -o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR) -t $(TOPICS_TEMPORARY_DIR)) # clean up temporary files $(Q) (rm -r $(TOPICS_TEMPORARY_DIR)) diff --git a/Tools/gencpp b/Tools/gencpp new file mode 160000 index 000000000..26a86f04b --- /dev/null +++ b/Tools/gencpp @@ -0,0 +1 @@ +Subproject commit 26a86f04bcec0018af6652b3ddd3f680e6e3fa2a diff --git a/Tools/genmsg b/Tools/genmsg new file mode 160000 index 000000000..72f0383f0 --- /dev/null +++ b/Tools/genmsg @@ -0,0 +1 @@ +Subproject commit 72f0383f0e6a489214c51802ae12d6e271b1e454 -- cgit v1.2.3 From 4c950eb76b0d63be7936dfcd68eb6eed266b91ad Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Dec 2014 08:57:24 +0100 Subject: mc att control: make the main app use the base class, move euroc functions into own class --- src/modules/mc_att_control/mc_att_control_base.cpp | 120 +-------- src/modules/mc_att_control/mc_att_control_base.h | 17 +- src/modules/mc_att_control/mc_att_control_main.cpp | 300 ++------------------- src/modules/mc_att_control/mc_att_control_sim.cpp | 107 ++++++++ src/modules/mc_att_control/mc_att_control_sim.h | 95 +++++++ src/modules/mc_att_control/module.mk | 3 +- 6 files changed, 241 insertions(+), 401 deletions(-) create mode 100644 src/modules/mc_att_control/mc_att_control_sim.cpp create mode 100644 src/modules/mc_att_control/mc_att_control_sim.h diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index baf2bfe65..d21deb715 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -31,7 +31,7 @@ /** * @file mc_att_control_base.h - * + * * @author Roman Bapst * */ @@ -47,13 +47,12 @@ using namespace std; #endif MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : + _task_should_exit(false), + _control_task(-1), - _task_should_exit(false), _control_task(-1), - - _actuators_0_circuit_breaker_enabled(false), + _actuators_0_circuit_breaker_enabled(false), - /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) + _publish_att_sp(false) { memset(&_v_att, 0, sizeof(_v_att)); @@ -82,38 +81,14 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : _att_control.zero(); _I.identity(); - - // setup standard gains - _params.att_p(0) = 5.0; - _params.rate_p(0) = 0.05; - _params.rate_i(0) = 0.0; - _params.rate_d(0) = 0.003; - /* pitch gains */ - _params.att_p(1) = 5.0; - _params.rate_p(1) = 0.05; - _params.rate_i(1) = 0.0; - _params.rate_d(1) = 0.003; - /* yaw gains */ - _params.att_p(2) = 2.8; - _params.rate_p(2) = 0.2; - _params.rate_i(2) = 0.1; - _params.rate_d(2) = 0.0; - _params.yaw_rate_max = 0.5; - _params.yaw_ff = 0.5; - _params.man_roll_max = 0.6; - _params.man_pitch_max = 0.6; - _params.man_yaw_max = 0.6; } MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() { } -void MulticopterAttitudeControlBase::vehicle_attitude_setpoint_poll() { -} - void MulticopterAttitudeControlBase::control_attitude(float dt) { float yaw_sp_move_rate = 0.0f; - bool publish_att_sp = false; + _publish_att_sp = false; if (_v_control_mode.flag_control_manual_enabled) { /* manual input, set or modify attitude setpoint */ @@ -127,7 +102,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { if (!_v_control_mode.flag_control_climb_rate_enabled) { /* pass throttle directly if not in altitude stabilized mode */ _v_att_sp.thrust = _manual_control_sp.z; - publish_att_sp = true; + _publish_att_sp = true; } if (!_armed.armed) { @@ -156,7 +131,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); } _v_att_sp.R_valid = false; - publish_att_sp = true; + _publish_att_sp = true; } /* reset yaw setpint to current position if needed */ @@ -164,7 +139,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { _reset_yaw_sp = false; _v_att_sp.yaw_body = _v_att.yaw; _v_att_sp.R_valid = false; - publish_att_sp = true; + _publish_att_sp = true; } if (!_v_control_mode.flag_control_velocity_enabled) { @@ -173,7 +148,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { _v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max; _v_att_sp.R_valid = false; - publish_att_sp = true; + _publish_att_sp = true; } } else { @@ -204,20 +179,6 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { _v_att_sp.R_valid = true; } -// /* 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); -// -// } else { -// _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), -// &_v_att_sp); -// } -// } - /* rotation matrix for current state */ math::Matrix<3, 3> R; R.set(_v_att.R); @@ -340,64 +301,3 @@ void MulticopterAttitudeControlBase::set_actuator_controls() { _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; } - -void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion attitude) { - math::Quaternion quat; - quat(0) = (float)attitude.w(); - quat(1) = (float)attitude.x(); - quat(2) = (float)attitude.y(); - quat(3) = (float)attitude.z(); - - _v_att.q[0] = quat(0); - _v_att.q[1] = quat(1); - _v_att.q[2] = quat(2); - _v_att.q[3] = quat(3); - - math::Matrix<3,3> Rot = quat.to_dcm(); - _v_att.R[0][0] = Rot(0,0); - _v_att.R[1][0] = Rot(1,0); - _v_att.R[2][0] = Rot(2,0); - _v_att.R[0][1] = Rot(0,1); - _v_att.R[1][1] = Rot(1,1); - _v_att.R[2][1] = Rot(2,1); - _v_att.R[0][2] = Rot(0,2); - _v_att.R[1][2] = Rot(1,2); - _v_att.R[2][2] = Rot(2,2); - - _v_att.R_valid = true; -} - -void MulticopterAttitudeControlBase::set_attitude_rates(const Eigen::Vector3d& angular_rate) { - // check if this is consistent !!! - _v_att.rollspeed = angular_rate(0); - _v_att.pitchspeed = angular_rate(1); - _v_att.yawspeed = angular_rate(2); -} - -void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) { - _v_att_sp.roll_body = control_attitude_thrust_reference(0); - _v_att_sp.pitch_body = control_attitude_thrust_reference(1); - _v_att_sp.yaw_body = control_attitude_thrust_reference(2); - _v_att_sp.thrust = (control_attitude_thrust_reference(3) -30)*(-1)/30; - - // setup rotation matrix - math::Matrix<3,3> Rot_sp; - Rot_sp.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body); - _v_att_sp.R_body[0][0] = Rot_sp(0,0); - _v_att_sp.R_body[1][0] = Rot_sp(1,0); - _v_att_sp.R_body[2][0] = Rot_sp(2,0); - _v_att_sp.R_body[0][1] = Rot_sp(0,1); - _v_att_sp.R_body[1][1] = Rot_sp(1,1); - _v_att_sp.R_body[2][1] = Rot_sp(2,1); - _v_att_sp.R_body[0][2] = Rot_sp(0,2); - _v_att_sp.R_body[1][2] = Rot_sp(1,2); - _v_att_sp.R_body[2][2] = Rot_sp(2,2); -} - -void MulticopterAttitudeControlBase::get_mixer_input(Eigen::Vector4d& motor_inputs) { - motor_inputs(0) = _actuators.control[0]; - motor_inputs(1) = _actuators.control[1]; - motor_inputs(2) = _actuators.control[2]; - motor_inputs(3) = _actuators.control[3]; -} - diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 515fb0c14..009f76268 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -34,7 +34,7 @@ /** * @file mc_att_control_base.h - * + * * @author Roman Bapst * */ @@ -83,12 +83,7 @@ public: */ void control_attitude(float dt); void control_attitude_rates(float dt); - - // setters and getters for interface with euroc-gazebo simulator - void set_attitude(const Eigen::Quaternion attitude); - void set_attitude_rates(const Eigen::Vector3d& angular_rate); - void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); - void get_mixer_input(Eigen::Vector4d& motor_inputs); + void set_actuator_controls(); protected: @@ -105,8 +100,6 @@ protected: struct actuator_controls_s _actuators; /**< actuator controls */ struct actuator_armed_s _armed; /**< actuator arming status */ - perf_counter_t _loop_perf; /**< loop performance counter */ - math::Vector<3> _rates_prev; /**< angular rates on previous step */ math::Vector<3> _rates_sp; /**< angular rates setpoint */ math::Vector<3> _rates_int; /**< angular rates integral error */ @@ -130,8 +123,10 @@ protected: float man_yaw_max; math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ } _params; - - void vehicle_attitude_setpoint_poll(); //provisional + + bool _publish_att_sp; + + virtual void vehicle_attitude_setpoint_poll() = 0; }; 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 81a13edb8..7bc638e5d 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -75,6 +75,7 @@ #include #include #include +#include "mc_att_control_base.h" /** * Multicopter attitude control app start / stop handling function @@ -87,7 +88,8 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f -class MulticopterAttitudeControl +class MulticopterAttitudeControl : + public MulticopterAttitudeControlBase { public: /** @@ -108,7 +110,6 @@ public: int start(); private: - bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ @@ -124,28 +125,6 @@ private: orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ - bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ - - struct vehicle_attitude_s _v_att; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ - struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ - struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ - struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */ - struct actuator_controls_s _actuators; /**< actuator controls */ - struct actuator_armed_s _armed; /**< actuator arming status */ - - perf_counter_t _loop_perf; /**< loop performance counter */ - - math::Vector<3> _rates_prev; /**< angular rates on previous step */ - math::Vector<3> _rates_sp; /**< angular rates setpoint */ - math::Vector<3> _rates_int; /**< angular rates integral error */ - float _thrust_sp; /**< thrust setpoint */ - math::Vector<3> _att_control; /**< attitude control vector */ - - math::Matrix<3, 3> _I; /**< identity matrix */ - - bool _reset_yaw_sp; /**< reset yaw setpoint flag */ - struct { param_t roll_p; param_t roll_rate_p; @@ -170,19 +149,7 @@ private: param_t acro_yaw_max; } _params_handles; /**< handles for interesting parameters */ - struct { - math::Vector<3> att_p; /**< P gain for angular error */ - math::Vector<3> rate_p; /**< P gain for angular rate error */ - math::Vector<3> rate_i; /**< I gain for angular rate error */ - math::Vector<3> rate_d; /**< D gain for angular rate error */ - float yaw_ff; /**< yaw control feed-forward */ - float yaw_rate_max; /**< max yaw rate */ - - float man_roll_max; - float man_pitch_max; - float man_yaw_max; - math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ - } _params; + perf_counter_t _loop_perf; /**< loop performance counter */ /** * Update our local parameter cache. @@ -219,16 +186,6 @@ private: */ void arming_status_poll(); - /** - * Attitude controller. - */ - void control_attitude(float dt); - - /** - * Attitude rates controller. - */ - void control_attitude_rates(float dt); - /** * Shim for calling task_main from task_create. */ @@ -253,11 +210,8 @@ MulticopterAttitudeControl *g_control; } MulticopterAttitudeControl::MulticopterAttitudeControl() : - - _task_should_exit(false), - _control_task(-1), - -/* subscriptions */ + MulticopterAttitudeControlBase(), + /* subscriptions */ _v_att_sub(-1), _v_att_sp_sub(-1), _v_control_mode_sub(-1), @@ -265,14 +219,12 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _manual_control_sp_sub(-1), _armed_sub(-1), -/* publications */ + /* publications */ _att_sp_pub(-1), _v_rates_sp_pub(-1), _actuators_0_pub(-1), - _actuators_0_circuit_breaker_enabled(false), - -/* performance counters */ + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) { @@ -489,229 +441,6 @@ MulticopterAttitudeControl::arming_status_poll() } } -/* - * Attitude controller. - * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode) - * Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes) - */ -void -MulticopterAttitudeControl::control_attitude(float dt) -{ - float yaw_sp_move_rate = 0.0f; - bool publish_att_sp = false; - - if (_v_control_mode.flag_control_manual_enabled) { - /* manual input, set or modify attitude setpoint */ - - if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) { - /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ - vehicle_attitude_setpoint_poll(); - } - - if (!_v_control_mode.flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude stabilized mode */ - _v_att_sp.thrust = _manual_control_sp.z; - publish_att_sp = true; - } - - if (!_armed.armed) { - /* reset yaw setpoint when disarmed */ - _reset_yaw_sp = true; - } - - /* move yaw setpoint in all modes */ - if (_v_att_sp.thrust < 0.1f) { - // TODO - //if (_status.condition_landed) { - /* reset yaw setpoint if on ground */ - // reset_yaw_sp = true; - //} - } else { - /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max; - _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.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.yaw_body - _v_att.yaw); - if (yaw_offs < - yaw_offs_max) { - _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max); - - } else if (yaw_offs > yaw_offs_max) { - _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); - } - _v_att_sp.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.yaw_body = _v_att.yaw; - _v_att_sp.R_valid = false; - publish_att_sp = true; - } - - if (!_v_control_mode.flag_control_velocity_enabled) { - /* update attitude setpoint if not in position control mode */ - _v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max; - _v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max; - _v_att_sp.R_valid = false; - publish_att_sp = true; - } - - } else { - /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ - vehicle_attitude_setpoint_poll(); - - /* reset yaw setpoint after non-manual control mode */ - _reset_yaw_sp = true; - } - - _thrust_sp = _v_att_sp.thrust; - - /* construct attitude setpoint rotation matrix */ - math::Matrix<3, 3> R_sp; - - if (_v_att_sp.R_valid) { - /* rotation matrix in _att_sp is valid, use it */ - R_sp.set(&_v_att_sp.R_body[0][0]); - - } else { - /* rotation matrix in _att_sp is not valid, use euler angles instead */ - R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); - - /* copy rotation matrix back to setpoint struct */ - memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body)); - _v_att_sp.R_valid = true; - } - - /* 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); - - } else { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); - } - } - - /* rotation matrix for current state */ - math::Matrix<3, 3> R; - R.set(_v_att.R); - - /* all input data is ready, run controller itself */ - - /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ - math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2)); - math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); - - /* axis and sin(angle) of desired rotation */ - math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z); - - /* calculate angle error */ - float e_R_z_sin = e_R.length(); - float e_R_z_cos = R_z * R_sp_z; - - /* calculate weight for yaw control */ - float yaw_w = R_sp(2, 2) * R_sp(2, 2); - - /* calculate rotation matrix after roll/pitch only rotation */ - math::Matrix<3, 3> R_rp; - - if (e_R_z_sin > 0.0f) { - /* get axis-angle representation */ - float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); - math::Vector<3> e_R_z_axis = e_R / e_R_z_sin; - - e_R = e_R_z_axis * e_R_z_angle; - - /* cross product matrix for e_R_axis */ - math::Matrix<3, 3> e_R_cp; - e_R_cp.zero(); - e_R_cp(0, 1) = -e_R_z_axis(2); - e_R_cp(0, 2) = e_R_z_axis(1); - e_R_cp(1, 0) = e_R_z_axis(2); - e_R_cp(1, 2) = -e_R_z_axis(0); - e_R_cp(2, 0) = -e_R_z_axis(1); - e_R_cp(2, 1) = e_R_z_axis(0); - - /* rotation matrix for roll/pitch only rotation */ - R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); - - } else { - /* zero roll/pitch rotation */ - R_rp = R; - } - - /* R_rp and R_sp has the same Z axis, calculate yaw error */ - math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); - math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); - e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; - - if (e_R_z_cos < 0.0f) { - /* for large thrust vector rotations use another rotation method: - * calculate angle and axis for R -> R_sp rotation directly */ - math::Quaternion q; - q.from_dcm(R.transposed() * R_sp); - math::Vector<3> e_R_d = q.imag(); - e_R_d.normalize(); - e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); - - /* use fusion of Z axis based rotation and direct rotation */ - float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; - e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; - } - - /* calculate angular rates setpoint */ - _rates_sp = _params.att_p.emult(e_R); - - /* limit yaw rate */ - _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max); - - /* feed forward yaw setpoint rate */ - _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; -} - -/* - * Attitude rates controller. - * Input: '_rates_sp' vector, '_thrust_sp' - * Output: '_att_control' vector - */ -void -MulticopterAttitudeControl::control_attitude_rates(float dt) -{ - /* reset integral if disarmed */ - if (!_armed.armed) { - _rates_int.zero(); - } - - /* current body angular rates */ - math::Vector<3> rates; - rates(0) = _v_att.rollspeed; - rates(1) = _v_att.pitchspeed; - rates(2) = _v_att.yawspeed; - - /* angular rates error */ - math::Vector<3> rates_err = _rates_sp - rates; - _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; - _rates_prev = rates; - - /* update integral only if not saturated on low limit */ - if (_thrust_sp > MIN_TAKEOFF_THRUST) { - for (int i = 0; i < 3; i++) { - if (fabsf(_att_control(i)) < _thrust_sp) { - float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; - - if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && - _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { - _rates_int(i) = rate_i; - } - } - } - } -} - void MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -787,6 +516,19 @@ MulticopterAttitudeControl::task_main() 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(); + + 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); + } + } + /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp new file mode 100644 index 000000000..61a4237fc --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_sim.cpp @@ -0,0 +1,107 @@ +/* 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_base.h + * + * @author Roman Bapst + * + */ + +#include "mc_att_control_sim.h" +#include +#include + +#ifdef CONFIG_ARCH_ARM +#else +#include +using namespace std; +#endif + +void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion attitude) { + math::Quaternion quat; + quat(0) = (float)attitude.w(); + quat(1) = (float)attitude.x(); + quat(2) = (float)attitude.y(); + quat(3) = (float)attitude.z(); + + _v_att.q[0] = quat(0); + _v_att.q[1] = quat(1); + _v_att.q[2] = quat(2); + _v_att.q[3] = quat(3); + + math::Matrix<3,3> Rot = quat.to_dcm(); + _v_att.R[0][0] = Rot(0,0); + _v_att.R[1][0] = Rot(1,0); + _v_att.R[2][0] = Rot(2,0); + _v_att.R[0][1] = Rot(0,1); + _v_att.R[1][1] = Rot(1,1); + _v_att.R[2][1] = Rot(2,1); + _v_att.R[0][2] = Rot(0,2); + _v_att.R[1][2] = Rot(1,2); + _v_att.R[2][2] = Rot(2,2); + + _v_att.R_valid = true; +} + +void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d& angular_rate) { + // check if this is consistent !!! + _v_att.rollspeed = angular_rate(0); + _v_att.pitchspeed = angular_rate(1); + _v_att.yawspeed = angular_rate(2); +} + +void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) { + _v_att_sp.roll_body = control_attitude_thrust_reference(0); + _v_att_sp.pitch_body = control_attitude_thrust_reference(1); + _v_att_sp.yaw_body = control_attitude_thrust_reference(2); + _v_att_sp.thrust = (control_attitude_thrust_reference(3) -30)*(-1)/30; + + // setup rotation matrix + math::Matrix<3,3> Rot_sp; + Rot_sp.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body); + _v_att_sp.R_body[0][0] = Rot_sp(0,0); + _v_att_sp.R_body[1][0] = Rot_sp(1,0); + _v_att_sp.R_body[2][0] = Rot_sp(2,0); + _v_att_sp.R_body[0][1] = Rot_sp(0,1); + _v_att_sp.R_body[1][1] = Rot_sp(1,1); + _v_att_sp.R_body[2][1] = Rot_sp(2,1); + _v_att_sp.R_body[0][2] = Rot_sp(0,2); + _v_att_sp.R_body[1][2] = Rot_sp(1,2); + _v_att_sp.R_body[2][2] = Rot_sp(2,2); +} + +void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d& motor_inputs) { + motor_inputs(0) = _actuators.control[0]; + motor_inputs(1) = _actuators.control[1]; + motor_inputs(2) = _actuators.control[2]; + motor_inputs(3) = _actuators.control[3]; +} diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h new file mode 100644 index 000000000..20752b054 --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_sim.h @@ -0,0 +1,95 @@ +#ifndef MC_ATT_CONTROL_BASE_H_ +#define MC_ATT_CONTROL_BASE_H_ + +/* 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_base.h + * + * @author Roman Bapst + * + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#inlcude "mc_att_control_base.h" + + + +#define YAW_DEADZONE 0.05f +#define MIN_TAKEOFF_THRUST 0.2f +#define RATES_I_LIMIT 0.3f + +class MulticopterAttitudeControlSim : + public MulticopterAttitudeControlBase + +{ +public: + /** + * Constructor + */ + MulticopterAttitudeControlSim(); + + /** + * Destructor + */ + ~MulticopterAttitudeControlSim(); + + /* setters and getters for interface with euroc-gazebo simulator */ + void set_attitude(const Eigen::Quaternion attitude); + void set_attitude_rates(const Eigen::Vector3d& angular_rate); + void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); + void get_mixer_input(Eigen::Vector4d& motor_inputs); + +protected: + void vehicle_attitude_setpoint_poll() {}; + + +}; + +#endif /* MC_ATT_CONTROL_BASE_H_ */ diff --git a/src/modules/mc_att_control/module.mk b/src/modules/mc_att_control/module.mk index 64b876f69..ecd251d45 100644 --- a/src/modules/mc_att_control/module.mk +++ b/src/modules/mc_att_control/module.mk @@ -38,4 +38,5 @@ MODULE_COMMAND = mc_att_control SRCS = mc_att_control_main.cpp \ - mc_att_control_params.c + mc_att_control_base.cpp \ + mc_att_control_params.c -- cgit v1.2.3 From 2f646e7082b49ef92bf9be1defd6fb7fdeec8480 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Dec 2014 09:02:06 +0100 Subject: mc att control: ran fix code style script --- src/modules/mc_att_control/mc_att_control_base.cpp | 36 ++++++++------ src/modules/mc_att_control/mc_att_control_base.h | 3 +- src/modules/mc_att_control/mc_att_control_main.cpp | 23 +++++---- src/modules/mc_att_control/mc_att_control_sim.cpp | 56 ++++++++++++---------- src/modules/mc_att_control/mc_att_control_sim.h | 6 +-- 5 files changed, 71 insertions(+), 53 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index d21deb715..a3b0340d2 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -83,10 +83,12 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : _I.identity(); } -MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() { +MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() +{ } -void MulticopterAttitudeControlBase::control_attitude(float dt) { +void MulticopterAttitudeControlBase::control_attitude(float dt) +{ float yaw_sp_move_rate = 0.0f; _publish_att_sp = false; @@ -94,7 +96,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { /* manual input, set or modify attitude setpoint */ if (_v_control_mode.flag_control_velocity_enabled - || _v_control_mode.flag_control_climb_rate_enabled) { + || _v_control_mode.flag_control_climb_rate_enabled) { /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ vehicle_attitude_setpoint_poll(); } @@ -121,15 +123,17 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { /* move yaw setpoint */ yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max; _v_att_sp.yaw_body = _wrap_pi( - _v_att_sp.yaw_body + yaw_sp_move_rate * dt); + _v_att_sp.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.yaw_body - _v_att.yaw); + if (yaw_offs < -yaw_offs_max) { _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max); } else if (yaw_offs > yaw_offs_max) { _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); } + _v_att_sp.R_valid = false; _publish_att_sp = true; } @@ -146,7 +150,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { /* update attitude setpoint if not in position control mode */ _v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max; _v_att_sp.pitch_body = -_manual_control_sp.x - * _params.man_pitch_max; + * _params.man_pitch_max; _v_att_sp.R_valid = false; _publish_att_sp = true; } @@ -175,7 +179,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { /* copy rotation matrix back to setpoint struct */ memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], - sizeof(_v_att_sp.R_body)); + sizeof(_v_att_sp.R_body)); _v_att_sp.R_valid = true; } @@ -221,8 +225,8 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { /* rotation matrix for roll/pitch only rotation */ R_rp = R - * (_I + e_R_cp * e_R_z_sin - + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + * (_I + e_R_cp * e_R_z_sin + + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); } else { /* zero roll/pitch rotation */ @@ -253,13 +257,14 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { /* limit yaw rate */ _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, - _params.yaw_rate_max); + _params.yaw_rate_max); /* feed forward yaw setpoint rate */ _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; } -void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { +void MulticopterAttitudeControlBase::control_attitude_rates(float dt) +{ /* reset integral if disarmed */ if (!_armed.armed) { _rates_int.zero(); @@ -274,7 +279,7 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { /* angular rates error */ math::Vector < 3 > rates_err = _rates_sp - rates; _att_control = _params.rate_p.emult(rates_err) - + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; + + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; _rates_prev = rates; /* update integral only if not saturated on low limit */ @@ -282,11 +287,11 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { for (int i = 0; i < 3; i++) { if (fabsf(_att_control(i)) < _thrust_sp) { float rate_i = _rates_int(i) - + _params.rate_i(i) * rates_err(i) * dt; + + _params.rate_i(i) * rates_err(i) * dt; if (isfinite( - rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && - _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { + rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { _rates_int(i) = rate_i; } } @@ -295,7 +300,8 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { } -void MulticopterAttitudeControlBase::set_actuator_controls() { +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; diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 009f76268..e7793e624 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -64,7 +64,8 @@ #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f -class MulticopterAttitudeControlBase { +class MulticopterAttitudeControlBase +{ public: /** * Constructor 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 7bc638e5d..d6a58f418 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -477,8 +477,9 @@ MulticopterAttitudeControl::task_main() int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit */ - if (pret == 0) + if (pret == 0) { continue; + } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { @@ -522,10 +523,11 @@ MulticopterAttitudeControl::task_main() if (_att_sp_pub > 0) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, - &_v_att_sp); + &_v_att_sp); + } else { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), - &_v_att_sp); + &_v_att_sp); } } @@ -547,7 +549,8 @@ MulticopterAttitudeControl::task_main() /* 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); + _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 */ @@ -630,18 +633,21 @@ MulticopterAttitudeControl::start() int mc_att_control_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { errx(1, "usage: mc_att_control {start|stop|status}"); + } if (!strcmp(argv[1], "start")) { - if (mc_att_control::g_control != nullptr) + if (mc_att_control::g_control != nullptr) { errx(1, "already running"); + } mc_att_control::g_control = new MulticopterAttitudeControl; - if (mc_att_control::g_control == nullptr) + if (mc_att_control::g_control == nullptr) { errx(1, "alloc failed"); + } if (OK != mc_att_control::g_control->start()) { delete mc_att_control::g_control; @@ -653,8 +659,9 @@ int mc_att_control_main(int argc, char *argv[]) } if (!strcmp(argv[1], "stop")) { - if (mc_att_control::g_control == nullptr) + if (mc_att_control::g_control == nullptr) { errx(1, "not running"); + } delete mc_att_control::g_control; mc_att_control::g_control = nullptr; diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp index 61a4237fc..faf729420 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.cpp +++ b/src/modules/mc_att_control/mc_att_control_sim.cpp @@ -46,7 +46,8 @@ using namespace std; #endif -void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion attitude) { +void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion attitude) +{ math::Quaternion quat; quat(0) = (float)attitude.w(); quat(1) = (float)attitude.x(); @@ -58,48 +59,51 @@ void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion _v_att.q[2] = quat(2); _v_att.q[3] = quat(3); - math::Matrix<3,3> Rot = quat.to_dcm(); - _v_att.R[0][0] = Rot(0,0); - _v_att.R[1][0] = Rot(1,0); - _v_att.R[2][0] = Rot(2,0); - _v_att.R[0][1] = Rot(0,1); - _v_att.R[1][1] = Rot(1,1); - _v_att.R[2][1] = Rot(2,1); - _v_att.R[0][2] = Rot(0,2); - _v_att.R[1][2] = Rot(1,2); - _v_att.R[2][2] = Rot(2,2); + math::Matrix<3, 3> Rot = quat.to_dcm(); + _v_att.R[0][0] = Rot(0, 0); + _v_att.R[1][0] = Rot(1, 0); + _v_att.R[2][0] = Rot(2, 0); + _v_att.R[0][1] = Rot(0, 1); + _v_att.R[1][1] = Rot(1, 1); + _v_att.R[2][1] = Rot(2, 1); + _v_att.R[0][2] = Rot(0, 2); + _v_att.R[1][2] = Rot(1, 2); + _v_att.R[2][2] = Rot(2, 2); _v_att.R_valid = true; } -void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d& angular_rate) { +void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d &angular_rate) +{ // check if this is consistent !!! _v_att.rollspeed = angular_rate(0); _v_att.pitchspeed = angular_rate(1); _v_att.yawspeed = angular_rate(2); } -void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) { +void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference) +{ _v_att_sp.roll_body = control_attitude_thrust_reference(0); _v_att_sp.pitch_body = control_attitude_thrust_reference(1); _v_att_sp.yaw_body = control_attitude_thrust_reference(2); - _v_att_sp.thrust = (control_attitude_thrust_reference(3) -30)*(-1)/30; + _v_att_sp.thrust = (control_attitude_thrust_reference(3) - 30) * (-1) / 30; // setup rotation matrix - math::Matrix<3,3> Rot_sp; - Rot_sp.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body); - _v_att_sp.R_body[0][0] = Rot_sp(0,0); - _v_att_sp.R_body[1][0] = Rot_sp(1,0); - _v_att_sp.R_body[2][0] = Rot_sp(2,0); - _v_att_sp.R_body[0][1] = Rot_sp(0,1); - _v_att_sp.R_body[1][1] = Rot_sp(1,1); - _v_att_sp.R_body[2][1] = Rot_sp(2,1); - _v_att_sp.R_body[0][2] = Rot_sp(0,2); - _v_att_sp.R_body[1][2] = Rot_sp(1,2); - _v_att_sp.R_body[2][2] = Rot_sp(2,2); + math::Matrix<3, 3> Rot_sp; + Rot_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); + _v_att_sp.R_body[0][0] = Rot_sp(0, 0); + _v_att_sp.R_body[1][0] = Rot_sp(1, 0); + _v_att_sp.R_body[2][0] = Rot_sp(2, 0); + _v_att_sp.R_body[0][1] = Rot_sp(0, 1); + _v_att_sp.R_body[1][1] = Rot_sp(1, 1); + _v_att_sp.R_body[2][1] = Rot_sp(2, 1); + _v_att_sp.R_body[0][2] = Rot_sp(0, 2); + _v_att_sp.R_body[1][2] = Rot_sp(1, 2); + _v_att_sp.R_body[2][2] = Rot_sp(2, 2); } -void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d& motor_inputs) { +void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d &motor_inputs) +{ motor_inputs(0) = _actuators.control[0]; motor_inputs(1) = _actuators.control[1]; motor_inputs(2) = _actuators.control[2]; diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h index 20752b054..0b48455af 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.h +++ b/src/modules/mc_att_control/mc_att_control_sim.h @@ -82,9 +82,9 @@ public: /* setters and getters for interface with euroc-gazebo simulator */ void set_attitude(const Eigen::Quaternion attitude); - void set_attitude_rates(const Eigen::Vector3d& angular_rate); - void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); - void get_mixer_input(Eigen::Vector4d& motor_inputs); + void set_attitude_rates(const Eigen::Vector3d &angular_rate); + void set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference); + void get_mixer_input(Eigen::Vector4d &motor_inputs); protected: void vehicle_attitude_setpoint_poll() {}; -- cgit v1.2.3 From eafc4b5f9eefabca5fba404d75b1e1425b2f579b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Dec 2014 08:57:24 +0100 Subject: mc att control: make the main app use the base class, move euroc functions into own class --- src/modules/mc_att_control/mc_att_control_base.cpp | 120 +-------- src/modules/mc_att_control/mc_att_control_base.h | 17 +- src/modules/mc_att_control/mc_att_control_main.cpp | 300 ++------------------- src/modules/mc_att_control/mc_att_control_sim.cpp | 107 ++++++++ src/modules/mc_att_control/mc_att_control_sim.h | 95 +++++++ src/modules/mc_att_control/module.mk | 3 +- 6 files changed, 241 insertions(+), 401 deletions(-) create mode 100644 src/modules/mc_att_control/mc_att_control_sim.cpp create mode 100644 src/modules/mc_att_control/mc_att_control_sim.h diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index baf2bfe65..d21deb715 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -31,7 +31,7 @@ /** * @file mc_att_control_base.h - * + * * @author Roman Bapst * */ @@ -47,13 +47,12 @@ using namespace std; #endif MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : + _task_should_exit(false), + _control_task(-1), - _task_should_exit(false), _control_task(-1), - - _actuators_0_circuit_breaker_enabled(false), + _actuators_0_circuit_breaker_enabled(false), - /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) + _publish_att_sp(false) { memset(&_v_att, 0, sizeof(_v_att)); @@ -82,38 +81,14 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : _att_control.zero(); _I.identity(); - - // setup standard gains - _params.att_p(0) = 5.0; - _params.rate_p(0) = 0.05; - _params.rate_i(0) = 0.0; - _params.rate_d(0) = 0.003; - /* pitch gains */ - _params.att_p(1) = 5.0; - _params.rate_p(1) = 0.05; - _params.rate_i(1) = 0.0; - _params.rate_d(1) = 0.003; - /* yaw gains */ - _params.att_p(2) = 2.8; - _params.rate_p(2) = 0.2; - _params.rate_i(2) = 0.1; - _params.rate_d(2) = 0.0; - _params.yaw_rate_max = 0.5; - _params.yaw_ff = 0.5; - _params.man_roll_max = 0.6; - _params.man_pitch_max = 0.6; - _params.man_yaw_max = 0.6; } MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() { } -void MulticopterAttitudeControlBase::vehicle_attitude_setpoint_poll() { -} - void MulticopterAttitudeControlBase::control_attitude(float dt) { float yaw_sp_move_rate = 0.0f; - bool publish_att_sp = false; + _publish_att_sp = false; if (_v_control_mode.flag_control_manual_enabled) { /* manual input, set or modify attitude setpoint */ @@ -127,7 +102,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { if (!_v_control_mode.flag_control_climb_rate_enabled) { /* pass throttle directly if not in altitude stabilized mode */ _v_att_sp.thrust = _manual_control_sp.z; - publish_att_sp = true; + _publish_att_sp = true; } if (!_armed.armed) { @@ -156,7 +131,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); } _v_att_sp.R_valid = false; - publish_att_sp = true; + _publish_att_sp = true; } /* reset yaw setpint to current position if needed */ @@ -164,7 +139,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { _reset_yaw_sp = false; _v_att_sp.yaw_body = _v_att.yaw; _v_att_sp.R_valid = false; - publish_att_sp = true; + _publish_att_sp = true; } if (!_v_control_mode.flag_control_velocity_enabled) { @@ -173,7 +148,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { _v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max; _v_att_sp.R_valid = false; - publish_att_sp = true; + _publish_att_sp = true; } } else { @@ -204,20 +179,6 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { _v_att_sp.R_valid = true; } -// /* 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); -// -// } else { -// _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), -// &_v_att_sp); -// } -// } - /* rotation matrix for current state */ math::Matrix<3, 3> R; R.set(_v_att.R); @@ -340,64 +301,3 @@ void MulticopterAttitudeControlBase::set_actuator_controls() { _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; } - -void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion attitude) { - math::Quaternion quat; - quat(0) = (float)attitude.w(); - quat(1) = (float)attitude.x(); - quat(2) = (float)attitude.y(); - quat(3) = (float)attitude.z(); - - _v_att.q[0] = quat(0); - _v_att.q[1] = quat(1); - _v_att.q[2] = quat(2); - _v_att.q[3] = quat(3); - - math::Matrix<3,3> Rot = quat.to_dcm(); - _v_att.R[0][0] = Rot(0,0); - _v_att.R[1][0] = Rot(1,0); - _v_att.R[2][0] = Rot(2,0); - _v_att.R[0][1] = Rot(0,1); - _v_att.R[1][1] = Rot(1,1); - _v_att.R[2][1] = Rot(2,1); - _v_att.R[0][2] = Rot(0,2); - _v_att.R[1][2] = Rot(1,2); - _v_att.R[2][2] = Rot(2,2); - - _v_att.R_valid = true; -} - -void MulticopterAttitudeControlBase::set_attitude_rates(const Eigen::Vector3d& angular_rate) { - // check if this is consistent !!! - _v_att.rollspeed = angular_rate(0); - _v_att.pitchspeed = angular_rate(1); - _v_att.yawspeed = angular_rate(2); -} - -void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) { - _v_att_sp.roll_body = control_attitude_thrust_reference(0); - _v_att_sp.pitch_body = control_attitude_thrust_reference(1); - _v_att_sp.yaw_body = control_attitude_thrust_reference(2); - _v_att_sp.thrust = (control_attitude_thrust_reference(3) -30)*(-1)/30; - - // setup rotation matrix - math::Matrix<3,3> Rot_sp; - Rot_sp.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body); - _v_att_sp.R_body[0][0] = Rot_sp(0,0); - _v_att_sp.R_body[1][0] = Rot_sp(1,0); - _v_att_sp.R_body[2][0] = Rot_sp(2,0); - _v_att_sp.R_body[0][1] = Rot_sp(0,1); - _v_att_sp.R_body[1][1] = Rot_sp(1,1); - _v_att_sp.R_body[2][1] = Rot_sp(2,1); - _v_att_sp.R_body[0][2] = Rot_sp(0,2); - _v_att_sp.R_body[1][2] = Rot_sp(1,2); - _v_att_sp.R_body[2][2] = Rot_sp(2,2); -} - -void MulticopterAttitudeControlBase::get_mixer_input(Eigen::Vector4d& motor_inputs) { - motor_inputs(0) = _actuators.control[0]; - motor_inputs(1) = _actuators.control[1]; - motor_inputs(2) = _actuators.control[2]; - motor_inputs(3) = _actuators.control[3]; -} - diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 515fb0c14..009f76268 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -34,7 +34,7 @@ /** * @file mc_att_control_base.h - * + * * @author Roman Bapst * */ @@ -83,12 +83,7 @@ public: */ void control_attitude(float dt); void control_attitude_rates(float dt); - - // setters and getters for interface with euroc-gazebo simulator - void set_attitude(const Eigen::Quaternion attitude); - void set_attitude_rates(const Eigen::Vector3d& angular_rate); - void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); - void get_mixer_input(Eigen::Vector4d& motor_inputs); + void set_actuator_controls(); protected: @@ -105,8 +100,6 @@ protected: struct actuator_controls_s _actuators; /**< actuator controls */ struct actuator_armed_s _armed; /**< actuator arming status */ - perf_counter_t _loop_perf; /**< loop performance counter */ - math::Vector<3> _rates_prev; /**< angular rates on previous step */ math::Vector<3> _rates_sp; /**< angular rates setpoint */ math::Vector<3> _rates_int; /**< angular rates integral error */ @@ -130,8 +123,10 @@ protected: float man_yaw_max; math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ } _params; - - void vehicle_attitude_setpoint_poll(); //provisional + + bool _publish_att_sp; + + virtual void vehicle_attitude_setpoint_poll() = 0; }; 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 19c10198c..4709f0487 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -75,6 +75,7 @@ #include #include #include +#include "mc_att_control_base.h" /** * Multicopter attitude control app start / stop handling function @@ -87,7 +88,8 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f -class MulticopterAttitudeControl +class MulticopterAttitudeControl : + public MulticopterAttitudeControlBase { public: /** @@ -108,7 +110,6 @@ public: int start(); private: - bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ @@ -124,28 +125,6 @@ private: orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ - bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ - - struct vehicle_attitude_s _v_att; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ - struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ - struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ - struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */ - struct actuator_controls_s _actuators; /**< actuator controls */ - struct actuator_armed_s _armed; /**< actuator arming status */ - - perf_counter_t _loop_perf; /**< loop performance counter */ - - math::Vector<3> _rates_prev; /**< angular rates on previous step */ - math::Vector<3> _rates_sp; /**< angular rates setpoint */ - math::Vector<3> _rates_int; /**< angular rates integral error */ - float _thrust_sp; /**< thrust setpoint */ - math::Vector<3> _att_control; /**< attitude control vector */ - - math::Matrix<3, 3> _I; /**< identity matrix */ - - bool _reset_yaw_sp; /**< reset yaw setpoint flag */ - struct { param_t roll_p; param_t roll_rate_p; @@ -170,19 +149,7 @@ private: param_t acro_yaw_max; } _params_handles; /**< handles for interesting parameters */ - struct { - math::Vector<3> att_p; /**< P gain for angular error */ - math::Vector<3> rate_p; /**< P gain for angular rate error */ - math::Vector<3> rate_i; /**< I gain for angular rate error */ - math::Vector<3> rate_d; /**< D gain for angular rate error */ - float yaw_ff; /**< yaw control feed-forward */ - float yaw_rate_max; /**< max yaw rate */ - - float man_roll_max; - float man_pitch_max; - float man_yaw_max; - math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ - } _params; + perf_counter_t _loop_perf; /**< loop performance counter */ /** * Update our local parameter cache. @@ -219,16 +186,6 @@ private: */ void arming_status_poll(); - /** - * Attitude controller. - */ - void control_attitude(float dt); - - /** - * Attitude rates controller. - */ - void control_attitude_rates(float dt); - /** * Shim for calling task_main from task_create. */ @@ -253,11 +210,8 @@ MulticopterAttitudeControl *g_control; } MulticopterAttitudeControl::MulticopterAttitudeControl() : - - _task_should_exit(false), - _control_task(-1), - -/* subscriptions */ + MulticopterAttitudeControlBase(), + /* subscriptions */ _v_att_sub(-1), _v_att_sp_sub(-1), _v_control_mode_sub(-1), @@ -265,14 +219,12 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _manual_control_sp_sub(-1), _armed_sub(-1), -/* publications */ + /* publications */ _att_sp_pub(-1), _v_rates_sp_pub(-1), _actuators_0_pub(-1), - _actuators_0_circuit_breaker_enabled(false), - -/* performance counters */ + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) { @@ -489,229 +441,6 @@ MulticopterAttitudeControl::arming_status_poll() } } -/* - * Attitude controller. - * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode) - * Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes) - */ -void -MulticopterAttitudeControl::control_attitude(float dt) -{ - float yaw_sp_move_rate = 0.0f; - bool publish_att_sp = false; - - if (_v_control_mode.flag_control_manual_enabled) { - /* manual input, set or modify attitude setpoint */ - - if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) { - /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ - vehicle_attitude_setpoint_poll(); - } - - if (!_v_control_mode.flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude stabilized mode */ - _v_att_sp.thrust = _manual_control_sp.z; - publish_att_sp = true; - } - - if (!_armed.armed) { - /* reset yaw setpoint when disarmed */ - _reset_yaw_sp = true; - } - - /* move yaw setpoint in all modes */ - if (_v_att_sp.thrust < 0.1f) { - // TODO - //if (_status.condition_landed) { - /* reset yaw setpoint if on ground */ - // reset_yaw_sp = true; - //} - } else { - /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max; - _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.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.yaw_body - _v_att.yaw); - if (yaw_offs < - yaw_offs_max) { - _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max); - - } else if (yaw_offs > yaw_offs_max) { - _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); - } - _v_att_sp.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.yaw_body = _v_att.yaw; - _v_att_sp.R_valid = false; - publish_att_sp = true; - } - - if (!_v_control_mode.flag_control_velocity_enabled) { - /* update attitude setpoint if not in position control mode */ - _v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max; - _v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max; - _v_att_sp.R_valid = false; - publish_att_sp = true; - } - - } else { - /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ - vehicle_attitude_setpoint_poll(); - - /* reset yaw setpoint after non-manual control mode */ - _reset_yaw_sp = true; - } - - _thrust_sp = _v_att_sp.thrust; - - /* construct attitude setpoint rotation matrix */ - math::Matrix<3, 3> R_sp; - - if (_v_att_sp.R_valid) { - /* rotation matrix in _att_sp is valid, use it */ - R_sp.set(&_v_att_sp.R_body[0][0]); - - } else { - /* rotation matrix in _att_sp is not valid, use euler angles instead */ - R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); - - /* copy rotation matrix back to setpoint struct */ - memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body)); - _v_att_sp.R_valid = true; - } - - /* 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); - - } else { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); - } - } - - /* rotation matrix for current state */ - math::Matrix<3, 3> R; - R.set(_v_att.R); - - /* all input data is ready, run controller itself */ - - /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ - math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2)); - math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); - - /* axis and sin(angle) of desired rotation */ - math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z); - - /* calculate angle error */ - float e_R_z_sin = e_R.length(); - float e_R_z_cos = R_z * R_sp_z; - - /* calculate weight for yaw control */ - float yaw_w = R_sp(2, 2) * R_sp(2, 2); - - /* calculate rotation matrix after roll/pitch only rotation */ - math::Matrix<3, 3> R_rp; - - if (e_R_z_sin > 0.0f) { - /* get axis-angle representation */ - float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); - math::Vector<3> e_R_z_axis = e_R / e_R_z_sin; - - e_R = e_R_z_axis * e_R_z_angle; - - /* cross product matrix for e_R_axis */ - math::Matrix<3, 3> e_R_cp; - e_R_cp.zero(); - e_R_cp(0, 1) = -e_R_z_axis(2); - e_R_cp(0, 2) = e_R_z_axis(1); - e_R_cp(1, 0) = e_R_z_axis(2); - e_R_cp(1, 2) = -e_R_z_axis(0); - e_R_cp(2, 0) = -e_R_z_axis(1); - e_R_cp(2, 1) = e_R_z_axis(0); - - /* rotation matrix for roll/pitch only rotation */ - R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); - - } else { - /* zero roll/pitch rotation */ - R_rp = R; - } - - /* R_rp and R_sp has the same Z axis, calculate yaw error */ - math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); - math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); - e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; - - if (e_R_z_cos < 0.0f) { - /* for large thrust vector rotations use another rotation method: - * calculate angle and axis for R -> R_sp rotation directly */ - math::Quaternion q; - q.from_dcm(R.transposed() * R_sp); - math::Vector<3> e_R_d = q.imag(); - e_R_d.normalize(); - e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); - - /* use fusion of Z axis based rotation and direct rotation */ - float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; - e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; - } - - /* calculate angular rates setpoint */ - _rates_sp = _params.att_p.emult(e_R); - - /* limit yaw rate */ - _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max); - - /* feed forward yaw setpoint rate */ - _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; -} - -/* - * Attitude rates controller. - * Input: '_rates_sp' vector, '_thrust_sp' - * Output: '_att_control' vector - */ -void -MulticopterAttitudeControl::control_attitude_rates(float dt) -{ - /* reset integral if disarmed */ - if (!_armed.armed) { - _rates_int.zero(); - } - - /* current body angular rates */ - math::Vector<3> rates; - rates(0) = _v_att.rollspeed; - rates(1) = _v_att.pitchspeed; - rates(2) = _v_att.yawspeed; - - /* angular rates error */ - math::Vector<3> rates_err = _rates_sp - rates; - _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; - _rates_prev = rates; - - /* update integral only if not saturated on low limit */ - if (_thrust_sp > MIN_TAKEOFF_THRUST) { - for (int i = 0; i < 3; i++) { - if (fabsf(_att_control(i)) < _thrust_sp) { - float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; - - if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && - _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { - _rates_int(i) = rate_i; - } - } - } - } -} - void MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -789,6 +518,19 @@ MulticopterAttitudeControl::task_main() 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(); + + 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); + } + } + /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp new file mode 100644 index 000000000..61a4237fc --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_sim.cpp @@ -0,0 +1,107 @@ +/* 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_base.h + * + * @author Roman Bapst + * + */ + +#include "mc_att_control_sim.h" +#include +#include + +#ifdef CONFIG_ARCH_ARM +#else +#include +using namespace std; +#endif + +void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion attitude) { + math::Quaternion quat; + quat(0) = (float)attitude.w(); + quat(1) = (float)attitude.x(); + quat(2) = (float)attitude.y(); + quat(3) = (float)attitude.z(); + + _v_att.q[0] = quat(0); + _v_att.q[1] = quat(1); + _v_att.q[2] = quat(2); + _v_att.q[3] = quat(3); + + math::Matrix<3,3> Rot = quat.to_dcm(); + _v_att.R[0][0] = Rot(0,0); + _v_att.R[1][0] = Rot(1,0); + _v_att.R[2][0] = Rot(2,0); + _v_att.R[0][1] = Rot(0,1); + _v_att.R[1][1] = Rot(1,1); + _v_att.R[2][1] = Rot(2,1); + _v_att.R[0][2] = Rot(0,2); + _v_att.R[1][2] = Rot(1,2); + _v_att.R[2][2] = Rot(2,2); + + _v_att.R_valid = true; +} + +void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d& angular_rate) { + // check if this is consistent !!! + _v_att.rollspeed = angular_rate(0); + _v_att.pitchspeed = angular_rate(1); + _v_att.yawspeed = angular_rate(2); +} + +void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) { + _v_att_sp.roll_body = control_attitude_thrust_reference(0); + _v_att_sp.pitch_body = control_attitude_thrust_reference(1); + _v_att_sp.yaw_body = control_attitude_thrust_reference(2); + _v_att_sp.thrust = (control_attitude_thrust_reference(3) -30)*(-1)/30; + + // setup rotation matrix + math::Matrix<3,3> Rot_sp; + Rot_sp.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body); + _v_att_sp.R_body[0][0] = Rot_sp(0,0); + _v_att_sp.R_body[1][0] = Rot_sp(1,0); + _v_att_sp.R_body[2][0] = Rot_sp(2,0); + _v_att_sp.R_body[0][1] = Rot_sp(0,1); + _v_att_sp.R_body[1][1] = Rot_sp(1,1); + _v_att_sp.R_body[2][1] = Rot_sp(2,1); + _v_att_sp.R_body[0][2] = Rot_sp(0,2); + _v_att_sp.R_body[1][2] = Rot_sp(1,2); + _v_att_sp.R_body[2][2] = Rot_sp(2,2); +} + +void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d& motor_inputs) { + motor_inputs(0) = _actuators.control[0]; + motor_inputs(1) = _actuators.control[1]; + motor_inputs(2) = _actuators.control[2]; + motor_inputs(3) = _actuators.control[3]; +} diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h new file mode 100644 index 000000000..20752b054 --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_sim.h @@ -0,0 +1,95 @@ +#ifndef MC_ATT_CONTROL_BASE_H_ +#define MC_ATT_CONTROL_BASE_H_ + +/* 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_base.h + * + * @author Roman Bapst + * + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#inlcude "mc_att_control_base.h" + + + +#define YAW_DEADZONE 0.05f +#define MIN_TAKEOFF_THRUST 0.2f +#define RATES_I_LIMIT 0.3f + +class MulticopterAttitudeControlSim : + public MulticopterAttitudeControlBase + +{ +public: + /** + * Constructor + */ + MulticopterAttitudeControlSim(); + + /** + * Destructor + */ + ~MulticopterAttitudeControlSim(); + + /* setters and getters for interface with euroc-gazebo simulator */ + void set_attitude(const Eigen::Quaternion attitude); + void set_attitude_rates(const Eigen::Vector3d& angular_rate); + void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); + void get_mixer_input(Eigen::Vector4d& motor_inputs); + +protected: + void vehicle_attitude_setpoint_poll() {}; + + +}; + +#endif /* MC_ATT_CONTROL_BASE_H_ */ diff --git a/src/modules/mc_att_control/module.mk b/src/modules/mc_att_control/module.mk index 64b876f69..ecd251d45 100644 --- a/src/modules/mc_att_control/module.mk +++ b/src/modules/mc_att_control/module.mk @@ -38,4 +38,5 @@ MODULE_COMMAND = mc_att_control SRCS = mc_att_control_main.cpp \ - mc_att_control_params.c + mc_att_control_base.cpp \ + mc_att_control_params.c -- cgit v1.2.3 From 1c19d75cf429639ff9ab9ff23ad9fbcd1c936e98 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Dec 2014 09:02:06 +0100 Subject: mc att control: ran fix code style script --- src/modules/mc_att_control/mc_att_control_base.cpp | 36 ++++++++------ src/modules/mc_att_control/mc_att_control_base.h | 3 +- src/modules/mc_att_control/mc_att_control_main.cpp | 23 +++++---- src/modules/mc_att_control/mc_att_control_sim.cpp | 56 ++++++++++++---------- src/modules/mc_att_control/mc_att_control_sim.h | 6 +-- 5 files changed, 71 insertions(+), 53 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index d21deb715..a3b0340d2 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -83,10 +83,12 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : _I.identity(); } -MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() { +MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() +{ } -void MulticopterAttitudeControlBase::control_attitude(float dt) { +void MulticopterAttitudeControlBase::control_attitude(float dt) +{ float yaw_sp_move_rate = 0.0f; _publish_att_sp = false; @@ -94,7 +96,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { /* manual input, set or modify attitude setpoint */ if (_v_control_mode.flag_control_velocity_enabled - || _v_control_mode.flag_control_climb_rate_enabled) { + || _v_control_mode.flag_control_climb_rate_enabled) { /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ vehicle_attitude_setpoint_poll(); } @@ -121,15 +123,17 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { /* move yaw setpoint */ yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max; _v_att_sp.yaw_body = _wrap_pi( - _v_att_sp.yaw_body + yaw_sp_move_rate * dt); + _v_att_sp.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.yaw_body - _v_att.yaw); + if (yaw_offs < -yaw_offs_max) { _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max); } else if (yaw_offs > yaw_offs_max) { _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); } + _v_att_sp.R_valid = false; _publish_att_sp = true; } @@ -146,7 +150,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { /* update attitude setpoint if not in position control mode */ _v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max; _v_att_sp.pitch_body = -_manual_control_sp.x - * _params.man_pitch_max; + * _params.man_pitch_max; _v_att_sp.R_valid = false; _publish_att_sp = true; } @@ -175,7 +179,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { /* copy rotation matrix back to setpoint struct */ memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], - sizeof(_v_att_sp.R_body)); + sizeof(_v_att_sp.R_body)); _v_att_sp.R_valid = true; } @@ -221,8 +225,8 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { /* rotation matrix for roll/pitch only rotation */ R_rp = R - * (_I + e_R_cp * e_R_z_sin - + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + * (_I + e_R_cp * e_R_z_sin + + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); } else { /* zero roll/pitch rotation */ @@ -253,13 +257,14 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) { /* limit yaw rate */ _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, - _params.yaw_rate_max); + _params.yaw_rate_max); /* feed forward yaw setpoint rate */ _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; } -void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { +void MulticopterAttitudeControlBase::control_attitude_rates(float dt) +{ /* reset integral if disarmed */ if (!_armed.armed) { _rates_int.zero(); @@ -274,7 +279,7 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { /* angular rates error */ math::Vector < 3 > rates_err = _rates_sp - rates; _att_control = _params.rate_p.emult(rates_err) - + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; + + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; _rates_prev = rates; /* update integral only if not saturated on low limit */ @@ -282,11 +287,11 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { for (int i = 0; i < 3; i++) { if (fabsf(_att_control(i)) < _thrust_sp) { float rate_i = _rates_int(i) - + _params.rate_i(i) * rates_err(i) * dt; + + _params.rate_i(i) * rates_err(i) * dt; if (isfinite( - rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && - _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { + rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { _rates_int(i) = rate_i; } } @@ -295,7 +300,8 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { } -void MulticopterAttitudeControlBase::set_actuator_controls() { +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; diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 009f76268..e7793e624 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -64,7 +64,8 @@ #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f -class MulticopterAttitudeControlBase { +class MulticopterAttitudeControlBase +{ public: /** * Constructor 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 4709f0487..7a03553f9 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -479,8 +479,9 @@ MulticopterAttitudeControl::task_main() int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit */ - if (pret == 0) + if (pret == 0) { continue; + } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { @@ -524,10 +525,11 @@ MulticopterAttitudeControl::task_main() if (_att_sp_pub > 0) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, - &_v_att_sp); + &_v_att_sp); + } else { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), - &_v_att_sp); + &_v_att_sp); } } @@ -549,7 +551,8 @@ MulticopterAttitudeControl::task_main() /* 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); + _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 */ @@ -632,18 +635,21 @@ MulticopterAttitudeControl::start() int mc_att_control_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { errx(1, "usage: mc_att_control {start|stop|status}"); + } if (!strcmp(argv[1], "start")) { - if (mc_att_control::g_control != nullptr) + if (mc_att_control::g_control != nullptr) { errx(1, "already running"); + } mc_att_control::g_control = new MulticopterAttitudeControl; - if (mc_att_control::g_control == nullptr) + if (mc_att_control::g_control == nullptr) { errx(1, "alloc failed"); + } if (OK != mc_att_control::g_control->start()) { delete mc_att_control::g_control; @@ -655,8 +661,9 @@ int mc_att_control_main(int argc, char *argv[]) } if (!strcmp(argv[1], "stop")) { - if (mc_att_control::g_control == nullptr) + if (mc_att_control::g_control == nullptr) { errx(1, "not running"); + } delete mc_att_control::g_control; mc_att_control::g_control = nullptr; diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp index 61a4237fc..faf729420 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.cpp +++ b/src/modules/mc_att_control/mc_att_control_sim.cpp @@ -46,7 +46,8 @@ using namespace std; #endif -void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion attitude) { +void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion attitude) +{ math::Quaternion quat; quat(0) = (float)attitude.w(); quat(1) = (float)attitude.x(); @@ -58,48 +59,51 @@ void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion _v_att.q[2] = quat(2); _v_att.q[3] = quat(3); - math::Matrix<3,3> Rot = quat.to_dcm(); - _v_att.R[0][0] = Rot(0,0); - _v_att.R[1][0] = Rot(1,0); - _v_att.R[2][0] = Rot(2,0); - _v_att.R[0][1] = Rot(0,1); - _v_att.R[1][1] = Rot(1,1); - _v_att.R[2][1] = Rot(2,1); - _v_att.R[0][2] = Rot(0,2); - _v_att.R[1][2] = Rot(1,2); - _v_att.R[2][2] = Rot(2,2); + math::Matrix<3, 3> Rot = quat.to_dcm(); + _v_att.R[0][0] = Rot(0, 0); + _v_att.R[1][0] = Rot(1, 0); + _v_att.R[2][0] = Rot(2, 0); + _v_att.R[0][1] = Rot(0, 1); + _v_att.R[1][1] = Rot(1, 1); + _v_att.R[2][1] = Rot(2, 1); + _v_att.R[0][2] = Rot(0, 2); + _v_att.R[1][2] = Rot(1, 2); + _v_att.R[2][2] = Rot(2, 2); _v_att.R_valid = true; } -void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d& angular_rate) { +void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d &angular_rate) +{ // check if this is consistent !!! _v_att.rollspeed = angular_rate(0); _v_att.pitchspeed = angular_rate(1); _v_att.yawspeed = angular_rate(2); } -void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) { +void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference) +{ _v_att_sp.roll_body = control_attitude_thrust_reference(0); _v_att_sp.pitch_body = control_attitude_thrust_reference(1); _v_att_sp.yaw_body = control_attitude_thrust_reference(2); - _v_att_sp.thrust = (control_attitude_thrust_reference(3) -30)*(-1)/30; + _v_att_sp.thrust = (control_attitude_thrust_reference(3) - 30) * (-1) / 30; // setup rotation matrix - math::Matrix<3,3> Rot_sp; - Rot_sp.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body); - _v_att_sp.R_body[0][0] = Rot_sp(0,0); - _v_att_sp.R_body[1][0] = Rot_sp(1,0); - _v_att_sp.R_body[2][0] = Rot_sp(2,0); - _v_att_sp.R_body[0][1] = Rot_sp(0,1); - _v_att_sp.R_body[1][1] = Rot_sp(1,1); - _v_att_sp.R_body[2][1] = Rot_sp(2,1); - _v_att_sp.R_body[0][2] = Rot_sp(0,2); - _v_att_sp.R_body[1][2] = Rot_sp(1,2); - _v_att_sp.R_body[2][2] = Rot_sp(2,2); + math::Matrix<3, 3> Rot_sp; + Rot_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); + _v_att_sp.R_body[0][0] = Rot_sp(0, 0); + _v_att_sp.R_body[1][0] = Rot_sp(1, 0); + _v_att_sp.R_body[2][0] = Rot_sp(2, 0); + _v_att_sp.R_body[0][1] = Rot_sp(0, 1); + _v_att_sp.R_body[1][1] = Rot_sp(1, 1); + _v_att_sp.R_body[2][1] = Rot_sp(2, 1); + _v_att_sp.R_body[0][2] = Rot_sp(0, 2); + _v_att_sp.R_body[1][2] = Rot_sp(1, 2); + _v_att_sp.R_body[2][2] = Rot_sp(2, 2); } -void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d& motor_inputs) { +void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d &motor_inputs) +{ motor_inputs(0) = _actuators.control[0]; motor_inputs(1) = _actuators.control[1]; motor_inputs(2) = _actuators.control[2]; diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h index 20752b054..0b48455af 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.h +++ b/src/modules/mc_att_control/mc_att_control_sim.h @@ -82,9 +82,9 @@ public: /* setters and getters for interface with euroc-gazebo simulator */ void set_attitude(const Eigen::Quaternion attitude); - void set_attitude_rates(const Eigen::Vector3d& angular_rate); - void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); - void get_mixer_input(Eigen::Vector4d& motor_inputs); + void set_attitude_rates(const Eigen::Vector3d &angular_rate); + void set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference); + void get_mixer_input(Eigen::Vector4d &motor_inputs); protected: void vehicle_attitude_setpoint_poll() {}; -- cgit v1.2.3 From 85aa6554ad90e392a9538006f8cc483d481f0bbd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Dec 2014 09:29:20 +0100 Subject: re-add hardcoded parameters for euroc --- src/modules/mc_att_control/mc_att_control_sim.cpp | 29 +++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp index faf729420..a8467f6b1 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.cpp +++ b/src/modules/mc_att_control/mc_att_control_sim.cpp @@ -46,6 +46,35 @@ using namespace std; #endif +MulticopterAttitudeControlSim::MulticopterAttitudeControlSim() +{ + /* setup standard gains */ + //XXX: make these configurable + _params.att_p(0) = 5.0; + _params.rate_p(0) = 0.05; + _params.rate_i(0) = 0.0; + _params.rate_d(0) = 0.003; + /* pitch gains */ + _params.att_p(1) = 5.0; + _params.rate_p(1) = 0.05; + _params.rate_i(1) = 0.0; + _params.rate_d(1) = 0.003; + /* yaw gains */ + _params.att_p(2) = 2.8; + _params.rate_p(2) = 0.2; + _params.rate_i(2) = 0.1; + _params.rate_d(2) = 0.0; + _params.yaw_rate_max = 0.5; + _params.yaw_ff = 0.5; + _params.man_roll_max = 0.6; + _params.man_pitch_max = 0.6; + _params.man_yaw_max = 0.6; +} + +MulticopterAttitudeControlSim::~MulticopterAttitudeControlSim() +{ +} + void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion attitude) { math::Quaternion quat; -- cgit v1.2.3 From 417bc91363c2607906c612089cb4b7197a60f531 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Dec 2014 09:29:57 +0100 Subject: mc att: more cleanup between base and main class --- src/modules/mc_att_control/mc_att_control_base.cpp | 10 +++++----- src/modules/mc_att_control/mc_att_control_base.h | 10 +++++----- src/modules/mc_att_control/mc_att_control_main.cpp | 12 ++++++++++-- 3 files changed, 20 insertions(+), 12 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index a3b0340d2..305fbaf89 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -32,6 +32,11 @@ /** * @file mc_att_control_base.h * + * @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * @author Thomas Gubler + * @author Julian Oes * @author Roman Bapst * */ @@ -47,11 +52,6 @@ using namespace std; #endif MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : - _task_should_exit(false), - _control_task(-1), - - _actuators_0_circuit_breaker_enabled(false), - _publish_att_sp(false) { diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index e7793e624..662d32ee4 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -35,6 +35,11 @@ /** * @file mc_att_control_base.h * + * @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * @author Thomas Gubler + * @author Julian Oes * @author Roman Bapst * */ @@ -88,11 +93,6 @@ public: void set_actuator_controls(); protected: - - bool _task_should_exit; /**< if true, sensor task should exit */ - int _control_task; /**< task handle for sensor task */ - bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ - struct vehicle_attitude_s _v_att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ 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 d6a58f418..887a53508 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -38,6 +38,9 @@ * @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. @@ -110,8 +113,10 @@ public: int start(); private: - bool _task_should_exit; /**< if true, sensor task should exit */ - int _control_task; /**< task handle for sensor task */ + bool _task_should_exit; /**< if true, sensor task should exit */ + int _control_task; /**< task handle for sensor task */ + bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ + int _v_att_sub; /**< vehicle attitude subscription */ int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ @@ -211,6 +216,9 @@ MulticopterAttitudeControl *g_control; MulticopterAttitudeControl::MulticopterAttitudeControl() : MulticopterAttitudeControlBase(), + _task_should_exit(false), + _control_task(-1), + _actuators_0_circuit_breaker_enabled(false), /* subscriptions */ _v_att_sub(-1), _v_att_sp_sub(-1), -- cgit v1.2.3 From dfd2098e04761ee65ab09a9a8e6656a334ccae41 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Dec 2014 09:29:20 +0100 Subject: re-add hardcoded parameters for euroc --- src/modules/mc_att_control/mc_att_control_sim.cpp | 29 +++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp index faf729420..a8467f6b1 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.cpp +++ b/src/modules/mc_att_control/mc_att_control_sim.cpp @@ -46,6 +46,35 @@ using namespace std; #endif +MulticopterAttitudeControlSim::MulticopterAttitudeControlSim() +{ + /* setup standard gains */ + //XXX: make these configurable + _params.att_p(0) = 5.0; + _params.rate_p(0) = 0.05; + _params.rate_i(0) = 0.0; + _params.rate_d(0) = 0.003; + /* pitch gains */ + _params.att_p(1) = 5.0; + _params.rate_p(1) = 0.05; + _params.rate_i(1) = 0.0; + _params.rate_d(1) = 0.003; + /* yaw gains */ + _params.att_p(2) = 2.8; + _params.rate_p(2) = 0.2; + _params.rate_i(2) = 0.1; + _params.rate_d(2) = 0.0; + _params.yaw_rate_max = 0.5; + _params.yaw_ff = 0.5; + _params.man_roll_max = 0.6; + _params.man_pitch_max = 0.6; + _params.man_yaw_max = 0.6; +} + +MulticopterAttitudeControlSim::~MulticopterAttitudeControlSim() +{ +} + void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion attitude) { math::Quaternion quat; -- cgit v1.2.3 From ebcad32e377824cb47186a400d22195cacd07e3b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Dec 2014 09:29:57 +0100 Subject: mc att: more cleanup between base and main class --- src/modules/mc_att_control/mc_att_control_base.cpp | 10 +++++----- src/modules/mc_att_control/mc_att_control_base.h | 10 +++++----- src/modules/mc_att_control/mc_att_control_main.cpp | 12 ++++++++++-- 3 files changed, 20 insertions(+), 12 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index a3b0340d2..305fbaf89 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -32,6 +32,11 @@ /** * @file mc_att_control_base.h * + * @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * @author Thomas Gubler + * @author Julian Oes * @author Roman Bapst * */ @@ -47,11 +52,6 @@ using namespace std; #endif MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : - _task_should_exit(false), - _control_task(-1), - - _actuators_0_circuit_breaker_enabled(false), - _publish_att_sp(false) { diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index e7793e624..662d32ee4 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -35,6 +35,11 @@ /** * @file mc_att_control_base.h * + * @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * @author Thomas Gubler + * @author Julian Oes * @author Roman Bapst * */ @@ -88,11 +93,6 @@ public: void set_actuator_controls(); protected: - - bool _task_should_exit; /**< if true, sensor task should exit */ - int _control_task; /**< task handle for sensor task */ - bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ - struct vehicle_attitude_s _v_att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ 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 7a03553f9..16a3b627f 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -38,6 +38,9 @@ * @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. @@ -110,8 +113,10 @@ public: int start(); private: - bool _task_should_exit; /**< if true, sensor task should exit */ - int _control_task; /**< task handle for sensor task */ + bool _task_should_exit; /**< if true, sensor task should exit */ + int _control_task; /**< task handle for sensor task */ + bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ + int _v_att_sub; /**< vehicle attitude subscription */ int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ @@ -211,6 +216,9 @@ MulticopterAttitudeControl *g_control; MulticopterAttitudeControl::MulticopterAttitudeControl() : MulticopterAttitudeControlBase(), + _task_should_exit(false), + _control_task(-1), + _actuators_0_circuit_breaker_enabled(false), /* subscriptions */ _v_att_sub(-1), _v_att_sp_sub(-1), -- cgit v1.2.3 From 0f1a7e7b5b699601886adec5215bf818b050a758 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Dec 2014 09:33:26 +0100 Subject: whitespace --- src/modules/mc_att_control/mc_att_control_base.cpp | 4 ++-- src/modules/mc_att_control/mc_att_control_base.h | 4 ++-- src/modules/mc_att_control/mc_att_control_sim.cpp | 4 ++-- src/modules/mc_att_control/mc_att_control_sim.h | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index 305fbaf89..17270884c 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -20,8 +20,8 @@ * 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 +* 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 diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 662d32ee4..115667eb9 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -23,8 +23,8 @@ * 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 +* 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 diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp index a8467f6b1..788f92661 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.cpp +++ b/src/modules/mc_att_control/mc_att_control_sim.cpp @@ -20,8 +20,8 @@ * 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 +* 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 diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h index 0b48455af..ecc065e68 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.h +++ b/src/modules/mc_att_control/mc_att_control_sim.h @@ -23,8 +23,8 @@ * 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 +* 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 -- cgit v1.2.3 From c64c184948bed07f354904c0549b93225c85ad58 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Dec 2014 09:33:26 +0100 Subject: whitespace --- src/modules/mc_att_control/mc_att_control_base.cpp | 4 ++-- src/modules/mc_att_control/mc_att_control_base.h | 4 ++-- src/modules/mc_att_control/mc_att_control_sim.cpp | 4 ++-- src/modules/mc_att_control/mc_att_control_sim.h | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index 305fbaf89..17270884c 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -20,8 +20,8 @@ * 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 +* 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 diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 662d32ee4..115667eb9 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -23,8 +23,8 @@ * 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 +* 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 diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp index a8467f6b1..788f92661 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.cpp +++ b/src/modules/mc_att_control/mc_att_control_sim.cpp @@ -20,8 +20,8 @@ * 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 +* 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 diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h index 0b48455af..ecc065e68 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.h +++ b/src/modules/mc_att_control/mc_att_control_sim.h @@ -23,8 +23,8 @@ * 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 +* 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 -- cgit v1.2.3 From 9c4fc14f230d4ebe2d82cf39ac5f4bd7f04d0c26 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Dec 2014 10:32:52 +0100 Subject: fix base and sim file descriptions --- src/modules/mc_att_control/mc_att_control_base.cpp | 4 +++- src/modules/mc_att_control/mc_att_control_base.h | 2 ++ src/modules/mc_att_control/mc_att_control_sim.cpp | 4 +++- src/modules/mc_att_control/mc_att_control_sim.h | 4 +++- 4 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index 17270884c..5e65e6508 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -30,7 +30,9 @@ ****************************************************************************/ /** - * @file mc_att_control_base.h + * @file mc_att_control_base.cpp + * + * MC Attitude Controller * * @author Tobias Naegeli * @author Lorenz Meier diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 115667eb9..1c2f3fb6f 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -34,6 +34,8 @@ /** * @file mc_att_control_base.h + * + * MC Attitude Controller * * @author Tobias Naegeli * @author Lorenz Meier diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp index 788f92661..d516d7e52 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.cpp +++ b/src/modules/mc_att_control/mc_att_control_sim.cpp @@ -30,7 +30,9 @@ ****************************************************************************/ /** - * @file mc_att_control_base.h + * @file mc_att_control_sim.cpp + * + * MC Attitude Controller Interface for usage in a simulator * * @author Roman Bapst * diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h index ecc065e68..a1bf44fc9 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.h +++ b/src/modules/mc_att_control/mc_att_control_sim.h @@ -33,7 +33,9 @@ ****************************************************************************/ /** - * @file mc_att_control_base.h + * @file mc_att_control_sim.h + * + * MC Attitude Controller Interface for usage in a simulator * * @author Roman Bapst * -- cgit v1.2.3 From 996438aafedbc23e86ca36171dc81ec23d322caa Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Dec 2014 10:32:52 +0100 Subject: fix base and sim file descriptions --- src/modules/mc_att_control/mc_att_control_base.cpp | 4 +++- src/modules/mc_att_control/mc_att_control_base.h | 2 ++ src/modules/mc_att_control/mc_att_control_sim.cpp | 4 +++- src/modules/mc_att_control/mc_att_control_sim.h | 4 +++- 4 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index 17270884c..5e65e6508 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -30,7 +30,9 @@ ****************************************************************************/ /** - * @file mc_att_control_base.h + * @file mc_att_control_base.cpp + * + * MC Attitude Controller * * @author Tobias Naegeli * @author Lorenz Meier diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 115667eb9..1c2f3fb6f 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -34,6 +34,8 @@ /** * @file mc_att_control_base.h + * + * MC Attitude Controller * * @author Tobias Naegeli * @author Lorenz Meier diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp index 788f92661..d516d7e52 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.cpp +++ b/src/modules/mc_att_control/mc_att_control_sim.cpp @@ -30,7 +30,9 @@ ****************************************************************************/ /** - * @file mc_att_control_base.h + * @file mc_att_control_sim.cpp + * + * MC Attitude Controller Interface for usage in a simulator * * @author Roman Bapst * diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h index ecc065e68..a1bf44fc9 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.h +++ b/src/modules/mc_att_control/mc_att_control_sim.h @@ -33,7 +33,9 @@ ****************************************************************************/ /** - * @file mc_att_control_base.h + * @file mc_att_control_sim.h + * + * MC Attitude Controller Interface for usage in a simulator * * @author Roman Bapst * -- cgit v1.2.3 From 52c35a8e20c99de4e7b6e27856d340a3a355250c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 6 Dec 2014 15:36:58 +0100 Subject: solve conflict for definiton of FILE --- NuttX | 2 +- makefiles/toolchain_gnu-arm-eabi.mk | 2 +- src/examples/subscriber/subscriber.cpp | 1 + 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/NuttX b/NuttX index ae4b05e2c..dbe128cea 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit ae4b05e2c51d07369b5d131052099ac346b0841c +Subproject commit dbe128ceaf0e486b2ef799f14c13d38586e6cca8 diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 84e5cd08a..b98b1a0af 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -121,7 +121,7 @@ INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) # Language-specific flags # ARCHCFLAGS = -std=gnu99 -ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ # Generic warnings # diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index 68003b6df..0d9ca1fa7 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -27,6 +27,7 @@ #include #include "subscriber_params.h" +#include using namespace px4; -- cgit v1.2.3 From fd01c7fc58db1fca43f06b277cb2a0094440ecb3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 6 Dec 2014 18:43:43 +0100 Subject: set NuttX submodule --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index dbe128cea..89a6776fc 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit dbe128ceaf0e486b2ef799f14c13d38586e6cca8 +Subproject commit 89a6776fc1b31d242c637d3f19072609bb98ec6c -- cgit v1.2.3 From 65629d09d5e2424c6fcaf261c95f6d6995c4afd7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 10:35:59 +0100 Subject: add mc att to cmakelist --- CMakeLists.txt | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 636cc07e3..e91f2cc37 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -136,6 +136,16 @@ target_link_libraries(subscriber px4 ) +add_executable(mc_att_control + src/modules/mc_att_control/mc_att_control_main.cpp + src/moudles/mc_att_control/mc_att_control_base.cpp) +add_dependencies(mc_att_control px4_generate_messages_cpp) +target_link_libraries(mc_att_control + ${catkin_LIBRARIES} + px4 +) + + ############# ## Install ## ############# -- cgit v1.2.3 From 87df7c3243412d4fc7a0c40967b2abe078f7a0b2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 10:37:01 +0100 Subject: move vehicle_attitude_setpoint to msg format --- msg/px4_msgs/vehicle_attitude_setpoint.msg | 21 ++++++++ src/modules/fw_att_control/fw_att_control_base.cpp | 3 +- src/modules/mavlink/mavlink_receiver.cpp | 2 +- src/modules/mc_att_control/mc_att_control_base.cpp | 4 +- src/modules/mc_att_control/mc_att_control_base.h | 7 +-- src/modules/mc_att_control/mc_att_control_main.cpp | 6 +-- src/modules/mc_pos_control/mc_pos_control_main.cpp | 22 ++++---- .../uORB/topics/vehicle_attitude_setpoint.h | 59 ++++++++-------------- src/platforms/px4_defines.h | 1 + src/platforms/px4_includes.h | 1 + src/platforms/px4_middleware.h | 4 +- 11 files changed, 66 insertions(+), 64 deletions(-) create mode 100644 msg/px4_msgs/vehicle_attitude_setpoint.msg diff --git a/msg/px4_msgs/vehicle_attitude_setpoint.msg b/msg/px4_msgs/vehicle_attitude_setpoint.msg new file mode 100644 index 000000000..1a8e6e3d5 --- /dev/null +++ b/msg/px4_msgs/vehicle_attitude_setpoint.msg @@ -0,0 +1,21 @@ + +uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data + +float32 roll_body # body angle in NED frame +float32 pitch_body # body angle in NED frame +float32 yaw_body # body angle in NED frame + +float32[9] R_body # Rotation matrix describing the setpoint as rotation from the current body frame +bool R_valid # Set to true if rotation matrix is valid + +# For quaternion-based attitude control +float32[4] q_d # Desired quaternion for quaternion control +bool q_d_valid # Set to true if quaternion vector is valid +float32[4] q_e # Attitude error in quaternion +bool q_e_valid # Set to true if quaternion error vector is valid + +float32 thrust # Thrust in Newton the power system should generate + +bool roll_reset_integral # Reset roll integral part (navigation logic change) +bool pitch_reset_integral # Reset pitch integral part (navigation logic change) +bool yaw_reset_integral # Reset yaw integral part (navigation logic change) diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp index 46fef3e67..f543c02f9 100644 --- a/src/modules/fw_att_control/fw_att_control_base.cpp +++ b/src/modules/fw_att_control/fw_att_control_base.cpp @@ -31,7 +31,7 @@ /** * @file mc_att_control_base.cpp - * + * * @author Roman Bapst * */ @@ -40,7 +40,6 @@ #include #include #include -#include using namespace std; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e98d72afe..cb55a25aa 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -778,7 +778,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); - mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body); + mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body); att_sp.R_valid = true; att_sp.thrust = set_attitude_target.thrust; memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d)); diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index 5e65e6508..f6b3268b5 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -172,7 +172,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) if (_v_att_sp.R_valid) { /* rotation matrix in _att_sp is valid, use it */ - R_sp.set(&_v_att_sp.R_body[0][0]); + R_sp.set(&_v_att_sp.R_body[0]); } else { /* rotation matrix in _att_sp is not valid, use euler angles instead */ @@ -180,7 +180,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) _v_att_sp.yaw_body); /* copy rotation matrix back to setpoint struct */ - memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], + memcpy(&_v_att_sp.R_body[0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body)); _v_att_sp.R_valid = true; } diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 1c2f3fb6f..c1ea52f63 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -34,7 +34,7 @@ /** * @file mc_att_control_base.h - * + * * MC Attitude Controller * * @author Tobias Naegeli @@ -45,23 +45,20 @@ * @author Roman Bapst * */ - +#include #include #include #include #include #include #include -#include -#include #include #include #include #include #include #include -#include #include #include 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 887a53508..c0e8957e0 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -52,8 +52,7 @@ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. */ -#include -#include +#include #include #include #include @@ -62,8 +61,6 @@ #include #include #include -#include -#include #include #include #include @@ -78,6 +75,7 @@ #include #include #include + #include "mc_att_control_base.h" /** 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 cea847603..4ea5fdfb6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -43,8 +43,9 @@ * @author Anton Babushkin */ -#include -#include +#include +#include +#include #include #include #include @@ -54,8 +55,7 @@ #include #include #include -#include -#include + #include #include #include @@ -67,8 +67,8 @@ #include #include #include -#include -#include +// #include +// #include #include #include #include @@ -532,9 +532,9 @@ MulticopterPositionControl::reset_pos_sp() if (_reset_pos_sp) { _reset_pos_sp = false; /* shift position setpoint to make attitude setpoint continuous */ - _pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0) + _pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0) - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); - _pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1) + _pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1) - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); } @@ -987,7 +987,7 @@ MulticopterPositionControl::task_main() if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; _att_sp.roll_body = 0.0f; @@ -1260,7 +1260,7 @@ MulticopterPositionControl::task_main() } /* copy rotation matrix to attitude setpoint topic */ - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; /* calculate euler angles, for logging only, must not be used for control */ @@ -1275,7 +1275,7 @@ MulticopterPositionControl::task_main() R.from_euler(0.0f, 0.0f, _att_sp.yaw_body); /* copy rotation matrix to attitude setpoint topic */ - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; _att_sp.roll_body = 0.0f; diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index a503aa0c6..29fb104df 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * 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 @@ -32,49 +31,37 @@ * ****************************************************************************/ -/** - * @file vehicle_attitude_setpoint.h - * Definition of the vehicle attitude setpoint uORB topic. - */ + /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/vehicle_attitude_setpoint.msg */ -#ifndef TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_ -#define TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_ + +#pragma once #include -#include -#include +#include "../uORB.h" + + /** * @addtogroup topics * @{ */ -/** - * vehicle attitude setpoint. - */ -struct vehicle_attitude_setpoint_s { - uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - - float roll_body; /**< body angle in NED frame */ - float pitch_body; /**< body angle in NED frame */ - float yaw_body; /**< body angle in NED frame */ - //float body_valid; /**< Set to true if body angles are valid */ - - float R_body[3][3]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ - bool R_valid; /**< Set to true if rotation matrix is valid */ - - //! For quaternion-based attitude control - float q_d[4]; /** Desired quaternion for quaternion control */ - bool q_d_valid; /**< Set to true if quaternion vector is valid */ - float q_e[4]; /** Attitude error in quaternion */ - bool q_e_valid; /**< Set to true if quaternion error vector is valid */ - - float thrust; /**< Thrust in Newton the power system should generate */ - - bool roll_reset_integral; /**< Reset roll integral part (navigation logic change) */ - bool pitch_reset_integral; /**< Reset pitch integral part (navigation logic change) */ - bool yaw_reset_integral; /**< Reset yaw integral part (navigation logic change) */ +struct vehicle_attitude_setpoint_s { + uint64_t timestamp; + float roll_body; + float pitch_body; + float yaw_body; + float R_body[9]; + bool R_valid; + float q_d[4]; + bool q_d_valid; + float q_e[4]; + bool q_e_valid; + float thrust; + bool roll_reset_integral; + bool pitch_reset_integral; + bool yaw_reset_integral; }; /** @@ -83,5 +70,3 @@ struct vehicle_attitude_setpoint_s { /* register this as object request broker structure */ ORB_DECLARE(vehicle_attitude_setpoint); - -#endif /* TOPIC_ARDRONE_CONTROL_H_ */ diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index af7188f32..ff302448a 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -115,6 +115,7 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) /* Parameter handle datatype */ +#include typedef param_t px4_param_t; /* Initialize a param, get param handle */ diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index a3b59b766..a9425077e 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -55,6 +55,7 @@ #include #include #include +#include #include #include diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index c1465b4b1..b0bc40417 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -53,13 +53,13 @@ __EXPORT uint64_t get_time_micros(); /** * Returns true if the app/task should continue to run */ -bool ok() { return ros::ok(); } +inline bool ok() { return ros::ok(); } #else extern bool task_should_exit; /** * Returns true if the app/task should continue to run */ -bool ok() { return !task_should_exit; } +inline bool ok() { return !task_should_exit; } #endif class Rate -- cgit v1.2.3 From b3600e5ee6fc4550533d65c1c25abceb6db4466e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 11:06:56 +0100 Subject: manual_control_setpoint as msg --- msg/px4_msgs/manual_control_setpoint.msg | 44 ++++++++++ src/modules/mc_att_control/mc_att_control_base.h | 1 - src/modules/mc_att_control/mc_att_control_main.cpp | 1 - src/modules/sensors/sensors.cpp | 8 +- src/modules/uORB/topics/manual_control_setpoint.h | 98 ++++++++-------------- src/platforms/px4_includes.h | 1 + 6 files changed, 82 insertions(+), 71 deletions(-) create mode 100644 msg/px4_msgs/manual_control_setpoint.msg diff --git a/msg/px4_msgs/manual_control_setpoint.msg b/msg/px4_msgs/manual_control_setpoint.msg new file mode 100644 index 000000000..d3cfb078b --- /dev/null +++ b/msg/px4_msgs/manual_control_setpoint.msg @@ -0,0 +1,44 @@ + +uint8 SWITCH_POS_NONE = 0 # switch is not mapped +uint8 SWITCH_POS_ON = 1 # switch activated (value = 1) +uint8 SWITCH_POS_MIDDLE = 2 # middle position (value = 0) +uint8 SWITCH_POS_OFF = 3 # switch not activated (value = -1) +uint64 timestamp + +# Any of the channels may not be available and be set to NaN +# to indicate that it does not contain valid data. +# The variable names follow the definition of the +# MANUAL_CONTROL mavlink message. +# The default range is from -1 to 1 (mavlink message -1000 to 1000) +# The range for the z variable is defined from 0 to 1. (The z field of +# the MANUAL_CONTROL mavlink message is defined from -1000 to 1000) + +float32 x # stick position in x direction -1..1 + # in general corresponds to forward/back motion or pitch of vehicle, + # in general a positive value means forward or negative pitch and + # a negative value means backward or positive pitch +float32 y # stick position in y direction -1..1 + # in general corresponds to right/left motion or roll of vehicle, + # in general a positive value means right or positive roll and + # a negative value means left or negative roll +float32 z # throttle stick position 0..1 + # in general corresponds to up/down motion or thrust of vehicle, + # in general the value corresponds to the demanded throttle by the user, + # if the input is used for setting the setpoint of a vertical position + # controller any value > 0.5 means up and any value < 0.5 means down +float32 r # yaw stick/twist positon, -1..1 + # in general corresponds to the righthand rotation around the vertical + # (downwards) axis of the vehicle +float32 flaps # flap position +float32 aux1 # default function: camera yaw / azimuth +float32 aux2 # default function: camera pitch / tilt +float32 aux3 # default function: camera trigger +float32 aux4 # default function: camera roll +float32 aux5 # default function: payload drop + +uint8 mode_switch # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO +uint8 return_switch # return to launch 2 position switch (mandatory): _NORMAL_, RTL +uint8 posctl_switch # position control 2 position switch (optional): _ALTCTL_, POSCTL +uint8 loiter_switch # loiter 2 position switch (optional): _MISSION_, LOITER +uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO +uint8 offboard_switch # offboard 2 position switch (optional): _NORMAL_, OFFBOARD diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index c1ea52f63..d42d672c0 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -53,7 +53,6 @@ #include #include -#include #include #include #include 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 c0e8957e0..a8a3ae6b0 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -61,7 +61,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 672dc52c3..fca72c304 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -187,8 +187,8 @@ private: /** * Get switch position for specified function. */ - switch_pos_t get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv); - switch_pos_t get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv); + uint8_t get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv); + uint8_t get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv); /** * Gather and publish RC input data. @@ -1512,7 +1512,7 @@ Sensors::get_rc_value(uint8_t func, float min_value, float max_value) } } -switch_pos_t +uint8_t Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv) { if (_rc.function[func] >= 0) { @@ -1533,7 +1533,7 @@ Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mi } } -switch_pos_t +uint8_t Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv) { if (_rc.function[func] >= 0) { diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 13138ebc9..440841f90 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -1,21 +1,20 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * 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. + * 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. + * 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. + * 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 @@ -32,73 +31,44 @@ * ****************************************************************************/ -/** - * @file manual_control_setpoint.h - * Definition of the manual_control_setpoint uORB topic. - */ + /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/manual_control_setpoint.msg */ + -#ifndef TOPIC_MANUAL_CONTROL_SETPOINT_H_ -#define TOPIC_MANUAL_CONTROL_SETPOINT_H_ +#pragma once #include -#include +#include "../uORB.h" + +#define SWITCH_POS_NONE 0 +#define SWITCH_POS_ON 1 +#define SWITCH_POS_MIDDLE 2 +#define SWITCH_POS_OFF 3 -/** - * Switch position - */ -typedef enum { - SWITCH_POS_NONE = 0, /**< switch is not mapped */ - SWITCH_POS_ON, /**< switch activated (value = 1) */ - SWITCH_POS_MIDDLE, /**< middle position (value = 0) */ - SWITCH_POS_OFF /**< switch not activated (value = -1) */ -} switch_pos_t; /** * @addtogroup topics * @{ */ + struct manual_control_setpoint_s { uint64_t timestamp; - - /** - * Any of the channels may not be available and be set to NaN - * to indicate that it does not contain valid data. - * The variable names follow the definition of the - * MANUAL_CONTROL mavlink message. - * The default range is from -1 to 1 (mavlink message -1000 to 1000) - * The range for the z variable is defined from 0 to 1. (The z field of - * the MANUAL_CONTROL mavlink message is defined from -1000 to 1000) - */ - float x; /**< stick position in x direction -1..1 - in general corresponds to forward/back motion or pitch of vehicle, - in general a positive value means forward or negative pitch and - a negative value means backward or positive pitch */ - float y; /**< stick position in y direction -1..1 - in general corresponds to right/left motion or roll of vehicle, - in general a positive value means right or positive roll and - a negative value means left or negative roll */ - float z; /**< throttle stick position 0..1 - in general corresponds to up/down motion or thrust of vehicle, - in general the value corresponds to the demanded throttle by the user, - if the input is used for setting the setpoint of a vertical position - controller any value > 0.5 means up and any value < 0.5 means down */ - float r; /**< yaw stick/twist positon, -1..1 - in general corresponds to the righthand rotation around the vertical - (downwards) axis of the vehicle */ - float flaps; /**< flap position */ - float aux1; /**< default function: camera yaw / azimuth */ - float aux2; /**< default function: camera pitch / tilt */ - float aux3; /**< default function: camera trigger */ - float aux4; /**< default function: camera roll */ - float aux5; /**< default function: payload drop */ - - switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */ - switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */ - switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */ - switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */ - switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */ - switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */ + float x; + float y; + float z; + float r; + float flaps; + float aux1; + float aux2; + float aux3; + float aux4; + float aux5; + uint8_t mode_switch; + uint8_t return_switch; + uint8_t posctl_switch; + uint8_t loiter_switch; + uint8_t acro_switch; + uint8_t offboard_switch; }; /** @@ -107,5 +77,3 @@ struct manual_control_setpoint_s { /* register this as object request broker structure */ ORB_DECLARE(manual_control_setpoint); - -#endif diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index a9425077e..2045bd2a8 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -56,6 +56,7 @@ #include #include #include +#include #include #include -- cgit v1.2.3 From dc945a3f3f9f7295c73fc8280d6a0048e059c469 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 12:12:23 +0100 Subject: actuator controls as msg --- msg/px4_msgs/actuator_controls.msg | 4 +++ msg/templates/msg.h.template | 3 +- src/modules/mc_att_control/mc_att_control_base.h | 2 +- src/modules/mc_att_control/mc_att_control_main.cpp | 6 ---- src/modules/uORB/topics/actuator_controls.h | 38 +++++++--------------- src/platforms/px4_defines.h | 7 ++++ src/platforms/px4_includes.h | 4 +++ src/systemcmds/esc_calib/esc_calib.c | 1 + 8 files changed, 30 insertions(+), 35 deletions(-) create mode 100644 msg/px4_msgs/actuator_controls.msg diff --git a/msg/px4_msgs/actuator_controls.msg b/msg/px4_msgs/actuator_controls.msg new file mode 100644 index 000000000..743f20cdf --- /dev/null +++ b/msg/px4_msgs/actuator_controls.msg @@ -0,0 +1,4 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +float32[8] control diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template index 5fce506b6..84dea37a3 100644 --- a/msg/templates/msg.h.template +++ b/msg/templates/msg.h.template @@ -104,7 +104,8 @@ type_map = {'int8': 'int8_t', 'uint32': 'uint32_t', 'uint64': 'uint64_t', 'float32': 'float', - 'bool': 'bool'} + 'bool': 'bool', + 'actuator_controls': 'actuator_controls'} # Function to print a standard ros type def print_field_def(field): diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index d42d672c0..c7853a0ec 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -53,11 +53,11 @@ #include #include -#include #include #include #include #include +#include #include #include 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 a8a3ae6b0..6ab4f95cd 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -61,12 +61,6 @@ #include #include #include -#include -#include -#include -#include -#include -#include #include #include #include diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index 43f7a59ee..3e43107ad 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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 @@ -31,48 +31,32 @@ * ****************************************************************************/ -/** - * @file actuator_controls.h - * - * Actuator control topics - mixer inputs. - * - * Values published to these topics are the outputs of the vehicle control - * system, and are expected to be mixed and used to drive the actuators - * (servos, speed controls, etc.) that operate the vehicle. - * - * Each topic can be published by a single controller - */ + /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/actuator_controls.msg */ + -#ifndef TOPIC_ACTUATOR_CONTROLS_H -#define TOPIC_ACTUATOR_CONTROLS_H +#pragma once #include -#include +#include "../uORB.h" -#define NUM_ACTUATOR_CONTROLS 8 -#define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ +#define NUM_ACTUATOR_CONTROLS 8 +#define NUM_ACTUATOR_CONTROL_GROUPS 4 -/* control sets with pre-defined applications */ -#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) /** * @addtogroup topics * @{ */ + struct actuator_controls_s { uint64_t timestamp; - float control[NUM_ACTUATOR_CONTROLS]; + float control[8]; }; /** * @} */ -/* actuator control sets; this list can be expanded as more controllers emerge */ -ORB_DECLARE(actuator_controls_0); -ORB_DECLARE(actuator_controls_1); -ORB_DECLARE(actuator_controls_2); -ORB_DECLARE(actuator_controls_3); - -#endif +/* register this as object request broker structure */ +ORB_DECLARE(actuator_controls); diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index ff302448a..daeff9cf3 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -139,3 +139,10 @@ typedef param_t px4_param_t; /* wrapper for rotation matrices stored in arrays */ #define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y) + +/* Diverese uORB header defiens */ //XXX: move to better location +#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) +ORB_DECLARE(actuator_controls_0); +ORB_DECLARE(actuator_controls_1); +ORB_DECLARE(actuator_controls_2); +ORB_DECLARE(actuator_controls_3); diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 2045bd2a8..3e94586c1 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -57,6 +57,10 @@ #include #include #include +#include +#include +#include +#include #include #include diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index 7d80af307..20967b541 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -39,6 +39,7 @@ */ #include +#include #include #include -- cgit v1.2.3 From fc4b722e729fda1ed45aa214360b5bb9b462a2b9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 12:17:30 +0100 Subject: fix headers, remove unneded uorb headers --- src/modules/mc_att_control/mc_att_control_base.h | 1 - src/modules/uORB/topics/actuator_controls.h | 62 ----------------- src/modules/uORB/topics/manual_control_setpoint.h | 79 ---------------------- .../uORB/topics/vehicle_attitude_setpoint.h | 72 -------------------- src/platforms/px4_includes.h | 6 +- 5 files changed, 2 insertions(+), 218 deletions(-) delete mode 100644 src/modules/uORB/topics/actuator_controls.h delete mode 100644 src/modules/uORB/topics/manual_control_setpoint.h delete mode 100644 src/modules/uORB/topics/vehicle_attitude_setpoint.h diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index c7853a0ec..5e9c0c4db 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -53,7 +53,6 @@ #include #include -#include #include #include #include diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h deleted file mode 100644 index 3e43107ad..000000000 --- a/src/modules/uORB/topics/actuator_controls.h +++ /dev/null @@ -1,62 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - - /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/actuator_controls.msg */ - - -#pragma once - -#include -#include "../uORB.h" - -#define NUM_ACTUATOR_CONTROLS 8 -#define NUM_ACTUATOR_CONTROL_GROUPS 4 - - -/** - * @addtogroup topics - * @{ - */ - - -struct actuator_controls_s { - uint64_t timestamp; - float control[8]; -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(actuator_controls); diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h deleted file mode 100644 index 440841f90..000000000 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ /dev/null @@ -1,79 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - - /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/manual_control_setpoint.msg */ - - -#pragma once - -#include -#include "../uORB.h" - -#define SWITCH_POS_NONE 0 -#define SWITCH_POS_ON 1 -#define SWITCH_POS_MIDDLE 2 -#define SWITCH_POS_OFF 3 - - -/** - * @addtogroup topics - * @{ - */ - - -struct manual_control_setpoint_s { - uint64_t timestamp; - float x; - float y; - float z; - float r; - float flaps; - float aux1; - float aux2; - float aux3; - float aux4; - float aux5; - uint8_t mode_switch; - uint8_t return_switch; - uint8_t posctl_switch; - uint8_t loiter_switch; - uint8_t acro_switch; - uint8_t offboard_switch; -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(manual_control_setpoint); diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h deleted file mode 100644 index 29fb104df..000000000 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ /dev/null @@ -1,72 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - - /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/vehicle_attitude_setpoint.msg */ - - -#pragma once - -#include -#include "../uORB.h" - - - -/** - * @addtogroup topics - * @{ - */ - - -struct vehicle_attitude_setpoint_s { - uint64_t timestamp; - float roll_body; - float pitch_body; - float yaw_body; - float R_body[9]; - bool R_valid; - float q_d[4]; - bool q_d_valid; - float q_e[4]; - bool q_e_valid; - float thrust; - bool roll_reset_integral; - bool pitch_reset_integral; - bool yaw_reset_integral; -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_attitude_setpoint); diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 3e94586c1..8b0c37402 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -57,10 +57,8 @@ #include #include #include -#include -#include -#include -#include +#include +#include #include #include -- cgit v1.2.3 From 99d89577cd4c7e663583773eccf0883c1df773c4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 12:23:27 +0100 Subject: vehicle rates sp as msg --- msg/px4_msgs/vehicle_rates_setpoint.msg | 6 +++ src/modules/uORB/topics/vehicle_rates_setpoint.h | 67 ------------------------ 2 files changed, 6 insertions(+), 67 deletions(-) create mode 100644 msg/px4_msgs/vehicle_rates_setpoint.msg delete mode 100644 src/modules/uORB/topics/vehicle_rates_setpoint.h diff --git a/msg/px4_msgs/vehicle_rates_setpoint.msg b/msg/px4_msgs/vehicle_rates_setpoint.msg new file mode 100644 index 000000000..834113c79 --- /dev/null +++ b/msg/px4_msgs/vehicle_rates_setpoint.msg @@ -0,0 +1,6 @@ +uint64 timestamp # in microseconds since system start + +float32 roll # body angular rates in NED frame +float32 pitch # body angular rates in NED frame +float32 yaw # body angular rates in NED frame +float32 thrust # thrust normalized to 0..1 diff --git a/src/modules/uORB/topics/vehicle_rates_setpoint.h b/src/modules/uORB/topics/vehicle_rates_setpoint.h deleted file mode 100644 index e5cecf02b..000000000 --- a/src/modules/uORB/topics/vehicle_rates_setpoint.h +++ /dev/null @@ -1,67 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * 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 vehicle_rates_setpoint.h - * Definition of the vehicle rates setpoint topic - */ - -#ifndef TOPIC_VEHICLE_RATES_SETPOINT_H_ -#define TOPIC_VEHICLE_RATES_SETPOINT_H_ - -#include -#include - -/** - * @addtogroup topics - * @{ - */ -struct vehicle_rates_setpoint_s { - uint64_t timestamp; /**< in microseconds since system start */ - - float roll; /**< body angular rates in NED frame */ - float pitch; /**< body angular rates in NED frame */ - float yaw; /**< body angular rates in NED frame */ - float thrust; /**< thrust normalized to 0..1 */ - -}; /**< vehicle_rates_setpoint */ - -/** -* @} -*/ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_rates_setpoint); - -#endif -- cgit v1.2.3 From a71e6ed3c64d5c1c94d6f5f445b4b746ccf37eff Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 12:25:39 +0100 Subject: change headers to use vehicle attitude msg --- src/modules/mc_att_control/mc_att_control_base.h | 1 - src/platforms/px4_includes.h | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 5e9c0c4db..6c4b5c707 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -53,7 +53,6 @@ #include #include -#include #include #include #include diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 8b0c37402..950f5cc79 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -59,6 +59,7 @@ #include #include #include +#include #include #include -- cgit v1.2.3 From b93fcca4339edc5312158dc4a70bf8abf2c8bb4b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 12:34:57 +0100 Subject: vehicle cotnrol mode as msg --- msg/px4_msgs/vehicle_control_mode.msg | 22 +++++ .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 1 + src/modules/mc_att_control/mc_att_control_base.h | 1 - src/modules/uORB/topics/vehicle_control_mode.h | 95 ---------------------- src/platforms/px4_includes.h | 1 + 5 files changed, 24 insertions(+), 96 deletions(-) create mode 100644 msg/px4_msgs/vehicle_control_mode.msg delete mode 100644 src/modules/uORB/topics/vehicle_control_mode.h diff --git a/msg/px4_msgs/vehicle_control_mode.msg b/msg/px4_msgs/vehicle_control_mode.msg new file mode 100644 index 000000000..153a642bb --- /dev/null +++ b/msg/px4_msgs/vehicle_control_mode.msg @@ -0,0 +1,22 @@ + +uint64 timestamp # in microseconds since system start + # is set whenever the writing thread stores new data + +bool flag_armed + +bool flag_external_manual_override_ok # external override non-fatal for system. Only true for fixed wing + +# XXX needs yet to be set by state machine helper +bool flag_system_hil_enabled + +bool flag_control_manual_enabled # true if manual input is mixed in +bool flag_control_auto_enabled # true if onboard autopilot should act +bool flag_control_offboard_enabled # true if offboard control should be used +bool flag_control_rates_enabled # true if rates are stabilized +bool flag_control_attitude_enabled # true if attitude stabilization is mixed in +bool flag_control_force_enabled # true if force control is mixed in +bool flag_control_velocity_enabled # true if horizontal velocity (implies direction) is controlled +bool flag_control_position_enabled # true if position is controlled +bool flag_control_altitude_enabled # true if altitude is controlled +bool flag_control_climb_rate_enabled # true if climb rate is controlled +bool flag_control_termination_enabled # true if flighttermination is enabled diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index e07bcc225..f8399d10b 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -77,6 +77,7 @@ #include #include #include +#include #include #include #include diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 6c4b5c707..036dd4c48 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -53,7 +53,6 @@ #include #include -#include #include #include #include diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h deleted file mode 100644 index 2dd8550bc..000000000 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ /dev/null @@ -1,95 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-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 vehicle_control_mode.h - * Definition of the vehicle_control_mode uORB topic. - * - * All control apps should depend their actions based on the flags set here. - * - * @author Lorenz Meier - * @author Petri Tanskanen - * @author Thomas Gubler - * @author Julian Oes - */ - -#ifndef VEHICLE_CONTROL_MODE -#define VEHICLE_CONTROL_MODE - -#include -#include -#include -#include "vehicle_status.h" - -/** - * @addtogroup topics @{ - */ - - -/** - * state machine / state of vehicle. - * - * Encodes the complete system state and is set by the commander app. - */ - -struct vehicle_control_mode_s { - uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - - bool flag_armed; - - bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ - - // XXX needs yet to be set by state machine helper - bool flag_system_hil_enabled; - - bool flag_control_manual_enabled; /**< true if manual input is mixed in */ - bool flag_control_auto_enabled; /**< true if onboard autopilot should act */ - bool flag_control_offboard_enabled; /**< true if offboard control should be used */ - bool flag_control_rates_enabled; /**< true if rates are stabilized */ - bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - bool flag_control_force_enabled; /**< true if force control is mixed in */ - bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */ - bool flag_control_position_enabled; /**< true if position is controlled */ - bool flag_control_altitude_enabled; /**< true if altitude is controlled */ - bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ - bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_control_mode); - -#endif diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 950f5cc79..a8b94076e 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -60,6 +60,7 @@ #include #include #include +#include #include #include -- cgit v1.2.3 From 356e6f1eebfba8bc8630299ef091570f0c337ba5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 13:42:40 +0100 Subject: WIP, move some ORB defines --- src/modules/uORB/uORB.h | 9 ++++++++- src/platforms/px4_defines.h | 7 ------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 82ff46ad2..578ebf9dd 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -125,7 +125,7 @@ typedef intptr_t orb_advert_t; * node in /obj if required and publishes the initial data. * * Any number of advertisers may publish to a topic; publications are atomic - * but co-ordination between publishers is not provided by the ORB. + * but co-ordination between publishers is not provided by the ORB. * * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. @@ -261,4 +261,11 @@ extern int orb_set_interval(int handle, unsigned interval) __EXPORT; __END_DECLS +/* Diverse uORB header defines */ //XXX: move to better location +#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) +ORB_DECLARE(actuator_controls_0); +ORB_DECLARE(actuator_controls_1); +ORB_DECLARE(actuator_controls_2); +ORB_DECLARE(actuator_controls_3); + #endif /* _UORB_UORB_H */ diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index daeff9cf3..ff302448a 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -139,10 +139,3 @@ typedef param_t px4_param_t; /* wrapper for rotation matrices stored in arrays */ #define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y) - -/* Diverese uORB header defiens */ //XXX: move to better location -#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) -ORB_DECLARE(actuator_controls_0); -ORB_DECLARE(actuator_controls_1); -ORB_DECLARE(actuator_controls_2); -ORB_DECLARE(actuator_controls_3); -- cgit v1.2.3 From 88f4931fd1ac301c18921a5c12884263ac6fd68c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 13:44:08 +0100 Subject: actuator armed as msg --- msg/px4_msgs/actuator_armed.msg | 6 +++++ msg/px4_msgs/unused/actuator_armed.msg | 6 ----- src/modules/mc_att_control/mc_att_control_base.h | 1 - src/modules/uORB/topics/actuator_armed.h | 32 ++++++++++-------------- src/platforms/px4_includes.h | 1 + 5 files changed, 20 insertions(+), 26 deletions(-) create mode 100644 msg/px4_msgs/actuator_armed.msg delete mode 100644 msg/px4_msgs/unused/actuator_armed.msg diff --git a/msg/px4_msgs/actuator_armed.msg b/msg/px4_msgs/actuator_armed.msg new file mode 100644 index 000000000..b83adb8f2 --- /dev/null +++ b/msg/px4_msgs/actuator_armed.msg @@ -0,0 +1,6 @@ + +uint64 timestamp # Microseconds since system boot +bool armed # Set to true if system is armed +bool ready_to_arm # Set to true if system is ready to be armed +bool lockdown # Set to true if actuators are forced to being disabled (due to emergency or HIL) +bool force_failsafe # Set to true if the actuators are forced to the failsafe position diff --git a/msg/px4_msgs/unused/actuator_armed.msg b/msg/px4_msgs/unused/actuator_armed.msg deleted file mode 100644 index f6bf58307..000000000 --- a/msg/px4_msgs/unused/actuator_armed.msg +++ /dev/null @@ -1,6 +0,0 @@ - -uint64 timestamp # Microseconds since system boot -bool armed # Set to true if system is armed -bool ready_to_arm # Set to true if system is ready to be armed -bool lockdown # Set to true if actuators are forced to being disabled (due to emergency or HIL) -bool force_failsafe # Set to true if the actuators are forced to the failsafe position diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 036dd4c48..49c8dad7d 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -53,7 +53,6 @@ #include #include -#include #include #include #include diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h index 1e10e0ad1..918a305fd 100644 --- a/src/modules/uORB/topics/actuator_armed.h +++ b/src/modules/uORB/topics/actuator_armed.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * 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 @@ -31,32 +31,28 @@ * ****************************************************************************/ -/** - * @file actuator_armed.h - * - * Actuator armed topic - * - */ + /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/actuator_armed.msg */ + -#ifndef TOPIC_ACTUATOR_ARMED_H -#define TOPIC_ACTUATOR_ARMED_H +#pragma once #include -#include +#include "../uORB.h" + + /** * @addtogroup topics * @{ */ -/** global 'actuator output is live' control. */ -struct actuator_armed_s { - uint64_t timestamp; /**< Microseconds since system boot */ - bool armed; /**< Set to true if system is armed */ - bool ready_to_arm; /**< Set to true if system is ready to be armed */ - bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ - bool force_failsafe; /**< Set to true if the actuators are forced to the failsafe position */ +struct actuator_armed_s { + uint64_t timestamp; + bool armed; + bool ready_to_arm; + bool lockdown; + bool force_failsafe; }; /** @@ -65,5 +61,3 @@ struct actuator_armed_s { /* register this as object request broker structure */ ORB_DECLARE(actuator_armed); - -#endif diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index a8b94076e..b7771072f 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -61,6 +61,7 @@ #include #include #include +#include #include #include -- cgit v1.2.3 From 77fd7b388b3b80881e405bec5ed56fb959082612 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 14:04:20 +0100 Subject: parameter update as msg --- msg/px4_msgs/parameter_update.msg | 1 + src/modules/mc_att_control/mc_att_control_base.h | 1 - src/modules/uORB/topics/parameter_update.h | 61 ------------------------ src/platforms/px4_includes.h | 1 + 4 files changed, 2 insertions(+), 62 deletions(-) create mode 100644 msg/px4_msgs/parameter_update.msg delete mode 100644 src/modules/uORB/topics/parameter_update.h diff --git a/msg/px4_msgs/parameter_update.msg b/msg/px4_msgs/parameter_update.msg new file mode 100644 index 000000000..39dc336e0 --- /dev/null +++ b/msg/px4_msgs/parameter_update.msg @@ -0,0 +1 @@ +uint64 timestamp # time at which the latest parameter was updated diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 49c8dad7d..f81a0d772 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -53,7 +53,6 @@ #include #include -#include #include #include diff --git a/src/modules/uORB/topics/parameter_update.h b/src/modules/uORB/topics/parameter_update.h deleted file mode 100644 index fe9c9070f..000000000 --- a/src/modules/uORB/topics/parameter_update.h +++ /dev/null @@ -1,61 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 parameter_update.h - * Notification about a parameter update. - */ - -#ifndef TOPIC_PARAMETER_UPDATE_H -#define TOPIC_PARAMETER_UPDATE_H - -#include -#include - -/** - * @addtogroup topics - * @{ - */ - -struct parameter_update_s { - /** time at which the latest parameter was updated */ - uint64_t timestamp; -}; - -/** - * @} - */ - -ORB_DECLARE(parameter_update); - -#endif \ No newline at end of file diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index b7771072f..b8c8d034e 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -62,6 +62,7 @@ #include #include #include +#include #include #include -- cgit v1.2.3 From 377030bd8a25f501c47aba573bf3125ad9061bd0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 14:20:11 +0100 Subject: mc att ctl: remove code which is already in base class --- src/modules/mc_att_control/mc_att_control_main.cpp | 27 ---------------------- 1 file changed, 27 deletions(-) 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 6ab4f95cd..9dec6b8e6 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -227,33 +227,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) { - memset(&_v_att, 0, sizeof(_v_att)); - memset(&_v_att_sp, 0, sizeof(_v_att_sp)); - memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); - memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); - memset(&_v_control_mode, 0, sizeof(_v_control_mode)); - memset(&_actuators, 0, sizeof(_actuators)); - memset(&_armed, 0, sizeof(_armed)); - - _params.att_p.zero(); - _params.rate_p.zero(); - _params.rate_i.zero(); - _params.rate_d.zero(); - _params.yaw_ff = 0.0f; - _params.yaw_rate_max = 0.0f; - _params.man_roll_max = 0.0f; - _params.man_pitch_max = 0.0f; - _params.man_yaw_max = 0.0f; - _params.acro_rate_max.zero(); - - _rates_prev.zero(); - _rates_sp.zero(); - _rates_int.zero(); - _thrust_sp = 0.0f; - _att_control.zero(); - - _I.identity(); - _params_handles.roll_p = param_find("MC_ROLL_P"); _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); -- cgit v1.2.3 From b500cfb1f11b2000ad58d8c8b5b0d19ab9ebb438 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 14:20:11 +0100 Subject: mc att ctl: remove code which is already in base class --- src/modules/mc_att_control/mc_att_control_main.cpp | 27 ---------------------- 1 file changed, 27 deletions(-) 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 887a53508..a5226eb7c 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -236,33 +236,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) { - memset(&_v_att, 0, sizeof(_v_att)); - memset(&_v_att_sp, 0, sizeof(_v_att_sp)); - memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); - memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); - memset(&_v_control_mode, 0, sizeof(_v_control_mode)); - memset(&_actuators, 0, sizeof(_actuators)); - memset(&_armed, 0, sizeof(_armed)); - - _params.att_p.zero(); - _params.rate_p.zero(); - _params.rate_i.zero(); - _params.rate_d.zero(); - _params.yaw_ff = 0.0f; - _params.yaw_rate_max = 0.0f; - _params.man_roll_max = 0.0f; - _params.man_pitch_max = 0.0f; - _params.man_yaw_max = 0.0f; - _params.acro_rate_max.zero(); - - _rates_prev.zero(); - _rates_sp.zero(); - _rates_int.zero(); - _thrust_sp = 0.0f; - _att_control.zero(); - - _I.identity(); - _params_handles.roll_p = param_find("MC_ROLL_P"); _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); -- cgit v1.2.3 From fe6663ad9aa0a5fdb18e2ce3e23dd57d47ae8569 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 15:09:31 +0100 Subject: add platforms/nuttx to default makefile --- src/modules/mc_att_control/mc_att_control_main.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) 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 9dec6b8e6..631ca57e0 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -103,6 +103,8 @@ public: */ int start(); + void handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg); + private: bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ @@ -422,11 +424,13 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) 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)); @@ -582,6 +586,10 @@ MulticopterAttitudeControl::task_main() _exit(0); } +void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) { + PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp); +} + int MulticopterAttitudeControl::start() { @@ -603,8 +611,10 @@ MulticopterAttitudeControl::start() return OK; } -int mc_att_control_main(int argc, char *argv[]) +PX4_MAIN_FUNCTION(mc_att_control) { + px4::init(argc, argv, "mc_att_control"); + if (argc < 1) { errx(1, "usage: mc_att_control {start|stop|status}"); } -- cgit v1.2.3 From afa835744cbef667279913bbf79a0c4697a5ec8d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 15:48:44 +0100 Subject: remove unnecessary type in msg.h template --- msg/templates/msg.h.template | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template index 84dea37a3..5fce506b6 100644 --- a/msg/templates/msg.h.template +++ b/msg/templates/msg.h.template @@ -104,8 +104,7 @@ type_map = {'int8': 'int8_t', 'uint32': 'uint32_t', 'uint64': 'uint64_t', 'float32': 'float', - 'bool': 'bool', - 'actuator_controls': 'actuator_controls'} + 'bool': 'bool'} # Function to print a standard ros type def print_field_def(field): -- cgit v1.2.3 From 9d5f06c9a779752cfb1662d5c794fa85fda3d494 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Dec 2014 15:50:30 +0100 Subject: remove actuator armed uorb topic --- src/modules/uORB/topics/actuator_armed.h | 63 -------------------------------- 1 file changed, 63 deletions(-) delete mode 100644 src/modules/uORB/topics/actuator_armed.h diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h deleted file mode 100644 index 918a305fd..000000000 --- a/src/modules/uORB/topics/actuator_armed.h +++ /dev/null @@ -1,63 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - - /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/actuator_armed.msg */ - - -#pragma once - -#include -#include "../uORB.h" - - - -/** - * @addtogroup topics - * @{ - */ - - -struct actuator_armed_s { - uint64_t timestamp; - bool armed; - bool ready_to_arm; - bool lockdown; - bool force_failsafe; -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(actuator_armed); -- cgit v1.2.3 From 76457e63c51695c814dda75c1cc351cb509c9a19 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 12:38:54 +0100 Subject: add nuttx platform to default makefile --- makefiles/config_px4fmu-v2_default.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index df6b6d0c5..38c5ebc7b 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -113,6 +113,7 @@ MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion MODULES += lib/launchdetection +MODULES += platforms/nuttx # # OBC challenge -- cgit v1.2.3 From 712e5797eba482592c372b06384e79d217f6bc05 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 12:39:33 +0100 Subject: Subscription: define more templates --- src/modules/uORB/Subscription.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 61609d009..fa0594c2e 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -95,5 +95,7 @@ template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; } // namespace uORB -- cgit v1.2.3 From 9ed57211cc684d5135a254d9230c6d82bb092f9b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 12:40:36 +0100 Subject: hack to define isspace in px4_defines, add macro for subscription without callback --- src/platforms/px4_defines.h | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index ff302448a..283dc5a53 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -96,6 +96,9 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) * Building for NuttX */ #include +#ifdef __cplusplus +#include +#endif /* Main entry point */ #define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) @@ -113,6 +116,7 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval) /* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) +#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), nullptr, _interval) /* Parameter handle datatype */ #include @@ -126,10 +130,10 @@ typedef param_t px4_param_t; #endif /* Shortcut for subscribing to topics - * Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback + * Overload the PX4_SUBSCRIBE macro to suppport methods, pure functions as callback and no callback at all */ #define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME -#define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC)(__VA_ARGS__) +#define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC, PX4_SUBSCRIBE_NOCB)(__VA_ARGS__) /* shortcut for advertising topics */ #define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise(PX4_TOPIC(_name)) @@ -139,3 +143,7 @@ typedef param_t px4_param_t; /* wrapper for rotation matrices stored in arrays */ #define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y) + +#define isspace(c) \ + ((c) == ' ' || (c) == '\t' || (c) == '\n' || \ + (c) == '\r' || (c) == '\f' || c== '\v') -- cgit v1.2.3 From 24fd5759b5443cf198f29eb6d5eae8c80cb04fe0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 12:41:11 +0100 Subject: add missing __EXPORT --- src/platforms/px4_middleware.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index b0bc40417..16e73ec04 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -59,7 +59,7 @@ extern bool task_should_exit; /** * Returns true if the app/task should continue to run */ -inline bool ok() { return !task_should_exit; } +__EXPORT inline bool ok() { return !task_should_exit; } #endif class Rate -- cgit v1.2.3 From e6224304606e20bbf79280978ba70326eabea575 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 12:41:54 +0100 Subject: px4 subscriber/nuttx: don't call null callback --- src/platforms/px4_subscriber.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index e995c6e49..5d0120f90 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -115,6 +115,10 @@ public: */ void update() { + if (_callback == nullptr) { + return; + } + if (!uORB::Subscription::updated()) { /* Topic not updated, do not call callback */ return; -- cgit v1.2.3 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(-) 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 From e43a05de3af19f8390aaf5432e7308cc5dabe5ff Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 15:06:17 +0100 Subject: add systemlib to px4 includes --- src/platforms/px4_includes.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index b8c8d034e..525f94aae 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -65,5 +65,6 @@ #include #include #include +#include #endif -- cgit v1.2.3 From cb77c16601338d2d0b22dc71ef057ec4d91652c4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 15:06:45 +0100 Subject: add sched.h to systemlib includes SCHED_RR and SCHED_FIFO are defined in sched.h --- src/modules/systemlib/systemlib.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 3728f2067..6e22a557e 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -41,6 +41,7 @@ #define SYSTEMLIB_H_ #include #include +#include __BEGIN_DECLS -- cgit v1.2.3 From 8ae1c9763d649470ae7c55e35507509dbb2612ff Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 16:22:19 +0100 Subject: write publisher example as class --- src/examples/publisher/module.mk | 3 +- src/examples/publisher/publisher.cpp | 100 ----------------- src/examples/publisher/publisher_example.cpp | 67 ++++++++++++ src/examples/publisher/publisher_example.h | 51 +++++++++ src/examples/publisher/publisher_main.cpp | 112 +++++++++++++++++++ src/examples/subscriber/module.mk | 3 +- src/examples/subscriber/subscriber.cpp | 120 --------------------- src/examples/subscriber/subscriber_example.cpp | 78 ++++++++++++++ src/examples/subscriber/subscriber_example.h | 58 ++++++++++ src/examples/subscriber/subscriber_main.cpp | 112 +++++++++++++++++++ src/modules/mc_att_control/mc_att_control_main.cpp | 3 +- 11 files changed, 483 insertions(+), 224 deletions(-) delete mode 100644 src/examples/publisher/publisher.cpp create mode 100644 src/examples/publisher/publisher_example.cpp create mode 100644 src/examples/publisher/publisher_example.h create mode 100644 src/examples/publisher/publisher_main.cpp delete mode 100644 src/examples/subscriber/subscriber.cpp create mode 100644 src/examples/subscriber/subscriber_example.cpp create mode 100644 src/examples/subscriber/subscriber_example.h create mode 100644 src/examples/subscriber/subscriber_main.cpp diff --git a/src/examples/publisher/module.mk b/src/examples/publisher/module.mk index 0fc4316ec..4f941cd43 100644 --- a/src/examples/publisher/module.mk +++ b/src/examples/publisher/module.mk @@ -37,6 +37,7 @@ MODULE_COMMAND = publisher -SRCS = publisher.cpp +SRCS = publisher_main.cpp \ + publisher_example.cpp MODULE_STACKSIZE = 1200 diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp deleted file mode 100644 index 8ef147493..000000000 --- a/src/examples/publisher/publisher.cpp +++ /dev/null @@ -1,100 +0,0 @@ -/* - * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * * 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. - * * Neither the names of Stanford University or Willow Garage, Inc. 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. - */ - -#include - -using namespace px4; - -/** - * This tutorial demonstrates simple sending of messages over the PX4 middleware system. - */ - -namespace px4 -{ -bool task_should_exit = false; -} - -PX4_MAIN_FUNCTION(publisher) -{ - - px4::init(argc, argv, "px4_publisher"); - - px4::NodeHandle n; - - /** - * The advertise() function is how you tell ROS that you want to - * publish on a given topic name. This invokes a call to the ROS - * master node, which keeps a registry of who is publishing and who - * is subscribing. After this advertise() call is made, the master - * node will notify anyone who is trying to subscribe to this topic name, - * and they will in turn negotiate a peer-to-peer connection with this - * node. advertise() returns a Publisher object which allows you to - * publish messages on that topic through a call to publish(). Once - * all copies of the returned Publisher object are destroyed, the topic - * will be automatically unadvertised. - * - * The second parameter to advertise() is the size of the message queue - * used for publishing messages. If messages are published more quickly - * than we can send them, the number here specifies how many messages to - * buffer up before throwing some away. - */ - px4::Publisher * rc_channels_pub = PX4_ADVERTISE(n, rc_channels); - - - px4::Rate loop_rate(10); - - /** - * A count of how many messages we have sent. This is used to create - * a unique string for each message. - */ - int count = 0; - - while (px4::ok()) { - /** - * This is a message object. You stuff it with data, and then publish it. - */ - PX4_TOPIC_T(rc_channels) msg; - - msg.timestamp_last_valid = px4::get_time_micros(); - PX4_INFO("%llu", msg.timestamp_last_valid); - - /** - * The publish() function is how you send messages. The parameter - * is the message object. The type of this object must agree with the type - * given as a template parameter to the advertise<>() call, as was done - * in the constructor above. - */ - rc_channels_pub->publish(msg); - - n.spinOnce(); - - loop_rate.sleep(); - ++count; - } - - return 0; -} diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp new file mode 100644 index 000000000..3c716291b --- /dev/null +++ b/src/examples/publisher/publisher_example.cpp @@ -0,0 +1,67 @@ + +/**************************************************************************** + * + * 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 publisher_example.cpp + * Example subscriber for ros and px4 + * + * @author Thomas Gubler + */ + +#include "publisher_example.h" + +PublisherExample::PublisherExample() : + _n(), + _rc_channels_pub(PX4_ADVERTISE(_n, rc_channels)) +{ + +} + +int PublisherExample::main() +{ + px4::Rate loop_rate(10); + + while (px4::ok()) { + PX4_TOPIC_T(rc_channels) msg; + msg.timestamp_last_valid = px4::get_time_micros(); + PX4_INFO("%llu", msg.timestamp_last_valid); + + _rc_channels_pub->publish(msg); + + _n.spinOnce(); + loop_rate.sleep(); + } + + return 0; +} diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h new file mode 100644 index 000000000..78c1ffc89 --- /dev/null +++ b/src/examples/publisher/publisher_example.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * 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 publisher.h + * Example publisher for ros and px4 + * + * @author Thomas Gubler + */ +#include +class PublisherExample { +public: + PublisherExample(); + + ~PublisherExample() {}; + + int main(); +protected: + px4::NodeHandle _n; + px4::Publisher * _rc_channels_pub; +}; diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp new file mode 100644 index 000000000..e1034fec5 --- /dev/null +++ b/src/examples/publisher/publisher_main.cpp @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * 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 publisher_main.cpp + * Example publisher for ros and px4 + * + * @author Thomas Gubler + */ +#include +#include +#include "publisher_example.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; + +int publisher_task_main(int argc, char *argv[]); + +PX4_MAIN_FUNCTION(publisher) +{ + px4::init(argc, argv, "publisher"); + + if (argc < 1) { + errx(1, "usage: publisher {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("publisher", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + publisher_task_main, + (argv) ? (const char **)&argv[2] : (const char **)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; +} + +int publisher_task_main(int argc, char *argv[]) +{ + warnx("starting"); + PublisherExample p; + thread_running = true; + p.main(); + + warnx("exiting."); + thread_running = false; + return 0; +} diff --git a/src/examples/subscriber/module.mk b/src/examples/subscriber/module.mk index 90b4d8ffc..3156ebb30 100644 --- a/src/examples/subscriber/module.mk +++ b/src/examples/subscriber/module.mk @@ -37,7 +37,8 @@ MODULE_COMMAND = subscriber -SRCS = subscriber.cpp \ +SRCS = subscriber_main.cpp \ + subscriber_example.cpp \ subscriber_params.c MODULE_STACKSIZE = 2400 diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp deleted file mode 100644 index 0d9ca1fa7..000000000 --- a/src/examples/subscriber/subscriber.cpp +++ /dev/null @@ -1,120 +0,0 @@ -/* - * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * * 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. - * * Neither the names of Stanford University or Willow Garage, Inc. 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. - */ - -#include -#include "subscriber_params.h" -#include - -using namespace px4; - -/** - * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. - */ -void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid); -} -void rc_channels_callback2(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("I heard (2): [%llu]", msg.timestamp_last_valid); -} - -class RCHandler { -public: - void callback(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp_last_valid); - } -}; - -RCHandler rchandler; - - -namespace px4 -{ -bool task_should_exit = false; -} - -PX4_MAIN_FUNCTION(subscriber) { - /** - * The ros::init() function needs to see argc and argv so that it can perform - * any ROS arguments and name remapping that were provided at the command line. For programmatic - * remappings you can use a different version of init() which takes remappings - * directly, but for most command-line programs, passing argc and argv is the easiest - * way to do it. The third argument to init() is the name of the node. - * - * You must call one of the versions of px4::init() before using any other - * part of the PX4/ ROS system. - */ - px4::init(argc, argv, "listener"); - - /** - * NodeHandle is the main access point to communications with the ROS system. - * The first NodeHandle constructed will fully initialize this node, and the last - * NodeHandle destructed will close down the node. - */ - px4::NodeHandle n; - - /* Define parameters */ - px4_param_t p_sub_interv = PX4_PARAM_INIT(SUB_INTERV); - px4_param_t p_test_float = PX4_PARAM_INIT(SUB_TESTF); - - /* Read the parameter back for testing */ - int32_t sub_interval; - float test_float; - PX4_PARAM_GET(p_sub_interv, &sub_interval); - PX4_INFO("Param SUB_INTERV = %d", sub_interval); - PX4_PARAM_GET(p_test_float, &test_float); - PX4_INFO("Param SUB_TESTF = %.3f", (double)test_float); - - /** - * The subscribe() call is how you tell ROS that you want to receive messages - * on a given topic. This invokes a call to the ROS - * master node, which keeps a registry of who is publishing and who - * is subscribing. Messages are passed to a callback function, here - * called chatterCallback. subscribe() returns a Subscriber object that you - * must hold on to until you want to unsubscribe. When all copies of the Subscriber - * object go out of scope, this callback will automatically be unsubscribed from - * this topic. - * - * The second parameter to the subscribe() function is the size of the message - * queue. If messages are arriving faster than they are being processed, this - * is the number of messages that will be buffered up before beginning to throw - * away the oldest ones. - */ - PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, sub_interval); - PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback2, 1000); - PX4_SUBSCRIBE(n, rc_channels, RCHandler::callback, rchandler, 1000); - PX4_INFO("subscribed"); - - /** - * 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; -} diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp new file mode 100644 index 000000000..cddab1035 --- /dev/null +++ b/src/examples/subscriber/subscriber_example.cpp @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * 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 subscriber.cpp + * Example subscriber for ros and px4 + * + * @author Thomas Gubler + */ + +#include "subscriber_params.h" +#include "subscriber_example.h" + +using namespace px4; + +void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) { + PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid); +} + +SubscriberExample::SubscriberExample() : + _n(), + _sub_interval(0), + _test_float(0.0f) +{ + /* Define parameters */ + _p_sub_interv = PX4_PARAM_INIT(SUB_INTERV); + _p_test_float = PX4_PARAM_INIT(SUB_TESTF); + + /* Read the parameter back as example */ + PX4_PARAM_GET(_p_sub_interv, &_sub_interval); + PX4_INFO("Param SUB_INTERV = %d", _sub_interval); + PX4_PARAM_GET(_p_test_float, &_test_float); + PX4_INFO("Param SUB_TESTF = %.3f", (double)_test_float); + + /* Do some subscriptions */ + /* Function */ + PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _sub_interval); + /* Class Method */ + PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000); + PX4_INFO("subscribed"); +} + +/** + * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. + */ +void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { + PX4_INFO("Subscriber callback: [%llu]", msg.timestamp_last_valid); +} diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h new file mode 100644 index 000000000..4bdc15c2d --- /dev/null +++ b/src/examples/subscriber/subscriber_example.h @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * 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 subscriber.h + * Example subscriber for ros and px4 + * + * @author Thomas Gubler + */ +#include +class SubscriberExample { +public: + SubscriberExample(); + + ~SubscriberExample() {}; + + void spin() {_n.spin();} +protected: + px4::NodeHandle _n; + px4_param_t _p_sub_interv; + int32_t _sub_interval; + px4_param_t _p_test_float; + float _test_float; + + void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg); + + +}; diff --git a/src/examples/subscriber/subscriber_main.cpp b/src/examples/subscriber/subscriber_main.cpp new file mode 100644 index 000000000..32de4fd6e --- /dev/null +++ b/src/examples/subscriber/subscriber_main.cpp @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * 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 subscriber_main.cpp + * Example subscriber for ros and px4 + * + * @author Thomas Gubler + */ +#include +#include +#include "subscriber_example.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; + +int subscriber_task_main(int argc, char *argv[]); + +PX4_MAIN_FUNCTION(subscriber) +{ + px4::init(argc, argv, "subscriber"); + + if (argc < 1) { + errx(1, "usage: subscriber {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("subscriber", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + subscriber_task_main, + (argv) ? (const char **)&argv[2] : (const char **)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; +} + +int subscriber_task_main(int argc, char *argv[]) +{ + warnx("starting"); + SubscriberExample s; + thread_running = true; + s.spin(); + + warnx("exiting."); + thread_running = false; + return 0; +} 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 7a3b61482..1103191e7 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -100,8 +100,7 @@ namespace px4 bool task_should_exit = false; } -// PX4_MAIN_FUNCTION(mc_att_control) { - // px4::init(argc, argv, "listener"); +// PX4_MAIN_FUNCTION(mc_att_control) { px4::init(argc, argv, "listener"); // px4::NodeHandle n; -- cgit v1.2.3 From da4cfad3c2dc14f17ccf0e9a8ce41309b76964e4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 16:25:10 +0100 Subject: subscriber example: improve init --- src/examples/subscriber/subscriber_example.cpp | 8 +++----- src/examples/subscriber/subscriber_example.h | 2 +- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index cddab1035..e6d63cab3 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file subscriber.cpp + * @file subscriber_example.cpp * Example subscriber for ros and px4 * * @author Thomas Gubler @@ -49,13 +49,11 @@ void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) { SubscriberExample::SubscriberExample() : _n(), + _p_sub_interv(PX4_PARAM_INIT(SUB_INTERV)), _sub_interval(0), + _p_test_float(PX4_PARAM_INIT(SUB_TESTF)), _test_float(0.0f) { - /* Define parameters */ - _p_sub_interv = PX4_PARAM_INIT(SUB_INTERV); - _p_test_float = PX4_PARAM_INIT(SUB_TESTF); - /* Read the parameter back as example */ PX4_PARAM_GET(_p_sub_interv, &_sub_interval); PX4_INFO("Param SUB_INTERV = %d", _sub_interval); diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index 4bdc15c2d..b52f89ba8 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file subscriber.h + * @file subscriber_example.h * Example subscriber for ros and px4 * * @author Thomas Gubler -- cgit v1.2.3 From f0ad2c9ef55dc2508e777384f8d0496042ade9e8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 17:02:49 +0100 Subject: px4 subscriber: uorb: check if callback null at correct location --- src/platforms/px4_subscriber.h | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 5d0120f90..45cb650b2 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -115,10 +115,6 @@ public: */ void update() { - if (_callback == nullptr) { - return; - } - if (!uORB::Subscription::updated()) { /* Topic not updated, do not call callback */ return; @@ -127,6 +123,12 @@ public: /* get latest data */ uORB::Subscription::update(); + + /* Check if there is a callback */ + if (_callback == nullptr) { + return; + } + /* Call callback which performs actions based on this data */ _callback(uORB::Subscription::getData()); -- cgit v1.2.3 From 1b6b6cd35c1f14766d49144bbc18d19d02656362 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 17:03:24 +0100 Subject: add no callback example to subscriber example --- src/examples/subscriber/subscriber_example.cpp | 15 ++++++++++----- src/examples/subscriber/subscriber_example.h | 3 ++- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index e6d63cab3..460a3267a 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -50,21 +50,24 @@ void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) { SubscriberExample::SubscriberExample() : _n(), _p_sub_interv(PX4_PARAM_INIT(SUB_INTERV)), - _sub_interval(0), + _interval(0), _p_test_float(PX4_PARAM_INIT(SUB_TESTF)), _test_float(0.0f) { /* Read the parameter back as example */ - PX4_PARAM_GET(_p_sub_interv, &_sub_interval); - PX4_INFO("Param SUB_INTERV = %d", _sub_interval); + PX4_PARAM_GET(_p_sub_interv, &_interval); + PX4_INFO("Param SUB_INTERV = %d", _interval); PX4_PARAM_GET(_p_test_float, &_test_float); PX4_INFO("Param SUB_TESTF = %.3f", (double)_test_float); /* Do some subscriptions */ /* Function */ - PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _sub_interval); + PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval); /* Class Method */ PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000); + /* No callback */ + _sub_rc_chan = PX4_SUBSCRIBE(_n, rc_channels, 500); + PX4_INFO("subscribed"); } @@ -72,5 +75,7 @@ SubscriberExample::SubscriberExample() : * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. */ void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("Subscriber callback: [%llu]", msg.timestamp_last_valid); + PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", + msg.timestamp_last_valid, + ((SubscriberPX4 *)_sub_rc_chan)->timestamp_last_valid); } diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index b52f89ba8..848d43f76 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -48,9 +48,10 @@ public: protected: px4::NodeHandle _n; px4_param_t _p_sub_interv; - int32_t _sub_interval; + int32_t _interval; px4_param_t _p_test_float; float _test_float; + px4::Subscriber * _sub_rc_chan; void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg); -- cgit v1.2.3 From d04bbf11ec5d273902d3920981ccac49489d7098 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 18:34:41 +0100 Subject: subscriber example: add comment --- src/examples/subscriber/subscriber_example.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 460a3267a..4e0350ff0 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -73,6 +73,7 @@ SubscriberExample::SubscriberExample() : /** * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. + * Also the current value of the _sub_rc_chan subscription is printed */ void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", -- cgit v1.2.3 From a1685ed6d093ec01214e6839ea66f9dd565e55ce Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 11 Dec 2014 09:27:11 +0100 Subject: change definition of px4 main function --- src/examples/publisher/publisher_main.cpp | 6 +++--- src/examples/subscriber/subscriber_main.cpp | 10 +++++----- src/platforms/px4_defines.h | 2 +- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp index e1034fec5..c4e79aaa6 100644 --- a/src/examples/publisher/publisher_main.cpp +++ b/src/examples/publisher/publisher_main.cpp @@ -49,9 +49,9 @@ bool task_should_exit = false; } using namespace px4; -int publisher_task_main(int argc, char *argv[]); +PX4_MAIN_FUNCTION(publisher); -PX4_MAIN_FUNCTION(publisher) +extern "C" __EXPORT int publisher_main(int argc, char *argv[]) { px4::init(argc, argv, "publisher"); @@ -99,7 +99,7 @@ PX4_MAIN_FUNCTION(publisher) return 1; } -int publisher_task_main(int argc, char *argv[]) +PX4_MAIN_FUNCTION(publisher) { warnx("starting"); PublisherExample p; diff --git a/src/examples/subscriber/subscriber_main.cpp b/src/examples/subscriber/subscriber_main.cpp index 32de4fd6e..0436dc9f2 100644 --- a/src/examples/subscriber/subscriber_main.cpp +++ b/src/examples/subscriber/subscriber_main.cpp @@ -49,12 +49,10 @@ bool task_should_exit = false; } using namespace px4; -int subscriber_task_main(int argc, char *argv[]); +PX4_MAIN_FUNCTION(subscriber); -PX4_MAIN_FUNCTION(subscriber) +extern "C" __EXPORT int subscriber_main(int argc, char *argv[]) { - px4::init(argc, argv, "subscriber"); - if (argc < 1) { errx(1, "usage: subscriber {start|stop|status}"); } @@ -99,8 +97,10 @@ PX4_MAIN_FUNCTION(subscriber) return 1; } -int subscriber_task_main(int argc, char *argv[]) +PX4_MAIN_FUNCTION(subscriber) { + px4::init(argc, argv, "subscriber"); + warnx("starting"); SubscriberExample s; thread_running = true; diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 283dc5a53..440f1b6fc 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -100,7 +100,7 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) #include #endif /* Main entry point */ -#define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) +#define PX4_MAIN_FUNCTION(_prefix) int _prefix##_task_main(int argc, char *argv[]) /* Print/output wrappers */ #define PX4_WARN warnx -- cgit v1.2.3 From 173b1b2a8bcf2344be92b1f6e5acbc089a43fcab Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 11 Dec 2014 10:51:19 +0100 Subject: WIP, make class based and extended subscriber/publisher example compile for ros --- CMakeLists.txt | 24 ++++++++++++++---------- src/examples/publisher/publisher_example.cpp | 2 ++ src/examples/publisher/publisher_example.h | 1 + src/examples/publisher/publisher_main.cpp | 6 ++++-- src/examples/subscriber/subscriber_example.cpp | 7 ++++--- src/examples/subscriber/subscriber_example.h | 3 +++ src/examples/subscriber/subscriber_main.cpp | 6 ++++-- src/platforms/px4_defines.h | 7 +++++-- src/platforms/px4_nodehandle.h | 13 +++++++++++++ 9 files changed, 50 insertions(+), 19 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e91f2cc37..f206a5aa2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -111,7 +111,9 @@ target_link_libraries(px4 ) ## Declare a test publisher -add_executable(publisher src/examples/publisher/publisher.cpp) +add_executable(publisher + src/examples/publisher/publisher_main.cpp + src/examples/publisher/publisher_example.cpp) ## Add cmake target dependencies of the executable/library ## as an example, message headers may need to be generated before nodes @@ -124,7 +126,9 @@ target_link_libraries(publisher ) ## Declare a test subscriber -add_executable(subscriber src/examples/subscriber/subscriber.cpp) +add_executable(subscriber + src/examples/subscriber/subscriber_main.cpp + src/examples/subscriber/subscriber_example.cpp) ## Add cmake target dependencies of the executable/library ## as an example, message headers may need to be generated before nodes @@ -136,14 +140,14 @@ target_link_libraries(subscriber px4 ) -add_executable(mc_att_control - src/modules/mc_att_control/mc_att_control_main.cpp - src/moudles/mc_att_control/mc_att_control_base.cpp) -add_dependencies(mc_att_control px4_generate_messages_cpp) -target_link_libraries(mc_att_control - ${catkin_LIBRARIES} - px4 -) +# add_executable(mc_att_control + # src/modules/mc_att_control/mc_att_control_main.cpp + # src/moudles/mc_att_control/mc_att_control_base.cpp) +# add_dependencies(mc_att_control px4_generate_messages_cpp) +# target_link_libraries(mc_att_control + # ${catkin_LIBRARIES} + # px4 +# ) ############# diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp index 3c716291b..2e5779ebe 100644 --- a/src/examples/publisher/publisher_example.cpp +++ b/src/examples/publisher/publisher_example.cpp @@ -41,6 +41,8 @@ #include "publisher_example.h" +using namespace px4; + PublisherExample::PublisherExample() : _n(), _rc_channels_pub(PX4_ADVERTISE(_n, rc_channels)) diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h index 78c1ffc89..304ecef47 100644 --- a/src/examples/publisher/publisher_example.h +++ b/src/examples/publisher/publisher_example.h @@ -38,6 +38,7 @@ * @author Thomas Gubler */ #include + class PublisherExample { public: PublisherExample(); diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp index c4e79aaa6..8b692d963 100644 --- a/src/examples/publisher/publisher_main.cpp +++ b/src/examples/publisher/publisher_main.cpp @@ -51,6 +51,7 @@ using namespace px4; PX4_MAIN_FUNCTION(publisher); +#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__)) extern "C" __EXPORT int publisher_main(int argc, char *argv[]) { px4::init(argc, argv, "publisher"); @@ -98,15 +99,16 @@ extern "C" __EXPORT int publisher_main(int argc, char *argv[]) warnx("unrecognized command"); return 1; } +#endif PX4_MAIN_FUNCTION(publisher) { - warnx("starting"); + PX4_INFO("starting"); PublisherExample p; thread_running = true; p.main(); - warnx("exiting."); + PX4_INFO("exiting."); thread_running = false; return 0; } diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 4e0350ff0..39d090752 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -76,7 +76,8 @@ SubscriberExample::SubscriberExample() : * Also the current value of the _sub_rc_chan subscription is printed */ void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", - msg.timestamp_last_valid, - ((SubscriberPX4 *)_sub_rc_chan)->timestamp_last_valid); + //XXX + // PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", + // msg.timestamp_last_valid, + // ((SubscriberPX4 *)_sub_rc_chan)->timestamp_last_valid); } diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index 848d43f76..73c739035 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -38,6 +38,9 @@ * @author Thomas Gubler */ #include + +using namespace px4; + class SubscriberExample { public: SubscriberExample(); diff --git a/src/examples/subscriber/subscriber_main.cpp b/src/examples/subscriber/subscriber_main.cpp index 0436dc9f2..716233739 100644 --- a/src/examples/subscriber/subscriber_main.cpp +++ b/src/examples/subscriber/subscriber_main.cpp @@ -51,6 +51,7 @@ using namespace px4; PX4_MAIN_FUNCTION(subscriber); +#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__)) extern "C" __EXPORT int subscriber_main(int argc, char *argv[]) { if (argc < 1) { @@ -96,17 +97,18 @@ extern "C" __EXPORT int subscriber_main(int argc, char *argv[]) warnx("unrecognized command"); return 1; } +#endif PX4_MAIN_FUNCTION(subscriber) { px4::init(argc, argv, "subscriber"); - warnx("starting"); + PX4_INFO("starting"); SubscriberExample s; thread_running = true; s.spin(); - warnx("exiting."); + PX4_INFO("exiting."); thread_running = false; return 0; } diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 440f1b6fc..819954d0e 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -66,9 +66,11 @@ #define PX4_TOPIC_T(_name) _name /* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */ -#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj); +#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _objptr, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, _objptr); /* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); +/* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */ +#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe(PX4_TOPIC(_name)); /* Parameter handle datatype */ typedef const char *px4_param_t; @@ -113,9 +115,10 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) #define PX4_TOPIC_T(_name) _name##_s /* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */ -#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval) +#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _objptr, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _objptr, std::placeholders::_1), _interval) /* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) +/* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), nullptr, _interval) /* Parameter handle datatype */ diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 5b7247b20..25b8e037d 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -99,6 +99,19 @@ public: return sub; } + /** + * Subscribe with no callback, just the latest value is stored on updates + * @param topic Name of the topic + */ + template + Subscriber *subscribe(const char *topic) + { + //XXX missing implementation + // Subscriber *sub = new Subscriber(ros_sub); + // _subs.push_back(sub); + return (Subscriber *)NULL; + } + /** * Advertise topic * @param topic Name of the topic -- cgit v1.2.3 From fd6a5fd38b194300788c597fd1636b2a97e24642 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 11 Dec 2014 10:58:47 +0100 Subject: move px4::init call --- src/examples/publisher/publisher_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp index 8b692d963..5b50ecc6c 100644 --- a/src/examples/publisher/publisher_main.cpp +++ b/src/examples/publisher/publisher_main.cpp @@ -54,8 +54,6 @@ PX4_MAIN_FUNCTION(publisher); #if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__)) extern "C" __EXPORT int publisher_main(int argc, char *argv[]) { - px4::init(argc, argv, "publisher"); - if (argc < 1) { errx(1, "usage: publisher {start|stop|status}"); } @@ -103,6 +101,8 @@ extern "C" __EXPORT int publisher_main(int argc, char *argv[]) PX4_MAIN_FUNCTION(publisher) { + px4::init(argc, argv, "publisher"); + PX4_INFO("starting"); PublisherExample p; thread_running = true; -- cgit v1.2.3 From c68c277c94cacd2a64b634dd9c45ace2a04c2911 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 11 Dec 2014 15:04:37 +0100 Subject: subscription class for ros now stores last message to avoid manual copy and support subscription with no callbacks --- CMakeLists.txt | 1 + src/examples/subscriber/subscriber_example.cpp | 7 +- src/platforms/px4_defines.h | 18 +++-- src/platforms/px4_nodehandle.h | 25 +++--- src/platforms/px4_subscriber.h | 107 +++++++++++++++++++------ 5 files changed, 113 insertions(+), 45 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f206a5aa2..489467db7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,6 @@ cmake_minimum_required(VERSION 2.8.3) project(px4) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 39d090752..95e208ca6 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -76,8 +76,7 @@ SubscriberExample::SubscriberExample() : * Also the current value of the _sub_rc_chan subscription is printed */ void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { - //XXX - // PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", - // msg.timestamp_last_valid, - // ((SubscriberPX4 *)_sub_rc_chan)->timestamp_last_valid); + PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", + msg.timestamp_last_valid, + ((PX4_SUBSCRIBER_T(rc_channels) *)_sub_rc_chan)->get_msg().timestamp_last_valid); } diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 819954d0e..fa3ef216a 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -70,7 +70,7 @@ /* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); /* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */ -#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe(PX4_TOPIC(_name)); +#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe(PX4_TOPIC(_name)); /* Parameter handle datatype */ typedef const char *px4_param_t; @@ -93,6 +93,9 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) /* Get value of parameter */ #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) +/* Get a subscriber class type based on the topic name */ +#define PX4_SUBSCRIBER_T(_name) SubscriberROS + #else /* * Building for NuttX @@ -130,6 +133,15 @@ typedef param_t px4_param_t; /* Get value of parameter */ #define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt) + +/* Get a subscriber class type based on the topic name */ +#define PX4_SUBSCRIBER_T(_name) SubscriberUORB + +/* XXX this is a hack to resolve conflicts with NuttX headers */ +#define isspace(c) \ + ((c) == ' ' || (c) == '\t' || (c) == '\n' || \ + (c) == '\r' || (c) == '\f' || c== '\v') + #endif /* Shortcut for subscribing to topics @@ -146,7 +158,3 @@ typedef param_t px4_param_t; /* wrapper for rotation matrices stored in arrays */ #define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y) - -#define isspace(c) \ - ((c) == ' ' || (c) == '\t' || (c) == '\n' || \ - (c) == '\r' || (c) == '\f' || c== '\v') diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 25b8e037d..c24a18303 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -77,10 +77,11 @@ public: * @param fb Callback, executed on receiving a new message */ template - Subscriber *subscribe(const char *topic, void(*fp)(M)) + Subscriber *subscribe(const char *topic, void(*fp)(const M&)) { - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp); - Subscriber *sub = new Subscriber(ros_sub); + Subscriber *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS*)sub); + ((SubscriberROS*)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); return sub; } @@ -91,10 +92,11 @@ public: * @param fb Callback, executed on receiving a new message */ template - Subscriber *subscribe(const char *topic, void(T::*fp)(M), T *obj) + Subscriber *subscribe(const char *topic, void(T::*fp)(const M&), T *obj) { - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj); - Subscriber *sub = new Subscriber(ros_sub); + Subscriber *sub = new SubscriberROS(std::bind(fp, obj, std::placeholders::_1)); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS*)sub); + ((SubscriberROS*)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); return sub; } @@ -106,10 +108,11 @@ public: template Subscriber *subscribe(const char *topic) { - //XXX missing implementation - // Subscriber *sub = new Subscriber(ros_sub); - // _subs.push_back(sub); - return (Subscriber *)NULL; + Subscriber *sub = new SubscriberROS(); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS*)sub); + ((SubscriberROS*)sub)->set_ros_sub(ros_sub); + _subs.push_back(sub); + return sub; } /** @@ -165,7 +168,7 @@ public: std::function callback, unsigned interval) { - SubscriberPX4 *sub_px4 = new SubscriberPX4(meta, interval, callback, &_subs); + SubscriberUORB *sub_px4 = new SubscriberUORB(meta, interval, callback, &_subs); /* Check if this is the smallest interval so far and update _sub_min_interval */ if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 45cb650b2..234b72f5b 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -36,6 +36,7 @@ * * PX4 Middleware Wrapper Subscriber */ +#include #pragma once #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) /* includes when building for ros */ @@ -44,59 +45,108 @@ /* includes when building for NuttX */ #include #include -#include +#include "px4_nodehandle.h" #endif namespace px4 { - -#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/** + * Subscriber class which is used by nodehandle + */ class Subscriber { public: - /** - * Construct Subscriber by providing a ros::Subscriber - * @param ros_sub the ros subscriber which will be used to perform the publications - */ - Subscriber(ros::Subscriber ros_sub) : - _ros_sub(ros_sub) - {} - + Subscriber() {}; ~Subscriber() {}; -private: - ros::Subscriber _ros_sub; /**< Handle to ros subscriber */ }; -#else - +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) /** - * Subscriber class which is used by nodehandle when building for NuttX + * Subscriber class that is templated with the ros n message type */ -class Subscriber +template +class SubscriberROS : + public Subscriber { +friend class NodeHandle; + public: - Subscriber() {}; - ~Subscriber() {}; -private: + /** + * Construct Subscriber by providing a callback function + */ + SubscriberROS(std::function cbf) : + Subscriber(), + _ros_sub(), + _cbf(cbf), + _msg_current() + {} + + /** + * Construct Subscriber without a callback function + */ + SubscriberROS() : + Subscriber(), + _ros_sub(), + _cbf(NULL), + _msg_current() + {} + + + ~SubscriberROS() {}; + + /* Accessors*/ + /** + * Get the last message value + */ + const M& get_msg() { return _msg_current; } + +protected: + /** + * Called on topic update, saves the current message and then calls the provided callback function + */ + void callback(const M &msg) { + /* Store data */ + _msg_current = msg; + + /* Call callback */ + if (_cbf != NULL) { + _cbf(msg); + } + + } + + /** + * Saves the ros subscriber to keep ros subscription alive + */ + void set_ros_sub(ros::Subscriber ros_sub) { + _ros_sub = ros_sub; + } + + ros::Subscriber _ros_sub; /**< Handle to ros subscriber */ + std::function _cbf; /**< Callback that the user provided on the subscription */ + M _msg_current; /**< Current Message value */ + }; +#else + /** * Subscriber class that is templated with the uorb subscription message type */ template -class SubscriberPX4 : +class SubscriberUORB : public Subscriber, public uORB::Subscription { public: /** - * Construct SubscriberPX4 by providing orb meta data + * Construct SubscriberUORB by providing orb meta data * @param meta orb metadata for the topic which is used * @param callback Callback, executed on receiving a new message * @param interval Minimal interval between calls to callback * @param list subscriber is added to this list */ - SubscriberPX4(const struct orb_metadata *meta, + SubscriberUORB(const struct orb_metadata *meta, unsigned interval, std::function callback, List *list) : @@ -106,7 +156,7 @@ public: //XXX store callback {} - ~SubscriberPX4() {}; + ~SubscriberUORB() {}; /** * Update Subscription @@ -133,7 +183,14 @@ public: _callback(uORB::Subscription::getData()); }; -private: + + /* Accessors*/ + /** + * Get the last message value + */ + const M& get_msg() { return uORB::Subscription::getData(); } + +protected: std::function _callback; /**< Callback handle, called when new data is available */ -- cgit v1.2.3 From 998646f03b229ae86bd6e57ff0bd15dd1763c342 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 11 Dec 2014 15:33:32 +0100 Subject: add base class and template subscriber class as well to improve interface to get last msg value --- src/examples/subscriber/subscriber_example.cpp | 2 +- src/examples/subscriber/subscriber_example.h | 2 +- src/platforms/px4_defines.h | 9 ++---- src/platforms/px4_nodehandle.h | 28 +++++++++---------- src/platforms/px4_publisher.h | 25 ++++++++++++++--- src/platforms/px4_subscriber.h | 38 ++++++++++++++++++++------ 6 files changed, 70 insertions(+), 34 deletions(-) diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 95e208ca6..83ead4ba5 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -78,5 +78,5 @@ SubscriberExample::SubscriberExample() : void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", msg.timestamp_last_valid, - ((PX4_SUBSCRIBER_T(rc_channels) *)_sub_rc_chan)->get_msg().timestamp_last_valid); + _sub_rc_chan->get_msg().timestamp_last_valid); } diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index 73c739035..c4b853d4d 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -54,7 +54,7 @@ protected: int32_t _interval; px4_param_t _p_test_float; float _test_float; - px4::Subscriber * _sub_rc_chan; + px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan; void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg); diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index fa3ef216a..9bb190380 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -93,9 +93,6 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) /* Get value of parameter */ #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) -/* Get a subscriber class type based on the topic name */ -#define PX4_SUBSCRIBER_T(_name) SubscriberROS - #else /* * Building for NuttX @@ -134,9 +131,6 @@ typedef param_t px4_param_t; /* Get value of parameter */ #define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt) -/* Get a subscriber class type based on the topic name */ -#define PX4_SUBSCRIBER_T(_name) SubscriberUORB - /* XXX this is a hack to resolve conflicts with NuttX headers */ #define isspace(c) \ ((c) == ' ' || (c) == '\t' || (c) == '\n' || \ @@ -150,6 +144,9 @@ typedef param_t px4_param_t; #define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME #define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC, PX4_SUBSCRIBE_NOCB)(__VA_ARGS__) +/* Get a subscriber class type based on the topic name */ +#define PX4_SUBSCRIBER(_name) Subscriber + /* shortcut for advertising topics */ #define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise(PX4_TOPIC(_name)) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index c24a18303..9bd792c69 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -77,13 +77,13 @@ public: * @param fb Callback, executed on receiving a new message */ template - Subscriber *subscribe(const char *topic, void(*fp)(const M&)) + Subscriber *subscribe(const char *topic, void(*fp)(const M&)) { - Subscriber *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); + SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS*)sub); ((SubscriberROS*)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); - return sub; + return (Subscriber *)sub; } /** @@ -92,13 +92,13 @@ public: * @param fb Callback, executed on receiving a new message */ template - Subscriber *subscribe(const char *topic, void(T::*fp)(const M&), T *obj) + Subscriber *subscribe(const char *topic, void(T::*fp)(const M&), T *obj) { - Subscriber *sub = new SubscriberROS(std::bind(fp, obj, std::placeholders::_1)); + SubscriberBase *sub = new SubscriberROS(std::bind(fp, obj, std::placeholders::_1)); ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS*)sub); ((SubscriberROS*)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); - return sub; + return (Subscriber *)sub; } /** @@ -106,13 +106,13 @@ public: * @param topic Name of the topic */ template - Subscriber *subscribe(const char *topic) + Subscriber *subscribe(const char *topic) { - Subscriber *sub = new SubscriberROS(); + SubscriberBase *sub = new SubscriberROS(); ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS*)sub); ((SubscriberROS*)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); - return sub; + return (Subscriber *)sub; } /** @@ -141,10 +141,10 @@ public: private: static const uint32_t kQueueSizeDefault = 1000; /**< Size of queue for ROS */ - std::list _subs; /**< Subcriptions of node */ - std::list _pubs; /**< Publications of node */ + std::list _subs; /**< Subcriptions of node */ + std::list _pubs; /**< Publications of node */ }; -#else +#else //Building for NuttX class __EXPORT NodeHandle { public: @@ -164,7 +164,7 @@ public: */ template - Subscriber *subscribe(const struct orb_metadata *meta, + Subscriber *subscribe(const struct orb_metadata *meta, std::function callback, unsigned interval) { @@ -175,7 +175,7 @@ public: _sub_min_interval = sub_px4; } - return (Subscriber *)sub_px4; + return (Subscriber *)sub_px4; } /** diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index cb15eeb58..6b6d8e39e 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -48,15 +48,30 @@ namespace px4 { + +/** + * Untemplated publisher base class + * */ +class PublisherBase +{ +public: + PublisherBase() {}; + ~PublisherBase() {}; + +}; + #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) -class Publisher +class Publisher : + public PublisherBase { public: /** * Construct Publisher by providing a ros::Publisher * @param ros_pub the ros publisher which will be used to perform the publications */ - Publisher(ros::Publisher ros_pub) : _ros_pub(ros_pub) + Publisher(ros::Publisher ros_pub) : + PublisherBase(), + _ros_pub(ros_pub) {} ~Publisher() {}; @@ -71,7 +86,8 @@ private: }; #else class Publisher : - public uORB::PublicationNode + public uORB::PublicationNode, + public PublisherBase { public: /** @@ -81,7 +97,8 @@ public: */ Publisher(const struct orb_metadata *meta, List *list) : - uORB::PublicationNode(meta, list) + uORB::PublicationNode(meta, list), + PublisherBase() {} ~Publisher() {}; diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 234b72f5b..59c0ef70b 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -50,14 +50,36 @@ namespace px4 { + +/** + * Untemplated subscriber base class + * */ +class SubscriberBase +{ +public: + SubscriberBase() {}; + ~SubscriberBase() {}; + +}; + /** * Subscriber class which is used by nodehandle */ -class Subscriber +template +class Subscriber : + public SubscriberBase { public: - Subscriber() {}; + Subscriber() : + SubscriberBase() + {}; ~Subscriber() {}; + + /* Accessors*/ + /** + * Get the last message value + */ + virtual const M& get_msg() = 0; }; #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) @@ -66,7 +88,7 @@ public: */ template class SubscriberROS : - public Subscriber + public Subscriber { friend class NodeHandle; @@ -75,7 +97,7 @@ public: * Construct Subscriber by providing a callback function */ SubscriberROS(std::function cbf) : - Subscriber(), + Subscriber(), _ros_sub(), _cbf(cbf), _msg_current() @@ -85,7 +107,7 @@ public: * Construct Subscriber without a callback function */ SubscriberROS() : - Subscriber(), + Subscriber(), _ros_sub(), _cbf(NULL), _msg_current() @@ -128,14 +150,14 @@ protected: }; -#else +#else // Building for NuttX /** * Subscriber class that is templated with the uorb subscription message type */ template class SubscriberUORB : - public Subscriber, + public Subscriber, public uORB::Subscription { public: @@ -150,7 +172,7 @@ public: unsigned interval, std::function callback, List *list) : - Subscriber(), + Subscriber(), uORB::Subscription(meta, interval, list), _callback(callback) //XXX store callback -- cgit v1.2.3 From 9c09a9e8d56b69797b64e5fa1f17d2bcf760e2bb Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 11 Dec 2014 16:11:04 +0100 Subject: mc att control: prepare for ros integration, move class into spearate file --- src/modules/mc_att_control/mc_att_control.cpp | 403 ++++++++++++++ src/modules/mc_att_control/mc_att_control.h | 174 ++++++ src/modules/mc_att_control/mc_att_control_base.cpp | 3 +- src/modules/mc_att_control/mc_att_control_base.h | 2 +- src/modules/mc_att_control/mc_att_control_main.cpp | 583 +-------------------- src/modules/mc_att_control/module.mk | 1 + 6 files changed, 609 insertions(+), 557 deletions(-) create mode 100644 src/modules/mc_att_control/mc_att_control.cpp create mode 100644 src/modules/mc_att_control/mc_att_control.h diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp new file mode 100644 index 000000000..31cdd99e4 --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control.cpp @@ -0,0 +1,403 @@ +/**************************************************************************** + * + * 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.cpp + * Multicopter attitude controller. + * + * @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * @author Thomas Gubler + * @author Julian Oes + * @author Roman Bapst + */ + +#include "mc_att_control.h" + +#define YAW_DEADZONE 0.05f +#define MIN_TAKEOFF_THRUST 0.2f +#define RATES_I_LIMIT 0.3f + +namespace mc_att_control +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +} + +MulticopterAttitudeControl::MulticopterAttitudeControl() : + MulticopterAttitudeControlBase(), + _task_should_exit(false), + _control_task(-1), + _actuators_0_circuit_breaker_enabled(false), + /* subscriptions */ + _v_att_sub(-1), + _v_att_sp_sub(-1), + _v_control_mode_sub(-1), + _params_sub(-1), + _manual_control_sp_sub(-1), + _armed_sub(-1), + + /* publications */ + _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")) + +{ + _params_handles.roll_p = param_find("MC_ROLL_P"); + _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); + _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); + _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); + _params_handles.pitch_p = param_find("MC_PITCH_P"); + _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); + _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); + _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); + _params_handles.yaw_p = param_find("MC_YAW_P"); + _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); + _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); + _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + _params_handles.yaw_ff = param_find("MC_YAW_FF"); + _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); + _params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); + _params_handles.man_pitch_max = param_find("MC_MAN_P_MAX"); + _params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX"); + _params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX"); + _params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX"); + _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); + + /* 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() +{ + if (_control_task != -1) { + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_control_task); + break; + } + } while (_control_task != -1); + } + + // mc_att_control::g_control = nullptr; +} + +int +MulticopterAttitudeControl::parameters_update() +{ + float v; + + /* roll gains */ + param_get(_params_handles.roll_p, &v); + _params.att_p(0) = v; + param_get(_params_handles.roll_rate_p, &v); + _params.rate_p(0) = v; + param_get(_params_handles.roll_rate_i, &v); + _params.rate_i(0) = v; + param_get(_params_handles.roll_rate_d, &v); + _params.rate_d(0) = v; + + /* pitch gains */ + param_get(_params_handles.pitch_p, &v); + _params.att_p(1) = v; + param_get(_params_handles.pitch_rate_p, &v); + _params.rate_p(1) = v; + param_get(_params_handles.pitch_rate_i, &v); + _params.rate_i(1) = v; + param_get(_params_handles.pitch_rate_d, &v); + _params.rate_d(1) = v; + + /* yaw gains */ + param_get(_params_handles.yaw_p, &v); + _params.att_p(2) = v; + param_get(_params_handles.yaw_rate_p, &v); + _params.rate_p(2) = v; + param_get(_params_handles.yaw_rate_i, &v); + _params.rate_i(2) = v; + param_get(_params_handles.yaw_rate_d, &v); + _params.rate_d(2) = v; + + param_get(_params_handles.yaw_ff, &_params.yaw_ff); + param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); + _params.yaw_rate_max = math::radians(_params.yaw_rate_max); + + /* manual control scale */ + param_get(_params_handles.man_roll_max, &_params.man_roll_max); + param_get(_params_handles.man_pitch_max, &_params.man_pitch_max); + param_get(_params_handles.man_yaw_max, &_params.man_yaw_max); + _params.man_roll_max = math::radians(_params.man_roll_max); + _params.man_pitch_max = math::radians(_params.man_pitch_max); + _params.man_yaw_max = math::radians(_params.man_yaw_max); + + /* acro control scale */ + param_get(_params_handles.acro_roll_max, &v); + _params.acro_rate_max(0) = math::radians(v); + param_get(_params_handles.acro_pitch_max, &v); + _params.acro_rate_max(1) = math::radians(v); + param_get(_params_handles.acro_yaw_max, &v); + _params.acro_rate_max(2) = math::radians(v); + + _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); + + return OK; +} + +void +MulticopterAttitudeControl::parameter_update_poll() +{ + bool updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_params_sub, &updated); + + if (updated) { + struct parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); + parameters_update(); + } +} + +void +MulticopterAttitudeControl::vehicle_control_mode_poll() +{ + bool updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_v_control_mode_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); + } +} + +void +MulticopterAttitudeControl::vehicle_manual_poll() +{ + bool updated; + + /* get pilots inputs */ + orb_check(_manual_control_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); + } +} + +void +MulticopterAttitudeControl::vehicle_attitude_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_v_att_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp); + } +} + +void +MulticopterAttitudeControl::vehicle_rates_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_v_rates_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp); + } +} + +void +MulticopterAttitudeControl::arming_status_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + } +} + + +void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) { + + perf_begin(_loop_perf); + + /* 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(); + + /* 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); + + /* 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); + + /* 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); + + } 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(); + + 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 */ + 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; + } + } + + if (_v_control_mode.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 = 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); + + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } + } + } +} diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h new file mode 100644 index 000000000..48df88771 --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control.h @@ -0,0 +1,174 @@ +/**************************************************************************** + * + * 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.h + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mc_att_control_base.h" + +class MulticopterAttitudeControl : + public MulticopterAttitudeControlBase +{ +public: + /** + * Constructor + */ + MulticopterAttitudeControl(); + + /** + * Destructor, also kills the sensors task. + */ + ~MulticopterAttitudeControl(); + + 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 */ + bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ + + + int _v_att_sub; /**< vehicle attitude subscription */ + int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ + int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */ + int _v_control_mode_sub; /**< vehicle control mode subscription */ + int _params_sub; /**< parameter updates subscription */ + int _manual_control_sp_sub; /**< manual control setpoint subscription */ + int _armed_sub; /**< arming status subscription */ + + orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ + 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; + param_t roll_rate_i; + param_t roll_rate_d; + param_t pitch_p; + param_t pitch_rate_p; + param_t pitch_rate_i; + param_t pitch_rate_d; + param_t yaw_p; + param_t yaw_rate_p; + param_t yaw_rate_i; + param_t yaw_rate_d; + param_t yaw_ff; + param_t yaw_rate_max; + + param_t man_roll_max; + param_t man_pitch_max; + param_t man_yaw_max; + param_t acro_roll_max; + param_t acro_pitch_max; + param_t acro_yaw_max; + } _params_handles; /**< handles for interesting parameters */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Check for parameter update and handle it. + */ + void parameter_update_poll(); + + /** + * Check for changes in vehicle control mode. + */ + void vehicle_control_mode_poll(); + + /** + * Check for changes in manual inputs. + */ + void vehicle_manual_poll(); + + /** + * Check for attitude setpoint updates. + */ + void vehicle_attitude_setpoint_poll(); + + /** + * Check for rates setpoint updates. + */ + void vehicle_rates_setpoint_poll(); + + /** + * Check for arming status updates. + */ + void arming_status_poll(); + +}; + diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index f6b3268b5..d0ab1bfbf 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -32,7 +32,7 @@ /** * @file mc_att_control_base.cpp * - * MC Attitude Controller + * MC Attitude Controller : Control and math code * * @author Tobias Naegeli * @author Lorenz Meier @@ -46,6 +46,7 @@ #include "mc_att_control_base.h" #include #include +#include #ifdef CONFIG_ARCH_ARM #else diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index f81a0d772..54a445e0d 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -35,7 +35,7 @@ /** * @file mc_att_control_base.h * - * MC Attitude Controller + * MC Attitude Controller : Control and math code * * @author Tobias Naegeli * @author Lorenz Meier 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 1103191e7..a1dca8a8c 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -52,548 +52,33 @@ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. */ -#include -#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "mc_att_control_base.h" +#include +#include "mc_att_control.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 - */ - -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 -{ -public: - /** - * Constructor - */ - MulticopterAttitudeControl(); - - /** - * Destructor, also kills the sensors task. - */ - ~MulticopterAttitudeControl(); - - 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 */ - bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ - - - int _v_att_sub; /**< vehicle attitude subscription */ - int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ - int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */ - int _v_control_mode_sub; /**< vehicle control mode subscription */ - int _params_sub; /**< parameter updates subscription */ - int _manual_control_sp_sub; /**< manual control setpoint subscription */ - int _armed_sub; /**< arming status subscription */ - - orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ - 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; - param_t roll_rate_i; - param_t roll_rate_d; - param_t pitch_p; - param_t pitch_rate_p; - param_t pitch_rate_i; - param_t pitch_rate_d; - param_t yaw_p; - param_t yaw_rate_p; - param_t yaw_rate_i; - param_t yaw_rate_d; - param_t yaw_ff; - param_t yaw_rate_max; - - param_t man_roll_max; - param_t man_pitch_max; - param_t man_yaw_max; - param_t acro_roll_max; - param_t acro_pitch_max; - param_t acro_yaw_max; - } _params_handles; /**< handles for interesting parameters */ - - perf_counter_t _loop_perf; /**< loop performance counter */ - - /** - * Update our local parameter cache. - */ - int parameters_update(); - - /** - * Check for parameter update and handle it. - */ - void parameter_update_poll(); - - /** - * Check for changes in vehicle control mode. - */ - void vehicle_control_mode_poll(); - - /** - * Check for changes in manual inputs. - */ - void vehicle_manual_poll(); - - /** - * Check for attitude setpoint updates. - */ - void vehicle_attitude_setpoint_poll(); - - /** - * Check for rates setpoint updates. - */ - void vehicle_rates_setpoint_poll(); - - /** - * Check for arming status updates. - */ - void arming_status_poll(); - -}; - -namespace mc_att_control -{ - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -} - -MulticopterAttitudeControl::MulticopterAttitudeControl() : - MulticopterAttitudeControlBase(), - _task_should_exit(false), - _control_task(-1), - _actuators_0_circuit_breaker_enabled(false), - /* subscriptions */ - _v_att_sub(-1), - _v_att_sp_sub(-1), - _v_control_mode_sub(-1), - _params_sub(-1), - _manual_control_sp_sub(-1), - _armed_sub(-1), - - /* publications */ - _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")) - -{ - _params_handles.roll_p = param_find("MC_ROLL_P"); - _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); - _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); - _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); - _params_handles.pitch_p = param_find("MC_PITCH_P"); - _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); - _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); - _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); - _params_handles.yaw_p = param_find("MC_YAW_P"); - _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); - _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); - _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); - _params_handles.yaw_ff = param_find("MC_YAW_FF"); - _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); - _params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); - _params_handles.man_pitch_max = param_find("MC_MAN_P_MAX"); - _params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX"); - _params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX"); - _params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX"); - _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); - - /* 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() -{ - if (_control_task != -1) { - /* task wakes up every 100ms or so at the longest */ - _task_should_exit = true; - - /* wait for a second for the task to quit at our request */ - unsigned i = 0; - - do { - /* wait 20ms */ - usleep(20000); - - /* if we have given up, kill it */ - if (++i > 50) { - task_delete(_control_task); - break; - } - } while (_control_task != -1); - } - - // mc_att_control::g_control = nullptr; -} - -int -MulticopterAttitudeControl::parameters_update() -{ - float v; - - /* roll gains */ - param_get(_params_handles.roll_p, &v); - _params.att_p(0) = v; - param_get(_params_handles.roll_rate_p, &v); - _params.rate_p(0) = v; - param_get(_params_handles.roll_rate_i, &v); - _params.rate_i(0) = v; - param_get(_params_handles.roll_rate_d, &v); - _params.rate_d(0) = v; - - /* pitch gains */ - param_get(_params_handles.pitch_p, &v); - _params.att_p(1) = v; - param_get(_params_handles.pitch_rate_p, &v); - _params.rate_p(1) = v; - param_get(_params_handles.pitch_rate_i, &v); - _params.rate_i(1) = v; - param_get(_params_handles.pitch_rate_d, &v); - _params.rate_d(1) = v; - - /* yaw gains */ - param_get(_params_handles.yaw_p, &v); - _params.att_p(2) = v; - param_get(_params_handles.yaw_rate_p, &v); - _params.rate_p(2) = v; - param_get(_params_handles.yaw_rate_i, &v); - _params.rate_i(2) = v; - param_get(_params_handles.yaw_rate_d, &v); - _params.rate_d(2) = v; - - param_get(_params_handles.yaw_ff, &_params.yaw_ff); - param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); - _params.yaw_rate_max = math::radians(_params.yaw_rate_max); - - /* manual control scale */ - param_get(_params_handles.man_roll_max, &_params.man_roll_max); - param_get(_params_handles.man_pitch_max, &_params.man_pitch_max); - param_get(_params_handles.man_yaw_max, &_params.man_yaw_max); - _params.man_roll_max = math::radians(_params.man_roll_max); - _params.man_pitch_max = math::radians(_params.man_pitch_max); - _params.man_yaw_max = math::radians(_params.man_yaw_max); - - /* acro control scale */ - param_get(_params_handles.acro_roll_max, &v); - _params.acro_rate_max(0) = math::radians(v); - param_get(_params_handles.acro_pitch_max, &v); - _params.acro_rate_max(1) = math::radians(v); - param_get(_params_handles.acro_yaw_max, &v); - _params.acro_rate_max(2) = math::radians(v); - - _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); - - return OK; -} - -void -MulticopterAttitudeControl::parameter_update_poll() -{ - bool updated; - - /* Check HIL state if vehicle status has changed */ - orb_check(_params_sub, &updated); - - if (updated) { - struct parameter_update_s param_update; - orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); - parameters_update(); - } -} - -void -MulticopterAttitudeControl::vehicle_control_mode_poll() -{ - bool updated; - - /* Check HIL state if vehicle status has changed */ - orb_check(_v_control_mode_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); - } -} - -void -MulticopterAttitudeControl::vehicle_manual_poll() -{ - bool updated; - - /* get pilots inputs */ - orb_check(_manual_control_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); - } -} - -void -MulticopterAttitudeControl::vehicle_attitude_setpoint_poll() -{ - /* check if there is a new setpoint */ - bool updated; - orb_check(_v_att_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp); - } -} - -void -MulticopterAttitudeControl::vehicle_rates_setpoint_poll() -{ - /* check if there is a new setpoint */ - bool updated; - orb_check(_v_rates_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp); - } -} - -void -MulticopterAttitudeControl::arming_status_poll() -{ - /* check if there is a new setpoint */ - bool updated; - orb_check(_armed_sub, &updated); +using namespace px4; - if (updated) { - orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - } +PX4_MAIN_FUNCTION(mc_att_control); +void handle_vehicle_attitude2(const PX4_TOPIC_T(rc_channels) &msg) { + PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp); } -// void -// MulticopterAttitudeControl::task_main() -// { - - - // [> wakeup source: vehicle attitude <] - // struct pollfd fds[1]; - - // fds[0].fd = _v_att_sub; - // fds[0].events = POLLIN; - - // while (!_task_should_exit) { - - - // perf_end(_loop_perf); - // } - - // warnx("exit"); - - // _control_task = -1; - // _exit(0); -// } - -void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) { - - perf_begin(_loop_perf); - - /* 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(); - - /* 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); - - /* 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); - - /* 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); - } 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(); - - 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 */ - 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; - } - } - - if (_v_control_mode.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 = 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); - - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); - } - } - } -} +#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__)) +/** + * Multicopter attitude control app start / stop handling function + * + * @ingroup apps + */ -PX4_MAIN_FUNCTION(mc_att_control) +extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]) { px4::init(argc, argv, "mc_att_control"); @@ -615,54 +100,42 @@ PX4_MAIN_FUNCTION(mc_att_control) SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, - mc_attitude_thread_main, + mc_att_control_task_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")) { + task_should_exit = true; + exit(0); + } - // delete mc_att_control::g_control; - // mc_att_control::g_control = nullptr; - // exit(0); - // } + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); - // if (!strcmp(argv[1], "status")) { - // if (mc_att_control::g_control) { - // errx(0, "running"); + } else { + warnx("not started"); + } - // } else { - // errx(1, "not running"); - // } - // } + exit(0); + } warnx("unrecognized command"); return 1; } +#endif -int mc_attitude_thread_main(int argc, char *argv[]) +PX4_MAIN_FUNCTION(mc_att_control) { - warnx("starting"); - MulticopterAttitudeControl attctl; - thread_running = true; - attctl.spin(); - // while (!task_should_exit) { - // attctl.update(); - // } - warnx("exiting."); - thread_running = false; - return 0; } diff --git a/src/modules/mc_att_control/module.mk b/src/modules/mc_att_control/module.mk index ecd251d45..29254f5bb 100644 --- a/src/modules/mc_att_control/module.mk +++ b/src/modules/mc_att_control/module.mk @@ -38,5 +38,6 @@ MODULE_COMMAND = mc_att_control SRCS = mc_att_control_main.cpp \ + mc_att_control.cpp \ mc_att_control_base.cpp \ mc_att_control_params.c -- cgit v1.2.3 From ce73a816027fb394d23ef36d6d2c71774b231717 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 11 Dec 2014 16:41:04 +0100 Subject: WIP remove unnecessary functions --- src/modules/mc_att_control/mc_att_control.cpp | 89 +--------------------- src/modules/mc_att_control/mc_att_control.h | 31 -------- src/modules/mc_att_control/mc_att_control_base.cpp | 4 +- src/modules/mc_att_control/mc_att_control_base.h | 2 - 4 files changed, 3 insertions(+), 123 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp index 31cdd99e4..002812741 100644 --- a/src/modules/mc_att_control/mc_att_control.cpp +++ b/src/modules/mc_att_control/mc_att_control.cpp @@ -211,84 +211,6 @@ MulticopterAttitudeControl::parameters_update() return OK; } -void -MulticopterAttitudeControl::parameter_update_poll() -{ - bool updated; - - /* Check HIL state if vehicle status has changed */ - orb_check(_params_sub, &updated); - - if (updated) { - struct parameter_update_s param_update; - orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); - parameters_update(); - } -} - -void -MulticopterAttitudeControl::vehicle_control_mode_poll() -{ - bool updated; - - /* Check HIL state if vehicle status has changed */ - orb_check(_v_control_mode_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); - } -} - -void -MulticopterAttitudeControl::vehicle_manual_poll() -{ - bool updated; - - /* get pilots inputs */ - orb_check(_manual_control_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); - } -} - -void -MulticopterAttitudeControl::vehicle_attitude_setpoint_poll() -{ - /* check if there is a new setpoint */ - bool updated; - orb_check(_v_att_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp); - } -} - -void -MulticopterAttitudeControl::vehicle_rates_setpoint_poll() -{ - /* check if there is a new setpoint */ - bool updated; - orb_check(_v_rates_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp); - } -} - -void -MulticopterAttitudeControl::arming_status_poll() -{ - /* check if there is a new setpoint */ - bool updated; - orb_check(_armed_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - } -} - - void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) { perf_begin(_loop_perf); @@ -306,15 +228,6 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi dt = 0.02f; } - /* 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(); - if (_v_control_mode.flag_control_attitude_enabled) { control_attitude(dt); @@ -373,7 +286,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi } else { /* attitude controller disabled, poll rates setpoint topic */ - vehicle_rates_setpoint_poll(); + //XXX 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; diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h index 48df88771..b2ec3083f 100644 --- a/src/modules/mc_att_control/mc_att_control.h +++ b/src/modules/mc_att_control/mc_att_control.h @@ -139,36 +139,5 @@ private: * Update our local parameter cache. */ int parameters_update(); - - /** - * Check for parameter update and handle it. - */ - void parameter_update_poll(); - - /** - * Check for changes in vehicle control mode. - */ - void vehicle_control_mode_poll(); - - /** - * Check for changes in manual inputs. - */ - void vehicle_manual_poll(); - - /** - * Check for attitude setpoint updates. - */ - void vehicle_attitude_setpoint_poll(); - - /** - * Check for rates setpoint updates. - */ - void vehicle_rates_setpoint_poll(); - - /** - * Check for arming status updates. - */ - void arming_status_poll(); - }; diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index d0ab1bfbf..0a05be924 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -101,7 +101,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) { /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ - vehicle_attitude_setpoint_poll(); + //XXX vehicle_attitude_setpoint_poll(); } if (!_v_control_mode.flag_control_climb_rate_enabled) { @@ -160,7 +160,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) } else { /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ - vehicle_attitude_setpoint_poll(); + //XXX vehicle_attitude_setpoint_poll(); /* reset yaw setpoint after non-manual control mode */ _reset_yaw_sp = true; diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 54a445e0d..0783bb71e 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -120,8 +120,6 @@ protected: bool _publish_att_sp; - virtual void vehicle_attitude_setpoint_poll() = 0; - }; #endif /* MC_ATT_CONTROL_BASE_H_ */ -- cgit v1.2.3 From 9d0ecc77bb1c3d1c9b5cf6bc5d517e45d9f20f97 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 11 Dec 2014 17:05:12 +0100 Subject: mc att: convert param definitions to be compatible with multiplatform build --- src/modules/mc_att_control/mc_att_control_params.c | 41 +++++++------- src/modules/mc_att_control/mc_att_control_params.h | 65 ++++++++++++++++++++++ 2 files changed, 86 insertions(+), 20 deletions(-) create mode 100644 src/modules/mc_att_control/mc_att_control_params.h diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 819847b40..bbbfd829b 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -40,6 +40,7 @@ * @author Anton Babushkin */ +#include "mc_att_control_params.h"" #include /** @@ -50,7 +51,7 @@ * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f); +PX4_PARAM_DEFINE_FLOAT(MC_ROLL_P); /** * Roll rate P gain @@ -60,7 +61,7 @@ PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); +PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_P); /** * Roll rate I gain @@ -70,7 +71,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); +PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_I); /** * Roll rate D gain @@ -80,7 +81,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); +PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_D); /** * Pitch P gain @@ -91,7 +92,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f); +PX4_PARAM_DEFINE_FLOAT(MC_PITCH_P); /** * Pitch rate P gain @@ -101,7 +102,7 @@ PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); +PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_P); /** * Pitch rate I gain @@ -111,7 +112,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); +PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_I); /** * Pitch rate D gain @@ -121,7 +122,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); +PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_D); /** * Yaw P gain @@ -132,7 +133,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); +PX4_PARAM_DEFINE_FLOAT(MC_YAW_P); /** * Yaw rate P gain @@ -142,7 +143,7 @@ PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); +PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_P); /** * Yaw rate I gain @@ -152,7 +153,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); +PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_I); /** * Yaw rate D gain @@ -162,7 +163,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); +PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_D); /** * Yaw feed forward @@ -173,7 +174,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); * @max 1.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); +PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF); /** * Max yaw rate @@ -185,7 +186,7 @@ PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f); +PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX); /** * Max manual roll @@ -195,7 +196,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f); * @max 90.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_MAN_R_MAX, 35.0f); +PX4_PARAM_DEFINE_FLOAT(MC_MAN_R_MAX); /** * Max manual pitch @@ -205,7 +206,7 @@ PARAM_DEFINE_FLOAT(MC_MAN_R_MAX, 35.0f); * @max 90.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_MAN_P_MAX, 35.0f); +PX4_PARAM_DEFINE_FLOAT(MC_MAN_P_MAX); /** * Max manual yaw rate @@ -214,7 +215,7 @@ PARAM_DEFINE_FLOAT(MC_MAN_P_MAX, 35.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX, 120.0f); +PX4_PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX); /** * Max acro roll rate @@ -224,7 +225,7 @@ PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX, 120.0f); * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f); +PX4_PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX); /** * Max acro pitch rate @@ -234,7 +235,7 @@ PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f); * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f); +PX4_PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX); /** * Max acro yaw rate @@ -243,4 +244,4 @@ PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 120.0f); +PX4_PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX); diff --git a/src/modules/mc_att_control/mc_att_control_params.h b/src/modules/mc_att_control/mc_att_control_params.h new file mode 100644 index 000000000..59dd5240f --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_params.h @@ -0,0 +1,65 @@ + +/**************************************************************************** + * + * 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_params.h + * Parameters for multicopter attitude controller. + * + * @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * @author Thomas Gubler + */ +#pragma once + +#define PARAM_MC_ROLL_P_DEFAULT 6.0f +#define PARAM_MC_ROLLRATE_P_DEFAULT 0.1f +#define PARAM_MC_ROLLRATE_I_DEFAULT 0.0f +#define PARAM_MC_ROLLRATE_D_DEFAULT 0.002f +#define PARAM_MC_PITCH_P_DEFAULT 6.0f +#define PARAM_MC_PITCHRATE_P_DEFAULT 0.1f +#define PARAM_MC_PITCHRATE_I_DEFAULT 0.0f +#define PARAM_MC_PITCHRATE_D_DEFAULT 0.002f +#define PARAM_MC_YAW_P_DEFAULT 2.0f +#define PARAM_MC_YAWRATE_P_DEFAULT 0.3f +#define PARAM_MC_YAWRATE_I_DEFAULT 0.0f +#define PARAM_MC_YAWRATE_D_DEFAULT 0.0f +#define PARAM_MC_YAW_FF_DEFAULT 0.5f +#define PARAM_MC_YAWRATE_MAX_DEFAULT 120.0f +#define PARAM_MC_MAN_R_MAX_DEFAULT 35.0f +#define PARAM_MC_MAN_P_MAX_DEFAULT 35.0f +#define PARAM_MC_MAN_Y_MAX_DEFAULT 120.0f +#define PARAM_MC_ACRO_R_MAX_DEFAULT 35.0f +#define PARAM_MC_ACRO_P_MAX_DEFAULT 35.0f +#define PARAM_MC_ACRO_Y_MAX_DEFAULT 120.0f -- cgit v1.2.3 From b755312a1ec3e1f73827ac2b581a597491601140 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 11 Dec 2014 17:12:03 +0100 Subject: mc att: use PX4 macros to init and get params --- src/modules/mc_att_control/mc_att_control.cpp | 80 +++++++++++++-------------- src/modules/mc_att_control/mc_att_control.h | 42 +++++++------- 2 files changed, 61 insertions(+), 61 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp index 002812741..d20bf24e1 100644 --- a/src/modules/mc_att_control/mc_att_control.cpp +++ b/src/modules/mc_att_control/mc_att_control.cpp @@ -83,26 +83,26 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) { - _params_handles.roll_p = param_find("MC_ROLL_P"); - _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); - _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); - _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); - _params_handles.pitch_p = param_find("MC_PITCH_P"); - _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); - _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); - _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); - _params_handles.yaw_p = param_find("MC_YAW_P"); - _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); - _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); - _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); - _params_handles.yaw_ff = param_find("MC_YAW_FF"); - _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); - _params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); - _params_handles.man_pitch_max = param_find("MC_MAN_P_MAX"); - _params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX"); - _params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX"); - _params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX"); - _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); + _params_handles.roll_p = PX4_PARAM_INIT("MC_ROLL_P"); + _params_handles.roll_rate_p = PX4_PARAM_INIT("MC_ROLLRATE_P"); + _params_handles.roll_rate_i = PX4_PARAM_INIT("MC_ROLLRATE_I"); + _params_handles.roll_rate_d = PX4_PARAM_INIT("MC_ROLLRATE_D"); + _params_handles.pitch_p = PX4_PARAM_INIT("MC_PITCH_P"); + _params_handles.pitch_rate_p = PX4_PARAM_INIT("MC_PITCHRATE_P"); + _params_handles.pitch_rate_i = PX4_PARAM_INIT("MC_PITCHRATE_I"); + _params_handles.pitch_rate_d = PX4_PARAM_INIT("MC_PITCHRATE_D"); + _params_handles.yaw_p = PX4_PARAM_INIT("MC_YAW_P"); + _params_handles.yaw_rate_p = PX4_PARAM_INIT("MC_YAWRATE_P"); + _params_handles.yaw_rate_i = PX4_PARAM_INIT("MC_YAWRATE_I"); + _params_handles.yaw_rate_d = PX4_PARAM_INIT("MC_YAWRATE_D"); + _params_handles.yaw_ff = PX4_PARAM_INIT("MC_YAW_FF"); + _params_handles.yaw_rate_max = PX4_PARAM_INIT("MC_YAWRATE_MAX"); + _params_handles.man_roll_max = PX4_PARAM_INIT("MC_MAN_R_MAX"); + _params_handles.man_pitch_max = PX4_PARAM_INIT("MC_MAN_P_MAX"); + _params_handles.man_yaw_max = PX4_PARAM_INIT("MC_MAN_Y_MAX"); + _params_handles.acro_roll_max = PX4_PARAM_INIT("MC_ACRO_R_MAX"); + _params_handles.acro_pitch_max = PX4_PARAM_INIT("MC_ACRO_P_MAX"); + _params_handles.acro_yaw_max = PX4_PARAM_INIT("MC_ACRO_Y_MAX"); /* fetch initial parameter values */ parameters_update(); @@ -157,53 +157,53 @@ MulticopterAttitudeControl::parameters_update() float v; /* roll gains */ - param_get(_params_handles.roll_p, &v); + PX4_PARAM_GET(_params_handles.roll_p, &v); _params.att_p(0) = v; - param_get(_params_handles.roll_rate_p, &v); + PX4_PARAM_GET(_params_handles.roll_rate_p, &v); _params.rate_p(0) = v; - param_get(_params_handles.roll_rate_i, &v); + PX4_PARAM_GET(_params_handles.roll_rate_i, &v); _params.rate_i(0) = v; - param_get(_params_handles.roll_rate_d, &v); + PX4_PARAM_GET(_params_handles.roll_rate_d, &v); _params.rate_d(0) = v; /* pitch gains */ - param_get(_params_handles.pitch_p, &v); + PX4_PARAM_GET(_params_handles.pitch_p, &v); _params.att_p(1) = v; - param_get(_params_handles.pitch_rate_p, &v); + PX4_PARAM_GET(_params_handles.pitch_rate_p, &v); _params.rate_p(1) = v; - param_get(_params_handles.pitch_rate_i, &v); + PX4_PARAM_GET(_params_handles.pitch_rate_i, &v); _params.rate_i(1) = v; - param_get(_params_handles.pitch_rate_d, &v); + PX4_PARAM_GET(_params_handles.pitch_rate_d, &v); _params.rate_d(1) = v; /* yaw gains */ - param_get(_params_handles.yaw_p, &v); + PX4_PARAM_GET(_params_handles.yaw_p, &v); _params.att_p(2) = v; - param_get(_params_handles.yaw_rate_p, &v); + PX4_PARAM_GET(_params_handles.yaw_rate_p, &v); _params.rate_p(2) = v; - param_get(_params_handles.yaw_rate_i, &v); + PX4_PARAM_GET(_params_handles.yaw_rate_i, &v); _params.rate_i(2) = v; - param_get(_params_handles.yaw_rate_d, &v); + PX4_PARAM_GET(_params_handles.yaw_rate_d, &v); _params.rate_d(2) = v; - param_get(_params_handles.yaw_ff, &_params.yaw_ff); - param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); + PX4_PARAM_GET(_params_handles.yaw_ff, &_params.yaw_ff); + PX4_PARAM_GET(_params_handles.yaw_rate_max, &_params.yaw_rate_max); _params.yaw_rate_max = math::radians(_params.yaw_rate_max); /* manual control scale */ - param_get(_params_handles.man_roll_max, &_params.man_roll_max); - param_get(_params_handles.man_pitch_max, &_params.man_pitch_max); - param_get(_params_handles.man_yaw_max, &_params.man_yaw_max); + PX4_PARAM_GET(_params_handles.man_roll_max, &_params.man_roll_max); + PX4_PARAM_GET(_params_handles.man_pitch_max, &_params.man_pitch_max); + PX4_PARAM_GET(_params_handles.man_yaw_max, &_params.man_yaw_max); _params.man_roll_max = math::radians(_params.man_roll_max); _params.man_pitch_max = math::radians(_params.man_pitch_max); _params.man_yaw_max = math::radians(_params.man_yaw_max); /* acro control scale */ - param_get(_params_handles.acro_roll_max, &v); + PX4_PARAM_GET(_params_handles.acro_roll_max, &v); _params.acro_rate_max(0) = math::radians(v); - param_get(_params_handles.acro_pitch_max, &v); + PX4_PARAM_GET(_params_handles.acro_pitch_max, &v); _params.acro_rate_max(1) = math::radians(v); - param_get(_params_handles.acro_yaw_max, &v); + PX4_PARAM_GET(_params_handles.acro_yaw_max, &v); _params.acro_rate_max(2) = math::radians(v); _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h index b2ec3083f..4e1dd0b57 100644 --- a/src/modules/mc_att_control/mc_att_control.h +++ b/src/modules/mc_att_control/mc_att_control.h @@ -110,27 +110,27 @@ private: px4::NodeHandle n; struct { - param_t roll_p; - param_t roll_rate_p; - param_t roll_rate_i; - param_t roll_rate_d; - param_t pitch_p; - param_t pitch_rate_p; - param_t pitch_rate_i; - param_t pitch_rate_d; - param_t yaw_p; - param_t yaw_rate_p; - param_t yaw_rate_i; - param_t yaw_rate_d; - param_t yaw_ff; - param_t yaw_rate_max; - - param_t man_roll_max; - param_t man_pitch_max; - param_t man_yaw_max; - param_t acro_roll_max; - param_t acro_pitch_max; - param_t acro_yaw_max; + px4_param_t roll_p; + px4_param_t roll_rate_p; + px4_param_t roll_rate_i; + px4_param_t roll_rate_d; + px4_param_t pitch_p; + px4_param_t pitch_rate_p; + px4_param_t pitch_rate_i; + px4_param_t pitch_rate_d; + px4_param_t yaw_p; + px4_param_t yaw_rate_p; + px4_param_t yaw_rate_i; + px4_param_t yaw_rate_d; + px4_param_t yaw_ff; + px4_param_t yaw_rate_max; + + px4_param_t man_roll_max; + px4_param_t man_pitch_max; + px4_param_t man_yaw_max; + px4_param_t acro_roll_max; + px4_param_t acro_pitch_max; + px4_param_t acro_yaw_max; } _params_handles; /**< handles for interesting parameters */ perf_counter_t _loop_perf; /**< loop performance counter */ -- cgit v1.2.3 From ab35b6470cd9ff8fe9fd9a1bf1f6617801189e83 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 11 Dec 2014 19:02:14 +0100 Subject: mc att: use multiplatform publisher --- src/modules/mc_att_control/mc_att_control.cpp | 48 +++++++++++++-------------- src/modules/mc_att_control/mc_att_control.h | 10 +++--- src/modules/uORB/uORB.h | 1 + src/platforms/px4_defines.h | 2 ++ 4 files changed, 31 insertions(+), 30 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp index d20bf24e1..89daa8b48 100644 --- a/src/modules/mc_att_control/mc_att_control.cpp +++ b/src/modules/mc_att_control/mc_att_control.cpp @@ -74,10 +74,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _armed_sub(-1), /* publications */ - _att_sp_pub(-1), - _v_rates_sp_pub(-1), - _actuators_0_pub(-1), - n(), + _att_sp_pub(nullptr), + _v_rates_sp_pub(nullptr), + _actuators_0_pub(nullptr), + _n(), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) @@ -111,19 +111,19 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : * do subscriptions */ // _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - PX4_SUBSCRIBE(n, vehicle_attitude, MulticopterAttitudeControl::handle_vehicle_attitude, this, 0); + 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); + 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); + 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); + PX4_SUBSCRIBE(_n, vehicle_control_mode, 0); // _params_sub = orb_subscribe(ORB_ID(parameter_update)); - PX4_SUBSCRIBE(n, parameter_update, 0); + PX4_SUBSCRIBE(_n, parameter_update, 0); // _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - PX4_SUBSCRIBE(n, manual_control_setpoint, 0); + PX4_SUBSCRIBE(_n, manual_control_setpoint, 0); // _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - PX4_SUBSCRIBE(n, actuator_armed, 0); + PX4_SUBSCRIBE(_n, actuator_armed, 0); } @@ -235,13 +235,11 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi 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 != nullptr) { + _att_sp_pub->publish(_v_att_sp); } else { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), - &_v_att_sp); + _att_sp_pub = PX4_ADVERTISE(_n, vehicle_attitude_setpoint); } } @@ -252,11 +250,11 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi _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 != nullptr) { + _v_rates_sp_pub->publish(_v_rates_sp); } else { - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint); } } else { @@ -277,11 +275,11 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi _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 != nullptr) { + _v_rates_sp_pub->publish(_v_rates_sp); } else { - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint); } } else { @@ -305,11 +303,11 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi _actuators.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 (_actuators_0_pub != nullptr) { + _actuators_0_pub->publish(_actuators); } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + _actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_0); } } } diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h index 4e1dd0b57..229e8e672 100644 --- a/src/modules/mc_att_control/mc_att_control.h +++ b/src/modules/mc_att_control/mc_att_control.h @@ -87,7 +87,7 @@ public: void handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg); - void spin() { n.spin(); } + void spin() { _n.spin(); } private: bool _task_should_exit; /**< if true, sensor task should exit */ @@ -103,11 +103,11 @@ private: int _manual_control_sp_sub; /**< manual control setpoint subscription */ int _armed_sub; /**< arming status subscription */ - orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ - orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ - orb_advert_t _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; + px4::NodeHandle _n; struct { px4_param_t roll_p; diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 578ebf9dd..0a99cce77 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -267,5 +267,6 @@ ORB_DECLARE(actuator_controls_0); ORB_DECLARE(actuator_controls_1); ORB_DECLARE(actuator_controls_2); ORB_DECLARE(actuator_controls_3); +typedef struct actuator_controls_s actuator_controls_0_s; #endif /* _UORB_UORB_H */ diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 9bb190380..a1bf9919e 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -138,6 +138,8 @@ typedef param_t px4_param_t; #endif +/* Defines for all platforms */ + /* Shortcut for subscribing to topics * Overload the PX4_SUBSCRIBE macro to suppport methods, pure functions as callback and no callback at all */ -- cgit v1.2.3 From 59d26f5c30c9f70cd1e8bb7152ab07c96231d4e2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 12 Dec 2014 07:34:23 +0100 Subject: mc att: remove subscriber handles --- src/modules/mc_att_control/mc_att_control.cpp | 14 -------------- src/modules/mc_att_control/mc_att_control.h | 9 --------- 2 files changed, 23 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp index 89daa8b48..bbda1f8aa 100644 --- a/src/modules/mc_att_control/mc_att_control.cpp +++ b/src/modules/mc_att_control/mc_att_control.cpp @@ -65,13 +65,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _task_should_exit(false), _control_task(-1), _actuators_0_circuit_breaker_enabled(false), - /* subscriptions */ - _v_att_sub(-1), - _v_att_sp_sub(-1), - _v_control_mode_sub(-1), - _params_sub(-1), - _manual_control_sp_sub(-1), - _armed_sub(-1), /* publications */ _att_sp_pub(nullptr), @@ -110,19 +103,12 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* * 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); } diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h index 229e8e672..24009a5e6 100644 --- a/src/modules/mc_att_control/mc_att_control.h +++ b/src/modules/mc_att_control/mc_att_control.h @@ -94,15 +94,6 @@ private: int _control_task; /**< task handle for sensor task */ bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ - - int _v_att_sub; /**< vehicle attitude subscription */ - int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ - int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */ - int _v_control_mode_sub; /**< vehicle control mode subscription */ - int _params_sub; /**< parameter updates subscription */ - int _manual_control_sp_sub; /**< manual control setpoint subscription */ - int _armed_sub; /**< arming status subscription */ - 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 */ -- cgit v1.2.3 From c0d386bce0a2fd7d119dd8495d1ca68d985ae411 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 12 Dec 2014 10:15:33 +0100 Subject: mc att: use multiplatform subscriptions type --- src/examples/subscriber/subscriber_example.cpp | 2 +- src/modules/mc_att_control/mc_att_control.cpp | 12 ++--- src/modules/mc_att_control/mc_att_control_base.cpp | 52 +++++++++++----------- src/modules/mc_att_control/mc_att_control_base.h | 15 ++++--- src/platforms/px4_subscriber.h | 6 +-- 5 files changed, 44 insertions(+), 43 deletions(-) diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 83ead4ba5..3c80561ca 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -78,5 +78,5 @@ SubscriberExample::SubscriberExample() : void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", msg.timestamp_last_valid, - _sub_rc_chan->get_msg().timestamp_last_valid); + _sub_rc_chan->get().timestamp_last_valid); } diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp index bbda1f8aa..59eaaec6d 100644 --- a/src/modules/mc_att_control/mc_att_control.cpp +++ b/src/modules/mc_att_control/mc_att_control.cpp @@ -214,7 +214,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi dt = 0.02f; } - if (_v_control_mode.flag_control_attitude_enabled) { + if (_v_control_mode->get().flag_control_attitude_enabled) { control_attitude(dt); /* publish the attitude setpoint if needed */ @@ -245,11 +245,11 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi } else { /* attitude controller disabled, poll rates setpoint topic */ - if (_v_control_mode.flag_control_manual_enabled) { + if (_v_control_mode->get().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; + _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; /* reset yaw setpoint after ACRO */ _reset_yaw_sp = true; @@ -278,7 +278,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi } } - if (_v_control_mode.flag_control_rates_enabled) { + if (_v_control_mode->get().flag_control_rates_enabled) { control_attitude_rates(dt); /* publish actuator controls */ diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index 0a05be924..b9a57ce69 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -58,13 +58,13 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : _publish_att_sp(false) { - memset(&_v_att, 0, sizeof(_v_att)); - memset(&_v_att_sp, 0, sizeof(_v_att_sp)); - memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); - memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); - memset(&_v_control_mode, 0, sizeof(_v_control_mode)); - memset(&_actuators, 0, sizeof(_actuators)); - memset(&_armed, 0, sizeof(_armed)); + // memset(&_v_att, 0, sizeof(_v_att)); + // memset(&_v_att_sp, 0, sizeof(_v_att_sp)); + // memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); + // memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); + // memset(&_v_control_mode, 0, sizeof(_v_control_mode)); + // memset(&_actuators, 0, sizeof(_actuators)); + // memset(&_armed, 0, sizeof(_armed)); _params.att_p.zero(); _params.rate_p.zero(); @@ -95,22 +95,22 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) float yaw_sp_move_rate = 0.0f; _publish_att_sp = false; - if (_v_control_mode.flag_control_manual_enabled) { + if (_v_control_mode->get().flag_control_manual_enabled) { /* manual input, set or modify attitude setpoint */ - if (_v_control_mode.flag_control_velocity_enabled - || _v_control_mode.flag_control_climb_rate_enabled) { + if (_v_control_mode->get().flag_control_velocity_enabled + || _v_control_mode->get().flag_control_climb_rate_enabled) { /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ //XXX vehicle_attitude_setpoint_poll(); } - if (!_v_control_mode.flag_control_climb_rate_enabled) { + if (!_v_control_mode->get().flag_control_climb_rate_enabled) { /* pass throttle directly if not in altitude stabilized mode */ - _v_att_sp.thrust = _manual_control_sp.z; + _v_att_sp.thrust = _manual_control_sp->get().z; _publish_att_sp = true; } - if (!_armed.armed) { + if (!_armed->get().armed) { /* reset yaw setpoint when disarmed */ _reset_yaw_sp = true; } @@ -124,17 +124,17 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) //} } else { /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max; + yaw_sp_move_rate = _manual_control_sp->get().r * _params.man_yaw_max; _v_att_sp.yaw_body = _wrap_pi( _v_att_sp.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.yaw_body - _v_att.yaw); + float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att->get().yaw); if (yaw_offs < -yaw_offs_max) { - _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max); + _v_att_sp.yaw_body = _wrap_pi(_v_att->get().yaw - yaw_offs_max); } else if (yaw_offs > yaw_offs_max) { - _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); + _v_att_sp.yaw_body = _wrap_pi(_v_att->get().yaw + yaw_offs_max); } _v_att_sp.R_valid = false; @@ -144,15 +144,15 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) /* reset yaw setpint to current position if needed */ if (_reset_yaw_sp) { _reset_yaw_sp = false; - _v_att_sp.yaw_body = _v_att.yaw; + _v_att_sp.yaw_body = _v_att->get().yaw; _v_att_sp.R_valid = false; _publish_att_sp = true; } - if (!_v_control_mode.flag_control_velocity_enabled) { + if (!_v_control_mode->get().flag_control_velocity_enabled) { /* update attitude setpoint if not in position control mode */ - _v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max; - _v_att_sp.pitch_body = -_manual_control_sp.x + _v_att_sp.roll_body = _manual_control_sp->get().y * _params.man_roll_max; + _v_att_sp.pitch_body = -_manual_control_sp->get().x * _params.man_pitch_max; _v_att_sp.R_valid = false; _publish_att_sp = true; @@ -188,7 +188,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) /* rotation matrix for current state */ math::Matrix<3, 3> R; - R.set(_v_att.R); + R.set(_v_att->get().R); /* all input data is ready, run controller itself */ @@ -269,15 +269,15 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { /* reset integral if disarmed */ - if (!_armed.armed) { + if (!_armed->get().armed) { _rates_int.zero(); } /* current body angular rates */ math::Vector < 3 > rates; - rates(0) = _v_att.rollspeed; - rates(1) = _v_att.pitchspeed; - rates(2) = _v_att.yawspeed; + rates(0) = _v_att->get().rollspeed; + rates(1) = _v_att->get().pitchspeed; + rates(2) = _v_att->get().yawspeed; /* angular rates error */ math::Vector < 3 > rates_err = _rates_sp - rates; diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 0783bb71e..05f89abe6 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -86,13 +86,14 @@ public: void set_actuator_controls(); protected: - struct vehicle_attitude_s _v_att; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ - struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ - struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ - struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */ - struct actuator_controls_s _actuators; /**< actuator controls */ - struct actuator_armed_s _armed; /**< actuator arming status */ + px4::PX4_SUBSCRIBER(vehicle_attitude) *_v_att; /**< vehicle attitude */ + px4::PX4_SUBSCRIBER(manual_control_setpoint) *_manual_control_sp; /**< manual control setpoint */ + px4::PX4_SUBSCRIBER(vehicle_control_mode) *_v_control_mode; /**< vehicle control mode */ + px4::PX4_SUBSCRIBER(actuator_armed) *_armed; /**< actuator arming status */ + + PX4_TOPIC_T(vehicle_attitude_setpoint) _v_att_sp; /**< vehicle attitude setpoint */ + PX4_TOPIC_T(vehicle_rates_setpoint) _v_rates_sp; /**< vehicle rates setpoint */ + PX4_TOPIC_T(actuator_controls) _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/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 59c0ef70b..7d8463cb5 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -79,7 +79,7 @@ public: /** * Get the last message value */ - virtual const M& get_msg() = 0; + virtual const M& get() = 0; }; #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) @@ -120,7 +120,7 @@ public: /** * Get the last message value */ - const M& get_msg() { return _msg_current; } + const M& get() { return _msg_current; } protected: /** @@ -210,7 +210,7 @@ public: /** * Get the last message value */ - const M& get_msg() { return uORB::Subscription::getData(); } + const M& get() { return uORB::Subscription::getData(); } protected: std::function _callback; /**< Callback handle, -- cgit v1.2.3 From 75a870153792248b0e32243b30213d69c3d8d9db Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 12 Dec 2014 11:21:43 +0100 Subject: mc att: correctly handle topics which are simultaneously subscribed and published --- src/modules/mc_att_control/mc_att_control.cpp | 37 +++++++------- src/modules/mc_att_control/mc_att_control_base.cpp | 57 ++++++++++------------ src/modules/mc_att_control/mc_att_control_base.h | 8 ++- 3 files changed, 51 insertions(+), 51 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp index 59eaaec6d..13bff9561 100644 --- a/src/modules/mc_att_control/mc_att_control.cpp +++ b/src/modules/mc_att_control/mc_att_control.cpp @@ -219,10 +219,10 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi /* publish the attitude setpoint if needed */ if (_publish_att_sp) { - _v_att_sp.timestamp = hrt_absolute_time(); + _v_att_sp_mod.timestamp = hrt_absolute_time(); if (_att_sp_pub != nullptr) { - _att_sp_pub->publish(_v_att_sp); + _att_sp_pub->publish(_v_att_sp_mod); } else { _att_sp_pub = PX4_ADVERTISE(_n, vehicle_attitude_setpoint); @@ -230,15 +230,14 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi } /* 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(); + _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 = hrt_absolute_time(); if (_v_rates_sp_pub != nullptr) { - _v_rates_sp_pub->publish(_v_rates_sp); - + _v_rates_sp_pub->publish(_v_rates_sp_mod); } else { _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint); } @@ -255,14 +254,14 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi _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(); + _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 = hrt_absolute_time(); if (_v_rates_sp_pub != nullptr) { - _v_rates_sp_pub->publish(_v_rates_sp); + _v_rates_sp_pub->publish(_v_rates_sp_mod); } else { _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint); @@ -271,10 +270,10 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi } else { /* attitude controller disabled, poll rates setpoint topic */ //XXX 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; + _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; } } diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index b9a57ce69..871f93ab8 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -58,13 +58,9 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : _publish_att_sp(false) { - // memset(&_v_att, 0, sizeof(_v_att)); - // memset(&_v_att_sp, 0, sizeof(_v_att_sp)); - // memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); - // memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); - // memset(&_v_control_mode, 0, sizeof(_v_control_mode)); - // memset(&_actuators, 0, sizeof(_actuators)); - // memset(&_armed, 0, sizeof(_armed)); + memset(&_v_rates_sp, 0, sizeof(_v_att_sp_mod)); + memset(&_v_rates_sp, 0, sizeof(_v_rates_sp_mod)); + memset(&_actuators, 0, sizeof(_actuators)); _params.att_p.zero(); _params.rate_p.zero(); @@ -95,18 +91,19 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) float yaw_sp_move_rate = 0.0f; _publish_att_sp = false; + if (_v_control_mode->get().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) { /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ - //XXX vehicle_attitude_setpoint_poll(); + _v_att_sp_mod = _v_att_sp->get(); } if (!_v_control_mode->get().flag_control_climb_rate_enabled) { /* pass throttle directly if not in altitude stabilized mode */ - _v_att_sp.thrust = _manual_control_sp->get().z; + _v_att_sp_mod.thrust = _manual_control_sp->get().z; _publish_att_sp = true; } @@ -116,7 +113,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) } /* move yaw setpoint in all modes */ - if (_v_att_sp.thrust < 0.1f) { + if (_v_att_sp_mod.thrust < 0.1f) { // TODO //if (_status.condition_landed) { /* reset yaw setpoint if on ground */ @@ -125,65 +122,65 @@ 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.yaw_body = _wrap_pi( - _v_att_sp.yaw_body + yaw_sp_move_rate * dt); + _v_att_sp_mod.yaw_body = _wrap_pi( + _v_att_sp_mod.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.yaw_body - _v_att->get().yaw); + float yaw_offs = _wrap_pi(_v_att_sp_mod.yaw_body - _v_att->get().yaw); if (yaw_offs < -yaw_offs_max) { - _v_att_sp.yaw_body = _wrap_pi(_v_att->get().yaw - yaw_offs_max); + _v_att_sp_mod.yaw_body = _wrap_pi(_v_att->get().yaw - yaw_offs_max); } else if (yaw_offs > yaw_offs_max) { - _v_att_sp.yaw_body = _wrap_pi(_v_att->get().yaw + yaw_offs_max); + _v_att_sp_mod.yaw_body = _wrap_pi(_v_att->get().yaw + yaw_offs_max); } - _v_att_sp.R_valid = false; + _v_att_sp_mod.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.yaw_body = _v_att->get().yaw; - _v_att_sp.R_valid = false; + _v_att_sp_mod.yaw_body = _v_att->get().yaw; + _v_att_sp_mod.R_valid = false; _publish_att_sp = true; } if (!_v_control_mode->get().flag_control_velocity_enabled) { /* update attitude setpoint if not in position control mode */ - _v_att_sp.roll_body = _manual_control_sp->get().y * _params.man_roll_max; - _v_att_sp.pitch_body = -_manual_control_sp->get().x + _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 * _params.man_pitch_max; - _v_att_sp.R_valid = false; + _v_att_sp_mod.R_valid = false; _publish_att_sp = true; } } else { /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ - //XXX vehicle_attitude_setpoint_poll(); + _v_att_sp_mod = _v_att_sp->get(); /* reset yaw setpoint after non-manual control mode */ _reset_yaw_sp = true; } - _thrust_sp = _v_att_sp.thrust; + _thrust_sp = _v_att_sp_mod.thrust; /* construct attitude setpoint rotation matrix */ math::Matrix<3, 3> R_sp; - if (_v_att_sp.R_valid) { + if (_v_att_sp_mod.R_valid) { /* rotation matrix in _att_sp is valid, use it */ - R_sp.set(&_v_att_sp.R_body[0]); + R_sp.set(&_v_att_sp_mod.R_body[0]); } else { /* rotation matrix in _att_sp is not valid, use euler angles instead */ - R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, - _v_att_sp.yaw_body); + R_sp.from_euler(_v_att_sp_mod.roll_body, _v_att_sp_mod.pitch_body, + _v_att_sp_mod.yaw_body); /* copy rotation matrix back to setpoint struct */ - memcpy(&_v_att_sp.R_body[0], &R_sp.data[0][0], - sizeof(_v_att_sp.R_body)); - _v_att_sp.R_valid = true; + 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; } /* rotation matrix for current state */ diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 05f89abe6..2135b61d8 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -90,9 +90,13 @@ protected: px4::PX4_SUBSCRIBER(manual_control_setpoint) *_manual_control_sp; /**< manual control setpoint */ px4::PX4_SUBSCRIBER(vehicle_control_mode) *_v_control_mode; /**< vehicle control mode */ px4::PX4_SUBSCRIBER(actuator_armed) *_armed; /**< actuator arming status */ + 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_TOPIC_T(vehicle_attitude_setpoint) _v_att_sp; /**< vehicle attitude setpoint */ - PX4_TOPIC_T(vehicle_rates_setpoint) _v_rates_sp; /**< vehicle rates setpoint */ + PX4_TOPIC_T(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 + that gets published eventually*/ PX4_TOPIC_T(actuator_controls) _actuators; /**< actuator controls */ math::Vector<3> _rates_prev; /**< angular rates on previous step */ -- cgit v1.2.3 From f19b8e570c2631a6f75b25e9a8e5b007cdc6000d Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 15 Dec 2014 09:06:58 +0100 Subject: added file to make PX4 math functions compatible with eigen --- src/platforms/ros/eigen_math.h | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) create mode 100755 src/platforms/ros/eigen_math.h diff --git a/src/platforms/ros/eigen_math.h b/src/platforms/ros/eigen_math.h new file mode 100755 index 000000000..60fc9b93c --- /dev/null +++ b/src/platforms/ros/eigen_math.h @@ -0,0 +1,20 @@ +/* + * eigen_math.h + * + * Created on: Aug 25, 2014 + * Author: roman + */ + +#ifndef EIGEN_MATH_H_ +#define EIGEN_MATH_H_ + + +struct eigen_matrix_instance +{ + int numRows; + int numCols; + float *pData; +}; + + +#endif /* EIGEN_MATH_H_ */ -- cgit v1.2.3 From 9980e4482146333340cc105b560bdbd26acb999f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 16 Dec 2014 08:22:58 +0100 Subject: moved msg files --- msg/actuator_armed.msg | 6 ++++ msg/actuator_controls.msg | 4 +++ msg/actuator_controls_0.msg | 4 +++ msg/manual_control_setpoint.msg | 44 ++++++++++++++++++++++++++++++ msg/parameter_update.msg | 1 + msg/px4_msgs/actuator_armed.msg | 6 ---- msg/px4_msgs/actuator_controls.msg | 4 --- msg/px4_msgs/manual_control_setpoint.msg | 44 ------------------------------ msg/px4_msgs/parameter_update.msg | 1 - msg/px4_msgs/rc_channels.msg | 24 ---------------- msg/px4_msgs/vehicle_attitude.msg | 18 ------------ msg/px4_msgs/vehicle_attitude_setpoint.msg | 21 -------------- msg/px4_msgs/vehicle_control_mode.msg | 22 --------------- msg/px4_msgs/vehicle_rates_setpoint.msg | 6 ---- msg/rc_channels.msg | 24 ++++++++++++++++ msg/templates/msg.h.template | 3 +- msg/vehicle_attitude.msg | 18 ++++++++++++ msg/vehicle_attitude_setpoint.msg | 21 ++++++++++++++ msg/vehicle_control_mode.msg | 22 +++++++++++++++ msg/vehicle_rates_setpoint.msg | 6 ++++ 20 files changed, 152 insertions(+), 147 deletions(-) create mode 100644 msg/actuator_armed.msg create mode 100644 msg/actuator_controls.msg create mode 100644 msg/actuator_controls_0.msg create mode 100644 msg/manual_control_setpoint.msg create mode 100644 msg/parameter_update.msg delete mode 100644 msg/px4_msgs/actuator_armed.msg delete mode 100644 msg/px4_msgs/actuator_controls.msg delete mode 100644 msg/px4_msgs/manual_control_setpoint.msg delete mode 100644 msg/px4_msgs/parameter_update.msg delete mode 100644 msg/px4_msgs/rc_channels.msg delete mode 100644 msg/px4_msgs/vehicle_attitude.msg delete mode 100644 msg/px4_msgs/vehicle_attitude_setpoint.msg delete mode 100644 msg/px4_msgs/vehicle_control_mode.msg delete mode 100644 msg/px4_msgs/vehicle_rates_setpoint.msg create mode 100644 msg/rc_channels.msg create mode 100644 msg/vehicle_attitude.msg create mode 100644 msg/vehicle_attitude_setpoint.msg create mode 100644 msg/vehicle_control_mode.msg create mode 100644 msg/vehicle_rates_setpoint.msg diff --git a/msg/actuator_armed.msg b/msg/actuator_armed.msg new file mode 100644 index 000000000..b83adb8f2 --- /dev/null +++ b/msg/actuator_armed.msg @@ -0,0 +1,6 @@ + +uint64 timestamp # Microseconds since system boot +bool armed # Set to true if system is armed +bool ready_to_arm # Set to true if system is ready to be armed +bool lockdown # Set to true if actuators are forced to being disabled (due to emergency or HIL) +bool force_failsafe # Set to true if the actuators are forced to the failsafe position diff --git a/msg/actuator_controls.msg b/msg/actuator_controls.msg new file mode 100644 index 000000000..743f20cdf --- /dev/null +++ b/msg/actuator_controls.msg @@ -0,0 +1,4 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +float32[8] control diff --git a/msg/actuator_controls_0.msg b/msg/actuator_controls_0.msg new file mode 100644 index 000000000..743f20cdf --- /dev/null +++ b/msg/actuator_controls_0.msg @@ -0,0 +1,4 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +float32[8] control diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg new file mode 100644 index 000000000..d3cfb078b --- /dev/null +++ b/msg/manual_control_setpoint.msg @@ -0,0 +1,44 @@ + +uint8 SWITCH_POS_NONE = 0 # switch is not mapped +uint8 SWITCH_POS_ON = 1 # switch activated (value = 1) +uint8 SWITCH_POS_MIDDLE = 2 # middle position (value = 0) +uint8 SWITCH_POS_OFF = 3 # switch not activated (value = -1) +uint64 timestamp + +# Any of the channels may not be available and be set to NaN +# to indicate that it does not contain valid data. +# The variable names follow the definition of the +# MANUAL_CONTROL mavlink message. +# The default range is from -1 to 1 (mavlink message -1000 to 1000) +# The range for the z variable is defined from 0 to 1. (The z field of +# the MANUAL_CONTROL mavlink message is defined from -1000 to 1000) + +float32 x # stick position in x direction -1..1 + # in general corresponds to forward/back motion or pitch of vehicle, + # in general a positive value means forward or negative pitch and + # a negative value means backward or positive pitch +float32 y # stick position in y direction -1..1 + # in general corresponds to right/left motion or roll of vehicle, + # in general a positive value means right or positive roll and + # a negative value means left or negative roll +float32 z # throttle stick position 0..1 + # in general corresponds to up/down motion or thrust of vehicle, + # in general the value corresponds to the demanded throttle by the user, + # if the input is used for setting the setpoint of a vertical position + # controller any value > 0.5 means up and any value < 0.5 means down +float32 r # yaw stick/twist positon, -1..1 + # in general corresponds to the righthand rotation around the vertical + # (downwards) axis of the vehicle +float32 flaps # flap position +float32 aux1 # default function: camera yaw / azimuth +float32 aux2 # default function: camera pitch / tilt +float32 aux3 # default function: camera trigger +float32 aux4 # default function: camera roll +float32 aux5 # default function: payload drop + +uint8 mode_switch # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO +uint8 return_switch # return to launch 2 position switch (mandatory): _NORMAL_, RTL +uint8 posctl_switch # position control 2 position switch (optional): _ALTCTL_, POSCTL +uint8 loiter_switch # loiter 2 position switch (optional): _MISSION_, LOITER +uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO +uint8 offboard_switch # offboard 2 position switch (optional): _NORMAL_, OFFBOARD diff --git a/msg/parameter_update.msg b/msg/parameter_update.msg new file mode 100644 index 000000000..39dc336e0 --- /dev/null +++ b/msg/parameter_update.msg @@ -0,0 +1 @@ +uint64 timestamp # time at which the latest parameter was updated diff --git a/msg/px4_msgs/actuator_armed.msg b/msg/px4_msgs/actuator_armed.msg deleted file mode 100644 index b83adb8f2..000000000 --- a/msg/px4_msgs/actuator_armed.msg +++ /dev/null @@ -1,6 +0,0 @@ - -uint64 timestamp # Microseconds since system boot -bool armed # Set to true if system is armed -bool ready_to_arm # Set to true if system is ready to be armed -bool lockdown # Set to true if actuators are forced to being disabled (due to emergency or HIL) -bool force_failsafe # Set to true if the actuators are forced to the failsafe position diff --git a/msg/px4_msgs/actuator_controls.msg b/msg/px4_msgs/actuator_controls.msg deleted file mode 100644 index 743f20cdf..000000000 --- a/msg/px4_msgs/actuator_controls.msg +++ /dev/null @@ -1,4 +0,0 @@ -uint8 NUM_ACTUATOR_CONTROLS = 8 -uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 -uint64 timestamp -float32[8] control diff --git a/msg/px4_msgs/manual_control_setpoint.msg b/msg/px4_msgs/manual_control_setpoint.msg deleted file mode 100644 index d3cfb078b..000000000 --- a/msg/px4_msgs/manual_control_setpoint.msg +++ /dev/null @@ -1,44 +0,0 @@ - -uint8 SWITCH_POS_NONE = 0 # switch is not mapped -uint8 SWITCH_POS_ON = 1 # switch activated (value = 1) -uint8 SWITCH_POS_MIDDLE = 2 # middle position (value = 0) -uint8 SWITCH_POS_OFF = 3 # switch not activated (value = -1) -uint64 timestamp - -# Any of the channels may not be available and be set to NaN -# to indicate that it does not contain valid data. -# The variable names follow the definition of the -# MANUAL_CONTROL mavlink message. -# The default range is from -1 to 1 (mavlink message -1000 to 1000) -# The range for the z variable is defined from 0 to 1. (The z field of -# the MANUAL_CONTROL mavlink message is defined from -1000 to 1000) - -float32 x # stick position in x direction -1..1 - # in general corresponds to forward/back motion or pitch of vehicle, - # in general a positive value means forward or negative pitch and - # a negative value means backward or positive pitch -float32 y # stick position in y direction -1..1 - # in general corresponds to right/left motion or roll of vehicle, - # in general a positive value means right or positive roll and - # a negative value means left or negative roll -float32 z # throttle stick position 0..1 - # in general corresponds to up/down motion or thrust of vehicle, - # in general the value corresponds to the demanded throttle by the user, - # if the input is used for setting the setpoint of a vertical position - # controller any value > 0.5 means up and any value < 0.5 means down -float32 r # yaw stick/twist positon, -1..1 - # in general corresponds to the righthand rotation around the vertical - # (downwards) axis of the vehicle -float32 flaps # flap position -float32 aux1 # default function: camera yaw / azimuth -float32 aux2 # default function: camera pitch / tilt -float32 aux3 # default function: camera trigger -float32 aux4 # default function: camera roll -float32 aux5 # default function: payload drop - -uint8 mode_switch # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO -uint8 return_switch # return to launch 2 position switch (mandatory): _NORMAL_, RTL -uint8 posctl_switch # position control 2 position switch (optional): _ALTCTL_, POSCTL -uint8 loiter_switch # loiter 2 position switch (optional): _MISSION_, LOITER -uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO -uint8 offboard_switch # offboard 2 position switch (optional): _NORMAL_, OFFBOARD diff --git a/msg/px4_msgs/parameter_update.msg b/msg/px4_msgs/parameter_update.msg deleted file mode 100644 index 39dc336e0..000000000 --- a/msg/px4_msgs/parameter_update.msg +++ /dev/null @@ -1 +0,0 @@ -uint64 timestamp # time at which the latest parameter was updated diff --git a/msg/px4_msgs/rc_channels.msg b/msg/px4_msgs/rc_channels.msg deleted file mode 100644 index 4e0e5b494..000000000 --- a/msg/px4_msgs/rc_channels.msg +++ /dev/null @@ -1,24 +0,0 @@ -int32 RC_CHANNELS_FUNCTION_MAX=18 -uint8 RC_CHANNELS_FUNCTION_THROTTLE=0 -uint8 RC_CHANNELS_FUNCTION_ROLL=1 -uint8 RC_CHANNELS_FUNCTION_PITCH=2 -uint8 RC_CHANNELS_FUNCTION_YAW=3 -uint8 RC_CHANNELS_FUNCTION_MODE=4 -uint8 RC_CHANNELS_FUNCTION_RETURN=5 -uint8 RC_CHANNELS_FUNCTION_POSCTL=6 -uint8 RC_CHANNELS_FUNCTION_LOITER=7 -uint8 RC_CHANNELS_FUNCTION_OFFBOARD=8 -uint8 RC_CHANNELS_FUNCTION_ACRO=9 -uint8 RC_CHANNELS_FUNCTION_FLAPS=10 -uint8 RC_CHANNELS_FUNCTION_AUX_1=11 -uint8 RC_CHANNELS_FUNCTION_AUX_2=12 -uint8 RC_CHANNELS_FUNCTION_AUX_3=13 -uint8 RC_CHANNELS_FUNCTION_AUX_4=14 -uint8 RC_CHANNELS_FUNCTION_AUX_5=15 -uint64 timestamp # Timestamp in microseconds since boot time -uint64 timestamp_last_valid # Timestamp of last valid RC signal -float32[18] channels # Scaled to -1..1 (throttle: 0..1) -uint8 channel_count # Number of valid channels -int8[18] function # Functions mapping -uint8 rssi # Receive signal strength index -bool signal_lost # Control signal lost, should be checked together with topic timeout diff --git a/msg/px4_msgs/vehicle_attitude.msg b/msg/px4_msgs/vehicle_attitude.msg deleted file mode 100644 index 98018a1df..000000000 --- a/msg/px4_msgs/vehicle_attitude.msg +++ /dev/null @@ -1,18 +0,0 @@ -# This is similar to the mavlink message ATTITUDE, but for onboard use */ -uint64 timestamp # in microseconds since system start -# @warning roll, pitch and yaw have always to be valid, the rotation matrix and quaternion are optional -float32 roll # Roll angle (rad, Tait-Bryan, NED) -float32 pitch # Pitch angle (rad, Tait-Bryan, NED) -float32 yaw # Yaw angle (rad, Tait-Bryan, NED) -float32 rollspeed # Roll angular speed (rad/s, Tait-Bryan, NED) -float32 pitchspeed # Pitch angular speed (rad/s, Tait-Bryan, NED) -float32 yawspeed # Yaw angular speed (rad/s, Tait-Bryan, NED) -float32 rollacc # Roll angular accelration (rad/s, Tait-Bryan, NED) -float32 pitchacc # Pitch angular acceleration (rad/s, Tait-Bryan, NED) -float32 yawacc # Yaw angular acceleration (rad/s, Tait-Bryan, NED) -float32[3] rate_offsets # Offsets of the body angular rates from zero -float32[9] R # Rotation matrix, body to world, (Tait-Bryan, NED) -float32[4] q # Quaternion (NED) -float32[3] g_comp # Compensated gravity vector -bool R_valid # Rotation matrix valid -bool q_valid # Quaternion valid diff --git a/msg/px4_msgs/vehicle_attitude_setpoint.msg b/msg/px4_msgs/vehicle_attitude_setpoint.msg deleted file mode 100644 index 1a8e6e3d5..000000000 --- a/msg/px4_msgs/vehicle_attitude_setpoint.msg +++ /dev/null @@ -1,21 +0,0 @@ - -uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data - -float32 roll_body # body angle in NED frame -float32 pitch_body # body angle in NED frame -float32 yaw_body # body angle in NED frame - -float32[9] R_body # Rotation matrix describing the setpoint as rotation from the current body frame -bool R_valid # Set to true if rotation matrix is valid - -# For quaternion-based attitude control -float32[4] q_d # Desired quaternion for quaternion control -bool q_d_valid # Set to true if quaternion vector is valid -float32[4] q_e # Attitude error in quaternion -bool q_e_valid # Set to true if quaternion error vector is valid - -float32 thrust # Thrust in Newton the power system should generate - -bool roll_reset_integral # Reset roll integral part (navigation logic change) -bool pitch_reset_integral # Reset pitch integral part (navigation logic change) -bool yaw_reset_integral # Reset yaw integral part (navigation logic change) diff --git a/msg/px4_msgs/vehicle_control_mode.msg b/msg/px4_msgs/vehicle_control_mode.msg deleted file mode 100644 index 153a642bb..000000000 --- a/msg/px4_msgs/vehicle_control_mode.msg +++ /dev/null @@ -1,22 +0,0 @@ - -uint64 timestamp # in microseconds since system start - # is set whenever the writing thread stores new data - -bool flag_armed - -bool flag_external_manual_override_ok # external override non-fatal for system. Only true for fixed wing - -# XXX needs yet to be set by state machine helper -bool flag_system_hil_enabled - -bool flag_control_manual_enabled # true if manual input is mixed in -bool flag_control_auto_enabled # true if onboard autopilot should act -bool flag_control_offboard_enabled # true if offboard control should be used -bool flag_control_rates_enabled # true if rates are stabilized -bool flag_control_attitude_enabled # true if attitude stabilization is mixed in -bool flag_control_force_enabled # true if force control is mixed in -bool flag_control_velocity_enabled # true if horizontal velocity (implies direction) is controlled -bool flag_control_position_enabled # true if position is controlled -bool flag_control_altitude_enabled # true if altitude is controlled -bool flag_control_climb_rate_enabled # true if climb rate is controlled -bool flag_control_termination_enabled # true if flighttermination is enabled diff --git a/msg/px4_msgs/vehicle_rates_setpoint.msg b/msg/px4_msgs/vehicle_rates_setpoint.msg deleted file mode 100644 index 834113c79..000000000 --- a/msg/px4_msgs/vehicle_rates_setpoint.msg +++ /dev/null @@ -1,6 +0,0 @@ -uint64 timestamp # in microseconds since system start - -float32 roll # body angular rates in NED frame -float32 pitch # body angular rates in NED frame -float32 yaw # body angular rates in NED frame -float32 thrust # thrust normalized to 0..1 diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg new file mode 100644 index 000000000..4e0e5b494 --- /dev/null +++ b/msg/rc_channels.msg @@ -0,0 +1,24 @@ +int32 RC_CHANNELS_FUNCTION_MAX=18 +uint8 RC_CHANNELS_FUNCTION_THROTTLE=0 +uint8 RC_CHANNELS_FUNCTION_ROLL=1 +uint8 RC_CHANNELS_FUNCTION_PITCH=2 +uint8 RC_CHANNELS_FUNCTION_YAW=3 +uint8 RC_CHANNELS_FUNCTION_MODE=4 +uint8 RC_CHANNELS_FUNCTION_RETURN=5 +uint8 RC_CHANNELS_FUNCTION_POSCTL=6 +uint8 RC_CHANNELS_FUNCTION_LOITER=7 +uint8 RC_CHANNELS_FUNCTION_OFFBOARD=8 +uint8 RC_CHANNELS_FUNCTION_ACRO=9 +uint8 RC_CHANNELS_FUNCTION_FLAPS=10 +uint8 RC_CHANNELS_FUNCTION_AUX_1=11 +uint8 RC_CHANNELS_FUNCTION_AUX_2=12 +uint8 RC_CHANNELS_FUNCTION_AUX_3=13 +uint8 RC_CHANNELS_FUNCTION_AUX_4=14 +uint8 RC_CHANNELS_FUNCTION_AUX_5=15 +uint64 timestamp # Timestamp in microseconds since boot time +uint64 timestamp_last_valid # Timestamp of last valid RC signal +float32[18] channels # Scaled to -1..1 (throttle: 0..1) +uint8 channel_count # Number of valid channels +int8[18] function # Functions mapping +uint8 rssi # Receive signal strength index +bool signal_lost # Control signal lost, should be checked together with topic timeout diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template index 5fce506b6..19068a3a1 100644 --- a/msg/templates/msg.h.template +++ b/msg/templates/msg.h.template @@ -104,7 +104,8 @@ type_map = {'int8': 'int8_t', 'uint32': 'uint32_t', 'uint64': 'uint64_t', 'float32': 'float', - 'bool': 'bool'} + 'bool': 'bool', + 'fence_vertex': 'fence_vertex'} # Function to print a standard ros type def print_field_def(field): diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg new file mode 100644 index 000000000..98018a1df --- /dev/null +++ b/msg/vehicle_attitude.msg @@ -0,0 +1,18 @@ +# This is similar to the mavlink message ATTITUDE, but for onboard use */ +uint64 timestamp # in microseconds since system start +# @warning roll, pitch and yaw have always to be valid, the rotation matrix and quaternion are optional +float32 roll # Roll angle (rad, Tait-Bryan, NED) +float32 pitch # Pitch angle (rad, Tait-Bryan, NED) +float32 yaw # Yaw angle (rad, Tait-Bryan, NED) +float32 rollspeed # Roll angular speed (rad/s, Tait-Bryan, NED) +float32 pitchspeed # Pitch angular speed (rad/s, Tait-Bryan, NED) +float32 yawspeed # Yaw angular speed (rad/s, Tait-Bryan, NED) +float32 rollacc # Roll angular accelration (rad/s, Tait-Bryan, NED) +float32 pitchacc # Pitch angular acceleration (rad/s, Tait-Bryan, NED) +float32 yawacc # Yaw angular acceleration (rad/s, Tait-Bryan, NED) +float32[3] rate_offsets # Offsets of the body angular rates from zero +float32[9] R # Rotation matrix, body to world, (Tait-Bryan, NED) +float32[4] q # Quaternion (NED) +float32[3] g_comp # Compensated gravity vector +bool R_valid # Rotation matrix valid +bool q_valid # Quaternion valid diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg new file mode 100644 index 000000000..1a8e6e3d5 --- /dev/null +++ b/msg/vehicle_attitude_setpoint.msg @@ -0,0 +1,21 @@ + +uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data + +float32 roll_body # body angle in NED frame +float32 pitch_body # body angle in NED frame +float32 yaw_body # body angle in NED frame + +float32[9] R_body # Rotation matrix describing the setpoint as rotation from the current body frame +bool R_valid # Set to true if rotation matrix is valid + +# For quaternion-based attitude control +float32[4] q_d # Desired quaternion for quaternion control +bool q_d_valid # Set to true if quaternion vector is valid +float32[4] q_e # Attitude error in quaternion +bool q_e_valid # Set to true if quaternion error vector is valid + +float32 thrust # Thrust in Newton the power system should generate + +bool roll_reset_integral # Reset roll integral part (navigation logic change) +bool pitch_reset_integral # Reset pitch integral part (navigation logic change) +bool yaw_reset_integral # Reset yaw integral part (navigation logic change) diff --git a/msg/vehicle_control_mode.msg b/msg/vehicle_control_mode.msg new file mode 100644 index 000000000..153a642bb --- /dev/null +++ b/msg/vehicle_control_mode.msg @@ -0,0 +1,22 @@ + +uint64 timestamp # in microseconds since system start + # is set whenever the writing thread stores new data + +bool flag_armed + +bool flag_external_manual_override_ok # external override non-fatal for system. Only true for fixed wing + +# XXX needs yet to be set by state machine helper +bool flag_system_hil_enabled + +bool flag_control_manual_enabled # true if manual input is mixed in +bool flag_control_auto_enabled # true if onboard autopilot should act +bool flag_control_offboard_enabled # true if offboard control should be used +bool flag_control_rates_enabled # true if rates are stabilized +bool flag_control_attitude_enabled # true if attitude stabilization is mixed in +bool flag_control_force_enabled # true if force control is mixed in +bool flag_control_velocity_enabled # true if horizontal velocity (implies direction) is controlled +bool flag_control_position_enabled # true if position is controlled +bool flag_control_altitude_enabled # true if altitude is controlled +bool flag_control_climb_rate_enabled # true if climb rate is controlled +bool flag_control_termination_enabled # true if flighttermination is enabled diff --git a/msg/vehicle_rates_setpoint.msg b/msg/vehicle_rates_setpoint.msg new file mode 100644 index 000000000..834113c79 --- /dev/null +++ b/msg/vehicle_rates_setpoint.msg @@ -0,0 +1,6 @@ +uint64 timestamp # in microseconds since system start + +float32 roll # body angular rates in NED frame +float32 pitch # body angular rates in NED frame +float32 yaw # body angular rates in NED frame +float32 thrust # thrust normalized to 0..1 -- cgit v1.2.3 From 9520983e08397c453af735d0ff0736cc007c2c45 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 16 Dec 2014 08:24:51 +0100 Subject: lots' of header juggling and small changes to make mc att control compile for NuttX and ROS --- CMakeLists.txt | 42 ++++-- Makefile | 2 +- makefiles/config_px4fmu-v2_default.mk | 4 +- package.xml | 2 + src/include/px4.h | 2 + src/lib/geo/geo.h | 5 +- src/lib/mathlib/math/Matrix.hpp | 12 +- src/lib/mathlib/math/Vector.hpp | 2 +- src/modules/bottle_drop/bottle_drop.cpp | 1 + src/modules/mc_att_control/mc_att_control.cpp | 75 ++++------ src/modules/mc_att_control/mc_att_control.h | 9 +- src/modules/mc_att_control/mc_att_control_main.cpp | 4 +- src/modules/systemlib/circuit_breaker.c | 17 ++- src/modules/systemlib/circuit_breaker.h | 8 +- src/modules/systemlib/circuit_breaker_params.h | 7 + src/modules/systemlib/perf_counter.h | 1 + src/platforms/px4_defines.h | 37 ++++- src/platforms/px4_includes.h | 13 ++ src/platforms/ros/circuit_breaker.cpp | 56 +++++++ src/platforms/ros/geo.cpp | 165 +++++++++++++++++++++ 20 files changed, 376 insertions(+), 88 deletions(-) create mode 100644 src/modules/systemlib/circuit_breaker_params.h create mode 100644 src/platforms/ros/circuit_breaker.cpp create mode 100644 src/platforms/ros/geo.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 489467db7..19a14f62a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,9 @@ find_package(catkin REQUIRED COMPONENTS rospy std_msgs message_generation + cmake_modules ) +find_package(Eigen REQUIRED) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -48,9 +50,17 @@ find_package(catkin REQUIRED COMPONENTS ## Generate messages in the 'msg' folder add_message_files( FILES - px4_msgs/rc_channels.msg - px4_msgs/vehicle_attitude.msg - px4_msgs/rc_channels.msg + rc_channels.msg + vehicle_attitude.msg + vehicle_attitude_setpoint.msg + manual_control_setpoint.msg + actuator_controls.msg + actuator_controls_0.msg + vehicle_rates_setpoint.msg + vehicle_attitude.msg + vehicle_control_mode.msg + actuator_armed.msg + parameter_update.msg ) ## Generate services in the 'srv' folder @@ -100,11 +110,19 @@ include_directories( ${catkin_INCLUDE_DIRS} src/platforms src/include + src/modules + src/ + src/lib + ${EIGEN_INCLUDE_DIRS} ) ## Declare a cpp library add_library(px4 src/platforms/ros/px4_ros_impl.cpp + src/platforms/ros/perf_counter.cpp + src/platforms/ros/geo.cpp + src/lib/mathlib/math/Limits.cpp + src/platforms/ros/circuit_breaker.cpp ) target_link_libraries(px4 @@ -141,14 +159,16 @@ target_link_libraries(subscriber px4 ) -# add_executable(mc_att_control - # src/modules/mc_att_control/mc_att_control_main.cpp - # src/moudles/mc_att_control/mc_att_control_base.cpp) -# add_dependencies(mc_att_control px4_generate_messages_cpp) -# target_link_libraries(mc_att_control - # ${catkin_LIBRARIES} - # px4 -# ) +## MC Attitude Control +add_executable(mc_att_control + src/modules/mc_att_control/mc_att_control_main.cpp + src/modules/mc_att_control/mc_att_control.cpp + src/modules/mc_att_control/mc_att_control_base.cpp) +add_dependencies(mc_att_control px4_generate_messages_cpp) +target_link_libraries(mc_att_control + ${catkin_LIBRARIES} + px4 +) ############# diff --git a/Makefile b/Makefile index f2e467e5a..910b785a3 100644 --- a/Makefile +++ b/Makefile @@ -224,7 +224,7 @@ updatesubmodules: $(Q) (git submodule init) $(Q) (git submodule update) -MSG_DIR = $(PX4_BASE)msg/px4_msgs +MSG_DIR = $(PX4_BASE)msg MSG_TEMPLATE_DIR = $(PX4_BASE)msg/templates TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics TOPICS_TEMPORARY_DIR = $(BUILD_DIR)topics_temporary diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 38c5ebc7b..edf4fe1a0 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -82,8 +82,8 @@ MODULES += modules/position_estimator_inav # Vehicle Control # #MODULES += modules/segway # XXX Needs GCC 4.7 fix -MODULES += modules/fw_pos_control_l1 -MODULES += modules/fw_att_control +#MODULES += modules/fw_pos_control_l1 +#MODULES += modules/fw_att_control MODULES += modules/mc_att_control MODULES += modules/mc_pos_control diff --git a/package.xml b/package.xml index bc23cdd18..666200390 100644 --- a/package.xml +++ b/package.xml @@ -43,9 +43,11 @@ roscpp rospy std_msgs + eigen roscpp rospy std_msgs + eigen diff --git a/src/include/px4.h b/src/include/px4.h index ca702d63c..34137ee6f 100644 --- a/src/include/px4.h +++ b/src/include/px4.h @@ -41,6 +41,8 @@ #include "../platforms/px4_includes.h" #include "../platforms/px4_defines.h" +#ifdef __cplusplus #include "../platforms/px4_middleware.h" #include "../platforms/px4_nodehandle.h" #include "../platforms/px4_subscriber.h" +#endif diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 2311e0a7c..012779646 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -44,10 +44,7 @@ */ #pragma once - -#include "uORB/topics/fence.h" -#include "uORB/topics/vehicle_global_position.h" - +#include __BEGIN_DECLS #include "geo_lookup/geo_mag_declination.h" diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 806f5933a..1e76aae60 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -49,9 +49,8 @@ #ifdef CONFIG_ARCH_ARM #include "../CMSIS/Include/arm_math.h" #else -#include +#include #include -#define M_PI_2_F 1.5707963267948966192f #endif namespace math @@ -122,6 +121,15 @@ public: memcpy(data, d, sizeof(data)); } +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) + /** + * set data from boost::array + */ + void set(const boost::array d) { + set(static_cast(d.data())); + } +#endif + /** * access by index */ diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 57b45e3ab..20f099831 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -49,7 +49,7 @@ #ifdef CONFIG_ARCH_ARM #include "../CMSIS/Include/arm_math.h" #else -#include +#include #endif namespace math diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index e0bcbc6e9..53ab74305 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -59,6 +59,7 @@ #include #include #include +#include #include #include #include diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp index 13bff9561..906bd0a47 100644 --- a/src/modules/mc_att_control/mc_att_control.cpp +++ b/src/modules/mc_att_control/mc_att_control.cpp @@ -44,6 +44,8 @@ */ #include "mc_att_control.h" +#include "mc_att_control_params.h" +#include "math.h" #define YAW_DEADZONE 0.05f #define MIN_TAKEOFF_THRUST 0.2f @@ -63,7 +65,6 @@ static const int ERROR = -1; MulticopterAttitudeControl::MulticopterAttitudeControl() : MulticopterAttitudeControlBase(), _task_should_exit(false), - _control_task(-1), _actuators_0_circuit_breaker_enabled(false), /* publications */ @@ -76,26 +77,26 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) { - _params_handles.roll_p = PX4_PARAM_INIT("MC_ROLL_P"); - _params_handles.roll_rate_p = PX4_PARAM_INIT("MC_ROLLRATE_P"); - _params_handles.roll_rate_i = PX4_PARAM_INIT("MC_ROLLRATE_I"); - _params_handles.roll_rate_d = PX4_PARAM_INIT("MC_ROLLRATE_D"); - _params_handles.pitch_p = PX4_PARAM_INIT("MC_PITCH_P"); - _params_handles.pitch_rate_p = PX4_PARAM_INIT("MC_PITCHRATE_P"); - _params_handles.pitch_rate_i = PX4_PARAM_INIT("MC_PITCHRATE_I"); - _params_handles.pitch_rate_d = PX4_PARAM_INIT("MC_PITCHRATE_D"); - _params_handles.yaw_p = PX4_PARAM_INIT("MC_YAW_P"); - _params_handles.yaw_rate_p = PX4_PARAM_INIT("MC_YAWRATE_P"); - _params_handles.yaw_rate_i = PX4_PARAM_INIT("MC_YAWRATE_I"); - _params_handles.yaw_rate_d = PX4_PARAM_INIT("MC_YAWRATE_D"); - _params_handles.yaw_ff = PX4_PARAM_INIT("MC_YAW_FF"); - _params_handles.yaw_rate_max = PX4_PARAM_INIT("MC_YAWRATE_MAX"); - _params_handles.man_roll_max = PX4_PARAM_INIT("MC_MAN_R_MAX"); - _params_handles.man_pitch_max = PX4_PARAM_INIT("MC_MAN_P_MAX"); - _params_handles.man_yaw_max = PX4_PARAM_INIT("MC_MAN_Y_MAX"); - _params_handles.acro_roll_max = PX4_PARAM_INIT("MC_ACRO_R_MAX"); - _params_handles.acro_pitch_max = PX4_PARAM_INIT("MC_ACRO_P_MAX"); - _params_handles.acro_yaw_max = PX4_PARAM_INIT("MC_ACRO_Y_MAX"); + _params_handles.roll_p = PX4_PARAM_INIT(MC_ROLL_P); + _params_handles.roll_rate_p = PX4_PARAM_INIT(MC_ROLLRATE_P); + _params_handles.roll_rate_i = PX4_PARAM_INIT(MC_ROLLRATE_I); + _params_handles.roll_rate_d = PX4_PARAM_INIT(MC_ROLLRATE_D); + _params_handles.pitch_p = PX4_PARAM_INIT(MC_PITCH_P); + _params_handles.pitch_rate_p = PX4_PARAM_INIT(MC_PITCHRATE_P); + _params_handles.pitch_rate_i = PX4_PARAM_INIT(MC_PITCHRATE_I); + _params_handles.pitch_rate_d = PX4_PARAM_INIT(MC_PITCHRATE_D); + _params_handles.yaw_p = PX4_PARAM_INIT(MC_YAW_P); + _params_handles.yaw_rate_p = PX4_PARAM_INIT(MC_YAWRATE_P); + _params_handles.yaw_rate_i = PX4_PARAM_INIT(MC_YAWRATE_I); + _params_handles.yaw_rate_d = PX4_PARAM_INIT(MC_YAWRATE_D); + _params_handles.yaw_ff = PX4_PARAM_INIT(MC_YAW_FF); + _params_handles.yaw_rate_max = PX4_PARAM_INIT(MC_YAWRATE_MAX); + _params_handles.man_roll_max = PX4_PARAM_INIT(MC_MAN_R_MAX); + _params_handles.man_pitch_max = PX4_PARAM_INIT(MC_MAN_P_MAX); + _params_handles.man_yaw_max = PX4_PARAM_INIT(MC_MAN_Y_MAX); + _params_handles.acro_roll_max = PX4_PARAM_INIT(MC_ACRO_R_MAX); + _params_handles.acro_pitch_max = PX4_PARAM_INIT(MC_ACRO_P_MAX); + _params_handles.acro_yaw_max = PX4_PARAM_INIT(MC_ACRO_Y_MAX); /* fetch initial parameter values */ parameters_update(); @@ -115,26 +116,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : MulticopterAttitudeControl::~MulticopterAttitudeControl() { - if (_control_task != -1) { - /* task wakes up every 100ms or so at the longest */ - _task_should_exit = true; - - /* wait for a second for the task to quit at our request */ - unsigned i = 0; - - do { - /* wait 20ms */ - usleep(20000); - - /* if we have given up, kill it */ - if (++i > 50) { - task_delete(_control_task); - break; - } - } while (_control_task != -1); - } - - // mc_att_control::g_control = nullptr; } int @@ -203,8 +184,8 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi /* 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(); + float dt = (px4::get_time_micros() - last_run) / 1000000.0f; + last_run = px4::get_time_micros(); /* guard against too small (< 2ms) and too large (> 20ms) dt's */ if (dt < 0.002f) { @@ -219,7 +200,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi /* publish the attitude setpoint if needed */ if (_publish_att_sp) { - _v_att_sp_mod.timestamp = hrt_absolute_time(); + _v_att_sp_mod.timestamp = px4::get_time_micros(); if (_att_sp_pub != nullptr) { _att_sp_pub->publish(_v_att_sp_mod); @@ -234,7 +215,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi _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 = hrt_absolute_time(); + _v_rates_sp_mod.timestamp = px4::get_time_micros(); if (_v_rates_sp_pub != nullptr) { _v_rates_sp_pub->publish(_v_rates_sp_mod); @@ -258,7 +239,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi _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 = hrt_absolute_time(); + _v_rates_sp_mod.timestamp = px4::get_time_micros(); if (_v_rates_sp_pub != nullptr) { _v_rates_sp_pub->publish(_v_rates_sp_mod); @@ -285,7 +266,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi _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(); + _actuators.timestamp = px4::get_time_micros(); if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub != nullptr) { diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h index 24009a5e6..1da738408 100644 --- a/src/modules/mc_att_control/mc_att_control.h +++ b/src/modules/mc_att_control/mc_att_control.h @@ -52,22 +52,16 @@ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. */ -#include #include #include #include #include #include #include -#include -#include -#include -#include #include -#include +// #include #include #include -#include #include "mc_att_control_base.h" @@ -91,7 +85,6 @@ public: private: bool _task_should_exit; /**< if true, sensor task should exit */ - int _control_task; /**< task handle for sensor task */ bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ px4::Publisher * _att_sp_pub; /**< attitude setpoint publication */ 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 a1dca8a8c..be627866e 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -129,12 +129,12 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]) PX4_MAIN_FUNCTION(mc_att_control) { - warnx("starting"); + PX4_INFO("starting"); MulticopterAttitudeControl attctl; thread_running = true; attctl.spin(); - warnx("exiting."); + PX4_INFO("exiting."); thread_running = false; return 0; } diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 12187d70e..1b3ffd59f 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -42,7 +42,8 @@ * parameter needs to set to the key (magic). */ -#include +#include +#include #include /** @@ -56,7 +57,7 @@ * @max 894281 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0); +PX4_PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK); /** * Circuit breaker for rate controller output @@ -69,7 +70,7 @@ PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0); * @max 140253 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); +PX4_PARAM_DEFINE_INT32(CBRK_RATE_CTRL); /** * Circuit breaker for IO safety @@ -81,7 +82,7 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); * @max 22027 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); +PX4_PARAM_DEFINE_INT32(CBRK_IO_SAFETY); /** * Circuit breaker for airspeed sensor @@ -93,7 +94,7 @@ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); * @max 162128 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); +PX4_PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK); /** * Circuit breaker for flight termination @@ -106,7 +107,7 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); * @max 121212 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); +PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM); /** * Circuit breaker for engine failure detection @@ -120,7 +121,7 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); * @max 284953 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); +PX4_PARAM_DEFINE_INT32(CBRK_ENGINEFAIL); /** * Circuit breaker for gps failure detection @@ -134,7 +135,7 @@ PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); * @max 240024 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); +PX4_PARAM_DEFINE_INT32(CBRK_GPSFAIL); bool circuit_breaker_enabled(const char* breaker, int32_t magic) { diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index b3431722f..012d8cb61 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -61,8 +61,14 @@ __BEGIN_DECLS +#ifdef __cplusplus +extern "C" { +#endif +__EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic); +#ifdef __cplusplus +} +#endif __EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic); - __END_DECLS #endif /* CIRCUIT_BREAKER_H_ */ diff --git a/src/modules/systemlib/circuit_breaker_params.h b/src/modules/systemlib/circuit_breaker_params.h new file mode 100644 index 000000000..768bf7f53 --- /dev/null +++ b/src/modules/systemlib/circuit_breaker_params.h @@ -0,0 +1,7 @@ +#define PARAM_CBRK_SUPPLY_CHK_DEFAULT 0 +#define PARAM_CBRK_RATE_CTRL_DEFAULT 0 +#define PARAM_CBRK_IO_SAFETY_DEFAULT 0 +#define PARAM_CBRK_AIRSPD_CHK_DEFAULT 0 +#define PARAM_CBRK_FLIGHTTERM_DEFAULT 121212 +#define PARAM_CBRK_ENGINEFAIL_DEFAULT 284953 +#define PARAM_CBRK_GPSFAIL_DEFAULT 240024 diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index 668d9dfdf..a9dfb13f8 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -40,6 +40,7 @@ #define _SYSTEMLIB_PERF_COUNTER_H value #include +#include /** * Counter types. diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index a1bf9919e..712e0dd63 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -51,7 +51,10 @@ * Building for running within the ROS environment */ #define __EXPORT +#define noreturn_function +#ifdef __cplusplus #include "ros/ros.h" +#endif /* Main entry point */ #define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv) @@ -63,7 +66,7 @@ #define PX4_TOPIC(_name) #_name /* Topic type */ -#define PX4_TOPIC_T(_name) _name +#define PX4_TOPIC_T(_name) px4::_name /* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _objptr, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, _objptr); @@ -93,6 +96,38 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) /* Get value of parameter */ #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) +#define OK 0 +#define ERROR -1 + +//XXX hack to be able to use isfinte from math.h, -D_GLIBCXX_USE_C99_MATH seems not to work +#define isfinite(_value) std::isfinite(_value) + +/* Useful constants. */ +#define M_E_F 2.7182818284590452354f +#define M_LOG2E_F 1.4426950408889634074f +#define M_LOG10E_F 0.43429448190325182765f +#define M_LN2_F _M_LN2_F +#define M_LN10_F 2.30258509299404568402f +#define M_PI_F 3.14159265358979323846f +#define M_TWOPI_F (M_PI_F * 2.0f) +#define M_PI_2_F 1.57079632679489661923f +#define M_PI_4_F 0.78539816339744830962f +#define M_3PI_4_F 2.3561944901923448370E0f +#define M_SQRTPI_F 1.77245385090551602792981f +#define M_1_PI_F 0.31830988618379067154f +#define M_2_PI_F 0.63661977236758134308f +#define M_2_SQRTPI_F 1.12837916709551257390f +#define M_DEG_TO_RAD_F 0.01745329251994f +#define M_RAD_TO_DEG_F 57.2957795130823f +#define M_SQRT2_F 1.41421356237309504880f +#define M_SQRT1_2_F 0.70710678118654752440f +#define M_LN2LO_F 1.9082149292705877000E-10f +#define M_LN2HI_F 6.9314718036912381649E-1f +#define M_SQRT3_F 1.73205080756887719000f +#define M_IVLN10_F 0.43429448190325182765f /* 1 / log(10) */ +#define M_LOG2_E_F _M_LN2_F +#define M_INVLN2_F 1.4426950408889633870E0f /* 1 / log(2) */ + #else /* * Building for NuttX diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 525f94aae..752c6b5fe 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -45,8 +45,21 @@ /* * Building for running within the ROS environment */ + +#ifdef __cplusplus #include "ros/ros.h" #include "px4/rc_channels.h" +#include "px4/vehicle_attitude.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#endif #else /* diff --git a/src/platforms/ros/circuit_breaker.cpp b/src/platforms/ros/circuit_breaker.cpp new file mode 100644 index 000000000..1e912d3ac --- /dev/null +++ b/src/platforms/ros/circuit_breaker.cpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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 circuit_breaker.c + * + * Circuit breaker parameters. + * Analog to real aviation circuit breakers these parameters + * allow to disable subsystems. They are not supported as standard + * operation procedure and are only provided for development purposes. + * To ensure they are not activated accidentally, the associated + * parameter needs to set to the key (magic). + */ + +#include +#include +#include + +bool circuit_breaker_enabled(const char* breaker, int32_t magic) +{ + int32_t val; + (void)PX4_PARAM_GET(breaker, &val); + + return (val == magic); +} + diff --git a/src/platforms/ros/geo.cpp b/src/platforms/ros/geo.cpp new file mode 100644 index 000000000..a7ee6fd84 --- /dev/null +++ b/src/platforms/ros/geo.cpp @@ -0,0 +1,165 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 geo.c + * + * Geo / math functions to perform geodesic calculations + * + * @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier + * @author Anton Babushkin + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +__EXPORT float _wrap_pi(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } + + int c = 0; + while (bearing >= M_PI_F) { + bearing -= M_TWOPI_F; + + if (c++ > 3) { + return NAN; + } + } + + c = 0; + while (bearing < -M_PI_F) { + bearing += M_TWOPI_F; + + if (c++ > 3) { + return NAN; + } + } + + return bearing; +} + +__EXPORT float _wrap_2pi(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } + + int c = 0; + while (bearing >= M_TWOPI_F) { + bearing -= M_TWOPI_F; + + if (c++ > 3) { + return NAN; + } + } + + c = 0; + while (bearing < 0.0f) { + bearing += M_TWOPI_F; + + if (c++ > 3) { + return NAN; + } + } + + return bearing; +} + +__EXPORT float _wrap_180(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } + + int c = 0; + while (bearing >= 180.0f) { + bearing -= 360.0f; + + if (c++ > 3) { + return NAN; + } + } + + c = 0; + while (bearing < -180.0f) { + bearing += 360.0f; + + if (c++ > 3) { + return NAN; + } + } + + return bearing; +} + +__EXPORT float _wrap_360(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } + + int c = 0; + while (bearing >= 360.0f) { + bearing -= 360.0f; + + if (c++ > 3) { + return NAN; + } + } + + c = 0; + while (bearing < 0.0f) { + bearing += 360.0f; + + if (c++ > 3) { + return NAN; + } + } + + return bearing; +} -- cgit v1.2.3 From e1ff89ad6131019a12114ca7b0b5fa40112b9645 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 16 Dec 2014 10:11:59 +0100 Subject: add nuttx platform to fmuv1 makefile --- makefiles/config_px4fmu-v1_default.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 18be47065..7eb090d73 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -110,6 +110,7 @@ MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion MODULES += lib/launchdetection +MODULES += platforms/nuttx # # Demo apps -- cgit v1.2.3 From 71f6a34367794a887704e2898f8a10101bacfb12 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 16 Dec 2014 10:12:50 +0100 Subject: mc att: increase stack size --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 be627866e..4188c7880 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -99,7 +99,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("mc_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2000, + 3000, mc_att_control_task_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); -- cgit v1.2.3 From be269520382adbd4bea59c439599897a53109ad7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 17 Dec 2014 08:03:22 +0100 Subject: px4 nodehandle: nuttx: call spin once also after timeout --- src/platforms/px4_nodehandle.h | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 9bd792c69..8879148a3 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -228,12 +228,7 @@ public: struct pollfd pfd; pfd.fd = _sub_min_interval->getHandle(); pfd.events = POLLIN; - - if (poll(&pfd, 1, timeout_ms) <= 0) { - /* timed out */ - continue; - } - + poll(&pfd, 1, timeout_ms); spinOnce(); } } -- cgit v1.2.3 From 9bad23e41852eba6657898d4ea46b0b303de4ae3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 17 Dec 2014 08:22:38 +0100 Subject: add explicit non-callback contructor for nuttx/uorb subscriber to work around linker issues --- src/platforms/px4_defines.h | 2 +- src/platforms/px4_nodehandle.h | 20 ++++++++++++++++++++ src/platforms/px4_subscriber.h | 15 +++++++++++++++ 3 files changed, 36 insertions(+), 1 deletion(-) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 712e0dd63..c76548381 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -154,7 +154,7 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) /* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) /* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */ -#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), nullptr, _interval) +#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _interval) /* Parameter handle datatype */ #include diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 8879148a3..160886810 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -178,6 +178,26 @@ public: return (Subscriber *)sub_px4; } + /** + * Subscribe without callback to function + * @param meta Describes the topic which nodehande should subscribe to + * @param interval Minimal interval between data fetches from orb + */ + + template + Subscriber *subscribe(const struct orb_metadata *meta, + unsigned interval) + { + SubscriberUORB *sub_px4 = new SubscriberUORB(meta, interval, &_subs); + + /* Check if this is the smallest interval so far and update _sub_min_interval */ + if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { + _sub_min_interval = sub_px4; + } + + return (Subscriber *)sub_px4; + } + /** * Advertise topic * @param meta Describes the topic which is advertised diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 7d8463cb5..5d4e67ad0 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -178,6 +178,21 @@ public: //XXX store callback {} + /** + * Construct SubscriberUORB by providing orb meta data without callback + * @param meta orb metadata for the topic which is used + * @param interval Minimal interval between calls to callback + * @param list subscriber is added to this list + */ + SubscriberUORB(const struct orb_metadata *meta, + unsigned interval, + List *list) : + Subscriber(), + uORB::Subscription(meta, interval, list), + _callback(nullptr) + //XXX store callback + {} + ~SubscriberUORB() {}; /** -- cgit v1.2.3 From bbfe78e4f60de5414cf3594250c5ed769f0a1433 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 17 Dec 2014 13:50:29 +0100 Subject: mc att ctl: fix subscription handlers, fix parameters --- src/modules/mc_att_control/mc_att_control.cpp | 20 ++++++++++++-------- src/modules/mc_att_control/mc_att_control.h | 4 +++- src/modules/mc_att_control/mc_att_control_base.h | 4 ++-- src/modules/mc_att_control/mc_att_control_params.c | 3 ++- 4 files changed, 19 insertions(+), 12 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp index 906bd0a47..89da8438b 100644 --- a/src/modules/mc_att_control/mc_att_control.cpp +++ b/src/modules/mc_att_control/mc_att_control.cpp @@ -104,13 +104,13 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* * do subscriptions */ - PX4_SUBSCRIBE(_n, vehicle_attitude, MulticopterAttitudeControl::handle_vehicle_attitude, this, 0); - PX4_SUBSCRIBE(_n, vehicle_attitude_setpoint, 0); - PX4_SUBSCRIBE(_n, vehicle_rates_setpoint, 0); - PX4_SUBSCRIBE(_n, vehicle_control_mode, 0); - PX4_SUBSCRIBE(_n, parameter_update, 0); - PX4_SUBSCRIBE(_n, manual_control_setpoint, 0); - PX4_SUBSCRIBE(_n, actuator_armed, 0); + _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); + 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); } @@ -178,6 +178,11 @@ MulticopterAttitudeControl::parameters_update() return OK; } +void MulticopterAttitudeControl::handle_parameter_update(const PX4_TOPIC_T(parameter_update) &msg) +{ + parameters_update(); +} + void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) { perf_begin(_loop_perf); @@ -250,7 +255,6 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi } else { /* attitude controller disabled, poll rates setpoint topic */ - //XXX vehicle_rates_setpoint_poll(); _rates_sp(0) = _v_rates_sp->get().roll; _rates_sp(1) = _v_rates_sp->get().pitch; _rates_sp(2) = _v_rates_sp->get().yaw; diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h index 1da738408..76c095cc1 100644 --- a/src/modules/mc_att_control/mc_att_control.h +++ b/src/modules/mc_att_control/mc_att_control.h @@ -79,7 +79,9 @@ public: */ ~MulticopterAttitudeControl(); - void handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg); + /* 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 spin() { _n.spin(); } diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 2135b61d8..17c779796 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -90,8 +90,8 @@ protected: px4::PX4_SUBSCRIBER(manual_control_setpoint) *_manual_control_sp; /**< manual control setpoint */ px4::PX4_SUBSCRIBER(vehicle_control_mode) *_v_control_mode; /**< vehicle control mode */ px4::PX4_SUBSCRIBER(actuator_armed) *_armed; /**< actuator arming status */ - 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_attitude_setpoint) *_v_att_sp; /**< vehicle attitude setpoint */ + px4::PX4_SUBSCRIBER(vehicle_rates_setpoint) *_v_rates_sp; /**< vehicle rates setpoint */ PX4_TOPIC_T(vehicle_attitude_setpoint) _v_att_sp_mod; /**< modified vehicle attitude setpoint that gets published eventually */ diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index bbbfd829b..a0b1706f2 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -40,7 +40,8 @@ * @author Anton Babushkin */ -#include "mc_att_control_params.h"" +#include +#include "mc_att_control_params.h" #include /** -- cgit v1.2.3 From 2d0f11783c1394b44d31d6bd6d07612c164dbe90 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 17 Dec 2014 15:47:18 +0100 Subject: fix header for cpp perf_counter dummy --- src/platforms/ros/perf_counter.cpp | 42 ++++++++++++++++++++++++++++++++++---- 1 file changed, 38 insertions(+), 4 deletions(-) diff --git a/src/platforms/ros/perf_counter.cpp b/src/platforms/ros/perf_counter.cpp index aa8d85c60..a71801397 100755 --- a/src/platforms/ros/perf_counter.cpp +++ b/src/platforms/ros/perf_counter.cpp @@ -1,9 +1,43 @@ -/* - * perf_counter.c +/**************************************************************************** + * + * Copyright (C) 2012 - 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 perf_counter.cpp + * + * Empty function calls for ros compatibility + * + * @author Roman Bapst * - * Created on: Sep 24, 2014 - * Author: roman */ #include #include -- cgit v1.2.3 From 7ad3b0335336d65736f707486b2b714d0fa91c2a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 17 Dec 2014 15:49:03 +0100 Subject: multi platform interface: macro to get param by name --- src/platforms/px4_defines.h | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index c76548381..188149008 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -93,9 +93,12 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) /* Initialize a param, in case of ROS the parameter is sent to the parameter server here */ #define PX4_PARAM_INIT(_name) PX4_ROS_PARAM_SET(#_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) -/* Get value of parameter */ +/* Get value of parameter by handle */ #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) +/* Get value of parameter by name, which is equal to the handle for ros */ +#define PX4_PARAM_GET_NAME(_name, _destpt) PX4_PARAM_GET(_name, _destpt) + #define OK 0 #define ERROR -1 @@ -163,9 +166,12 @@ typedef param_t px4_param_t; /* Initialize a param, get param handle */ #define PX4_PARAM_INIT(_name) param_find(#_name) -/* Get value of parameter */ +/* Get value of parameter by handle */ #define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt) +/* Get value of parameter by name */ +#define PX4_PARAM_GET_NAME(_name, _destpt) param_get(PX4_PARAM_INIT(_name), _destpt) + /* XXX this is a hack to resolve conflicts with NuttX headers */ #define isspace(c) \ ((c) == ' ' || (c) == '\t' || (c) == '\n' || \ -- cgit v1.2.3 From 0a1e94d504b38d30c2d7428300c5728798db094d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 17 Dec 2014 15:50:01 +0100 Subject: circuit breaker: move to cpp, all platforms use the same file --- CMakeLists.txt | 2 +- src/modules/systemlib/circuit_breaker.c | 147 ------------------------- src/modules/systemlib/circuit_breaker.cpp | 56 ++++++++++ src/modules/systemlib/circuit_breaker.h | 10 +- src/modules/systemlib/circuit_breaker_params.c | 123 +++++++++++++++++++++ src/modules/systemlib/module.mk | 3 +- src/platforms/ros/circuit_breaker.cpp | 56 ---------- 7 files changed, 184 insertions(+), 213 deletions(-) delete mode 100644 src/modules/systemlib/circuit_breaker.c create mode 100644 src/modules/systemlib/circuit_breaker.cpp create mode 100644 src/modules/systemlib/circuit_breaker_params.c delete mode 100644 src/platforms/ros/circuit_breaker.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 19a14f62a..3184bb2ac 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -122,7 +122,7 @@ add_library(px4 src/platforms/ros/perf_counter.cpp src/platforms/ros/geo.cpp src/lib/mathlib/math/Limits.cpp - src/platforms/ros/circuit_breaker.cpp + src/modules/systemlib/circuit_breaker.cpp ) target_link_libraries(px4 diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c deleted file mode 100644 index 1b3ffd59f..000000000 --- a/src/modules/systemlib/circuit_breaker.c +++ /dev/null @@ -1,147 +0,0 @@ -/**************************************************************************** - * - * 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 circuit_breaker.c - * - * Circuit breaker parameters. - * Analog to real aviation circuit breakers these parameters - * allow to disable subsystems. They are not supported as standard - * operation procedure and are only provided for development purposes. - * To ensure they are not activated accidentally, the associated - * parameter needs to set to the key (magic). - */ - -#include -#include -#include - -/** - * Circuit breaker for power supply check - * - * Setting this parameter to 894281 will disable the power valid - * checks in the commander. - * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK - * - * @min 0 - * @max 894281 - * @group Circuit Breaker - */ -PX4_PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK); - -/** - * Circuit breaker for rate controller output - * - * Setting this parameter to 140253 will disable the rate - * controller uORB publication. - * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK - * - * @min 0 - * @max 140253 - * @group Circuit Breaker - */ -PX4_PARAM_DEFINE_INT32(CBRK_RATE_CTRL); - -/** - * Circuit breaker for IO safety - * - * Setting this parameter to 894281 will disable IO safety. - * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK - * - * @min 0 - * @max 22027 - * @group Circuit Breaker - */ -PX4_PARAM_DEFINE_INT32(CBRK_IO_SAFETY); - -/** - * Circuit breaker for airspeed sensor - * - * Setting this parameter to 162128 will disable the check for an airspeed sensor. - * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK - * - * @min 0 - * @max 162128 - * @group Circuit Breaker - */ -PX4_PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK); - -/** - * Circuit breaker for flight termination - * - * Setting this parameter to 121212 will disable the flight termination action. - * --> The IO driver will not do flight termination if requested by the FMU - * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK - * - * @min 0 - * @max 121212 - * @group Circuit Breaker - */ -PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM); - -/** - * Circuit breaker for engine failure detection - * - * Setting this parameter to 284953 will disable the engine failure detection. - * If the aircraft is in engine failure mode the enine failure flag will be - * set to healthy - * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK - * - * @min 0 - * @max 284953 - * @group Circuit Breaker - */ -PX4_PARAM_DEFINE_INT32(CBRK_ENGINEFAIL); - -/** - * Circuit breaker for gps failure detection - * - * Setting this parameter to 240024 will disable the gps failure detection. - * If the aircraft is in gps failure mode the gps failure flag will be - * set to healthy - * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK - * - * @min 0 - * @max 240024 - * @group Circuit Breaker - */ -PX4_PARAM_DEFINE_INT32(CBRK_GPSFAIL); - -bool circuit_breaker_enabled(const char* breaker, int32_t magic) -{ - int32_t val; - (void)param_get(param_find(breaker), &val); - - return (val == magic); -} - diff --git a/src/modules/systemlib/circuit_breaker.cpp b/src/modules/systemlib/circuit_breaker.cpp new file mode 100644 index 000000000..2df13ebff --- /dev/null +++ b/src/modules/systemlib/circuit_breaker.cpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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 circuit_breaker.c + * + * Circuit breaker parameters. + * Analog to real aviation circuit breakers these parameters + * allow to disable subsystems. They are not supported as standard + * operation procedure and are only provided for development purposes. + * To ensure they are not activated accidentally, the associated + * parameter needs to set to the key (magic). + */ + +#include +#include + +bool circuit_breaker_enabled(const char* breaker, int32_t magic) +{ + int32_t val; + /* (void)param_get(param_find(breaker), &val); */ + (void)PX4_PARAM_GET_NAME(breaker, &val); + + return (val == magic); +} + diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index 012d8cb61..c97dbc26f 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -61,14 +61,8 @@ __BEGIN_DECLS -#ifdef __cplusplus -extern "C" { -#endif -__EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic); -#ifdef __cplusplus -} -#endif -__EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic); +extern "C" __EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic); + __END_DECLS #endif /* CIRCUIT_BREAKER_H_ */ diff --git a/src/modules/systemlib/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c new file mode 100644 index 000000000..e499ae27a --- /dev/null +++ b/src/modules/systemlib/circuit_breaker_params.c @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * 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 circuit_breaker.c + * + * Circuit breaker parameters. + * Analog to real aviation circuit breakers these parameters + * allow to disable subsystems. They are not supported as standard + * operation procedure and are only provided for development purposes. + * To ensure they are not activated accidentally, the associated + * parameter needs to set to the key (magic). + */ + +#include +#include + +/** + * Circuit breaker for power supply check + * + * Setting this parameter to 894281 will disable the power valid + * checks in the commander. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 894281 + * @group Circuit Breaker + */ +PX4_PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK); + +/** + * Circuit breaker for rate controller output + * + * Setting this parameter to 140253 will disable the rate + * controller uORB publication. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 140253 + * @group Circuit Breaker + */ +PX4_PARAM_DEFINE_INT32(CBRK_RATE_CTRL); + +/** + * Circuit breaker for IO safety + * + * Setting this parameter to 894281 will disable IO safety. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 22027 + * @group Circuit Breaker + */ +PX4_PARAM_DEFINE_INT32(CBRK_IO_SAFETY); + +/** + * Circuit breaker for airspeed sensor + * + * Setting this parameter to 162128 will disable the check for an airspeed sensor. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 162128 + * @group Circuit Breaker + */ +PX4_PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK); + +/** + * Circuit breaker for flight termination + * + * Setting this parameter to 121212 will disable the flight termination action. + * --> The IO driver will not do flight termination if requested by the FMU + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 121212 + * @group Circuit Breaker + */ +PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM); + +/** + * Circuit breaker for engine failure detection + * + * Setting this parameter to 284953 will disable the engine failure detection. + * If the aircraft is in engine failure mode the enine failure flag will be + * set to healthy + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 284953 + * @group Circuit Breaker + */ +PX4_PARAM_DEFINE_INT32(CBRK_ENGINEFAIL); diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index fe8b7e75a..1e0a9c007 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -53,7 +53,8 @@ SRCS = err.c \ otp.c \ board_serial.c \ pwm_limit/pwm_limit.c \ - circuit_breaker.c \ + circuit_breaker.cpp \ + circuit_breaker_params.c \ mcu_version.c MAXOPTIMIZATION = -Os diff --git a/src/platforms/ros/circuit_breaker.cpp b/src/platforms/ros/circuit_breaker.cpp deleted file mode 100644 index 1e912d3ac..000000000 --- a/src/platforms/ros/circuit_breaker.cpp +++ /dev/null @@ -1,56 +0,0 @@ -/**************************************************************************** - * - * 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 circuit_breaker.c - * - * Circuit breaker parameters. - * Analog to real aviation circuit breakers these parameters - * allow to disable subsystems. They are not supported as standard - * operation procedure and are only provided for development purposes. - * To ensure they are not activated accidentally, the associated - * parameter needs to set to the key (magic). - */ - -#include -#include -#include - -bool circuit_breaker_enabled(const char* breaker, int32_t magic) -{ - int32_t val; - (void)PX4_PARAM_GET(breaker, &val); - - return (val == magic); -} - -- cgit v1.2.3 From 4402d7106bc63b2a02a1e4c22f54e072b3c48fc7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 17 Dec 2014 16:31:40 +0100 Subject: improve and fix multiplatform param by name macro --- src/modules/systemlib/circuit_breaker.cpp | 3 +-- src/platforms/px4_defines.h | 4 ++-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/systemlib/circuit_breaker.cpp b/src/modules/systemlib/circuit_breaker.cpp index 2df13ebff..ea478a60f 100644 --- a/src/modules/systemlib/circuit_breaker.cpp +++ b/src/modules/systemlib/circuit_breaker.cpp @@ -48,8 +48,7 @@ bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; - /* (void)param_get(param_find(breaker), &val); */ - (void)PX4_PARAM_GET_NAME(breaker, &val); + (void)PX4_PARAM_GET_BYNAME(breaker, &val); return (val == magic); } diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 188149008..c1a0782c4 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -97,7 +97,7 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) /* Get value of parameter by name, which is equal to the handle for ros */ -#define PX4_PARAM_GET_NAME(_name, _destpt) PX4_PARAM_GET(_name, _destpt) +#define PX4_PARAM_GET_BYNAME(_name, _destpt) PX4_PARAM_GET(_name, _destpt) #define OK 0 #define ERROR -1 @@ -170,7 +170,7 @@ typedef param_t px4_param_t; #define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt) /* Get value of parameter by name */ -#define PX4_PARAM_GET_NAME(_name, _destpt) param_get(PX4_PARAM_INIT(_name), _destpt) +#define PX4_PARAM_GET_BYNAME(_name, _destpt) param_get(param_find(_name), _destpt) /* XXX this is a hack to resolve conflicts with NuttX headers */ #define isspace(c) \ -- cgit v1.2.3 From e697413667579ffb1c5900193a03da257c05d11a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 18 Dec 2014 12:11:35 +0100 Subject: multiplatform subscription: uorb: separate class for no callback case --- src/platforms/px4_nodehandle.h | 2 +- src/platforms/px4_subscriber.h | 61 +++++++++++++++++++++++++++--------------- 2 files changed, 41 insertions(+), 22 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 160886810..97fb25563 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -168,7 +168,7 @@ public: std::function callback, unsigned interval) { - SubscriberUORB *sub_px4 = new SubscriberUORB(meta, interval, callback, &_subs); + SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(meta, interval, callback, &_subs); /* Check if this is the smallest interval so far and update _sub_min_interval */ if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 5d4e67ad0..8c299c748 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -161,46 +161,72 @@ class SubscriberUORB : public uORB::Subscription { public: + /** - * Construct SubscriberUORB by providing orb meta data + * Construct SubscriberUORB by providing orb meta data without callback * @param meta orb metadata for the topic which is used - * @param callback Callback, executed on receiving a new message * @param interval Minimal interval between calls to callback * @param list subscriber is added to this list */ SubscriberUORB(const struct orb_metadata *meta, unsigned interval, - std::function callback, List *list) : Subscriber(), - uORB::Subscription(meta, interval, list), - _callback(callback) - //XXX store callback + uORB::Subscription(meta, interval, list) {} + ~SubscriberUORB() {}; + /** - * Construct SubscriberUORB by providing orb meta data without callback + * Update Subscription + * Invoked by the list traversal in NodeHandle::spinOnce + */ + virtual void update() + { + if (!uORB::Subscription::updated()) { + /* Topic not updated */ + return; + } + + /* get latest data */ + uORB::Subscription::update(); + }; + + /* Accessors*/ + /** + * Get the last message value + */ + const M& get() { return uORB::Subscription::getData(); } +}; + +template +class SubscriberUORBCallback : + public SubscriberUORB +{ +public: + /** + * Construct SubscriberUORBCallback by providing orb meta data * @param meta orb metadata for the topic which is used + * @param callback Callback, executed on receiving a new message * @param interval Minimal interval between calls to callback * @param list subscriber is added to this list */ - SubscriberUORB(const struct orb_metadata *meta, + SubscriberUORBCallback(const struct orb_metadata *meta, unsigned interval, + std::function callback, List *list) : - Subscriber(), - uORB::Subscription(meta, interval, list), - _callback(nullptr) - //XXX store callback + SubscriberUORB(meta, interval, list), + _callback(callback) {} - ~SubscriberUORB() {}; + ~SubscriberUORBCallback() {}; /** * Update Subscription * Invoked by the list traversal in NodeHandle::spinOnce * If new data is available the callback is called */ - void update() + virtual void update() { if (!uORB::Subscription::updated()) { /* Topic not updated, do not call callback */ @@ -221,16 +247,9 @@ public: }; - /* Accessors*/ - /** - * Get the last message value - */ - const M& get() { return uORB::Subscription::getData(); } - protected: std::function _callback; /**< Callback handle, called when new data is available */ - }; #endif -- cgit v1.2.3 From ad189cf7d69b8de16244b90d398e1d84ed6d0f4b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 19 Dec 2014 16:02:13 +0100 Subject: fix small compile error after merge --- src/modules/fw_att_control/fw_att_control_main.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 53e854ae5..0025aae7f 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -744,15 +744,15 @@ FixedwingAttitudeControl::task_main() _att.roll = euler_angles(0); _att.pitch = euler_angles(1); _att.yaw = euler_angles(2); - _att.R[0][0] = R_adapted(0, 0); - _att.R[0][1] = R_adapted(0, 1); - _att.R[0][2] = R_adapted(0, 2); - _att.R[1][0] = R_adapted(1, 0); - _att.R[1][1] = R_adapted(1, 1); - _att.R[1][2] = R_adapted(1, 2); - _att.R[2][0] = R_adapted(2, 0); - _att.R[2][1] = R_adapted(2, 1); - _att.R[2][2] = R_adapted(2, 2); + PX4_R(_att.R, 0, 0) = R_adapted(0, 0); + PX4_R(_att.R, 0, 1) = R_adapted(0, 1); + PX4_R(_att.R, 0, 2) = R_adapted(0, 2); + PX4_R(_att.R, 1, 0) = R_adapted(1, 0); + PX4_R(_att.R, 1, 1) = R_adapted(1, 1); + PX4_R(_att.R, 1, 2) = R_adapted(1, 2); + PX4_R(_att.R, 2, 0) = R_adapted(2, 0); + PX4_R(_att.R, 2, 1) = R_adapted(2, 1); + PX4_R(_att.R, 2, 2) = R_adapted(2, 2); // lastly, roll- and yawspeed have to be swaped float helper = _att.rollspeed; @@ -850,7 +850,7 @@ FixedwingAttitudeControl::task_main() _yaw_ctrl.reset_integrator(); } } else if (_vcontrol_mode.flag_control_velocity_enabled) { - /* + /* * Velocity should be controlled and manual is enabled. */ roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) -- cgit v1.2.3 From e31e143047712aa2f43a4a900a0a1210082a36cf Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 25 Dec 2014 10:00:09 +0100 Subject: fix include --- src/platforms/px4_includes.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 6c36b692a..9c8c24764 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -59,7 +59,7 @@ #include #include #include -#include +#include #endif #else -- cgit v1.2.3 From 4567512d7a341659bcb19956d58201aeaf0871b5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 25 Dec 2014 10:01:20 +0100 Subject: add actuator_controls_virtual_mc as msg, fix msg setup in CMakeList --- CMakeLists.txt | 2 ++ msg/actuator_controls_virtual_mc.msg | 4 ++++ 2 files changed, 6 insertions(+) create mode 100644 msg/actuator_controls_virtual_mc.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 3184bb2ac..dfcbd4adc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -56,11 +56,13 @@ add_message_files( manual_control_setpoint.msg actuator_controls.msg actuator_controls_0.msg + actuator_controls_virtual_mc.msg vehicle_rates_setpoint.msg vehicle_attitude.msg vehicle_control_mode.msg actuator_armed.msg parameter_update.msg + vehicle_status.msg ) ## Generate services in the 'srv' folder diff --git a/msg/actuator_controls_virtual_mc.msg b/msg/actuator_controls_virtual_mc.msg new file mode 100644 index 000000000..743f20cdf --- /dev/null +++ b/msg/actuator_controls_virtual_mc.msg @@ -0,0 +1,4 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +float32[8] control -- cgit v1.2.3 From 382158d87b170e2b1848928c745073c209ac64f3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 25 Dec 2014 10:03:46 +0100 Subject: fix merge error in .gitmodules --- .gitmodules | 3 --- 1 file changed, 3 deletions(-) diff --git a/.gitmodules b/.gitmodules index 577181281..d0039b38d 100644 --- a/.gitmodules +++ b/.gitmodules @@ -13,6 +13,3 @@ [submodule "Tools/gencpp"] path = Tools/gencpp url = https://github.com/ros/gencpp.git -[submodule "src/lib/uavcan"] - path = src/lib/uavcan - url = git://github.com/pavel-kirienko/uavcan.git -- cgit v1.2.3 From ef61ba5454153c8cff21345f4930203cc22505c7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 25 Dec 2014 10:43:19 +0100 Subject: fix compile/merge error in mc_att_control --- src/modules/mc_att_control/mc_att_control.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp index 5dfe18cf2..66ceb6d77 100644 --- a/src/modules/mc_att_control/mc_att_control.cpp +++ b/src/modules/mc_att_control/mc_att_control.cpp @@ -224,7 +224,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi if (_v_rates_sp_pub != nullptr) { _v_rates_sp_pub->publish(_v_rates_sp_mod); } else { - if (_v_status->get()._is_vtol) { + if (_v_status->get().is_vtol) { _v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint); } else { _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint); @@ -253,7 +253,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi _v_rates_sp_pub->publish(_v_rates_sp_mod); } else { - if (_v_status->get()._is_vtol) { + if (_v_status->get().is_vtol) { _v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint); } else { _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint); @@ -284,7 +284,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi _actuators_0_pub->publish(_actuators); } else { - if (_v_status()->get()._is_vtol) { + if (_v_status->get().is_vtol) { _actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_virtual_mc); } else { _actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_0); -- cgit v1.2.3 From a93362c70013281457fb44ab0494157325eb28f9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 25 Dec 2014 11:50:41 +0100 Subject: re-add accidentally removed parameter init --- src/modules/mc_att_control/mc_att_control.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp index 66ceb6d77..822a504b5 100644 --- a/src/modules/mc_att_control/mc_att_control.cpp +++ b/src/modules/mc_att_control/mc_att_control.cpp @@ -98,6 +98,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.acro_pitch_max = PX4_PARAM_INIT(MC_ACRO_P_MAX); _params_handles.acro_yaw_max = PX4_PARAM_INIT(MC_ACRO_Y_MAX); + /* fetch initial parameter values */ + parameters_update(); + /* * do subscriptions */ -- cgit v1.2.3 From 195472fddf0242bfb881aef9e53d910c81a2b003 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 25 Dec 2014 12:51:38 +0100 Subject: fix init of published structs --- src/modules/mc_att_control/mc_att_control_base.cpp | 8 ++++---- src/platforms/px4_subscriber.h | 12 ++++++++++++ 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index d7fbf3acd..2315e0a99 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -58,8 +58,8 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : _publish_att_sp(false) { - memset(&_v_rates_sp, 0, sizeof(_v_att_sp_mod)); - memset(&_v_rates_sp, 0, sizeof(_v_rates_sp_mod)); + memset(&_v_att_sp_mod, 0, sizeof(_v_att_sp_mod)); + memset(&_v_rates_sp_mod, 0, sizeof(_v_rates_sp_mod)); memset(&_actuators, 0, sizeof(_actuators)); _params.att_p.zero(); @@ -98,7 +98,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) if (_v_control_mode->get().flag_control_velocity_enabled || _v_control_mode->get().flag_control_climb_rate_enabled) { /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ - _v_att_sp_mod = _v_att_sp->get(); + memcpy(&_v_att_sp_mod, _v_att_sp->get_void_ptr(), sizeof(_v_att_sp_mod)); } if (!_v_control_mode->get().flag_control_climb_rate_enabled) { @@ -157,7 +157,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) } else { /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ - _v_att_sp_mod = _v_att_sp->get(); + memcpy(&_v_att_sp_mod, _v_att_sp->get_void_ptr(), sizeof(_v_att_sp_mod)); /* reset yaw setpoint after non-manual control mode */ _reset_yaw_sp = true; diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 8c299c748..e7e344201 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -80,6 +80,10 @@ public: * Get the last message value */ virtual const M& get() = 0; + /** + * Get void pointer to last message value + */ + virtual void * get_void_ptr() = 0; }; #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) @@ -121,6 +125,10 @@ public: * Get the last message value */ const M& get() { return _msg_current; } + /** + * Get void pointer to last message value + */ + void * get_void_ptr() { return (void*)&_msg_current; } protected: /** @@ -197,6 +205,10 @@ public: * Get the last message value */ const M& get() { return uORB::Subscription::getData(); } + /** + * Get void pointer to last message value + */ + void * get_void_ptr() { return uORB::Subscription::getDataVoidPtr(); } }; template -- cgit v1.2.3 From acb4844939f971a1a33515316f5e6c7c8f668f12 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 25 Dec 2014 18:21:24 +0100 Subject: don't publish att sp in altctl because it can mask the setpoint from the pos controller --- src/modules/mc_att_control/mc_att_control_base.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index 2315e0a99..aff59778e 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -135,7 +135,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) } _v_att_sp_mod.R_valid = false; - _publish_att_sp = true; + // _publish_att_sp = true; } /* reset yaw setpint to current position if needed */ @@ -143,7 +143,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) _reset_yaw_sp = false; _v_att_sp_mod.yaw_body = _v_att->get().yaw; _v_att_sp_mod.R_valid = false; - _publish_att_sp = true; + // _publish_att_sp = true; } if (!_v_control_mode->get().flag_control_velocity_enabled) { @@ -152,7 +152,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) _v_att_sp_mod.pitch_body = -_manual_control_sp->get().x * _params.man_pitch_max; _v_att_sp_mod.R_valid = false; - _publish_att_sp = true; + // _publish_att_sp = true; } } else { -- cgit v1.2.3 From ac8b47b0c32d41cb4cf0f03d996bc8a4c77a0f49 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 29 Dec 2014 09:49:53 +0100 Subject: add missing msg and includes --- CMakeLists.txt | 1 + msg/mc_virtual_rates_setpoint.msg | 6 ++++++ src/platforms/px4_includes.h | 2 ++ 3 files changed, 9 insertions(+) create mode 100644 msg/mc_virtual_rates_setpoint.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index dfcbd4adc..519afa0e2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -58,6 +58,7 @@ add_message_files( actuator_controls_0.msg actuator_controls_virtual_mc.msg vehicle_rates_setpoint.msg + mc_virtual_rates_setpoint.msg vehicle_attitude.msg vehicle_control_mode.msg actuator_armed.msg diff --git a/msg/mc_virtual_rates_setpoint.msg b/msg/mc_virtual_rates_setpoint.msg new file mode 100644 index 000000000..834113c79 --- /dev/null +++ b/msg/mc_virtual_rates_setpoint.msg @@ -0,0 +1,6 @@ +uint64 timestamp # in microseconds since system start + +float32 roll # body angular rates in NED frame +float32 pitch # body angular rates in NED frame +float32 yaw # body angular rates in NED frame +float32 thrust # thrust normalized to 0..1 diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 9c8c24764..4d29bc3cd 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -54,7 +54,9 @@ #include #include #include +#include #include +#include #include #include #include -- cgit v1.2.3 From 9d30c297f40b06779db12e44f2068b482a7fae64 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 29 Dec 2014 10:17:25 +0100 Subject: add ros launch files --- launch/example.launch | 8 ++++++++ launch/multicopter.launch | 7 +++++++ 2 files changed, 15 insertions(+) create mode 100644 launch/example.launch create mode 100644 launch/multicopter.launch diff --git a/launch/example.launch b/launch/example.launch new file mode 100644 index 000000000..39da4b3d4 --- /dev/null +++ b/launch/example.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/launch/multicopter.launch b/launch/multicopter.launch new file mode 100644 index 000000000..57b7653d5 --- /dev/null +++ b/launch/multicopter.launch @@ -0,0 +1,7 @@ + + + + + + + -- cgit v1.2.3 From b04c80aec7ee91820e5ffa5a09e40aa30b285bc7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 29 Dec 2014 10:35:46 +0100 Subject: mc att: fix init call --- src/modules/mc_att_control/mc_att_control_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 4188c7880..080f0ed65 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -80,8 +80,6 @@ void handle_vehicle_attitude2(const PX4_TOPIC_T(rc_channels) &msg) { extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]) { - px4::init(argc, argv, "mc_att_control"); - if (argc < 1) { errx(1, "usage: mc_att_control {start|stop|status}"); } @@ -129,6 +127,8 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]) PX4_MAIN_FUNCTION(mc_att_control) { + px4::init(argc, argv, "mc_att_control"); + PX4_INFO("starting"); MulticopterAttitudeControl attctl; thread_running = true; -- cgit v1.2.3 From 2c12a524de2c16b33722d2d128515cfd21367b4d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 29 Dec 2014 11:57:45 +0100 Subject: dummy estimator skeleton code --- CMakeLists.txt | 9 ++++ .../ros/nodes/att_estimator/att_estimator.cpp | 61 ++++++++++++++++++++++ 2 files changed, 70 insertions(+) create mode 100644 src/platforms/ros/nodes/att_estimator/att_estimator.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 519afa0e2..5e0f9b29c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -173,6 +173,15 @@ target_link_libraries(mc_att_control px4 ) +## Attitude Estimator dummy +add_executable(att_estimator + src/platforms/ros/nodes/att_estimator/att_estimator.cpp) +add_dependencies(att_estimator px4_generate_messages_cpp) +target_link_libraries(att_estimator + ${catkin_LIBRARIES} + px4 +) + ############# ## Install ## diff --git a/src/platforms/ros/nodes/att_estimator/att_estimator.cpp b/src/platforms/ros/nodes/att_estimator/att_estimator.cpp new file mode 100644 index 000000000..986412a1d --- /dev/null +++ b/src/platforms/ros/nodes/att_estimator/att_estimator.cpp @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * 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 att_estimator.cpp + * Dummy attitude estimator that forwards attitude from gazebo to px4 topic + * + * @author Thomas Gubler +*/ + +#include "ros/ros.h" +#include "std_msgs/String.h" + +/** + * This tutorial demonstrates simple receipt of messages over the ROS system. + */ +void chatterCallback(const std_msgs::String::ConstPtr& msg) +{ + ROS_INFO("I heard: [%s]", msg->data.c_str()); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "att_estimator"); + ros::NodeHandle n; + ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback); + + ros::spin(); + + return 0; +} -- cgit v1.2.3 From 3e941a2d1f4919f05beee0672211efb35bfb816d Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 29 Dec 2014 14:03:37 +0100 Subject: progress on att_estimator node --- src/platforms/ros/nodes/att_estimator/att_estimator.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/platforms/ros/nodes/att_estimator/att_estimator.cpp b/src/platforms/ros/nodes/att_estimator/att_estimator.cpp index 986412a1d..fa021fff0 100644 --- a/src/platforms/ros/nodes/att_estimator/att_estimator.cpp +++ b/src/platforms/ros/nodes/att_estimator/att_estimator.cpp @@ -39,14 +39,15 @@ */ #include "ros/ros.h" -#include "std_msgs/String.h" +#include +//#include "std_msgs/String.h" /** * This tutorial demonstrates simple receipt of messages over the ROS system. */ -void chatterCallback(const std_msgs::String::ConstPtr& msg) +void chatterCallback(const gazebo_msgs::ModelStates &msg) { - ROS_INFO("I heard: [%s]", msg->data.c_str()); + // try to read out message here } int main(int argc, char **argv) -- cgit v1.2.3 From 1c6da49e3f2ea805d6b0a228dd59d95a24d70d9c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 29 Dec 2014 14:11:33 +0100 Subject: add gazebo_msgs --- CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5e0f9b29c..7893c28b1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS std_msgs message_generation cmake_modules + gazebo_msgs ) find_package(Eigen REQUIRED) @@ -84,6 +85,7 @@ add_message_files( generate_messages( DEPENDENCIES std_msgs + gazebo_msgs ) ################################### -- cgit v1.2.3 From c9b0dfaaa118e6ec42fb6384e35a40299a078079 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Dec 2014 08:17:59 +0100 Subject: skeleton code for manual input node --- CMakeLists.txt | 10 +++ launch/multicopter.launch | 2 + .../ros/nodes/manual_input/manual_input.cpp | 71 ++++++++++++++++++++++ .../ros/nodes/manual_input/manual_input.h | 61 +++++++++++++++++++ 4 files changed, 144 insertions(+) create mode 100644 src/platforms/ros/nodes/manual_input/manual_input.cpp create mode 100644 src/platforms/ros/nodes/manual_input/manual_input.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 7893c28b1..258868027 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS message_generation cmake_modules gazebo_msgs + sensor_msgs ) find_package(Eigen REQUIRED) @@ -184,6 +185,15 @@ target_link_libraries(att_estimator px4 ) +## Manual input +add_executable(manual_input + src/platforms/ros/nodes/manual_input/manual_input.cpp) +add_dependencies(manual_input px4_generate_messages_cpp) +target_link_libraries(manual_input + ${catkin_LIBRARIES} + px4 +) + ############# ## Install ## diff --git a/launch/multicopter.launch b/launch/multicopter.launch index 57b7653d5..5db0e6415 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -1,6 +1,8 @@ + + diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp new file mode 100644 index 000000000..531ffa640 --- /dev/null +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * 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 manual_input.cpp + * Reads from the ros joystick topic and publishes to the px4 manual control input topic. + * + * @author Thomas Gubler +*/ + +#include "manual_input.h" + +#include + +ManualInput::ManualInput() : + _n(), + _sub_joy(_n.subscribe("joy", 10, &ManualInput::JoyCallback, this)), + _man_ctrl_sp_pub(_n.advertise("manual_control_setpoint", 1000)) +{ +} + + +void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg) +{ + px4::manual_control_setpoint msg_out; + + /* Fill px4 manual control setpoint topic with contents from ros joystick */ + //XXX + + _man_ctrl_sp_pub.publish(msg_out); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "manual_input"); + ManualInput m; + + ros::spin(); + + return 0; +} diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h new file mode 100644 index 000000000..1721d5e81 --- /dev/null +++ b/src/platforms/ros/nodes/manual_input/manual_input.h @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * 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 manual_input.h + * Reads from the ros joystick topic and publishes to the px4 manual control setpoint topic. + * + * @author Thomas Gubler +*/ + +#include "ros/ros.h" +#include + +class ManualInput { +public: + ManualInput(); + + ~ManualInput() {} + +protected: + /** + * Takes ROS joystick message and converts/publishes to px4 manual control setpoint topic + */ + void JoyCallback(const sensor_msgs::JoyConstPtr& msg); + + ros::NodeHandle _n; + ros::Subscriber _sub_joy; + ros::Publisher _man_ctrl_sp_pub; + + +}; -- cgit v1.2.3 From b632d128aa513c4602ec3da22ccfff937d8a5e7f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Dec 2014 08:22:18 +0100 Subject: add ros helper nodes readme --- src/platforms/ros/nodes/README.md | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 src/platforms/ros/nodes/README.md diff --git a/src/platforms/ros/nodes/README.md b/src/platforms/ros/nodes/README.md new file mode 100644 index 000000000..277f57bfd --- /dev/null +++ b/src/platforms/ros/nodes/README.md @@ -0,0 +1,4 @@ +# PX4 Nodes + +This directory contains several small ROS nodes which are intended to run alongside the PX4 multi-platform nodes in +ROS. They act as a bridge between the PX4 specific topics and the ROS topics. -- cgit v1.2.3 From 1f8fd5d1207512711c47859e06ac23c340be1551 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Dec 2014 09:13:20 +0100 Subject: new dummy attitude estimator skeleton --- CMakeLists.txt | 8 +-- launch/multicopter.launch | 2 +- .../ros/nodes/att_estimator/att_estimator.cpp | 62 ------------------- .../attitude_estimator/attitude_estimator.cpp | 70 ++++++++++++++++++++++ .../nodes/attitude_estimator/attitude_estimator.h | 61 +++++++++++++++++++ .../ros/nodes/manual_input/manual_input.cpp | 3 +- 6 files changed, 137 insertions(+), 69 deletions(-) delete mode 100644 src/platforms/ros/nodes/att_estimator/att_estimator.cpp create mode 100644 src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp create mode 100644 src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 258868027..d506cadb1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -177,10 +177,10 @@ target_link_libraries(mc_att_control ) ## Attitude Estimator dummy -add_executable(att_estimator - src/platforms/ros/nodes/att_estimator/att_estimator.cpp) -add_dependencies(att_estimator px4_generate_messages_cpp) -target_link_libraries(att_estimator +add_executable(attitude_estimator + src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp) +add_dependencies(attitude_estimator px4_generate_messages_cpp) +target_link_libraries(attitude_estimator ${catkin_LIBRARIES} px4 ) diff --git a/launch/multicopter.launch b/launch/multicopter.launch index 5db0e6415..8cc89450d 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -2,7 +2,7 @@ - + diff --git a/src/platforms/ros/nodes/att_estimator/att_estimator.cpp b/src/platforms/ros/nodes/att_estimator/att_estimator.cpp deleted file mode 100644 index fa021fff0..000000000 --- a/src/platforms/ros/nodes/att_estimator/att_estimator.cpp +++ /dev/null @@ -1,62 +0,0 @@ -/**************************************************************************** - * - * 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 att_estimator.cpp - * Dummy attitude estimator that forwards attitude from gazebo to px4 topic - * - * @author Thomas Gubler -*/ - -#include "ros/ros.h" -#include -//#include "std_msgs/String.h" - -/** - * This tutorial demonstrates simple receipt of messages over the ROS system. - */ -void chatterCallback(const gazebo_msgs::ModelStates &msg) -{ - // try to read out message here -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "att_estimator"); - ros::NodeHandle n; - ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback); - - ros::spin(); - - return 0; -} diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp new file mode 100644 index 000000000..7504091eb --- /dev/null +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * 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 att_estimator.cpp + * Dummy attitude estimator that forwards attitude from gazebo to px4 topic + * + * @author Thomas Gubler +*/ + +#include "attitude_estimator.h" + +#include + +AttitudeEstimator::AttitudeEstimator() : + _n(), + _sub_modelstates(_n.subscribe("joy", 10, &AttitudeEstimator::ModelStatesCallback, this)), + _vehicle_attitude_pub(_n.advertise("vehicle_attitude", 10)) +{ +} + +void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr& msg) +{ + px4::vehicle_attitude msg_out; + + /* Fill px4 attitude topic with contents from modelstates topic */ + //XXX + + _vehicle_attitude_pub.publish(msg_out); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "attitude_estimator"); + AttitudeEstimator m; + + ros::spin(); + + return 0; +} diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h new file mode 100644 index 000000000..e3326b715 --- /dev/null +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * 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 att_estimator.h + * Dummy attitude estimator that forwards attitude from gazebo to px4 topic + * + * @author Thomas Gubler +*/ + +#include "ros/ros.h" +#include + +class AttitudeEstimator { +public: + AttitudeEstimator(); + + ~AttitudeEstimator() {} + +protected: + /** + * Takes ROS joystick message and converts/publishes to px4 manual control setpoint topic + */ + void ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr& msg); + + ros::NodeHandle _n; + ros::Subscriber _sub_modelstates; + ros::Publisher _vehicle_attitude_pub; + + +}; diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index 531ffa640..41e597ab7 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.cpp +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -45,11 +45,10 @@ ManualInput::ManualInput() : _n(), _sub_joy(_n.subscribe("joy", 10, &ManualInput::JoyCallback, this)), - _man_ctrl_sp_pub(_n.advertise("manual_control_setpoint", 1000)) + _man_ctrl_sp_pub(_n.advertise("manual_control_setpoint", 10)) { } - void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg) { px4::manual_control_setpoint msg_out; -- cgit v1.2.3 From 4debf45e06ada59a5675f7fa2bc180b829464c02 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Dec 2014 09:58:30 +0100 Subject: add joystick to launch file --- launch/multicopter.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/launch/multicopter.launch b/launch/multicopter.launch index 8cc89450d..e54ebb3e2 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -1,6 +1,7 @@ + -- cgit v1.2.3 From 27203beaa7b9c9110d93951ad4c068164d96f866 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Dec 2014 09:58:45 +0100 Subject: add some info in README about joystick --- src/platforms/ros/nodes/README.md | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/platforms/ros/nodes/README.md b/src/platforms/ros/nodes/README.md index 277f57bfd..aafc647ff 100644 --- a/src/platforms/ros/nodes/README.md +++ b/src/platforms/ros/nodes/README.md @@ -2,3 +2,21 @@ This directory contains several small ROS nodes which are intended to run alongside the PX4 multi-platform nodes in ROS. They act as a bridge between the PX4 specific topics and the ROS topics. + +## Joystick Input + +You will need to install the ros joystick packages +See http://wiki.ros.org/joy/Tutorials/ConfiguringALinuxJoystick + +### Arch Linux +```sh +yaourt -Sy ros-indigo-joystick-drivers --noconfirm +``` +check joystick +```sh +ls /dev/input/ +ls -l /dev/input/js0 +``` +(replace 0 by the number you find with the first command) + +make sure the joystick is accessible by the `input` group and that your user is in the `input` group -- cgit v1.2.3 From 4c511210793c7b2479291bb2abb1de7ce1362b9e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Dec 2014 10:19:33 +0100 Subject: add example ROS_INFO --- src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp index 7504091eb..e64ba335d 100644 --- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp @@ -44,7 +44,7 @@ AttitudeEstimator::AttitudeEstimator() : _n(), - _sub_modelstates(_n.subscribe("joy", 10, &AttitudeEstimator::ModelStatesCallback, this)), + _sub_modelstates(_n.subscribe("gazebo/model_states", 10, &AttitudeEstimator::ModelStatesCallback, this)), _vehicle_attitude_pub(_n.advertise("vehicle_attitude", 10)) { } @@ -54,6 +54,7 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP px4::vehicle_attitude msg_out; /* Fill px4 attitude topic with contents from modelstates topic */ + ROS_INFO("Test x: %.4f", msg->pose[0].orientation.x); //XXX _vehicle_attitude_pub.publish(msg_out); -- cgit v1.2.3 From 67c49303150b61403ad49018f617d870b703221e Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 30 Dec 2014 10:21:21 +0100 Subject: added mixer node --- src/platforms/ros/nodes/mc_mixer.cpp | 72 ++++++++++++++++++++++++++++++++++++ 1 file changed, 72 insertions(+) create mode 100644 src/platforms/ros/nodes/mc_mixer.cpp diff --git a/src/platforms/ros/nodes/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer.cpp new file mode 100644 index 000000000..ddafde3b9 --- /dev/null +++ b/src/platforms/ros/nodes/mc_mixer.cpp @@ -0,0 +1,72 @@ +e/**************************************************************************** + * + * 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 att_estimator.cpp + * Dummy attitude estimator that forwards attitude from gazebo to px4 topic + * + * @author Roman Bapst +*/ + #include "ros/ros.h" + + struct { + float control[8]; + }inputs; + + struct { + float control[8]; + }outputs; + +void mix(void *input) { + + +} + + void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg) +{ + // read message + memcpy(inputs,msg,sizeof(inputs)); + // mix + +} + + int main(int argc, char **argv) +{ + ros::init(argc, argv, "mc_mixer"); + ros::NodeHandle n; + ros::Subscriber sub = n.subscribe("actuator_controls_0", 1000, actuatorControlsCallback); + + ros::spin(); + + return 0; +} -- cgit v1.2.3 From 484020177d947bf308466cb0bd5342cee0dd014a Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 30 Dec 2014 11:06:40 +0100 Subject: further progress on mixer node --- CMakeLists.txt | 9 ++++ src/platforms/ros/nodes/mc_mixer.cpp | 101 +++++++++++++++++++++++++++++++++-- 2 files changed, 106 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5e0f9b29c..57d1bc440 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -182,6 +182,15 @@ target_link_libraries(att_estimator px4 ) +## Multicopter Mixer dummy +add_executable(mc_mixer + src/platforms/ros/nodes/mc_mixer.cpp) +add_dependencies(mc_mixer px4_generate_messages_cpp) +target_link_libraries(mc_mixer + ${catkin_LIBRARIES} + px4 +) + ############# ## Install ## diff --git a/src/platforms/ros/nodes/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer.cpp index ddafde3b9..a131440a8 100644 --- a/src/platforms/ros/nodes/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer.cpp @@ -1,4 +1,4 @@ -e/**************************************************************************** +/**************************************************************************** * * Copyright (c) 2014 PX4 Development Team. All rights reserved. * @@ -37,7 +37,19 @@ e/**************************************************************************** * * @author Roman Bapst */ - #include "ros/ros.h" + #include + #include + #include + #include + + #define _rotor_count 4 + + struct Rotor { + float roll_scale; + float pitch_scale; + float yaw_scale; +}; + struct { float control[8]; @@ -47,16 +59,96 @@ e/**************************************************************************** float control[8]; }outputs; -void mix(void *input) { +const Rotor _config_x[] = { + { 0.000000, -1.000000, -1.00 }, + { -0.000000, 1.000000, -1.00 }, + { 1.000000, 0.000000, 1.00 }, + { -1.000000, 0.000000, 1.00 }, +}; + + const Rotor *_rotors = { &_config_x[0] + + }; + + +void mix() { + float roll = math::constrain(inputs.control[0], -1.0f, 1.0f); + float pitch = math::constrain(inputs.control[1], -1.0f, 1.0f); + float yaw = math::constrain(inputs.control[2], -1.0f, 1.0f); + float thrust = math::constrain(inputs.control[3], 0.0f, 1.0f); + float min_out = 0.0f; + float max_out = 0.0f; + + /* perform initial mix pass yielding unbounded outputs, ignore yaw */ + for (unsigned i = 0; i < _rotor_count; i++) { + float out = roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale + thrust; + /* limit yaw if it causes outputs clipping */ + if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) { + yaw = -out / _rotors[i].yaw_scale; + } + + /* calculate min and max output values */ + if (out < min_out) { + min_out = out; + } + if (out > max_out) { + max_out = out; + } + + outputs.control[i] = out; + } + /* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */ + if (min_out < 0.0f) { + float scale_in = thrust / (thrust - min_out); + + /* mix again with adjusted controls */ + for (unsigned i = 0; i < _rotor_count; i++) { + outputs.control[i] = scale_in + * (roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale) + thrust; + } + + } else { + /* roll/pitch mixed without limiting, add yaw control */ + for (unsigned i = 0; i < _rotor_count; i++) { + outputs.control[i] += yaw * _rotors[i].yaw_scale; + } + } + + /* scale down all outputs if some outputs are too large, reduce total thrust */ + float scale_out; + if (max_out > 1.0f) { + scale_out = 1.0f / max_out; + + } else { + scale_out = 1.0f; + } + + /* scale outputs to range _idle_speed..1, and do final limiting */ + for (unsigned i = 0; i < _rotor_count; i++) { + outputs.control[i] = math::constrain(outputs.control[i], 0.0f, 1.0f); + } } void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg) { // read message - memcpy(inputs,msg,sizeof(inputs)); + for(int i = 0;i < msg.NUM_ACTUATOR_CONTROLS;i++) { + inputs.control[i] = msg.control[i]; + } + // mix + mix(); + + // publish message + mav_msgs::MotorSpeed rotor_vel_msg; + for (int i = 0; i < _rotor_count; i++) { + rotor_vel_msg.motor_speed.push_back(outputs.control[i]); + } + pub.publish(rotor_vel_msg); } @@ -65,6 +157,7 @@ void mix(void *input) { ros::init(argc, argv, "mc_mixer"); ros::NodeHandle n; ros::Subscriber sub = n.subscribe("actuator_controls_0", 1000, actuatorControlsCallback); + ros::Publisher pub = n.advertise("mixed_motor_commands",10); ros::spin(); -- cgit v1.2.3 From 824446bf2f91d76743d9c8c17d710d4ac51bfaf8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Dec 2014 11:15:46 +0100 Subject: typo --- src/platforms/px4_defines.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index c1a0782c4..d6d23d013 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -78,7 +78,7 @@ /* Parameter handle datatype */ typedef const char *px4_param_t; -/* Helper fucntions to set ROS params, only int and float supported */ +/* Helper functions to set ROS params, only int and float supported */ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value) { ros::param::set(name, value); -- cgit v1.2.3 From c01680459409d0119e72becc48d497fdf59e7023 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Dec 2014 11:16:53 +0100 Subject: manual input: map axes --- .../ros/nodes/manual_input/manual_input.cpp | 27 +++++++++++++++++++++- .../ros/nodes/manual_input/manual_input.h | 10 ++++++++ 2 files changed, 36 insertions(+), 1 deletion(-) diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index 41e597ab7..8fc158504 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.cpp +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -47,6 +47,22 @@ ManualInput::ManualInput() : _sub_joy(_n.subscribe("joy", 10, &ManualInput::JoyCallback, this)), _man_ctrl_sp_pub(_n.advertise("manual_control_setpoint", 10)) { + /* Load parameters, default values work for Microsoft XBox Controller */ + _n.param("map_x", param_axes_map[0], 4); + _n.param("scale_x", param_axes_scale[0], 1.0); + _n.param("off_x", param_axes_offset[0], 0.0); + + _n.param("map_y", param_axes_map[1], 3); + _n.param("scale_y", param_axes_scale[1], -1.0); + _n.param("off_y", param_axes_offset[1], 0.0); + + _n.param("map_z", param_axes_map[2], 2); + _n.param("scale_z", param_axes_scale[2], -0.5); + _n.param("off_z", param_axes_offset[2], 0.5); + + _n.param("map_r", param_axes_map[3], 0); + _n.param("scale_r", param_axes_scale[3], -1.0); + _n.param("off_r", param_axes_offset[3], 0.0); } void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg) @@ -54,11 +70,20 @@ void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg) px4::manual_control_setpoint msg_out; /* Fill px4 manual control setpoint topic with contents from ros joystick */ - //XXX + MapAxis(msg, param_axes_map[0], param_axes_scale[0], param_axes_offset[0], msg_out.x); + MapAxis(msg, param_axes_map[1], param_axes_scale[1], param_axes_offset[1], msg_out.y); + MapAxis(msg, param_axes_map[2], param_axes_scale[2], param_axes_offset[2], msg_out.z); + MapAxis(msg, param_axes_map[3], param_axes_scale[3], param_axes_offset[3], msg_out.r); + ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_out.x, msg_out.y, msg_out.z, msg_out.r); _man_ctrl_sp_pub.publish(msg_out); } +void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, double scale, double offset, float &out) +{ + out = (float)(msg->axes[map_index] * scale + offset); +} + int main(int argc, char **argv) { ros::init(argc, argv, "manual_input"); diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h index 1721d5e81..fbe635cf2 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.h +++ b/src/platforms/ros/nodes/manual_input/manual_input.h @@ -53,9 +53,19 @@ protected: */ void JoyCallback(const sensor_msgs::JoyConstPtr& msg); + /** + * Helper function to map and scale joystick input + */ + void MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, double scale, double offset, float &out); + ros::NodeHandle _n; ros::Subscriber _sub_joy; ros::Publisher _man_ctrl_sp_pub; + /* Parameters */ + int param_axes_map[4]; + double param_axes_scale[4]; + double param_axes_offset[4]; + }; -- cgit v1.2.3 From 502952e034624e789755368b6a8ab6f8b88ba262 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Dec 2014 11:25:27 +0100 Subject: make clear that switches are hardcoded to manual mode --- src/platforms/ros/nodes/manual_input/manual_input.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index 8fc158504..2ee2dadbc 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.cpp +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -70,11 +70,22 @@ void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg) px4::manual_control_setpoint msg_out; /* Fill px4 manual control setpoint topic with contents from ros joystick */ + /* Map sticks to x, y, z, r */ MapAxis(msg, param_axes_map[0], param_axes_scale[0], param_axes_offset[0], msg_out.x); MapAxis(msg, param_axes_map[1], param_axes_scale[1], param_axes_offset[1], msg_out.y); MapAxis(msg, param_axes_map[2], param_axes_scale[2], param_axes_offset[2], msg_out.z); MapAxis(msg, param_axes_map[3], param_axes_scale[3], param_axes_offset[3], msg_out.r); - ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_out.x, msg_out.y, msg_out.z, msg_out.r); + //ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_out.x, msg_out.y, msg_out.z, msg_out.r); + + /* Map buttons to switches */ + //XXX todo + /* for now just publish switches in position for manual flight */ + msg_out.mode_switch = 0; + msg_out.return_switch = 0; + msg_out.posctl_switch = 0; + msg_out.loiter_switch = 0; + msg_out.acro_switch = 0; + msg_out.offboard_switch = 0; _man_ctrl_sp_pub.publish(msg_out); } -- cgit v1.2.3 From fa1f09b850f754ff3e0da7f4c5e56e3ee58fce11 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 30 Dec 2014 11:34:37 +0100 Subject: made class for mc_mixer and moved into folder --- src/platforms/ros/nodes/mc_mixer.cpp | 165 ---------------------- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 192 ++++++++++++++++++++++++++ 2 files changed, 192 insertions(+), 165 deletions(-) delete mode 100644 src/platforms/ros/nodes/mc_mixer.cpp create mode 100644 src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp diff --git a/src/platforms/ros/nodes/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer.cpp deleted file mode 100644 index a131440a8..000000000 --- a/src/platforms/ros/nodes/mc_mixer.cpp +++ /dev/null @@ -1,165 +0,0 @@ -/**************************************************************************** - * - * 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 att_estimator.cpp - * Dummy attitude estimator that forwards attitude from gazebo to px4 topic - * - * @author Roman Bapst -*/ - #include - #include - #include - #include - - #define _rotor_count 4 - - struct Rotor { - float roll_scale; - float pitch_scale; - float yaw_scale; -}; - - - struct { - float control[8]; - }inputs; - - struct { - float control[8]; - }outputs; - - -const Rotor _config_x[] = { - { 0.000000, -1.000000, -1.00 }, - { -0.000000, 1.000000, -1.00 }, - { 1.000000, 0.000000, 1.00 }, - { -1.000000, 0.000000, 1.00 }, -}; - - const Rotor *_rotors = { &_config_x[0] - - }; - - -void mix() { - float roll = math::constrain(inputs.control[0], -1.0f, 1.0f); - float pitch = math::constrain(inputs.control[1], -1.0f, 1.0f); - float yaw = math::constrain(inputs.control[2], -1.0f, 1.0f); - float thrust = math::constrain(inputs.control[3], 0.0f, 1.0f); - float min_out = 0.0f; - float max_out = 0.0f; - - /* perform initial mix pass yielding unbounded outputs, ignore yaw */ - for (unsigned i = 0; i < _rotor_count; i++) { - float out = roll * _rotors[i].roll_scale - + pitch * _rotors[i].pitch_scale + thrust; - - /* limit yaw if it causes outputs clipping */ - if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) { - yaw = -out / _rotors[i].yaw_scale; - } - - /* calculate min and max output values */ - if (out < min_out) { - min_out = out; - } - if (out > max_out) { - max_out = out; - } - - outputs.control[i] = out; - } - /* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */ - if (min_out < 0.0f) { - float scale_in = thrust / (thrust - min_out); - - /* mix again with adjusted controls */ - for (unsigned i = 0; i < _rotor_count; i++) { - outputs.control[i] = scale_in - * (roll * _rotors[i].roll_scale - + pitch * _rotors[i].pitch_scale) + thrust; - } - - } else { - /* roll/pitch mixed without limiting, add yaw control */ - for (unsigned i = 0; i < _rotor_count; i++) { - outputs.control[i] += yaw * _rotors[i].yaw_scale; - } - } - - /* scale down all outputs if some outputs are too large, reduce total thrust */ - float scale_out; - if (max_out > 1.0f) { - scale_out = 1.0f / max_out; - - } else { - scale_out = 1.0f; - } - - /* scale outputs to range _idle_speed..1, and do final limiting */ - for (unsigned i = 0; i < _rotor_count; i++) { - outputs.control[i] = math::constrain(outputs.control[i], 0.0f, 1.0f); - } -} - - void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg) -{ - // read message - for(int i = 0;i < msg.NUM_ACTUATOR_CONTROLS;i++) { - inputs.control[i] = msg.control[i]; - } - - // mix - mix(); - - // publish message - mav_msgs::MotorSpeed rotor_vel_msg; - for (int i = 0; i < _rotor_count; i++) { - rotor_vel_msg.motor_speed.push_back(outputs.control[i]); - } - pub.publish(rotor_vel_msg); - -} - - int main(int argc, char **argv) -{ - ros::init(argc, argv, "mc_mixer"); - ros::NodeHandle n; - ros::Subscriber sub = n.subscribe("actuator_controls_0", 1000, actuatorControlsCallback); - ros::Publisher pub = n.advertise("mixed_motor_commands",10); - - ros::spin(); - - return 0; -} diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp new file mode 100644 index 000000000..79eaf19e5 --- /dev/null +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -0,0 +1,192 @@ +/**************************************************************************** + * + * 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 att_estimator.cpp + * Dummy attitude estimator that forwards attitude from gazebo to px4 topic + * + * @author Roman Bapst +*/ + #include + #include + #include + #include + + class MultirotorMixer { +public: + + MultirotorMixer(); + + struct Rotor { + float roll_scale; + float pitch_scale; + float yaw_scale; + + }; + + void mix(); + void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg); + +private: + + ros::NodeHandle _n; + ros::Subscriber _sub; + ros::Publisher _pub; + + const Rotor *_rotors; + + unsigned _rotor_count; + + struct { + float control[6]; + }inputs; + + struct { + float control[6]; + }outputs; + + + +}; + + + +const MultirotorMixer::Rotor _config_x[] = { + { 0.000000, -1.000000, -1.00 }, + { -0.000000, 1.000000, -1.00 }, + { 1.000000, 0.000000, 1.00 }, + { -1.000000, 0.000000, 1.00 }, +}; + + const MultirotorMixer::Rotor *_config_index = { &_config_x[0] + + }; + +MultirotorMixer::MultirotorMixer(): + _rotor_count(4), + _rotors(_config_index) +{ + _sub = _n.subscribe("actuator_controls_0", 1000, &MultirotorMixer::actuatorControlsCallback,this); + _pub = _n.advertise("mixed_motor_commands",10); +} + + +void MultirotorMixer::mix() { + float roll = math::constrain(inputs.control[0], -1.0f, 1.0f); + float pitch = math::constrain(inputs.control[1], -1.0f, 1.0f); + float yaw = math::constrain(inputs.control[2], -1.0f, 1.0f); + float thrust = math::constrain(inputs.control[3], 0.0f, 1.0f); + float min_out = 0.0f; + float max_out = 0.0f; + + /* perform initial mix pass yielding unbounded outputs, ignore yaw */ + for (unsigned i = 0; i < _rotor_count; i++) { + float out = roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale + thrust; + + /* limit yaw if it causes outputs clipping */ + if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) { + yaw = -out / _rotors[i].yaw_scale; + } + + /* calculate min and max output values */ + if (out < min_out) { + min_out = out; + } + if (out > max_out) { + max_out = out; + } + + outputs.control[i] = out; + } + /* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */ + if (min_out < 0.0f) { + float scale_in = thrust / (thrust - min_out); + + /* mix again with adjusted controls */ + for (unsigned i = 0; i < _rotor_count; i++) { + outputs.control[i] = scale_in + * (roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale) + thrust; + } + + } else { + /* roll/pitch mixed without limiting, add yaw control */ + for (unsigned i = 0; i < _rotor_count; i++) { + outputs.control[i] += yaw * _rotors[i].yaw_scale; + } + } + + /* scale down all outputs if some outputs are too large, reduce total thrust */ + float scale_out; + if (max_out > 1.0f) { + scale_out = 1.0f / max_out; + + } else { + scale_out = 1.0f; + } + + /* scale outputs to range _idle_speed..1, and do final limiting */ + for (unsigned i = 0; i < _rotor_count; i++) { + outputs.control[i] = math::constrain(outputs.control[i], 0.0f, 1.0f); + } +} + + void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg) +{ + // read message + for(int i = 0;i < msg.NUM_ACTUATOR_CONTROLS;i++) { + inputs.control[i] = msg.control[i]; + } + + // mix + mix(); + + // publish message + mav_msgs::MotorSpeed rotor_vel_msg; + for (int i = 0; i < _rotor_count; i++) { + rotor_vel_msg.motor_speed.push_back(outputs.control[i]); + } + _pub.publish(rotor_vel_msg); + +} + + int main(int argc, char **argv) +{ + ros::init(argc, argv, "mc_mixer"); + MultirotorMixer mixer; + + ros::spin(); + + return 0; +} -- cgit v1.2.3 From bfc398442663a943ff1e0fa21ec50b9960abb5a7 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 30 Dec 2014 11:41:28 +0100 Subject: cleanup --- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 38 +++++++++++---------------- 1 file changed, 15 insertions(+), 23 deletions(-) diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 79eaf19e5..7092a8017 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -41,20 +41,18 @@ #include #include #include - + class MultirotorMixer { public: MultirotorMixer(); struct Rotor { - float roll_scale; - float pitch_scale; - float yaw_scale; - - }; + float roll_scale; + float pitch_scale; + float yaw_scale; + }; - void mix(); void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg); private: @@ -62,7 +60,7 @@ private: ros::NodeHandle _n; ros::Subscriber _sub; ros::Publisher _pub; - + const Rotor *_rotors; unsigned _rotor_count; @@ -75,21 +73,18 @@ private: float control[6]; }outputs; + void mix(); - }; - - const MultirotorMixer::Rotor _config_x[] = { - { 0.000000, -1.000000, -1.00 }, - { -0.000000, 1.000000, -1.00 }, + { 0.000000, -1.000000, -1.00 }, + { -0.000000, 1.000000, -1.00 }, { 1.000000, 0.000000, 1.00 }, - { -1.000000, 0.000000, 1.00 }, + { -1.000000, 0.000000, 1.00 }, }; const MultirotorMixer::Rotor *_config_index = { &_config_x[0] - }; MultirotorMixer::MultirotorMixer(): @@ -97,10 +92,9 @@ MultirotorMixer::MultirotorMixer(): _rotors(_config_index) { _sub = _n.subscribe("actuator_controls_0", 1000, &MultirotorMixer::actuatorControlsCallback,this); - _pub = _n.advertise("mixed_motor_commands",10); + _pub = _n.advertise("mixed_motor_commands",10); } - void MultirotorMixer::mix() { float roll = math::constrain(inputs.control[0], -1.0f, 1.0f); float pitch = math::constrain(inputs.control[1], -1.0f, 1.0f); @@ -164,28 +158,26 @@ void MultirotorMixer::mix() { void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg) { - // read message + // read message for(int i = 0;i < msg.NUM_ACTUATOR_CONTROLS;i++) { inputs.control[i] = msg.control[i]; } - + // mix mix(); // publish message mav_msgs::MotorSpeed rotor_vel_msg; for (int i = 0; i < _rotor_count; i++) { - rotor_vel_msg.motor_speed.push_back(outputs.control[i]); + rotor_vel_msg.motor_speed.push_back(outputs.control[i]); } _pub.publish(rotor_vel_msg); - } int main(int argc, char **argv) { ros::init(argc, argv, "mc_mixer"); - MultirotorMixer mixer; - + MultirotorMixer mixer; ros::spin(); return 0; -- cgit v1.2.3 From 4307b48c03779a196f8b16697ac91213f4b39792 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 30 Dec 2014 11:46:18 +0100 Subject: fixed description --- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 7092a8017..1e5ef99a3 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -32,8 +32,8 @@ ****************************************************************************/ /** - * @file att_estimator.cpp - * Dummy attitude estimator that forwards attitude from gazebo to px4 topic + * @file mc_mixer.cpp + * Dummy multicopter mixer for euroc simulator (gazebo) * * @author Roman Bapst */ -- cgit v1.2.3 From 6f425ca7fcaa12f30ff475b9578473ede369d2c1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Dec 2014 12:27:29 +0100 Subject: commander dummy node --- CMakeLists.txt | 9 +++ src/platforms/px4_middleware.h | 4 ++ src/platforms/ros/nodes/commander/commander.cpp | 96 +++++++++++++++++++++++++ src/platforms/ros/nodes/commander/commander.h | 62 ++++++++++++++++ 4 files changed, 171 insertions(+) create mode 100644 src/platforms/ros/nodes/commander/commander.cpp create mode 100644 src/platforms/ros/nodes/commander/commander.h diff --git a/CMakeLists.txt b/CMakeLists.txt index d506cadb1..4c504bff4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -194,6 +194,15 @@ target_link_libraries(manual_input px4 ) +## Commander +add_executable(commander + src/platforms/ros/nodes/commander/commander.cpp) +add_dependencies(manual_input px4_generate_messages_cpp) +target_link_libraries(commander + ${catkin_LIBRARIES} + px4 +) + ############# ## Install ## diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index a8f3ad666..54050de8b 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -42,6 +42,10 @@ #include #include +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +#define __EXPORT +#endif + namespace px4 { diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp new file mode 100644 index 000000000..f767bbb36 --- /dev/null +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * 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 commander.cpp + * Dummy commander node that publishes the various status topics + * + * @author Thomas Gubler +*/ + +#include "commander.h" + +#include +#include +#include +#include +#include + +Commander::Commander() : + _n(), + _man_ctrl_sp_sub(_n.subscribe("manual_control_setpoint", 10, &Commander::ManualControlInputCallback, this)), + _vehicle_control_mode_pub(_n.advertise("vehicle_control_mode", 10)), + _actuator_armed_pub(_n.advertise("actuator_armed", 10)), + _vehicle_status_pub(_n.advertise("vehicle_status", 10)) +{ +} + +void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr& msg) +{ + px4::vehicle_control_mode msg_vehicle_control_mode; + px4::actuator_armed msg_actuator_armed; + px4::vehicle_status msg_vehicle_status; + + /* fill vehicle control mode */ + //XXX hardcoded + msg_vehicle_control_mode.timestamp = px4::get_time_micros(); + msg_vehicle_control_mode.flag_control_manual_enabled = true; + msg_vehicle_control_mode.flag_control_rates_enabled = true; + msg_vehicle_control_mode.flag_control_attitude_enabled = true; + + /* fill actuator armed */ + //XXX hardcoded + msg_actuator_armed.timestamp = px4::get_time_micros(); + msg_actuator_armed.armed = true; + + /* fill vehicle status */ + //XXX hardcoded + msg_vehicle_status.timestamp = px4::get_time_micros(); + msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_MANUAL; + msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_MANUAL; + msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED; + msg_vehicle_status.hil_state = msg_vehicle_status.HIL_STATE_OFF; + msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; + + _vehicle_control_mode_pub.publish(msg_vehicle_control_mode); + _actuator_armed_pub.publish(msg_actuator_armed); + _vehicle_status_pub.publish(msg_vehicle_status); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "commander"); + Commander m; + ros::spin(); + return 0; +} diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h new file mode 100644 index 000000000..d7fe0a4ca --- /dev/null +++ b/src/platforms/ros/nodes/commander/commander.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * 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 commander.h + * Dummy commander node that publishes the various status topics + * + * @author Thomas Gubler +*/ + +#include "ros/ros.h" +#include + +class Commander { +public: + Commander(); + + ~Commander() {} + +protected: + /** + * Based on manual control input the status will be set + */ + void ManualControlInputCallback(const px4::manual_control_setpointConstPtr& msg); + + ros::NodeHandle _n; + ros::Subscriber _man_ctrl_sp_sub; + ros::Publisher _vehicle_control_mode_pub; + ros::Publisher _actuator_armed_pub; + ros::Publisher _vehicle_status_pub; + +}; -- cgit v1.2.3 From 838f8603475def949646ececc8059e6b924bda28 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Dec 2014 12:27:45 +0100 Subject: variable rename --- src/platforms/ros/nodes/manual_input/manual_input.cpp | 7 ++++--- src/platforms/ros/nodes/manual_input/manual_input.h | 2 +- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index 2ee2dadbc..b125bb180 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.cpp +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -41,10 +41,11 @@ #include "manual_input.h" #include +#include ManualInput::ManualInput() : _n(), - _sub_joy(_n.subscribe("joy", 10, &ManualInput::JoyCallback, this)), + joy_sub(_n.subscribe("joy", 10, &ManualInput::JoyCallback, this)), _man_ctrl_sp_pub(_n.advertise("manual_control_setpoint", 10)) { /* Load parameters, default values work for Microsoft XBox Controller */ @@ -87,6 +88,8 @@ void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg) msg_out.acro_switch = 0; msg_out.offboard_switch = 0; + msg_out.timestamp = px4::get_time_micros(); + _man_ctrl_sp_pub.publish(msg_out); } @@ -99,8 +102,6 @@ int main(int argc, char **argv) { ros::init(argc, argv, "manual_input"); ManualInput m; - ros::spin(); - return 0; } diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h index fbe635cf2..22a158985 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.h +++ b/src/platforms/ros/nodes/manual_input/manual_input.h @@ -59,7 +59,7 @@ protected: void MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, double scale, double offset, float &out); ros::NodeHandle _n; - ros::Subscriber _sub_joy; + ros::Subscriber joy_sub; ros::Publisher _man_ctrl_sp_pub; /* Parameters */ -- cgit v1.2.3 From f968e7355d62bbf8df426c51dc2aba98d2bc72b7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Dec 2014 12:31:24 +0100 Subject: fix mixer path --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a7277b02a..220cd762b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -196,7 +196,7 @@ target_link_libraries(manual_input ## Multicopter Mixer dummy add_executable(mc_mixer - src/platforms/ros/nodes/mc_mixer.cpp) + src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp) add_dependencies(mc_mixer px4_generate_messages_cpp) target_link_libraries(mc_mixer ${catkin_LIBRARIES} -- cgit v1.2.3 From 16c66669c257767173d52febd0f526c0e718e004 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Dec 2014 14:53:53 +0100 Subject: dummy attitude estimator copies attitude from gazebo --- src/lib/mathlib/math/Matrix.hpp | 1 + src/lib/mathlib/math/Vector.hpp | 2 ++ .../attitude_estimator/attitude_estimator.cpp | 31 ++++++++++++++++++---- 3 files changed, 29 insertions(+), 5 deletions(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 1e76aae60..ddadf707c 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -52,6 +52,7 @@ #include #include #endif +#include namespace math { diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 20f099831..781c14d53 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -52,6 +52,8 @@ #include #endif +#include + namespace math { diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp index e64ba335d..99d1ae024 100644 --- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp @@ -33,14 +33,16 @@ /** * @file att_estimator.cpp - * Dummy attitude estimator that forwards attitude from gazebo to px4 topic * * @author Thomas Gubler + * @author Roman Bapst */ #include "attitude_estimator.h" #include +#include +#include AttitudeEstimator::AttitudeEstimator() : _n(), @@ -51,13 +53,32 @@ AttitudeEstimator::AttitudeEstimator() : void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr& msg) { - px4::vehicle_attitude msg_out; + px4::vehicle_attitude msg_v_att; /* Fill px4 attitude topic with contents from modelstates topic */ - ROS_INFO("Test x: %.4f", msg->pose[0].orientation.x); - //XXX - _vehicle_attitude_pub.publish(msg_out); + /* Convert quaternion to rotation matrix */ + math::Quaternion quat; + quat(0) = (float)msg->pose[0].orientation.w; + quat(1) = (float)msg->pose[0].orientation.x; + quat(2) = (float)msg->pose[0].orientation.y; + quat(3) = (float)msg->pose[0].orientation.z; + + msg_v_att.q[0] = quat(0); + msg_v_att.q[1] = quat(1); + msg_v_att.q[2] = quat(2); + msg_v_att.q[3] = quat(3); + + math::Matrix<3, 3> Rot = quat.to_dcm(); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + PX4_R(msg_v_att.R, i, j) = Rot(i, j); + } + } + + msg_v_att.R_valid = true; + + _vehicle_attitude_pub.publish(msg_v_att); } int main(int argc, char **argv) -- cgit v1.2.3 From 481f18cb3e20ed74b03dc02c919648f7a15b99e6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Dec 2014 14:58:00 +0100 Subject: include commander and mixer in launch file --- launch/multicopter.launch | 2 ++ 1 file changed, 2 insertions(+) diff --git a/launch/multicopter.launch b/launch/multicopter.launch index e54ebb3e2..e7f49319c 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -3,6 +3,8 @@ + + -- cgit v1.2.3 From 6f446d9abcf6eaf057183f64f7dbeb1bc75dbc26 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Dec 2014 15:31:44 +0100 Subject: add first version of simulator launch file --- launch/gazebo_multicopter.launch | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 launch/gazebo_multicopter.launch diff --git a/launch/gazebo_multicopter.launch b/launch/gazebo_multicopter.launch new file mode 100644 index 000000000..c01ad163a --- /dev/null +++ b/launch/gazebo_multicopter.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + -- cgit v1.2.3 From 133842e89d83bc9a4dd0523263bea6e855caf08e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Dec 2014 15:43:23 +0100 Subject: fix launch files --- launch/gazebo_multicopter.launch | 4 ++-- launch/multicopter.launch | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/launch/gazebo_multicopter.launch b/launch/gazebo_multicopter.launch index c01ad163a..263ee025b 100644 --- a/launch/gazebo_multicopter.launch +++ b/launch/gazebo_multicopter.launch @@ -1,7 +1,7 @@ + - - + diff --git a/launch/multicopter.launch b/launch/multicopter.launch index e7f49319c..96ff3ad99 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -1,6 +1,6 @@ - + -- cgit v1.2.3 From 0ea60f56e9f52ea2ee5eac41183fea09720d9e7c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Dec 2014 15:55:39 +0100 Subject: attitude estimator: fix readin of pose --- .../ros/nodes/attitude_estimator/attitude_estimator.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp index 99d1ae024..bde436aca 100644 --- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp @@ -46,7 +46,7 @@ AttitudeEstimator::AttitudeEstimator() : _n(), - _sub_modelstates(_n.subscribe("gazebo/model_states", 10, &AttitudeEstimator::ModelStatesCallback, this)), + _sub_modelstates(_n.subscribe("/gazebo/model_states", 10, &AttitudeEstimator::ModelStatesCallback, this)), _vehicle_attitude_pub(_n.advertise("vehicle_attitude", 10)) { } @@ -59,10 +59,11 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP /* Convert quaternion to rotation matrix */ math::Quaternion quat; - quat(0) = (float)msg->pose[0].orientation.w; - quat(1) = (float)msg->pose[0].orientation.x; - quat(2) = (float)msg->pose[0].orientation.y; - quat(3) = (float)msg->pose[0].orientation.z; + //XXX: search for vtol or other (other than 'plane') vehicle here + quat(0) = (float)msg->pose[1].orientation.w; + quat(1) = (float)msg->pose[1].orientation.x; + quat(2) = (float)msg->pose[1].orientation.y; + quat(3) = (float)msg->pose[1].orientation.z; msg_v_att.q[0] = quat(0); msg_v_att.q[1] = quat(1); -- cgit v1.2.3 From 60b02477e5ae72c6afc6fd609e28c6e74c955060 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Dec 2014 16:01:52 +0100 Subject: fix mixed motor commands publication --- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 1e5ef99a3..f92712c55 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -92,7 +92,7 @@ MultirotorMixer::MultirotorMixer(): _rotors(_config_index) { _sub = _n.subscribe("actuator_controls_0", 1000, &MultirotorMixer::actuatorControlsCallback,this); - _pub = _n.advertise("mixed_motor_commands",10); + _pub = _n.advertise("/mixed_motor_commands",10); } void MultirotorMixer::mix() { -- cgit v1.2.3 From a2edc284b2bc0d5e462e9732bb1adcfb6b0a59a3 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 30 Dec 2014 16:47:38 +0100 Subject: fixed launch file --- launch/gazebo_multicopter.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/gazebo_multicopter.launch b/launch/gazebo_multicopter.launch index 263ee025b..9c0e96e04 100644 --- a/launch/gazebo_multicopter.launch +++ b/launch/gazebo_multicopter.launch @@ -1,5 +1,5 @@ - + -- cgit v1.2.3 From dbaf6af257ca5811d779859a9ba4067758f07eec Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Dec 2014 16:55:34 +0100 Subject: scale dummy mixer output --- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index f92712c55..69d69dcf8 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -169,7 +169,7 @@ void MultirotorMixer::mix() { // publish message mav_msgs::MotorSpeed rotor_vel_msg; for (int i = 0; i < _rotor_count; i++) { - rotor_vel_msg.motor_speed.push_back(outputs.control[i]); + rotor_vel_msg.motor_speed.push_back(outputs.control[i] * 1000); } _pub.publish(rotor_vel_msg); } -- cgit v1.2.3 From a0260b6f6ebe7a4709a55818c324145f17937145 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Dec 2014 16:56:03 +0100 Subject: use joystick launch file (joystick will not be used actually from euroc directly) --- launch/gazebo_multicopter.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/gazebo_multicopter.launch b/launch/gazebo_multicopter.launch index 263ee025b..9c0e96e04 100644 --- a/launch/gazebo_multicopter.launch +++ b/launch/gazebo_multicopter.launch @@ -1,5 +1,5 @@ - + -- cgit v1.2.3 From bc5fe30270d450ae6e22f69b2fd6b7ff252e8bb1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 31 Dec 2014 09:14:59 +0100 Subject: make mixer to radps scaling a param and raise default --- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 69d69dcf8..61ee13ecd 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -74,7 +74,6 @@ private: }outputs; void mix(); - }; const MultirotorMixer::Rotor _config_x[] = { @@ -88,11 +87,15 @@ const MultirotorMixer::Rotor _config_x[] = { }; MultirotorMixer::MultirotorMixer(): + _n(), _rotor_count(4), _rotors(_config_index) { _sub = _n.subscribe("actuator_controls_0", 1000, &MultirotorMixer::actuatorControlsCallback,this); _pub = _n.advertise("/mixed_motor_commands",10); + if (!_n.hasParam("motor_scaling_radps")) { + _n.setParam("motor_scaling_radps", 2000.0); + } } void MultirotorMixer::mix() { @@ -168,8 +171,10 @@ void MultirotorMixer::mix() { // publish message mav_msgs::MotorSpeed rotor_vel_msg; + double scaling; + _n.getParamCached("motor_scaling_radps", scaling); for (int i = 0; i < _rotor_count; i++) { - rotor_vel_msg.motor_speed.push_back(outputs.control[i] * 1000); + rotor_vel_msg.motor_speed.push_back(outputs.control[i] * scaling); } _pub.publish(rotor_vel_msg); } -- cgit v1.2.3 From f8214f3e0162fcbd0ca9382b0d00bdab2f5863ee Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 31 Dec 2014 09:19:57 +0100 Subject: manual input fix variable names --- .../ros/nodes/manual_input/manual_input.cpp | 34 +++++++++++----------- .../ros/nodes/manual_input/manual_input.h | 8 ++--- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index b125bb180..bd2017033 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.cpp +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -45,25 +45,25 @@ ManualInput::ManualInput() : _n(), - joy_sub(_n.subscribe("joy", 10, &ManualInput::JoyCallback, this)), + _joy_sub(_n.subscribe("joy", 10, &ManualInput::JoyCallback, this)), _man_ctrl_sp_pub(_n.advertise("manual_control_setpoint", 10)) { /* Load parameters, default values work for Microsoft XBox Controller */ - _n.param("map_x", param_axes_map[0], 4); - _n.param("scale_x", param_axes_scale[0], 1.0); - _n.param("off_x", param_axes_offset[0], 0.0); + _n.param("map_x", _param_axes_map[0], 4); + _n.param("scale_x", _param_axes_scale[0], 1.0); + _n.param("off_x", _param_axes_offset[0], 0.0); - _n.param("map_y", param_axes_map[1], 3); - _n.param("scale_y", param_axes_scale[1], -1.0); - _n.param("off_y", param_axes_offset[1], 0.0); + _n.param("map_y", _param_axes_map[1], 3); + _n.param("scale_y", _param_axes_scale[1], -1.0); + _n.param("off_y", _param_axes_offset[1], 0.0); - _n.param("map_z", param_axes_map[2], 2); - _n.param("scale_z", param_axes_scale[2], -0.5); - _n.param("off_z", param_axes_offset[2], 0.5); + _n.param("map_z", _param_axes_map[2], 2); + _n.param("scale_z", _param_axes_scale[2], -0.5); + _n.param("off_z", _param_axes_offset[2], 0.5); - _n.param("map_r", param_axes_map[3], 0); - _n.param("scale_r", param_axes_scale[3], -1.0); - _n.param("off_r", param_axes_offset[3], 0.0); + _n.param("map_r", _param_axes_map[3], 0); + _n.param("scale_r", _param_axes_scale[3], -1.0); + _n.param("off_r", _param_axes_offset[3], 0.0); } void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg) @@ -72,10 +72,10 @@ void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg) /* Fill px4 manual control setpoint topic with contents from ros joystick */ /* Map sticks to x, y, z, r */ - MapAxis(msg, param_axes_map[0], param_axes_scale[0], param_axes_offset[0], msg_out.x); - MapAxis(msg, param_axes_map[1], param_axes_scale[1], param_axes_offset[1], msg_out.y); - MapAxis(msg, param_axes_map[2], param_axes_scale[2], param_axes_offset[2], msg_out.z); - MapAxis(msg, param_axes_map[3], param_axes_scale[3], param_axes_offset[3], msg_out.r); + MapAxis(msg, _param_axes_map[0], _param_axes_scale[0], _param_axes_offset[0], msg_out.x); + MapAxis(msg, _param_axes_map[1], _param_axes_scale[1], _param_axes_offset[1], msg_out.y); + MapAxis(msg, _param_axes_map[2], _param_axes_scale[2], _param_axes_offset[2], msg_out.z); + MapAxis(msg, _param_axes_map[3], _param_axes_scale[3], _param_axes_offset[3], msg_out.r); //ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_out.x, msg_out.y, msg_out.z, msg_out.r); /* Map buttons to switches */ diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h index 22a158985..f579acbb7 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.h +++ b/src/platforms/ros/nodes/manual_input/manual_input.h @@ -59,13 +59,13 @@ protected: void MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, double scale, double offset, float &out); ros::NodeHandle _n; - ros::Subscriber joy_sub; + ros::Subscriber _joy_sub; ros::Publisher _man_ctrl_sp_pub; /* Parameters */ - int param_axes_map[4]; - double param_axes_scale[4]; - double param_axes_offset[4]; + int _param_axes_map[4]; + double _param_axes_scale[4]; + double _param_axes_offset[4]; }; -- cgit v1.2.3 From 3684ac6bf1fdbaeb18686768d013f28ad75122ff Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 31 Dec 2014 14:43:55 +0100 Subject: fix dummy mixer --- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 23 ++++++++++++++++------- 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 61ee13ecd..04d7888fb 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -77,24 +77,33 @@ private: }; const MultirotorMixer::Rotor _config_x[] = { - { 0.000000, -1.000000, -1.00 }, - { -0.000000, 1.000000, -1.00 }, - { 1.000000, 0.000000, 1.00 }, + { -0.707107, 0.707107, 1.00 }, + { 0.707107, -0.707107, 1.00 }, + { 0.707107, 0.707107, -1.00 }, + { -0.707107, -0.707107, -1.00 }, +}; + +const MultirotorMixer::Rotor _config_quad_plus[] = { { -1.000000, 0.000000, 1.00 }, + { 1.000000, 0.000000, 1.00 }, + { 0.000000, 1.000000, -1.00 }, + { -0.000000, -1.000000, -1.00 }, }; - const MultirotorMixer::Rotor *_config_index = { &_config_x[0] - }; +const MultirotorMixer::Rotor *_config_index[3] = { + &_config_x[0], + &_config_quad_plus[0], +}; MultirotorMixer::MultirotorMixer(): _n(), _rotor_count(4), - _rotors(_config_index) + _rotors(_config_index[1]) //XXX +config hardcoded { _sub = _n.subscribe("actuator_controls_0", 1000, &MultirotorMixer::actuatorControlsCallback,this); _pub = _n.advertise("/mixed_motor_commands",10); if (!_n.hasParam("motor_scaling_radps")) { - _n.setParam("motor_scaling_radps", 2000.0); + _n.setParam("motor_scaling_radps", 1500.0); } } -- cgit v1.2.3 From 134f41c7077ea8592a55c07b1635bac6e23fe3bd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 31 Dec 2014 15:23:09 +0100 Subject: make ros params from launch files work --- src/platforms/px4_defines.h | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index d6d23d013..ef43c55b8 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -81,12 +81,16 @@ typedef const char *px4_param_t; /* Helper functions to set ROS params, only int and float supported */ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value) { - ros::param::set(name, value); + if (!ros::param::has(name)) { + ros::param::set(name, value); + } return (px4_param_t)name; }; static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) { - ros::param::set(name, value); + if (!ros::param::has(name)) { + ros::param::set(name, value); + } return (px4_param_t)name; }; -- cgit v1.2.3 From 1b0446ed41eb6db676106debeef2f895edc30d01 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 31 Dec 2014 15:23:28 +0100 Subject: improve launch files --- launch/gazebo_multicopter.launch | 11 ++--------- launch/multicopter.launch | 5 +++++ 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/launch/gazebo_multicopter.launch b/launch/gazebo_multicopter.launch index 9c0e96e04..febf7bdc0 100644 --- a/launch/gazebo_multicopter.launch +++ b/launch/gazebo_multicopter.launch @@ -1,13 +1,6 @@ - - - - - - - - - + + diff --git a/launch/multicopter.launch b/launch/multicopter.launch index 96ff3ad99..2ccd1338b 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -7,6 +7,11 @@ + + + + + -- cgit v1.2.3 From e826187efe6f514a632ab2ef2e0183d17792c0a9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 31 Dec 2014 16:50:30 +0100 Subject: reduce mixer queue size --- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 04d7888fb..966c5cfb7 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -100,7 +100,7 @@ MultirotorMixer::MultirotorMixer(): _rotor_count(4), _rotors(_config_index[1]) //XXX +config hardcoded { - _sub = _n.subscribe("actuator_controls_0", 1000, &MultirotorMixer::actuatorControlsCallback,this); + _sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback,this); _pub = _n.advertise("/mixed_motor_commands",10); if (!_n.hasParam("motor_scaling_radps")) { _n.setParam("motor_scaling_radps", 1500.0); -- cgit v1.2.3 From 2ab6eefc2966b615f66f3c8b366337fa293bc7a5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 31 Dec 2014 16:51:33 +0100 Subject: reduce manualinput queue size --- src/platforms/ros/nodes/manual_input/manual_input.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index bd2017033..eddacf55f 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.cpp +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -45,8 +45,8 @@ ManualInput::ManualInput() : _n(), - _joy_sub(_n.subscribe("joy", 10, &ManualInput::JoyCallback, this)), - _man_ctrl_sp_pub(_n.advertise("manual_control_setpoint", 10)) + _joy_sub(_n.subscribe("joy", 1, &ManualInput::JoyCallback, this)), + _man_ctrl_sp_pub(_n.advertise("manual_control_setpoint", 1)) { /* Load parameters, default values work for Microsoft XBox Controller */ _n.param("map_x", _param_axes_map[0], 4); -- cgit v1.2.3 From a5890662c53d700832783f31477d1fa26f5b2d05 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 31 Dec 2014 16:52:00 +0100 Subject: attitude estimator: fix signs --- .../nodes/attitude_estimator/attitude_estimator.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp index bde436aca..163b111be 100644 --- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp @@ -46,8 +46,8 @@ AttitudeEstimator::AttitudeEstimator() : _n(), - _sub_modelstates(_n.subscribe("/gazebo/model_states", 10, &AttitudeEstimator::ModelStatesCallback, this)), - _vehicle_attitude_pub(_n.advertise("vehicle_attitude", 10)) + _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &AttitudeEstimator::ModelStatesCallback, this)), + _vehicle_attitude_pub(_n.advertise("vehicle_attitude", 1)) { } @@ -62,23 +62,27 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP //XXX: search for vtol or other (other than 'plane') vehicle here quat(0) = (float)msg->pose[1].orientation.w; quat(1) = (float)msg->pose[1].orientation.x; - quat(2) = (float)msg->pose[1].orientation.y; - quat(3) = (float)msg->pose[1].orientation.z; + quat(2) = (float)-msg->pose[1].orientation.y; + quat(3) = (float)-msg->pose[1].orientation.z; msg_v_att.q[0] = quat(0); msg_v_att.q[1] = quat(1); msg_v_att.q[2] = quat(2); msg_v_att.q[3] = quat(3); - math::Matrix<3, 3> Rot = quat.to_dcm(); + math::Matrix<3, 3> rot = quat.to_dcm(); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { - PX4_R(msg_v_att.R, i, j) = Rot(i, j); + PX4_R(msg_v_att.R, i, j) = rot(i, j); } } - msg_v_att.R_valid = true; + math::Vector<3> euler = rot.to_euler(); + msg_v_att.roll = euler(0); + msg_v_att.pitch = euler(1); + msg_v_att.yaw = euler(2); + _vehicle_attitude_pub.publish(msg_v_att); } -- cgit v1.2.3 From 9586a8f5b11314380f1e07ef6a09bff3c430c7e6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 2 Jan 2015 09:08:30 +0100 Subject: add missing mab_msgs dependency --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6312fa055..cbd646556 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS cmake_modules gazebo_msgs sensor_msgs + mav_msgs ) find_package(Eigen REQUIRED) -- cgit v1.2.3 From 92729b3020afe204aebaeb14d502654ad9251e45 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 2 Jan 2015 09:08:49 +0100 Subject: commander dummy node: publish param update at low freq to make other nodes update their params --- src/platforms/ros/nodes/commander/commander.cpp | 10 +++++++++- src/platforms/ros/nodes/commander/commander.h | 4 ++++ 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index f767bbb36..1972d8cfa 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -51,7 +51,9 @@ Commander::Commander() : _man_ctrl_sp_sub(_n.subscribe("manual_control_setpoint", 10, &Commander::ManualControlInputCallback, this)), _vehicle_control_mode_pub(_n.advertise("vehicle_control_mode", 10)), _actuator_armed_pub(_n.advertise("actuator_armed", 10)), - _vehicle_status_pub(_n.advertise("vehicle_status", 10)) + _vehicle_status_pub(_n.advertise("vehicle_status", 10)), + _parameter_update_pub(_n.advertise("parameter_update", 10)), + _msg_parameter_update() { } @@ -85,6 +87,12 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon _vehicle_control_mode_pub.publish(msg_vehicle_control_mode); _actuator_armed_pub.publish(msg_actuator_armed); _vehicle_status_pub.publish(msg_vehicle_status); + + /* Fill parameter update */ + if (px4::get_time_micros() - _msg_parameter_update.timestamp > 1e6) { + _msg_parameter_update.timestamp = px4::get_time_micros(); + _parameter_update_pub.publish(_msg_parameter_update); + } } int main(int argc, char **argv) diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h index d7fe0a4ca..cd9be5135 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -40,6 +40,7 @@ #include "ros/ros.h" #include +#include class Commander { public: @@ -58,5 +59,8 @@ protected: ros::Publisher _vehicle_control_mode_pub; ros::Publisher _actuator_armed_pub; ros::Publisher _vehicle_status_pub; + ros::Publisher _parameter_update_pub; + + px4::parameter_update _msg_parameter_update; }; -- cgit v1.2.3 From f05dc608748fa4714b7bb1239dc5ca4743602f17 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 2 Jan 2015 12:22:06 +0100 Subject: ros nodes: add mixer for euroc --- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 966c5cfb7..e8c65905f 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -90,15 +90,23 @@ const MultirotorMixer::Rotor _config_quad_plus[] = { { -0.000000, -1.000000, -1.00 }, }; +const MultirotorMixer::Rotor _config_quad_plus_euroc[] = { + { 0.000000, 1.000000, 1.00 }, + { -0.000000, -1.000000, 1.00 }, + { 1.000000, 0.000000, -1.00 }, + { -1.000000, 0.000000, -1.00 }, +}; + const MultirotorMixer::Rotor *_config_index[3] = { &_config_x[0], &_config_quad_plus[0], + &_config_quad_plus_euroc[0] }; MultirotorMixer::MultirotorMixer(): _n(), _rotor_count(4), - _rotors(_config_index[1]) //XXX +config hardcoded + _rotors(_config_index[2]) //XXX + eurocconfig hardcoded { _sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback,this); _pub = _n.advertise("/mixed_motor_commands",10); -- cgit v1.2.3 From 3932013777c232c8c91e34144f54f6fd2f635175 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 2 Jan 2015 12:22:06 +0100 Subject: ros nodes: add mixer for euroc --- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 966c5cfb7..e8c65905f 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -90,15 +90,23 @@ const MultirotorMixer::Rotor _config_quad_plus[] = { { -0.000000, -1.000000, -1.00 }, }; +const MultirotorMixer::Rotor _config_quad_plus_euroc[] = { + { 0.000000, 1.000000, 1.00 }, + { -0.000000, -1.000000, 1.00 }, + { 1.000000, 0.000000, -1.00 }, + { -1.000000, 0.000000, -1.00 }, +}; + const MultirotorMixer::Rotor *_config_index[3] = { &_config_x[0], &_config_quad_plus[0], + &_config_quad_plus_euroc[0] }; MultirotorMixer::MultirotorMixer(): _n(), _rotor_count(4), - _rotors(_config_index[1]) //XXX +config hardcoded + _rotors(_config_index[2]) //XXX + eurocconfig hardcoded { _sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback,this); _pub = _n.advertise("/mixed_motor_commands",10); -- cgit v1.2.3 From 530c38b6fa3bc0294d66801a0d7a36e8888e903f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 2 Jan 2015 16:11:16 +0100 Subject: dummy commander and mixer: armed status --- src/platforms/ros/nodes/commander/commander.cpp | 24 +++++++++++++++++------- src/platforms/ros/nodes/commander/commander.h | 2 ++ src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 18 +++++++++++++++++- 3 files changed, 36 insertions(+), 8 deletions(-) diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index 1972d8cfa..d8ff739b9 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -42,7 +42,6 @@ #include #include -#include #include #include @@ -53,14 +52,14 @@ Commander::Commander() : _actuator_armed_pub(_n.advertise("actuator_armed", 10)), _vehicle_status_pub(_n.advertise("vehicle_status", 10)), _parameter_update_pub(_n.advertise("parameter_update", 10)), - _msg_parameter_update() + _msg_parameter_update(), + _msg_actuator_armed() { } void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr& msg) { px4::vehicle_control_mode msg_vehicle_control_mode; - px4::actuator_armed msg_actuator_armed; px4::vehicle_status msg_vehicle_status; /* fill vehicle control mode */ @@ -71,9 +70,19 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon msg_vehicle_control_mode.flag_control_attitude_enabled = true; /* fill actuator armed */ - //XXX hardcoded - msg_actuator_armed.timestamp = px4::get_time_micros(); - msg_actuator_armed.armed = true; + float arm_th = 0.95; + _msg_actuator_armed.timestamp = px4::get_time_micros(); + if (_msg_actuator_armed.armed) { + /* Check for disarm */ + if (msg->r < -arm_th && msg->z < (1-arm_th)) { + _msg_actuator_armed.armed = false; + } + } else { + /* Check for arm */ + if (msg->r > arm_th && msg->z < (1-arm_th)) { + _msg_actuator_armed.armed = true; + } + } /* fill vehicle status */ //XXX hardcoded @@ -83,9 +92,10 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED; msg_vehicle_status.hil_state = msg_vehicle_status.HIL_STATE_OFF; msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; + msg_vehicle_status.is_rotary_wing = true; _vehicle_control_mode_pub.publish(msg_vehicle_control_mode); - _actuator_armed_pub.publish(msg_actuator_armed); + _actuator_armed_pub.publish(_msg_actuator_armed); _vehicle_status_pub.publish(msg_vehicle_status); /* Fill parameter update */ diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h index cd9be5135..f3c2f6f1a 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -41,6 +41,7 @@ #include "ros/ros.h" #include #include +#include class Commander { public: @@ -62,5 +63,6 @@ protected: ros::Publisher _parameter_update_pub; px4::parameter_update _msg_parameter_update; + px4::actuator_armed _msg_actuator_armed; }; diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index e8c65905f..4dc428a27 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -54,6 +54,7 @@ public: }; void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg); + void actuatorArmedCallback(const PX4_TOPIC_T(actuator_armed) &msg); private: @@ -73,6 +74,9 @@ private: float control[6]; }outputs; + bool _armed; + ros::Subscriber _sub_actuator_armed; + void mix(); }; @@ -113,6 +117,7 @@ MultirotorMixer::MultirotorMixer(): if (!_n.hasParam("motor_scaling_radps")) { _n.setParam("motor_scaling_radps", 1500.0); } + _sub_actuator_armed = _n.subscribe("actuator_armed", 1, &MultirotorMixer::actuatorArmedCallback,this); } void MultirotorMixer::mix() { @@ -184,7 +189,13 @@ void MultirotorMixer::mix() { } // mix - mix(); + if (_armed) { + mix(); + } else { + for (unsigned i = 0; i < _rotor_count; i++) { + outputs.control[i] = 0.0f; + } + } // publish message mav_msgs::MotorSpeed rotor_vel_msg; @@ -204,3 +215,8 @@ void MultirotorMixer::mix() { return 0; } + +void MultirotorMixer::actuatorArmedCallback(const PX4_TOPIC_T(actuator_armed) &msg) +{ + _armed = msg.armed; +} -- cgit v1.2.3 From 48b781ca3df159cc69358b08f8f5647f2a0d4f3e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 2 Jan 2015 16:11:47 +0100 Subject: manual input: deadzones --- .../ros/nodes/manual_input/manual_input.cpp | 24 +++++++++++++++------- .../ros/nodes/manual_input/manual_input.h | 4 +++- 2 files changed, 20 insertions(+), 8 deletions(-) diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index eddacf55f..475d0c4d2 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.cpp +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -48,22 +48,28 @@ ManualInput::ManualInput() : _joy_sub(_n.subscribe("joy", 1, &ManualInput::JoyCallback, this)), _man_ctrl_sp_pub(_n.advertise("manual_control_setpoint", 1)) { + double dz_default = 0.2; /* Load parameters, default values work for Microsoft XBox Controller */ _n.param("map_x", _param_axes_map[0], 4); _n.param("scale_x", _param_axes_scale[0], 1.0); _n.param("off_x", _param_axes_offset[0], 0.0); + _n.param("dz_x", _param_axes_dz[0], dz_default); _n.param("map_y", _param_axes_map[1], 3); _n.param("scale_y", _param_axes_scale[1], -1.0); _n.param("off_y", _param_axes_offset[1], 0.0); + _n.param("dz_y", _param_axes_dz[1], dz_default); _n.param("map_z", _param_axes_map[2], 2); _n.param("scale_z", _param_axes_scale[2], -0.5); - _n.param("off_z", _param_axes_offset[2], 0.5); + _n.param("off_z", _param_axes_offset[2], -1.0); + _n.param("dz_z", _param_axes_dz[2], dz_default); _n.param("map_r", _param_axes_map[3], 0); _n.param("scale_r", _param_axes_scale[3], -1.0); _n.param("off_r", _param_axes_offset[3], 0.0); + _n.param("dz_r", _param_axes_dz[3], dz_default); + } void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg) @@ -72,10 +78,10 @@ void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg) /* Fill px4 manual control setpoint topic with contents from ros joystick */ /* Map sticks to x, y, z, r */ - MapAxis(msg, _param_axes_map[0], _param_axes_scale[0], _param_axes_offset[0], msg_out.x); - MapAxis(msg, _param_axes_map[1], _param_axes_scale[1], _param_axes_offset[1], msg_out.y); - MapAxis(msg, _param_axes_map[2], _param_axes_scale[2], _param_axes_offset[2], msg_out.z); - MapAxis(msg, _param_axes_map[3], _param_axes_scale[3], _param_axes_offset[3], msg_out.r); + MapAxis(msg, _param_axes_map[0], _param_axes_scale[0], _param_axes_offset[0], _param_axes_dz[0], msg_out.x); + MapAxis(msg, _param_axes_map[1], _param_axes_scale[1], _param_axes_offset[1], _param_axes_dz[1], msg_out.y); + MapAxis(msg, _param_axes_map[2], _param_axes_scale[2], _param_axes_offset[2], _param_axes_dz[2], msg_out.z); + MapAxis(msg, _param_axes_map[3], _param_axes_scale[3], _param_axes_offset[3], _param_axes_dz[3], msg_out.r); //ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_out.x, msg_out.y, msg_out.z, msg_out.r); /* Map buttons to switches */ @@ -93,9 +99,13 @@ void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg) _man_ctrl_sp_pub.publish(msg_out); } -void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, double scale, double offset, float &out) +void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, double scale, double offset, + double deadzone, float &out) { - out = (float)(msg->axes[map_index] * scale + offset); + double val = msg->axes[map_index]; + if (val + offset > deadzone || val + offset < -deadzone) { + out = (float)((val + offset)) * scale; + } } int main(int argc, char **argv) diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h index f579acbb7..fb516d375 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.h +++ b/src/platforms/ros/nodes/manual_input/manual_input.h @@ -56,7 +56,8 @@ protected: /** * Helper function to map and scale joystick input */ - void MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, double scale, double offset, float &out); + void MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, double scale, double offset, double deadzone, + float &out); ros::NodeHandle _n; ros::Subscriber _joy_sub; @@ -66,6 +67,7 @@ protected: int _param_axes_map[4]; double _param_axes_scale[4]; double _param_axes_offset[4]; + double _param_axes_dz[4]; }; -- cgit v1.2.3 From 06879bff38a88e6468c73dbef5cf6c2ecfdd72b2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 2 Jan 2015 16:12:21 +0100 Subject: reduce default queue size --- src/platforms/px4_nodehandle.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 97fb25563..0e77852f2 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -140,7 +140,7 @@ public: private: - static const uint32_t kQueueSizeDefault = 1000; /**< Size of queue for ROS */ + static const uint32_t kQueueSizeDefault = 1; /**< Size of queue for ROS */ std::list _subs; /**< Subcriptions of node */ std::list _pubs; /**< Publications of node */ }; -- cgit v1.2.3 From e16c4ff76e7eff2da88bcbef5d05dd4ba11e7203 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 2 Jan 2015 22:46:50 +0100 Subject: read attitude and attitude speed from imu message --- .../attitude_estimator/attitude_estimator.cpp | 58 ++++++++++++++++++++-- .../nodes/attitude_estimator/attitude_estimator.h | 6 +-- 2 files changed, 56 insertions(+), 8 deletions(-) diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp index 163b111be..46c836b36 100644 --- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp @@ -46,7 +46,8 @@ AttitudeEstimator::AttitudeEstimator() : _n(), - _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &AttitudeEstimator::ModelStatesCallback, this)), + // _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &AttitudeEstimator::ModelStatesCallback, this)), + _sub_imu(_n.subscribe("/vtol/imu", 1, &AttitudeEstimator::ImuCallback, this)), _vehicle_attitude_pub(_n.advertise("vehicle_attitude", 1)) { } @@ -60,10 +61,11 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP /* Convert quaternion to rotation matrix */ math::Quaternion quat; //XXX: search for vtol or other (other than 'plane') vehicle here - quat(0) = (float)msg->pose[1].orientation.w; - quat(1) = (float)msg->pose[1].orientation.x; - quat(2) = (float)-msg->pose[1].orientation.y; - quat(3) = (float)-msg->pose[1].orientation.z; + int index = 1; + quat(0) = (float)msg->pose[index].orientation.w; + quat(1) = (float)msg->pose[index].orientation.x; + quat(2) = (float)-msg->pose[index].orientation.y; + quat(3) = (float)-msg->pose[index].orientation.z; msg_v_att.q[0] = quat(0); msg_v_att.q[1] = quat(1); @@ -83,9 +85,55 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP msg_v_att.pitch = euler(1); msg_v_att.yaw = euler(2); + //XXX this is in inertial frame + // msg_v_att.rollspeed = (float)msg->twist[index].angular.x; + // msg_v_att.pitchspeed = -(float)msg->twist[index].angular.y; + // msg_v_att.yawspeed = -(float)msg->twist[index].angular.z; + _vehicle_attitude_pub.publish(msg_v_att); } +void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr& msg) +{ + px4::vehicle_attitude msg_v_att; + + /* Fill px4 attitude topic with contents from modelstates topic */ + + /* Convert quaternion to rotation matrix */ + math::Quaternion quat; + //XXX: search for vtol or other (other than 'plane') vehicle here + int index = 1; + quat(0) = (float)msg->orientation.w; + quat(1) = (float)msg->orientation.x; + quat(2) = (float)-msg->orientation.y; + quat(3) = (float)-msg->orientation.z; + + msg_v_att.q[0] = quat(0); + msg_v_att.q[1] = quat(1); + msg_v_att.q[2] = quat(2); + msg_v_att.q[3] = quat(3); + + math::Matrix<3, 3> rot = quat.to_dcm(); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + PX4_R(msg_v_att.R, i, j) = rot(i, j); + } + } + msg_v_att.R_valid = true; + + math::Vector<3> euler = rot.to_euler(); + msg_v_att.roll = euler(0); + msg_v_att.pitch = euler(1); + msg_v_att.yaw = euler(2); + + msg_v_att.rollspeed = (float)msg->angular_velocity.x; + msg_v_att.pitchspeed = -(float)msg->angular_velocity.y; + msg_v_att.yawspeed = -(float)msg->angular_velocity.z; + + _vehicle_attitude_pub.publish(msg_v_att); +} + + int main(int argc, char **argv) { ros::init(argc, argv, "attitude_estimator"); diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h index e3326b715..7c09985f3 100644 --- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h @@ -40,6 +40,7 @@ #include "ros/ros.h" #include +#include class AttitudeEstimator { public: @@ -48,13 +49,12 @@ public: ~AttitudeEstimator() {} protected: - /** - * Takes ROS joystick message and converts/publishes to px4 manual control setpoint topic - */ void ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr& msg); + void ImuCallback(const sensor_msgs::ImuConstPtr& msg); ros::NodeHandle _n; ros::Subscriber _sub_modelstates; + ros::Subscriber _sub_imu; ros::Publisher _vehicle_attitude_pub; -- cgit v1.2.3 From ef485177acb123483fe42d49b3ab23671ee3aa31 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 Jan 2015 10:25:39 +0100 Subject: make Matrix.hpp more consistent with upstream --- src/lib/mathlib/math/Matrix.hpp | 29 ++++++++--------------------- 1 file changed, 8 insertions(+), 21 deletions(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index fc57d7964..43ba6b9d9 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -178,9 +178,8 @@ public: bool operator !=(const Matrix &m) const { for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m.data[i][j]) { + if (data[i][j] != m.data[i][j]) return true; - } return false; } @@ -202,7 +201,6 @@ public: for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) res.data[i][j] = -data[i][j]; - } return res; } @@ -216,7 +214,6 @@ public: for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) res.data[i][j] = data[i][j] + m.data[i][j]; - } return res; } @@ -225,7 +222,6 @@ public: for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) data[i][j] += m.data[i][j]; - } return *static_cast*>(this); } @@ -237,9 +233,8 @@ public: Matrix res; for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) { + for (unsigned int j = 0; j < N; j++) res.data[i][j] = data[i][j] - m.data[i][j]; - } return res; } @@ -248,7 +243,6 @@ public: for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) data[i][j] -= m.data[i][j]; - } return *static_cast*>(this); } @@ -260,19 +254,16 @@ public: Matrix res; for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) { + for (unsigned int j = 0; j < N; j++) res.data[i][j] = data[i][j] * num; - } return res; - } Matrix &operator *=(const float num) { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) { + for (unsigned int j = 0; j < N; j++) data[i][j] *= num; - } return *static_cast*>(this); } @@ -281,18 +272,16 @@ public: Matrix res; for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) { + for (unsigned int j = 0; j < N; j++) res[i][j] = data[i][j] / num; - } return res; } Matrix &operator /=(const float num) { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) { + for (unsigned int j = 0; j < N; j++) data[i][j] /= num; - } return *static_cast*>(this); } @@ -365,18 +354,16 @@ public: memset(data, 0, sizeof(data)); unsigned int n = (M < N) ? M : N; - for (unsigned int i = 0; i < n; i++) { + for (unsigned int i = 0; i < n; i++) data[i][i] = 1; - } } void print(void) { for (unsigned int i = 0; i < M; i++) { printf("[ "); - for (unsigned int j = 0; j < N; j++) { + for (unsigned int j = 0; j < N; j++) printf("%.3f\t", data[i][j]); - } printf(" ]\n"); } -- cgit v1.2.3 From 17e544ebf3c65338db891edc3615e6fd942f6ab4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 Jan 2015 13:09:32 +0100 Subject: dummy mixer: add offset param --- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 25 +++++++++++++++---------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 4dc428a27..5db180052 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -115,7 +115,10 @@ MultirotorMixer::MultirotorMixer(): _sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback,this); _pub = _n.advertise("/mixed_motor_commands",10); if (!_n.hasParam("motor_scaling_radps")) { - _n.setParam("motor_scaling_radps", 1500.0); + _n.setParam("motor_scaling_radps", 150.0); + } + if (!_n.hasParam("motor_offset_radps")) { + _n.setParam("motor_offset_radps", 600.0); } _sub_actuator_armed = _n.subscribe("actuator_armed", 1, &MultirotorMixer::actuatorArmedCallback,this); } @@ -189,20 +192,22 @@ void MultirotorMixer::mix() { } // mix - if (_armed) { - mix(); - } else { - for (unsigned i = 0; i < _rotor_count; i++) { - outputs.control[i] = 0.0f; - } - } + mix(); // publish message mav_msgs::MotorSpeed rotor_vel_msg; double scaling; + double offset; _n.getParamCached("motor_scaling_radps", scaling); - for (int i = 0; i < _rotor_count; i++) { - rotor_vel_msg.motor_speed.push_back(outputs.control[i] * scaling); + _n.getParamCached("motor_offset_radps", offset); + if (_armed) { + for (int i = 0; i < _rotor_count; i++) { + rotor_vel_msg.motor_speed.push_back(outputs.control[i] * scaling + offset); + } + } else { + for (int i = 0; i < _rotor_count; i++) { + rotor_vel_msg.motor_speed.push_back(0.0); + } } _pub.publish(rotor_vel_msg); } -- cgit v1.2.3 From 8d6e1c4455dea2dc13a1a9b08b997ee3df8c7309 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 Jan 2015 14:29:10 +0100 Subject: update ros parameters --- launch/multicopter.launch | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/launch/multicopter.launch b/launch/multicopter.launch index 2ccd1338b..9956c871d 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -11,7 +11,14 @@ + + + + + + + -- cgit v1.2.3 From 5745189e7b27a7b0b190c746f632318e3c1ee4d7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 Jan 2015 15:22:08 +0100 Subject: fix some errors/warnings in multiplatform examples --- src/examples/publisher/publisher_main.cpp | 5 +++-- src/examples/subscriber/subscriber_example.h | 2 ++ src/examples/subscriber/subscriber_main.cpp | 5 +++-- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp index 5b50ecc6c..5cac42250 100644 --- a/src/examples/publisher/publisher_main.cpp +++ b/src/examples/publisher/publisher_main.cpp @@ -52,7 +52,8 @@ using namespace px4; PX4_MAIN_FUNCTION(publisher); #if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__)) -extern "C" __EXPORT int publisher_main(int argc, char *argv[]) +extern "C" __EXPORT int publisher_main(int argc, char *argv[]); +int publisher_main(int argc, char *argv[]) { if (argc < 1) { errx(1, "usage: publisher {start|stop|status}"); @@ -73,7 +74,7 @@ extern "C" __EXPORT int publisher_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 5, 2000, publisher_task_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char* const*)&argv[2] : (char* const*)NULL); exit(0); } diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index c4b853d4d..eb2c956e0 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -41,6 +41,8 @@ using namespace px4; +void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg); + class SubscriberExample { public: SubscriberExample(); diff --git a/src/examples/subscriber/subscriber_main.cpp b/src/examples/subscriber/subscriber_main.cpp index 716233739..c824e34e3 100644 --- a/src/examples/subscriber/subscriber_main.cpp +++ b/src/examples/subscriber/subscriber_main.cpp @@ -52,7 +52,8 @@ using namespace px4; PX4_MAIN_FUNCTION(subscriber); #if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__)) -extern "C" __EXPORT int subscriber_main(int argc, char *argv[]) +extern "C" __EXPORT int subscriber_main(int argc, char *argv[]); +int subscriber_main(int argc, char *argv[]) { if (argc < 1) { errx(1, "usage: subscriber {start|stop|status}"); @@ -73,7 +74,7 @@ extern "C" __EXPORT int subscriber_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 5, 2000, subscriber_task_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char* const*)&argv[2] : (char* const*)NULL); exit(0); } -- cgit v1.2.3 From 1fe70a845ddd5ba9721748ceced81327efa47193 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 Jan 2015 15:22:43 +0100 Subject: turn off werror for now --- makefiles/toolchain_gnu-arm-eabi.mk | 1 - 1 file changed, 1 deletion(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index ef70d51f2..caaf4ca87 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -127,7 +127,6 @@ ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics # ARCHWARNINGS = -Wall \ -Wextra \ - -Werror \ -Wdouble-promotion \ -Wshadow \ -Wfloat-equal \ -- cgit v1.2.3 From a35818a2884987c0cabddb673638799d420118bd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 Jan 2015 15:23:12 +0100 Subject: update rc_channels.msg to latest master rc_channels.h --- msg/rc_channels.msg | 3 +++ 1 file changed, 3 insertions(+) diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg index 4e0e5b494..861f3dcaa 100644 --- a/msg/rc_channels.msg +++ b/msg/rc_channels.msg @@ -15,6 +15,9 @@ uint8 RC_CHANNELS_FUNCTION_AUX_2=12 uint8 RC_CHANNELS_FUNCTION_AUX_3=13 uint8 RC_CHANNELS_FUNCTION_AUX_4=14 uint8 RC_CHANNELS_FUNCTION_AUX_5=15 +uint8 RC_CHANNELS_FUNCTION_PARAM_1=16 +uint8 RC_CHANNELS_FUNCTION_PARAM_2=17 +uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18 uint64 timestamp # Timestamp in microseconds since boot time uint64 timestamp_last_valid # Timestamp of last valid RC signal float32[18] channels # Scaled to -1..1 (throttle: 0..1) -- cgit v1.2.3 From 88a498d8f8598c1ad480df057064d7c2eda2836f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 Jan 2015 15:23:49 +0100 Subject: sensor: fix variable names --- src/modules/sensors/sensors.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 1ca9ecd8f..f27fc127f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -834,7 +834,7 @@ Sensors::parameters_update() _rc.function[RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1; for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - _rc.function[PARAM_1 + i] = _parameters.rc_map_param[i] - 1; + _rc.function[RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1; } /* gyro offsets */ @@ -891,8 +891,8 @@ Sensors::parameters_update() return ERROR; } close(flowfd); - } - + } + get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); @@ -1498,7 +1498,7 @@ Sensors::rc_parameter_map_poll(bool forced) orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map); /* update paramter handles to which the RC channels are mapped */ for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - if (_rc.function[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { + if (_rc.function[RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) * or no request to map this channel to a param has been sent via mavlink */ @@ -1710,14 +1710,14 @@ Sensors::set_params_from_rc() { static float param_rc_values[RC_PARAM_MAP_NCHAN] = {}; for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - if (_rc.function[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { + if (_rc.function[RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) * or no request to map this channel to a param has been sent via mavlink */ continue; } - float rc_val = get_rc_value((enum RC_CHANNELS_FUNCTION)(PARAM_1 + i), -1.0, 1.0); + float rc_val = get_rc_value((enum RC_CHANNELS_FUNCTION)(RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0); /* Check if the value has changed, * maybe we need to introduce a more aggressive limit here */ if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) { -- cgit v1.2.3 From 7507e4a4b5dd11bc23fd3ccebc9bcf3aa1162821 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 Jan 2015 15:31:28 +0100 Subject: sensors: argument is not an enum anymore --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f27fc127f..f6ffedc6b 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1717,7 +1717,7 @@ Sensors::set_params_from_rc() continue; } - float rc_val = get_rc_value((enum RC_CHANNELS_FUNCTION)(RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0); + float rc_val = get_rc_value((RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0); /* Check if the value has changed, * maybe we need to introduce a more aggressive limit here */ if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) { -- cgit v1.2.3 From 9c06450d94a2a462872e5e59ac5285c2d7fe6323 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 Jan 2015 15:31:56 +0100 Subject: fix RC_CHANNELS_FUNCTION_MAX --- msg/rc_channels.msg | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg index 861f3dcaa..0fa5ed2fc 100644 --- a/msg/rc_channels.msg +++ b/msg/rc_channels.msg @@ -1,4 +1,4 @@ -int32 RC_CHANNELS_FUNCTION_MAX=18 +int32 RC_CHANNELS_FUNCTION_MAX=19 uint8 RC_CHANNELS_FUNCTION_THROTTLE=0 uint8 RC_CHANNELS_FUNCTION_ROLL=1 uint8 RC_CHANNELS_FUNCTION_PITCH=2 @@ -20,8 +20,8 @@ uint8 RC_CHANNELS_FUNCTION_PARAM_2=17 uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18 uint64 timestamp # Timestamp in microseconds since boot time uint64 timestamp_last_valid # Timestamp of last valid RC signal -float32[18] channels # Scaled to -1..1 (throttle: 0..1) +float32[19] channels # Scaled to -1..1 (throttle: 0..1) uint8 channel_count # Number of valid channels -int8[18] function # Functions mapping +int8[19] function # Functions mapping uint8 rssi # Receive signal strength index bool signal_lost # Control signal lost, should be checked together with topic timeout -- cgit v1.2.3 From 941ff05720b4ba8c282a84e1f8933469ae7fc39e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 Jan 2015 15:58:38 +0100 Subject: uavcan gives compile errors, disable for now --- makefiles/config_px4fmu-v2_default.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 86b2f7cf2..e0bd99285 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -70,7 +70,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led -MODULES += modules/uavcan +# MODULES += modules/uavcan # # Estimation modules (EKF/ SO3 / other filters) -- cgit v1.2.3 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 --- CMakeLists.txt | 6 +- makefiles/config_px4fmu-v2_default.mk | 1 + src/modules/mc_att_control/mc_att_control.cpp | 298 ------- src/modules/mc_att_control/mc_att_control.h | 131 --- src/modules/mc_att_control/mc_att_control_base.cpp | 309 ------- src/modules/mc_att_control/mc_att_control_base.h | 134 --- src/modules/mc_att_control/mc_att_control_main.cpp | 942 +++++++++++++++++++-- src/modules/mc_att_control/mc_att_control_params.c | 42 +- src/modules/mc_att_control/mc_att_control_params.h | 65 -- src/modules/mc_att_control/mc_att_control_sim.cpp | 142 ---- src/modules/mc_att_control/mc_att_control_sim.h | 97 --- src/modules/mc_att_control/module.mk | 4 +- .../mc_att_control.cpp | 298 +++++++ .../mc_att_control_multiplatform/mc_att_control.h | 131 +++ .../mc_att_control_base.cpp | 309 +++++++ .../mc_att_control_base.h | 134 +++ .../mc_att_control_main.cpp | 139 +++ .../mc_att_control_params.c | 248 ++++++ .../mc_att_control_params.h | 65 ++ .../mc_att_control_sim.cpp | 142 ++++ .../mc_att_control_sim.h | 97 +++ src/modules/mc_att_control_multiplatform/module.mk | 43 + 22 files changed, 2519 insertions(+), 1258 deletions(-) delete mode 100644 src/modules/mc_att_control/mc_att_control.cpp delete mode 100644 src/modules/mc_att_control/mc_att_control.h delete mode 100644 src/modules/mc_att_control/mc_att_control_base.cpp delete mode 100644 src/modules/mc_att_control/mc_att_control_base.h delete mode 100644 src/modules/mc_att_control/mc_att_control_params.h delete mode 100644 src/modules/mc_att_control/mc_att_control_sim.cpp delete mode 100644 src/modules/mc_att_control/mc_att_control_sim.h create mode 100644 src/modules/mc_att_control_multiplatform/mc_att_control.cpp create mode 100644 src/modules/mc_att_control_multiplatform/mc_att_control.h create mode 100644 src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp create mode 100644 src/modules/mc_att_control_multiplatform/mc_att_control_base.h create mode 100644 src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp create mode 100644 src/modules/mc_att_control_multiplatform/mc_att_control_params.c create mode 100644 src/modules/mc_att_control_multiplatform/mc_att_control_params.h create mode 100644 src/modules/mc_att_control_multiplatform/mc_att_control_sim.cpp create mode 100644 src/modules/mc_att_control_multiplatform/mc_att_control_sim.h create mode 100644 src/modules/mc_att_control_multiplatform/module.mk diff --git a/CMakeLists.txt b/CMakeLists.txt index cbd646556..95a6f12fe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -168,9 +168,9 @@ target_link_libraries(subscriber ## MC Attitude Control add_executable(mc_att_control - src/modules/mc_att_control/mc_att_control_main.cpp - src/modules/mc_att_control/mc_att_control.cpp - src/modules/mc_att_control/mc_att_control_base.cpp) + src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp + src/modules/mc_att_control_multiplatform/mc_att_control.cpp + src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp) add_dependencies(mc_att_control px4_generate_messages_cpp) target_link_libraries(mc_att_control ${catkin_LIBRARIES} diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index e0bd99285..d5c3f4a22 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -86,6 +86,7 @@ MODULES += modules/position_estimator_inav #MODULES += modules/fw_pos_control_l1 #MODULES += modules/fw_att_control MODULES += modules/mc_att_control +MODULES += modules/mc_att_control_multiplatform MODULES += modules/mc_pos_control MODULES += modules/vtol_att_control diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp deleted file mode 100644 index 822a504b5..000000000 --- a/src/modules/mc_att_control/mc_att_control.cpp +++ /dev/null @@ -1,298 +0,0 @@ -/**************************************************************************** - * - * 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.cpp - * Multicopter attitude controller. - * - * @author Tobias Naegeli - * @author Lorenz Meier - * @author Anton Babushkin - * @author Thomas Gubler - * @author Julian Oes - * @author Roman Bapst - */ - -#include "mc_att_control.h" -#include "mc_att_control_params.h" -#include "math.h" - -#define YAW_DEADZONE 0.05f -#define MIN_TAKEOFF_THRUST 0.2f -#define RATES_I_LIMIT 0.3f - -namespace mc_att_control -{ - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -} - -MulticopterAttitudeControl::MulticopterAttitudeControl() : - MulticopterAttitudeControlBase(), - _task_should_exit(false), - _actuators_0_circuit_breaker_enabled(false), - - /* publications */ - _att_sp_pub(nullptr), - _v_rates_sp_pub(nullptr), - _actuators_0_pub(nullptr), - _n(), - - /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) - -{ - _params_handles.roll_p = PX4_PARAM_INIT(MC_ROLL_P); - _params_handles.roll_rate_p = PX4_PARAM_INIT(MC_ROLLRATE_P); - _params_handles.roll_rate_i = PX4_PARAM_INIT(MC_ROLLRATE_I); - _params_handles.roll_rate_d = PX4_PARAM_INIT(MC_ROLLRATE_D); - _params_handles.pitch_p = PX4_PARAM_INIT(MC_PITCH_P); - _params_handles.pitch_rate_p = PX4_PARAM_INIT(MC_PITCHRATE_P); - _params_handles.pitch_rate_i = PX4_PARAM_INIT(MC_PITCHRATE_I); - _params_handles.pitch_rate_d = PX4_PARAM_INIT(MC_PITCHRATE_D); - _params_handles.yaw_p = PX4_PARAM_INIT(MC_YAW_P); - _params_handles.yaw_rate_p = PX4_PARAM_INIT(MC_YAWRATE_P); - _params_handles.yaw_rate_i = PX4_PARAM_INIT(MC_YAWRATE_I); - _params_handles.yaw_rate_d = PX4_PARAM_INIT(MC_YAWRATE_D); - _params_handles.yaw_ff = PX4_PARAM_INIT(MC_YAW_FF); - _params_handles.yaw_rate_max = PX4_PARAM_INIT(MC_YAWRATE_MAX); - _params_handles.man_roll_max = PX4_PARAM_INIT(MC_MAN_R_MAX); - _params_handles.man_pitch_max = PX4_PARAM_INIT(MC_MAN_P_MAX); - _params_handles.man_yaw_max = PX4_PARAM_INIT(MC_MAN_Y_MAX); - _params_handles.acro_roll_max = PX4_PARAM_INIT(MC_ACRO_R_MAX); - _params_handles.acro_pitch_max = PX4_PARAM_INIT(MC_ACRO_P_MAX); - _params_handles.acro_yaw_max = PX4_PARAM_INIT(MC_ACRO_Y_MAX); - - /* fetch initial parameter values */ - parameters_update(); - - /* - * 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); - -} - -MulticopterAttitudeControl::~MulticopterAttitudeControl() -{ -} - -int -MulticopterAttitudeControl::parameters_update() -{ - float v; - - /* roll gains */ - PX4_PARAM_GET(_params_handles.roll_p, &v); - _params.att_p(0) = v; - PX4_PARAM_GET(_params_handles.roll_rate_p, &v); - _params.rate_p(0) = v; - PX4_PARAM_GET(_params_handles.roll_rate_i, &v); - _params.rate_i(0) = v; - PX4_PARAM_GET(_params_handles.roll_rate_d, &v); - _params.rate_d(0) = v; - - /* pitch gains */ - PX4_PARAM_GET(_params_handles.pitch_p, &v); - _params.att_p(1) = v; - PX4_PARAM_GET(_params_handles.pitch_rate_p, &v); - _params.rate_p(1) = v; - PX4_PARAM_GET(_params_handles.pitch_rate_i, &v); - _params.rate_i(1) = v; - PX4_PARAM_GET(_params_handles.pitch_rate_d, &v); - _params.rate_d(1) = v; - - /* yaw gains */ - PX4_PARAM_GET(_params_handles.yaw_p, &v); - _params.att_p(2) = v; - PX4_PARAM_GET(_params_handles.yaw_rate_p, &v); - _params.rate_p(2) = v; - PX4_PARAM_GET(_params_handles.yaw_rate_i, &v); - _params.rate_i(2) = v; - PX4_PARAM_GET(_params_handles.yaw_rate_d, &v); - _params.rate_d(2) = v; - - PX4_PARAM_GET(_params_handles.yaw_ff, &_params.yaw_ff); - PX4_PARAM_GET(_params_handles.yaw_rate_max, &_params.yaw_rate_max); - _params.yaw_rate_max = math::radians(_params.yaw_rate_max); - - /* manual control scale */ - PX4_PARAM_GET(_params_handles.man_roll_max, &_params.man_roll_max); - PX4_PARAM_GET(_params_handles.man_pitch_max, &_params.man_pitch_max); - PX4_PARAM_GET(_params_handles.man_yaw_max, &_params.man_yaw_max); - _params.man_roll_max = math::radians(_params.man_roll_max); - _params.man_pitch_max = math::radians(_params.man_pitch_max); - _params.man_yaw_max = math::radians(_params.man_yaw_max); - - /* acro control scale */ - PX4_PARAM_GET(_params_handles.acro_roll_max, &v); - _params.acro_rate_max(0) = math::radians(v); - PX4_PARAM_GET(_params_handles.acro_pitch_max, &v); - _params.acro_rate_max(1) = math::radians(v); - PX4_PARAM_GET(_params_handles.acro_yaw_max, &v); - _params.acro_rate_max(2) = math::radians(v); - - _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); - - return OK; -} - -void MulticopterAttitudeControl::handle_parameter_update(const PX4_TOPIC_T(parameter_update) &msg) -{ - parameters_update(); -} - -void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) { - - perf_begin(_loop_perf); - - /* run controller on attitude changes */ - static uint64_t last_run = 0; - float dt = (px4::get_time_micros() - last_run) / 1000000.0f; - last_run = px4::get_time_micros(); - - /* 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; - } - - if (_v_control_mode->get().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 (_att_sp_pub != nullptr) { - _att_sp_pub->publish(_v_att_sp_mod); - - } else { - _att_sp_pub = PX4_ADVERTISE(_n, vehicle_attitude_setpoint); - } - } - - /* 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(); - - 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); - } else { - _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint); - } - } - - } else { - /* attitude controller disabled, poll rates setpoint topic */ - if (_v_control_mode->get().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; - - /* 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(); - - 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); - } else { - _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint); - } - } - - } 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; - } - } - - if (_v_control_mode->get().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(); - - 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); - } else { - _actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_0); - } - } - } - } -} diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h deleted file mode 100644 index bff5289fd..000000000 --- a/src/modules/mc_att_control/mc_att_control.h +++ /dev/null @@ -1,131 +0,0 @@ -/**************************************************************************** - * - * 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.h - * 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 -#include -#include -#include -#include -// #include -#include -#include - -#include "mc_att_control_base.h" - -class MulticopterAttitudeControl : - public MulticopterAttitudeControlBase -{ -public: - /** - * Constructor - */ - MulticopterAttitudeControl(); - - /** - * Destructor, also kills the sensors task. - */ - ~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 spin() { _n.spin(); } - -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::NodeHandle _n; - - struct { - px4_param_t roll_p; - px4_param_t roll_rate_p; - px4_param_t roll_rate_i; - px4_param_t roll_rate_d; - px4_param_t pitch_p; - px4_param_t pitch_rate_p; - px4_param_t pitch_rate_i; - px4_param_t pitch_rate_d; - px4_param_t yaw_p; - px4_param_t yaw_rate_p; - px4_param_t yaw_rate_i; - px4_param_t yaw_rate_d; - px4_param_t yaw_ff; - px4_param_t yaw_rate_max; - - px4_param_t man_roll_max; - px4_param_t man_pitch_max; - px4_param_t man_yaw_max; - px4_param_t acro_roll_max; - px4_param_t acro_pitch_max; - px4_param_t acro_yaw_max; - - px4_param_t autostart_id; - } _params_handles; /**< handles for interesting parameters */ - - perf_counter_t _loop_perf; /**< loop performance counter */ - - /** - * Update our local parameter cache. - */ - int parameters_update(); -}; - diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp deleted file mode 100644 index aff59778e..000000000 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ /dev/null @@ -1,309 +0,0 @@ -/* 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_base.cpp - * - * MC Attitude Controller : Control and math code - * - * @author Tobias Naegeli - * @author Lorenz Meier - * @author Anton Babushkin - * @author Thomas Gubler - * @author Julian Oes - * @author Roman Bapst - * - */ - -#include "mc_att_control_base.h" -#include -#include -#include - -#ifdef CONFIG_ARCH_ARM -#else -#include -using namespace std; -#endif - -MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : - _publish_att_sp(false) - -{ - memset(&_v_att_sp_mod, 0, sizeof(_v_att_sp_mod)); - memset(&_v_rates_sp_mod, 0, sizeof(_v_rates_sp_mod)); - memset(&_actuators, 0, sizeof(_actuators)); - - _params.att_p.zero(); - _params.rate_p.zero(); - _params.rate_i.zero(); - _params.rate_d.zero(); - _params.yaw_ff = 0.0f; - _params.yaw_rate_max = 0.0f; - _params.man_roll_max = 0.0f; - _params.man_pitch_max = 0.0f; - _params.man_yaw_max = 0.0f; - _params.acro_rate_max.zero(); - - _rates_prev.zero(); - _rates_sp.zero(); - _rates_int.zero(); - _thrust_sp = 0.0f; - _att_control.zero(); - - _I.identity(); -} - -MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() -{ -} - -void MulticopterAttitudeControlBase::control_attitude(float dt) -{ - float yaw_sp_move_rate = 0.0f; - _publish_att_sp = false; - - - if (_v_control_mode->get().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) { - /* 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)); - } - - if (!_v_control_mode->get().flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude stabilized mode */ - _v_att_sp_mod.thrust = _manual_control_sp->get().z; - _publish_att_sp = true; - } - - if (!_armed->get().armed) { - /* reset yaw setpoint when disarmed */ - _reset_yaw_sp = true; - } - - /* move yaw setpoint in all modes */ - if (_v_att_sp_mod.thrust < 0.1f) { - // TODO - //if (_status.condition_landed) { - /* reset yaw setpoint if on ground */ - // reset_yaw_sp = true; - //} - } 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); - 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); - - if (yaw_offs < -yaw_offs_max) { - _v_att_sp_mod.yaw_body = _wrap_pi(_v_att->get().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.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; - // _publish_att_sp = true; - } - - if (!_v_control_mode->get().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 - * _params.man_pitch_max; - _v_att_sp_mod.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)); - - /* reset yaw setpoint after non-manual control mode */ - _reset_yaw_sp = true; - } - - _thrust_sp = _v_att_sp_mod.thrust; - - /* construct attitude setpoint rotation matrix */ - math::Matrix<3, 3> R_sp; - - if (_v_att_sp_mod.R_valid) { - /* rotation matrix in _att_sp is valid, use it */ - R_sp.set(&_v_att_sp_mod.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); - - /* 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; - } - - /* rotation matrix for current state */ - math::Matrix<3, 3> R; - R.set(_v_att->get().R); - - /* all input data is ready, run controller itself */ - - /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ - math::Vector < 3 > R_z(R(0, 2), R(1, 2), R(2, 2)); - math::Vector < 3 > R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); - - /* axis and sin(angle) of desired rotation */ - math::Vector < 3 > e_R = R.transposed() * (R_z % R_sp_z); - - /* calculate angle error */ - float e_R_z_sin = e_R.length(); - float e_R_z_cos = R_z * R_sp_z; - - /* calculate weight for yaw control */ - float yaw_w = R_sp(2, 2) * R_sp(2, 2); - - /* calculate rotation matrix after roll/pitch only rotation */ - math::Matrix<3, 3> R_rp; - - if (e_R_z_sin > 0.0f) { - /* get axis-angle representation */ - float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); - math::Vector < 3 > e_R_z_axis = e_R / e_R_z_sin; - - e_R = e_R_z_axis * e_R_z_angle; - - /* cross product matrix for e_R_axis */ - math::Matrix<3, 3> e_R_cp; - e_R_cp.zero(); - e_R_cp(0, 1) = -e_R_z_axis(2); - e_R_cp(0, 2) = e_R_z_axis(1); - e_R_cp(1, 0) = e_R_z_axis(2); - e_R_cp(1, 2) = -e_R_z_axis(0); - e_R_cp(2, 0) = -e_R_z_axis(1); - e_R_cp(2, 1) = e_R_z_axis(0); - - /* rotation matrix for roll/pitch only rotation */ - R_rp = R - * (_I + e_R_cp * e_R_z_sin - + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); - - } else { - /* zero roll/pitch rotation */ - R_rp = R; - } - - /* R_rp and R_sp has the same Z axis, calculate yaw error */ - math::Vector < 3 > R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); - math::Vector < 3 > R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); - e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; - - if (e_R_z_cos < 0.0f) { - /* for large thrust vector rotations use another rotation method: - * calculate angle and axis for R -> R_sp rotation directly */ - math::Quaternion q; - q.from_dcm(R.transposed() * R_sp); - math::Vector < 3 > e_R_d = q.imag(); - e_R_d.normalize(); - e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); - - /* use fusion of Z axis based rotation and direct rotation */ - float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; - e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; - } - - /* calculate angular rates setpoint */ - _rates_sp = _params.att_p.emult(e_R); - - /* limit yaw rate */ - _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, - _params.yaw_rate_max); - - /* feed forward yaw setpoint rate */ - _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; -} - -void MulticopterAttitudeControlBase::control_attitude_rates(float dt) -{ - /* reset integral if disarmed */ - if (!_armed->get().armed || !_v_status->get().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; - - /* angular rates error */ - math::Vector < 3 > rates_err = _rates_sp - rates; - _att_control = _params.rate_p.emult(rates_err) - + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; - _rates_prev = rates; - - /* update integral only if not saturated on low limit */ - if (_thrust_sp > MIN_TAKEOFF_THRUST) { - for (int i = 0; i < 3; i++) { - if (fabsf(_att_control(i)) < _thrust_sp) { - float rate_i = _rates_int(i) - + _params.rate_i(i) * rates_err(i) * dt; - - if (isfinite( - rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && - _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { - _rates_int(i) = rate_i; - } - } - } - } - -} - -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; -} diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h deleted file mode 100644 index cf4c726a7..000000000 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ /dev/null @@ -1,134 +0,0 @@ -#ifndef MC_ATT_CONTROL_BASE_H_ -#define MC_ATT_CONTROL_BASE_H_ - -/* 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_base.h - * - * MC Attitude Controller : Control and math code - * - * @author Tobias Naegeli - * @author Lorenz Meier - * @author Anton Babushkin - * @author Thomas Gubler - * @author Julian Oes - * @author Roman Bapst - * - */ -#include -#include -#include -#include -#include -#include -#include - -#include -#include - - - -#define YAW_DEADZONE 0.05f -#define MIN_TAKEOFF_THRUST 0.2f -#define RATES_I_LIMIT 0.3f - -class MulticopterAttitudeControlBase -{ -public: - /** - * Constructor - */ - MulticopterAttitudeControlBase(); - - /** - * Destructor - */ - ~MulticopterAttitudeControlBase(); - - /** - * Start the sensors task. - * - * @return OK on success. - */ - void control_attitude(float dt); - void control_attitude_rates(float dt); - - 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 - that gets published eventually */ - PX4_TOPIC_T(vehicle_rates_setpoint) _v_rates_sp_mod; /**< vehicle rates setpoint - that gets published eventually*/ - PX4_TOPIC_T(actuator_controls) _actuators; /**< actuator controls */ - - math::Vector<3> _rates_prev; /**< angular rates on previous step */ - math::Vector<3> _rates_sp; /**< angular rates setpoint */ - math::Vector<3> _rates_int; /**< angular rates integral error */ - float _thrust_sp; /**< thrust setpoint */ - math::Vector<3> _att_control; /**< attitude control vector */ - - math::Matrix<3, 3> _I; /**< identity matrix */ - - bool _reset_yaw_sp; /**< reset yaw setpoint flag */ - - struct { - math::Vector<3> att_p; /**< P gain for angular error */ - math::Vector<3> rate_p; /**< P gain for angular rate error */ - math::Vector<3> rate_i; /**< I gain for angular rate error */ - math::Vector<3> rate_d; /**< D gain for angular rate error */ - float yaw_ff; /**< yaw control feed-forward */ - float yaw_rate_max; /**< max yaw rate */ - - float man_roll_max; - float man_pitch_max; - float man_yaw_max; - math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ - - int32_t autostart_id; - } _params; - - bool _publish_att_sp; - -}; - -#endif /* MC_ATT_CONTROL_BASE_H_ */ 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 080f0ed65..67ae90877 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -38,9 +38,6 @@ * @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. @@ -52,91 +49,928 @@ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. */ +#include +#include +#include #include -#include -#include "mc_att_control.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -static bool thread_running = false; /**< Deamon status flag */ -static int daemon_task; /**< Handle of deamon task / thread */ -namespace px4 +/** + * Multicopter attitude control app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); + +#define YAW_DEADZONE 0.05f +#define MIN_TAKEOFF_THRUST 0.2f +#define RATES_I_LIMIT 0.3f + +class MulticopterAttitudeControl { -bool task_should_exit = false; +public: + /** + * Constructor + */ + MulticopterAttitudeControl(); + + /** + * Destructor, also kills the main task + */ + ~MulticopterAttitudeControl(); + + /** + * Start the multicopter attitude control task. + * + * @return OK on success. + */ + int start(); + +private: + + bool _task_should_exit; /**< if true, task_main() should exit */ + int _control_task; /**< task handle */ + + int _v_att_sub; /**< vehicle attitude subscription */ + int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ + int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */ + int _v_control_mode_sub; /**< vehicle control mode subscription */ + int _params_sub; /**< parameter updates subscription */ + int _manual_control_sp_sub; /**< manual control setpoint subscription */ + int _armed_sub; /**< arming status subscription */ + int _vehicle_status_sub; /**< vehicle status subscription */ + + orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ + orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ + orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ + + orb_id_t _rates_sp_id; /**< pointer to correct rates setpoint uORB metadata structure */ + orb_id_t _actuators_id; /**< pointer to correct actuator controls0 uORB metadata structure */ + + bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ + + struct vehicle_attitude_s _v_att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ + struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ + struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ + struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator controls */ + struct actuator_armed_s _armed; /**< actuator arming status */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + math::Vector<3> _rates_prev; /**< angular rates on previous step */ + math::Vector<3> _rates_sp; /**< angular rates setpoint */ + math::Vector<3> _rates_int; /**< angular rates integral error */ + float _thrust_sp; /**< thrust setpoint */ + math::Vector<3> _att_control; /**< attitude control vector */ + + math::Matrix<3, 3> _I; /**< identity matrix */ + + bool _reset_yaw_sp; /**< reset yaw setpoint flag */ + + struct { + param_t roll_p; + param_t roll_rate_p; + param_t roll_rate_i; + param_t roll_rate_d; + param_t pitch_p; + param_t pitch_rate_p; + param_t pitch_rate_i; + param_t pitch_rate_d; + param_t yaw_p; + param_t yaw_rate_p; + param_t yaw_rate_i; + param_t yaw_rate_d; + param_t yaw_ff; + param_t yaw_rate_max; + + param_t man_roll_max; + param_t man_pitch_max; + param_t man_yaw_max; + param_t acro_roll_max; + param_t acro_pitch_max; + param_t acro_yaw_max; + + } _params_handles; /**< handles for interesting parameters */ + + struct { + math::Vector<3> att_p; /**< P gain for angular error */ + math::Vector<3> rate_p; /**< P gain for angular rate error */ + math::Vector<3> rate_i; /**< I gain for angular rate error */ + math::Vector<3> rate_d; /**< D gain for angular rate error */ + float yaw_ff; /**< yaw control feed-forward */ + float yaw_rate_max; /**< max yaw rate */ + + float man_roll_max; + float man_pitch_max; + float man_yaw_max; + math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ + + } _params; + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Check for parameter update and handle it. + */ + void parameter_update_poll(); + + /** + * Check for changes in vehicle control mode. + */ + void vehicle_control_mode_poll(); + + /** + * Check for changes in manual inputs. + */ + void vehicle_manual_poll(); + + /** + * Check for attitude setpoint updates. + */ + void vehicle_attitude_setpoint_poll(); + + /** + * Check for rates setpoint updates. + */ + void vehicle_rates_setpoint_poll(); + + /** + * Check for arming status updates. + */ + void arming_status_poll(); + + /** + * Attitude controller. + */ + void control_attitude(float dt); + + /** + * Attitude rates controller. + */ + void control_attitude_rates(float dt); + + /** + * Check for vehicle status updates. + */ + void vehicle_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 +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +MulticopterAttitudeControl *g_control; } -using namespace px4; +MulticopterAttitudeControl::MulticopterAttitudeControl() : + + _task_should_exit(false), + _control_task(-1), + +/* subscriptions */ + _v_att_sub(-1), + _v_att_sp_sub(-1), + _v_control_mode_sub(-1), + _params_sub(-1), + _manual_control_sp_sub(-1), + _armed_sub(-1), + _vehicle_status_sub(-1), + +/* publications */ + _att_sp_pub(-1), + _v_rates_sp_pub(-1), + _actuators_0_pub(-1), + _rates_sp_id(0), + _actuators_id(0), -PX4_MAIN_FUNCTION(mc_att_control); -void handle_vehicle_attitude2(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp); + _actuators_0_circuit_breaker_enabled(false), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) + +{ + memset(&_v_att, 0, sizeof(_v_att)); + memset(&_v_att_sp, 0, sizeof(_v_att_sp)); + memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); + memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); + memset(&_v_control_mode, 0, sizeof(_v_control_mode)); + memset(&_actuators, 0, sizeof(_actuators)); + memset(&_armed, 0, sizeof(_armed)); + memset(&_vehicle_status, 0, sizeof(_vehicle_status)); + _vehicle_status.is_rotary_wing = true; + + _params.att_p.zero(); + _params.rate_p.zero(); + _params.rate_i.zero(); + _params.rate_d.zero(); + _params.yaw_ff = 0.0f; + _params.yaw_rate_max = 0.0f; + _params.man_roll_max = 0.0f; + _params.man_pitch_max = 0.0f; + _params.man_yaw_max = 0.0f; + _params.acro_rate_max.zero(); + + _rates_prev.zero(); + _rates_sp.zero(); + _rates_int.zero(); + _thrust_sp = 0.0f; + _att_control.zero(); + + _I.identity(); + + _params_handles.roll_p = param_find("MC_ROLL_P"); + _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); + _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); + _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); + _params_handles.pitch_p = param_find("MC_PITCH_P"); + _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); + _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); + _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); + _params_handles.yaw_p = param_find("MC_YAW_P"); + _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); + _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); + _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + _params_handles.yaw_ff = param_find("MC_YAW_FF"); + _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); + _params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); + _params_handles.man_pitch_max = param_find("MC_MAN_P_MAX"); + _params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX"); + _params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX"); + _params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX"); + _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); + + /* fetch initial parameter values */ + parameters_update(); } +MulticopterAttitudeControl::~MulticopterAttitudeControl() +{ + if (_control_task != -1) { + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; -#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__)) -/** - * Multicopter attitude control app start / stop handling function - * - * @ingroup apps + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_control_task); + break; + } + } while (_control_task != -1); + } + + mc_att_control::g_control = nullptr; +} + +int +MulticopterAttitudeControl::parameters_update() +{ + float v; + + /* roll gains */ + param_get(_params_handles.roll_p, &v); + _params.att_p(0) = v; + param_get(_params_handles.roll_rate_p, &v); + _params.rate_p(0) = v; + param_get(_params_handles.roll_rate_i, &v); + _params.rate_i(0) = v; + param_get(_params_handles.roll_rate_d, &v); + _params.rate_d(0) = v; + + /* pitch gains */ + param_get(_params_handles.pitch_p, &v); + _params.att_p(1) = v; + param_get(_params_handles.pitch_rate_p, &v); + _params.rate_p(1) = v; + param_get(_params_handles.pitch_rate_i, &v); + _params.rate_i(1) = v; + param_get(_params_handles.pitch_rate_d, &v); + _params.rate_d(1) = v; + + /* yaw gains */ + param_get(_params_handles.yaw_p, &v); + _params.att_p(2) = v; + param_get(_params_handles.yaw_rate_p, &v); + _params.rate_p(2) = v; + param_get(_params_handles.yaw_rate_i, &v); + _params.rate_i(2) = v; + param_get(_params_handles.yaw_rate_d, &v); + _params.rate_d(2) = v; + + param_get(_params_handles.yaw_ff, &_params.yaw_ff); + param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); + _params.yaw_rate_max = math::radians(_params.yaw_rate_max); + + /* manual control scale */ + param_get(_params_handles.man_roll_max, &_params.man_roll_max); + param_get(_params_handles.man_pitch_max, &_params.man_pitch_max); + param_get(_params_handles.man_yaw_max, &_params.man_yaw_max); + _params.man_roll_max = math::radians(_params.man_roll_max); + _params.man_pitch_max = math::radians(_params.man_pitch_max); + _params.man_yaw_max = math::radians(_params.man_yaw_max); + + /* acro control scale */ + param_get(_params_handles.acro_roll_max, &v); + _params.acro_rate_max(0) = math::radians(v); + param_get(_params_handles.acro_pitch_max, &v); + _params.acro_rate_max(1) = math::radians(v); + param_get(_params_handles.acro_yaw_max, &v); + _params.acro_rate_max(2) = math::radians(v); + + _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); + + return OK; +} + +void +MulticopterAttitudeControl::parameter_update_poll() +{ + bool updated; + + /* Check if parameters have changed */ + orb_check(_params_sub, &updated); + + if (updated) { + struct parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); + parameters_update(); + } +} + +void +MulticopterAttitudeControl::vehicle_control_mode_poll() +{ + bool updated; + + /* Check if vehicle control mode has changed */ + orb_check(_v_control_mode_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); + } +} + +void +MulticopterAttitudeControl::vehicle_manual_poll() +{ + bool updated; + + /* get pilots inputs */ + orb_check(_manual_control_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); + } +} + +void +MulticopterAttitudeControl::vehicle_attitude_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_v_att_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp); + } +} + +void +MulticopterAttitudeControl::vehicle_rates_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_v_rates_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp); + } +} + +void +MulticopterAttitudeControl::arming_status_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + } +} + +void +MulticopterAttitudeControl::vehicle_status_poll() +{ + /* check if there is new status information */ + bool vehicle_status_updated; + orb_check(_vehicle_status_sub, &vehicle_status_updated); + + if (vehicle_status_updated) { + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + /* set correct uORB ID, depending on if vehicle is VTOL or not */ + if (!_rates_sp_id) { + if (_vehicle_status.is_vtol) { + _rates_sp_id = ORB_ID(mc_virtual_rates_setpoint); + _actuators_id = ORB_ID(actuator_controls_virtual_mc); + } else { + _rates_sp_id = ORB_ID(vehicle_rates_setpoint); + _actuators_id = ORB_ID(actuator_controls_0); + } + } + } +} + +/* + * Attitude controller. + * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode) + * Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes) */ +void +MulticopterAttitudeControl::control_attitude(float dt) +{ + float yaw_sp_move_rate = 0.0f; + bool publish_att_sp = false; + + if (_v_control_mode.flag_control_manual_enabled) { + /* manual input, set or modify attitude setpoint */ + + if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) { + /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ + vehicle_attitude_setpoint_poll(); + } + + if (!_v_control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude stabilized mode */ + _v_att_sp.thrust = _manual_control_sp.z; + publish_att_sp = true; + } -extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]) + if (!_armed.armed) { + /* reset yaw setpoint when disarmed */ + _reset_yaw_sp = true; + } + + /* move yaw setpoint in all modes */ + if (_v_att_sp.thrust < 0.1f) { + // TODO + //if (_status.condition_landed) { + /* reset yaw setpoint if on ground */ + // reset_yaw_sp = true; + //} + } else { + /* move yaw setpoint */ + yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max; + _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.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.yaw_body - _v_att.yaw); + if (yaw_offs < - yaw_offs_max) { + _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max); + + } else if (yaw_offs > yaw_offs_max) { + _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); + } + _v_att_sp.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.yaw_body = _v_att.yaw; + _v_att_sp.R_valid = false; + publish_att_sp = true; + } + + if (!_v_control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + _v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max; + _v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max; + _v_att_sp.R_valid = false; + publish_att_sp = true; + } + + } else { + /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ + vehicle_attitude_setpoint_poll(); + + /* reset yaw setpoint after non-manual control mode */ + _reset_yaw_sp = true; + } + + _thrust_sp = _v_att_sp.thrust; + + /* construct attitude setpoint rotation matrix */ + math::Matrix<3, 3> R_sp; + + if (_v_att_sp.R_valid) { + /* rotation matrix in _att_sp is valid, use it */ + R_sp.set(&_v_att_sp.R_body[0]); + + } else { + /* rotation matrix in _att_sp is not valid, use euler angles instead */ + R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); + + /* copy rotation matrix back to setpoint struct */ + memcpy(&_v_att_sp.R_body[0], &R_sp.data[0], sizeof(_v_att_sp.R_body)); + _v_att_sp.R_valid = true; + } + + /* publish the attitude setpoint if needed */ + if (publish_att_sp && _vehicle_status.is_rotary_wing) { + _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); + + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); + } + } + + /* rotation matrix for current state */ + math::Matrix<3, 3> R; + R.set(_v_att.R); + + /* all input data is ready, run controller itself */ + + /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ + math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2)); + math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); + + /* axis and sin(angle) of desired rotation */ + math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z); + + /* calculate angle error */ + float e_R_z_sin = e_R.length(); + float e_R_z_cos = R_z * R_sp_z; + + /* calculate weight for yaw control */ + float yaw_w = R_sp(2, 2) * R_sp(2, 2); + + /* calculate rotation matrix after roll/pitch only rotation */ + math::Matrix<3, 3> R_rp; + + if (e_R_z_sin > 0.0f) { + /* get axis-angle representation */ + float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); + math::Vector<3> e_R_z_axis = e_R / e_R_z_sin; + + e_R = e_R_z_axis * e_R_z_angle; + + /* cross product matrix for e_R_axis */ + math::Matrix<3, 3> e_R_cp; + e_R_cp.zero(); + e_R_cp(0, 1) = -e_R_z_axis(2); + e_R_cp(0, 2) = e_R_z_axis(1); + e_R_cp(1, 0) = e_R_z_axis(2); + e_R_cp(1, 2) = -e_R_z_axis(0); + e_R_cp(2, 0) = -e_R_z_axis(1); + e_R_cp(2, 1) = e_R_z_axis(0); + + /* rotation matrix for roll/pitch only rotation */ + R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + + } else { + /* zero roll/pitch rotation */ + R_rp = R; + } + + /* R_rp and R_sp has the same Z axis, calculate yaw error */ + math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); + math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); + e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; + + if (e_R_z_cos < 0.0f) { + /* for large thrust vector rotations use another rotation method: + * calculate angle and axis for R -> R_sp rotation directly */ + math::Quaternion q; + q.from_dcm(R.transposed() * R_sp); + math::Vector<3> e_R_d = q.imag(); + e_R_d.normalize(); + e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); + + /* use fusion of Z axis based rotation and direct rotation */ + float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; + e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; + } + + /* calculate angular rates setpoint */ + _rates_sp = _params.att_p.emult(e_R); + + /* limit yaw rate */ + _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max); + + /* feed forward yaw setpoint rate */ + _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; +} + +/* + * Attitude rates controller. + * Input: '_rates_sp' vector, '_thrust_sp' + * Output: '_att_control' vector + */ +void +MulticopterAttitudeControl::control_attitude_rates(float dt) { - if (argc < 1) { - errx(1, "usage: mc_att_control {start|stop|status}"); + /* reset integral if disarmed */ + if (!_armed.armed || !_vehicle_status.is_rotary_wing) { + _rates_int.zero(); } - if (!strcmp(argv[1], "start")) { + /* current body angular rates */ + math::Vector<3> rates; + rates(0) = _v_att.rollspeed; + rates(1) = _v_att.pitchspeed; + rates(2) = _v_att.yawspeed; + + /* angular rates error */ + math::Vector<3> rates_err = _rates_sp - rates; + _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; + _rates_prev = rates; + + /* update integral only if not saturated on low limit */ + if (_thrust_sp > MIN_TAKEOFF_THRUST) { + for (int i = 0; i < 3; i++) { + if (fabsf(_att_control(i)) < _thrust_sp) { + float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; + + if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { + _rates_int(i) = rate_i; + } + } + } + } +} + +void +MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) +{ + mc_att_control::g_control->task_main(); +} + +void +MulticopterAttitudeControl::task_main() +{ - if (thread_running) { - warnx("already running"); - /* this is not an error */ - exit(0); + /* + * do subscriptions + */ + _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _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)); + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + + /* initialize parameters cache */ + parameters_update(); + + /* wakeup source: vehicle attitude */ + struct pollfd fds[1]; + + fds[0].fd = _v_att_sub; + fds[0].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 100ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit */ + if (pret == 0) + continue; + + /* 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; + } + + perf_begin(_loop_perf); + + /* 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(); + + /* 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); + + /* check for updates in other topics */ + parameter_update_poll(); + vehicle_control_mode_poll(); + arming_status_poll(); + vehicle_manual_poll(); + vehicle_status_poll(); + + if (_v_control_mode.flag_control_attitude_enabled) { + control_attitude(dt); + + /* 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(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); + + } else if (_rates_sp_id) { + _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_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(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); + + } else if (_rates_sp_id) { + _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_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; + } + } + + if (_v_control_mode.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 = hrt_absolute_time(); + + if (!_actuators_0_circuit_breaker_enabled) { + if (_actuators_0_pub > 0) { + orb_publish(_actuators_id, _actuators_0_pub, &_actuators); + + } else if (_actuators_id) { + _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); + } + + } + } } - task_should_exit = false; + perf_end(_loop_perf); + } + + warnx("exit"); + + _control_task = -1; + _exit(0); +} + +int +MulticopterAttitudeControl::start() +{ + ASSERT(_control_task == -1); - daemon_task = task_spawn_cmd("mc_att_control", + /* start the task */ + _control_task = task_spawn_cmd("mc_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 3000, - mc_att_control_task_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + 2000, + (main_t)&MulticopterAttitudeControl::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int mc_att_control_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: mc_att_control {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (mc_att_control::g_control != nullptr) + errx(1, "already running"); + + mc_att_control::g_control = new MulticopterAttitudeControl; + + if (mc_att_control::g_control == nullptr) + errx(1, "alloc failed"); + + if (OK != mc_att_control::g_control->start()) { + delete mc_att_control::g_control; + mc_att_control::g_control = nullptr; + err(1, "start failed"); + } exit(0); } if (!strcmp(argv[1], "stop")) { - task_should_exit = true; + 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); } if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("is running"); + if (mc_att_control::g_control) { + errx(0, "running"); } else { - warnx("not started"); + errx(1, "not running"); } - - exit(0); } warnx("unrecognized command"); return 1; } -#endif - -PX4_MAIN_FUNCTION(mc_att_control) -{ - px4::init(argc, argv, "mc_att_control"); - - PX4_INFO("starting"); - MulticopterAttitudeControl attctl; - thread_running = true; - attctl.spin(); - - PX4_INFO("exiting."); - thread_running = false; - return 0; -} - - diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index a0b1706f2..819847b40 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -40,8 +40,6 @@ * @author Anton Babushkin */ -#include -#include "mc_att_control_params.h" #include /** @@ -52,7 +50,7 @@ * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_ROLL_P); +PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f); /** * Roll rate P gain @@ -62,7 +60,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ROLL_P); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_P); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); /** * Roll rate I gain @@ -72,7 +70,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_P); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_I); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); /** * Roll rate D gain @@ -82,7 +80,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_I); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_D); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); /** * Pitch P gain @@ -93,7 +91,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_D); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_PITCH_P); +PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f); /** * Pitch rate P gain @@ -103,7 +101,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_PITCH_P); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_P); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); /** * Pitch rate I gain @@ -113,7 +111,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_P); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_I); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); /** * Pitch rate D gain @@ -123,7 +121,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_I); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_D); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); /** * Yaw P gain @@ -134,7 +132,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_D); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_YAW_P); +PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); /** * Yaw rate P gain @@ -144,7 +142,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAW_P); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_P); +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); /** * Yaw rate I gain @@ -154,7 +152,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_P); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_I); +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); /** * Yaw rate D gain @@ -164,7 +162,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_I); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_D); +PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); /** * Yaw feed forward @@ -175,7 +173,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_D); * @max 1.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF); +PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); /** * Max yaw rate @@ -187,7 +185,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF); * @max 360.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX); +PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f); /** * Max manual roll @@ -197,7 +195,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX); * @max 90.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_MAN_R_MAX); +PARAM_DEFINE_FLOAT(MC_MAN_R_MAX, 35.0f); /** * Max manual pitch @@ -207,7 +205,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_MAN_R_MAX); * @max 90.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_MAN_P_MAX); +PARAM_DEFINE_FLOAT(MC_MAN_P_MAX, 35.0f); /** * Max manual yaw rate @@ -216,7 +214,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_MAN_P_MAX); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX); +PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX, 120.0f); /** * Max acro roll rate @@ -226,7 +224,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX); * @max 360.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX); +PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f); /** * Max acro pitch rate @@ -236,7 +234,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX); * @max 360.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX); +PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f); /** * Max acro yaw rate @@ -245,4 +243,4 @@ PX4_PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX); +PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 120.0f); diff --git a/src/modules/mc_att_control/mc_att_control_params.h b/src/modules/mc_att_control/mc_att_control_params.h deleted file mode 100644 index 59dd5240f..000000000 --- a/src/modules/mc_att_control/mc_att_control_params.h +++ /dev/null @@ -1,65 +0,0 @@ - -/**************************************************************************** - * - * 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_params.h - * Parameters for multicopter attitude controller. - * - * @author Tobias Naegeli - * @author Lorenz Meier - * @author Anton Babushkin - * @author Thomas Gubler - */ -#pragma once - -#define PARAM_MC_ROLL_P_DEFAULT 6.0f -#define PARAM_MC_ROLLRATE_P_DEFAULT 0.1f -#define PARAM_MC_ROLLRATE_I_DEFAULT 0.0f -#define PARAM_MC_ROLLRATE_D_DEFAULT 0.002f -#define PARAM_MC_PITCH_P_DEFAULT 6.0f -#define PARAM_MC_PITCHRATE_P_DEFAULT 0.1f -#define PARAM_MC_PITCHRATE_I_DEFAULT 0.0f -#define PARAM_MC_PITCHRATE_D_DEFAULT 0.002f -#define PARAM_MC_YAW_P_DEFAULT 2.0f -#define PARAM_MC_YAWRATE_P_DEFAULT 0.3f -#define PARAM_MC_YAWRATE_I_DEFAULT 0.0f -#define PARAM_MC_YAWRATE_D_DEFAULT 0.0f -#define PARAM_MC_YAW_FF_DEFAULT 0.5f -#define PARAM_MC_YAWRATE_MAX_DEFAULT 120.0f -#define PARAM_MC_MAN_R_MAX_DEFAULT 35.0f -#define PARAM_MC_MAN_P_MAX_DEFAULT 35.0f -#define PARAM_MC_MAN_Y_MAX_DEFAULT 120.0f -#define PARAM_MC_ACRO_R_MAX_DEFAULT 35.0f -#define PARAM_MC_ACRO_P_MAX_DEFAULT 35.0f -#define PARAM_MC_ACRO_Y_MAX_DEFAULT 120.0f diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp deleted file mode 100644 index d516d7e52..000000000 --- a/src/modules/mc_att_control/mc_att_control_sim.cpp +++ /dev/null @@ -1,142 +0,0 @@ -/* 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_sim.cpp - * - * MC Attitude Controller Interface for usage in a simulator - * - * @author Roman Bapst - * - */ - -#include "mc_att_control_sim.h" -#include -#include - -#ifdef CONFIG_ARCH_ARM -#else -#include -using namespace std; -#endif - -MulticopterAttitudeControlSim::MulticopterAttitudeControlSim() -{ - /* setup standard gains */ - //XXX: make these configurable - _params.att_p(0) = 5.0; - _params.rate_p(0) = 0.05; - _params.rate_i(0) = 0.0; - _params.rate_d(0) = 0.003; - /* pitch gains */ - _params.att_p(1) = 5.0; - _params.rate_p(1) = 0.05; - _params.rate_i(1) = 0.0; - _params.rate_d(1) = 0.003; - /* yaw gains */ - _params.att_p(2) = 2.8; - _params.rate_p(2) = 0.2; - _params.rate_i(2) = 0.1; - _params.rate_d(2) = 0.0; - _params.yaw_rate_max = 0.5; - _params.yaw_ff = 0.5; - _params.man_roll_max = 0.6; - _params.man_pitch_max = 0.6; - _params.man_yaw_max = 0.6; -} - -MulticopterAttitudeControlSim::~MulticopterAttitudeControlSim() -{ -} - -void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion attitude) -{ - math::Quaternion quat; - quat(0) = (float)attitude.w(); - quat(1) = (float)attitude.x(); - quat(2) = (float)attitude.y(); - quat(3) = (float)attitude.z(); - - _v_att.q[0] = quat(0); - _v_att.q[1] = quat(1); - _v_att.q[2] = quat(2); - _v_att.q[3] = quat(3); - - math::Matrix<3, 3> Rot = quat.to_dcm(); - _v_att.R[0][0] = Rot(0, 0); - _v_att.R[1][0] = Rot(1, 0); - _v_att.R[2][0] = Rot(2, 0); - _v_att.R[0][1] = Rot(0, 1); - _v_att.R[1][1] = Rot(1, 1); - _v_att.R[2][1] = Rot(2, 1); - _v_att.R[0][2] = Rot(0, 2); - _v_att.R[1][2] = Rot(1, 2); - _v_att.R[2][2] = Rot(2, 2); - - _v_att.R_valid = true; -} - -void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d &angular_rate) -{ - // check if this is consistent !!! - _v_att.rollspeed = angular_rate(0); - _v_att.pitchspeed = angular_rate(1); - _v_att.yawspeed = angular_rate(2); -} - -void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference) -{ - _v_att_sp.roll_body = control_attitude_thrust_reference(0); - _v_att_sp.pitch_body = control_attitude_thrust_reference(1); - _v_att_sp.yaw_body = control_attitude_thrust_reference(2); - _v_att_sp.thrust = (control_attitude_thrust_reference(3) - 30) * (-1) / 30; - - // setup rotation matrix - math::Matrix<3, 3> Rot_sp; - Rot_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); - _v_att_sp.R_body[0][0] = Rot_sp(0, 0); - _v_att_sp.R_body[1][0] = Rot_sp(1, 0); - _v_att_sp.R_body[2][0] = Rot_sp(2, 0); - _v_att_sp.R_body[0][1] = Rot_sp(0, 1); - _v_att_sp.R_body[1][1] = Rot_sp(1, 1); - _v_att_sp.R_body[2][1] = Rot_sp(2, 1); - _v_att_sp.R_body[0][2] = Rot_sp(0, 2); - _v_att_sp.R_body[1][2] = Rot_sp(1, 2); - _v_att_sp.R_body[2][2] = Rot_sp(2, 2); -} - -void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d &motor_inputs) -{ - motor_inputs(0) = _actuators.control[0]; - motor_inputs(1) = _actuators.control[1]; - motor_inputs(2) = _actuators.control[2]; - motor_inputs(3) = _actuators.control[3]; -} diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h deleted file mode 100644 index a1bf44fc9..000000000 --- a/src/modules/mc_att_control/mc_att_control_sim.h +++ /dev/null @@ -1,97 +0,0 @@ -#ifndef MC_ATT_CONTROL_BASE_H_ -#define MC_ATT_CONTROL_BASE_H_ - -/* 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_sim.h - * - * MC Attitude Controller Interface for usage in a simulator - * - * @author Roman Bapst - * - */ - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#inlcude "mc_att_control_base.h" - - - -#define YAW_DEADZONE 0.05f -#define MIN_TAKEOFF_THRUST 0.2f -#define RATES_I_LIMIT 0.3f - -class MulticopterAttitudeControlSim : - public MulticopterAttitudeControlBase - -{ -public: - /** - * Constructor - */ - MulticopterAttitudeControlSim(); - - /** - * Destructor - */ - ~MulticopterAttitudeControlSim(); - - /* setters and getters for interface with euroc-gazebo simulator */ - void set_attitude(const Eigen::Quaternion attitude); - void set_attitude_rates(const Eigen::Vector3d &angular_rate); - void set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference); - void get_mixer_input(Eigen::Vector4d &motor_inputs); - -protected: - void vehicle_attitude_setpoint_poll() {}; - - -}; - -#endif /* MC_ATT_CONTROL_BASE_H_ */ diff --git a/src/modules/mc_att_control/module.mk b/src/modules/mc_att_control/module.mk index 29254f5bb..64b876f69 100644 --- a/src/modules/mc_att_control/module.mk +++ b/src/modules/mc_att_control/module.mk @@ -38,6 +38,4 @@ MODULE_COMMAND = mc_att_control SRCS = mc_att_control_main.cpp \ - mc_att_control.cpp \ - mc_att_control_base.cpp \ - mc_att_control_params.c + mc_att_control_params.c diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp new file mode 100644 index 000000000..822a504b5 --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp @@ -0,0 +1,298 @@ +/**************************************************************************** + * + * 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.cpp + * Multicopter attitude controller. + * + * @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * @author Thomas Gubler + * @author Julian Oes + * @author Roman Bapst + */ + +#include "mc_att_control.h" +#include "mc_att_control_params.h" +#include "math.h" + +#define YAW_DEADZONE 0.05f +#define MIN_TAKEOFF_THRUST 0.2f +#define RATES_I_LIMIT 0.3f + +namespace mc_att_control +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +} + +MulticopterAttitudeControl::MulticopterAttitudeControl() : + MulticopterAttitudeControlBase(), + _task_should_exit(false), + _actuators_0_circuit_breaker_enabled(false), + + /* publications */ + _att_sp_pub(nullptr), + _v_rates_sp_pub(nullptr), + _actuators_0_pub(nullptr), + _n(), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) + +{ + _params_handles.roll_p = PX4_PARAM_INIT(MC_ROLL_P); + _params_handles.roll_rate_p = PX4_PARAM_INIT(MC_ROLLRATE_P); + _params_handles.roll_rate_i = PX4_PARAM_INIT(MC_ROLLRATE_I); + _params_handles.roll_rate_d = PX4_PARAM_INIT(MC_ROLLRATE_D); + _params_handles.pitch_p = PX4_PARAM_INIT(MC_PITCH_P); + _params_handles.pitch_rate_p = PX4_PARAM_INIT(MC_PITCHRATE_P); + _params_handles.pitch_rate_i = PX4_PARAM_INIT(MC_PITCHRATE_I); + _params_handles.pitch_rate_d = PX4_PARAM_INIT(MC_PITCHRATE_D); + _params_handles.yaw_p = PX4_PARAM_INIT(MC_YAW_P); + _params_handles.yaw_rate_p = PX4_PARAM_INIT(MC_YAWRATE_P); + _params_handles.yaw_rate_i = PX4_PARAM_INIT(MC_YAWRATE_I); + _params_handles.yaw_rate_d = PX4_PARAM_INIT(MC_YAWRATE_D); + _params_handles.yaw_ff = PX4_PARAM_INIT(MC_YAW_FF); + _params_handles.yaw_rate_max = PX4_PARAM_INIT(MC_YAWRATE_MAX); + _params_handles.man_roll_max = PX4_PARAM_INIT(MC_MAN_R_MAX); + _params_handles.man_pitch_max = PX4_PARAM_INIT(MC_MAN_P_MAX); + _params_handles.man_yaw_max = PX4_PARAM_INIT(MC_MAN_Y_MAX); + _params_handles.acro_roll_max = PX4_PARAM_INIT(MC_ACRO_R_MAX); + _params_handles.acro_pitch_max = PX4_PARAM_INIT(MC_ACRO_P_MAX); + _params_handles.acro_yaw_max = PX4_PARAM_INIT(MC_ACRO_Y_MAX); + + /* fetch initial parameter values */ + parameters_update(); + + /* + * 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); + +} + +MulticopterAttitudeControl::~MulticopterAttitudeControl() +{ +} + +int +MulticopterAttitudeControl::parameters_update() +{ + float v; + + /* roll gains */ + PX4_PARAM_GET(_params_handles.roll_p, &v); + _params.att_p(0) = v; + PX4_PARAM_GET(_params_handles.roll_rate_p, &v); + _params.rate_p(0) = v; + PX4_PARAM_GET(_params_handles.roll_rate_i, &v); + _params.rate_i(0) = v; + PX4_PARAM_GET(_params_handles.roll_rate_d, &v); + _params.rate_d(0) = v; + + /* pitch gains */ + PX4_PARAM_GET(_params_handles.pitch_p, &v); + _params.att_p(1) = v; + PX4_PARAM_GET(_params_handles.pitch_rate_p, &v); + _params.rate_p(1) = v; + PX4_PARAM_GET(_params_handles.pitch_rate_i, &v); + _params.rate_i(1) = v; + PX4_PARAM_GET(_params_handles.pitch_rate_d, &v); + _params.rate_d(1) = v; + + /* yaw gains */ + PX4_PARAM_GET(_params_handles.yaw_p, &v); + _params.att_p(2) = v; + PX4_PARAM_GET(_params_handles.yaw_rate_p, &v); + _params.rate_p(2) = v; + PX4_PARAM_GET(_params_handles.yaw_rate_i, &v); + _params.rate_i(2) = v; + PX4_PARAM_GET(_params_handles.yaw_rate_d, &v); + _params.rate_d(2) = v; + + PX4_PARAM_GET(_params_handles.yaw_ff, &_params.yaw_ff); + PX4_PARAM_GET(_params_handles.yaw_rate_max, &_params.yaw_rate_max); + _params.yaw_rate_max = math::radians(_params.yaw_rate_max); + + /* manual control scale */ + PX4_PARAM_GET(_params_handles.man_roll_max, &_params.man_roll_max); + PX4_PARAM_GET(_params_handles.man_pitch_max, &_params.man_pitch_max); + PX4_PARAM_GET(_params_handles.man_yaw_max, &_params.man_yaw_max); + _params.man_roll_max = math::radians(_params.man_roll_max); + _params.man_pitch_max = math::radians(_params.man_pitch_max); + _params.man_yaw_max = math::radians(_params.man_yaw_max); + + /* acro control scale */ + PX4_PARAM_GET(_params_handles.acro_roll_max, &v); + _params.acro_rate_max(0) = math::radians(v); + PX4_PARAM_GET(_params_handles.acro_pitch_max, &v); + _params.acro_rate_max(1) = math::radians(v); + PX4_PARAM_GET(_params_handles.acro_yaw_max, &v); + _params.acro_rate_max(2) = math::radians(v); + + _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); + + return OK; +} + +void MulticopterAttitudeControl::handle_parameter_update(const PX4_TOPIC_T(parameter_update) &msg) +{ + parameters_update(); +} + +void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) { + + perf_begin(_loop_perf); + + /* run controller on attitude changes */ + static uint64_t last_run = 0; + float dt = (px4::get_time_micros() - last_run) / 1000000.0f; + last_run = px4::get_time_micros(); + + /* 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; + } + + if (_v_control_mode->get().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 (_att_sp_pub != nullptr) { + _att_sp_pub->publish(_v_att_sp_mod); + + } else { + _att_sp_pub = PX4_ADVERTISE(_n, vehicle_attitude_setpoint); + } + } + + /* 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(); + + 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); + } else { + _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint); + } + } + + } else { + /* attitude controller disabled, poll rates setpoint topic */ + if (_v_control_mode->get().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; + + /* 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(); + + 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); + } else { + _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint); + } + } + + } 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; + } + } + + if (_v_control_mode->get().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(); + + 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); + } else { + _actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_0); + } + } + } + } +} diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.h b/src/modules/mc_att_control_multiplatform/mc_att_control.h new file mode 100644 index 000000000..bff5289fd --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.h @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * 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.h + * 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 +#include +#include +#include +#include +// #include +#include +#include + +#include "mc_att_control_base.h" + +class MulticopterAttitudeControl : + public MulticopterAttitudeControlBase +{ +public: + /** + * Constructor + */ + MulticopterAttitudeControl(); + + /** + * Destructor, also kills the sensors task. + */ + ~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 spin() { _n.spin(); } + +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::NodeHandle _n; + + struct { + px4_param_t roll_p; + px4_param_t roll_rate_p; + px4_param_t roll_rate_i; + px4_param_t roll_rate_d; + px4_param_t pitch_p; + px4_param_t pitch_rate_p; + px4_param_t pitch_rate_i; + px4_param_t pitch_rate_d; + px4_param_t yaw_p; + px4_param_t yaw_rate_p; + px4_param_t yaw_rate_i; + px4_param_t yaw_rate_d; + px4_param_t yaw_ff; + px4_param_t yaw_rate_max; + + px4_param_t man_roll_max; + px4_param_t man_pitch_max; + px4_param_t man_yaw_max; + px4_param_t acro_roll_max; + px4_param_t acro_pitch_max; + px4_param_t acro_yaw_max; + + px4_param_t autostart_id; + } _params_handles; /**< handles for interesting parameters */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + /** + * Update our local parameter cache. + */ + int parameters_update(); +}; + 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 new file mode 100644 index 000000000..aff59778e --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp @@ -0,0 +1,309 @@ +/* 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_base.cpp + * + * MC Attitude Controller : Control and math code + * + * @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * @author Thomas Gubler + * @author Julian Oes + * @author Roman Bapst + * + */ + +#include "mc_att_control_base.h" +#include +#include +#include + +#ifdef CONFIG_ARCH_ARM +#else +#include +using namespace std; +#endif + +MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : + _publish_att_sp(false) + +{ + memset(&_v_att_sp_mod, 0, sizeof(_v_att_sp_mod)); + memset(&_v_rates_sp_mod, 0, sizeof(_v_rates_sp_mod)); + memset(&_actuators, 0, sizeof(_actuators)); + + _params.att_p.zero(); + _params.rate_p.zero(); + _params.rate_i.zero(); + _params.rate_d.zero(); + _params.yaw_ff = 0.0f; + _params.yaw_rate_max = 0.0f; + _params.man_roll_max = 0.0f; + _params.man_pitch_max = 0.0f; + _params.man_yaw_max = 0.0f; + _params.acro_rate_max.zero(); + + _rates_prev.zero(); + _rates_sp.zero(); + _rates_int.zero(); + _thrust_sp = 0.0f; + _att_control.zero(); + + _I.identity(); +} + +MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() +{ +} + +void MulticopterAttitudeControlBase::control_attitude(float dt) +{ + float yaw_sp_move_rate = 0.0f; + _publish_att_sp = false; + + + if (_v_control_mode->get().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) { + /* 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)); + } + + if (!_v_control_mode->get().flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude stabilized mode */ + _v_att_sp_mod.thrust = _manual_control_sp->get().z; + _publish_att_sp = true; + } + + if (!_armed->get().armed) { + /* reset yaw setpoint when disarmed */ + _reset_yaw_sp = true; + } + + /* move yaw setpoint in all modes */ + if (_v_att_sp_mod.thrust < 0.1f) { + // TODO + //if (_status.condition_landed) { + /* reset yaw setpoint if on ground */ + // reset_yaw_sp = true; + //} + } 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); + 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); + + if (yaw_offs < -yaw_offs_max) { + _v_att_sp_mod.yaw_body = _wrap_pi(_v_att->get().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.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; + // _publish_att_sp = true; + } + + if (!_v_control_mode->get().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 + * _params.man_pitch_max; + _v_att_sp_mod.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)); + + /* reset yaw setpoint after non-manual control mode */ + _reset_yaw_sp = true; + } + + _thrust_sp = _v_att_sp_mod.thrust; + + /* construct attitude setpoint rotation matrix */ + math::Matrix<3, 3> R_sp; + + if (_v_att_sp_mod.R_valid) { + /* rotation matrix in _att_sp is valid, use it */ + R_sp.set(&_v_att_sp_mod.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); + + /* 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; + } + + /* rotation matrix for current state */ + math::Matrix<3, 3> R; + R.set(_v_att->get().R); + + /* all input data is ready, run controller itself */ + + /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ + math::Vector < 3 > R_z(R(0, 2), R(1, 2), R(2, 2)); + math::Vector < 3 > R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); + + /* axis and sin(angle) of desired rotation */ + math::Vector < 3 > e_R = R.transposed() * (R_z % R_sp_z); + + /* calculate angle error */ + float e_R_z_sin = e_R.length(); + float e_R_z_cos = R_z * R_sp_z; + + /* calculate weight for yaw control */ + float yaw_w = R_sp(2, 2) * R_sp(2, 2); + + /* calculate rotation matrix after roll/pitch only rotation */ + math::Matrix<3, 3> R_rp; + + if (e_R_z_sin > 0.0f) { + /* get axis-angle representation */ + float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); + math::Vector < 3 > e_R_z_axis = e_R / e_R_z_sin; + + e_R = e_R_z_axis * e_R_z_angle; + + /* cross product matrix for e_R_axis */ + math::Matrix<3, 3> e_R_cp; + e_R_cp.zero(); + e_R_cp(0, 1) = -e_R_z_axis(2); + e_R_cp(0, 2) = e_R_z_axis(1); + e_R_cp(1, 0) = e_R_z_axis(2); + e_R_cp(1, 2) = -e_R_z_axis(0); + e_R_cp(2, 0) = -e_R_z_axis(1); + e_R_cp(2, 1) = e_R_z_axis(0); + + /* rotation matrix for roll/pitch only rotation */ + R_rp = R + * (_I + e_R_cp * e_R_z_sin + + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + + } else { + /* zero roll/pitch rotation */ + R_rp = R; + } + + /* R_rp and R_sp has the same Z axis, calculate yaw error */ + math::Vector < 3 > R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); + math::Vector < 3 > R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); + e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; + + if (e_R_z_cos < 0.0f) { + /* for large thrust vector rotations use another rotation method: + * calculate angle and axis for R -> R_sp rotation directly */ + math::Quaternion q; + q.from_dcm(R.transposed() * R_sp); + math::Vector < 3 > e_R_d = q.imag(); + e_R_d.normalize(); + e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); + + /* use fusion of Z axis based rotation and direct rotation */ + float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; + e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; + } + + /* calculate angular rates setpoint */ + _rates_sp = _params.att_p.emult(e_R); + + /* limit yaw rate */ + _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, + _params.yaw_rate_max); + + /* feed forward yaw setpoint rate */ + _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; +} + +void MulticopterAttitudeControlBase::control_attitude_rates(float dt) +{ + /* reset integral if disarmed */ + if (!_armed->get().armed || !_v_status->get().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; + + /* angular rates error */ + math::Vector < 3 > rates_err = _rates_sp - rates; + _att_control = _params.rate_p.emult(rates_err) + + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; + _rates_prev = rates; + + /* update integral only if not saturated on low limit */ + if (_thrust_sp > MIN_TAKEOFF_THRUST) { + for (int i = 0; i < 3; i++) { + if (fabsf(_att_control(i)) < _thrust_sp) { + float rate_i = _rates_int(i) + + _params.rate_i(i) * rates_err(i) * dt; + + if (isfinite( + rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { + _rates_int(i) = rate_i; + } + } + } + } + +} + +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; +} 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 new file mode 100644 index 000000000..cf4c726a7 --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h @@ -0,0 +1,134 @@ +#ifndef MC_ATT_CONTROL_BASE_H_ +#define MC_ATT_CONTROL_BASE_H_ + +/* 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_base.h + * + * MC Attitude Controller : Control and math code + * + * @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * @author Thomas Gubler + * @author Julian Oes + * @author Roman Bapst + * + */ +#include +#include +#include +#include +#include +#include +#include + +#include +#include + + + +#define YAW_DEADZONE 0.05f +#define MIN_TAKEOFF_THRUST 0.2f +#define RATES_I_LIMIT 0.3f + +class MulticopterAttitudeControlBase +{ +public: + /** + * Constructor + */ + MulticopterAttitudeControlBase(); + + /** + * Destructor + */ + ~MulticopterAttitudeControlBase(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + void control_attitude(float dt); + void control_attitude_rates(float dt); + + 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 + that gets published eventually */ + PX4_TOPIC_T(vehicle_rates_setpoint) _v_rates_sp_mod; /**< vehicle rates setpoint + that gets published eventually*/ + PX4_TOPIC_T(actuator_controls) _actuators; /**< actuator controls */ + + math::Vector<3> _rates_prev; /**< angular rates on previous step */ + math::Vector<3> _rates_sp; /**< angular rates setpoint */ + math::Vector<3> _rates_int; /**< angular rates integral error */ + float _thrust_sp; /**< thrust setpoint */ + math::Vector<3> _att_control; /**< attitude control vector */ + + math::Matrix<3, 3> _I; /**< identity matrix */ + + bool _reset_yaw_sp; /**< reset yaw setpoint flag */ + + struct { + math::Vector<3> att_p; /**< P gain for angular error */ + math::Vector<3> rate_p; /**< P gain for angular rate error */ + math::Vector<3> rate_i; /**< I gain for angular rate error */ + math::Vector<3> rate_d; /**< D gain for angular rate error */ + float yaw_ff; /**< yaw control feed-forward */ + float yaw_rate_max; /**< max yaw rate */ + + float man_roll_max; + float man_pitch_max; + float man_yaw_max; + math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ + + int32_t autostart_id; + } _params; + + bool _publish_att_sp; + +}; + +#endif /* MC_ATT_CONTROL_BASE_H_ */ 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; +} + + diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.c b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c new file mode 100644 index 000000000..a0b1706f2 --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c @@ -0,0 +1,248 @@ +/**************************************************************************** + * + * 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_params.c + * Parameters for multicopter attitude controller. + * + * @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + */ + +#include +#include "mc_att_control_params.h" +#include + +/** + * Roll P gain + * + * Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_ROLL_P); + +/** + * Roll rate P gain + * + * Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_P); + +/** + * Roll rate I gain + * + * Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_I); + +/** + * Roll rate D gain + * + * Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_D); + +/** + * Pitch P gain + * + * Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @unit 1/s + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_PITCH_P); + +/** + * Pitch rate P gain + * + * Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_P); + +/** + * Pitch rate I gain + * + * Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_I); + +/** + * Pitch rate D gain + * + * Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_D); + +/** + * Yaw P gain + * + * Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @unit 1/s + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_YAW_P); + +/** + * Yaw rate P gain + * + * Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_P); + +/** + * Yaw rate I gain + * + * Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_I); + +/** + * Yaw rate D gain + * + * Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_D); + +/** + * Yaw feed forward + * + * Feed forward weight for manual yaw control. 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF); + +/** + * Max yaw rate + * + * Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation. + * + * @unit deg/s + * @min 0.0 + * @max 360.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX); + +/** + * Max manual roll + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_MAN_R_MAX); + +/** + * Max manual pitch + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_MAN_P_MAX); + +/** + * Max manual yaw rate + * + * @unit deg/s + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX); + +/** + * Max acro roll rate + * + * @unit deg/s + * @min 0.0 + * @max 360.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX); + +/** + * Max acro pitch rate + * + * @unit deg/s + * @min 0.0 + * @max 360.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX); + +/** + * Max acro yaw rate + * + * @unit deg/s + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX); diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.h b/src/modules/mc_att_control_multiplatform/mc_att_control_params.h new file mode 100644 index 000000000..59dd5240f --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_params.h @@ -0,0 +1,65 @@ + +/**************************************************************************** + * + * 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_params.h + * Parameters for multicopter attitude controller. + * + * @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * @author Thomas Gubler + */ +#pragma once + +#define PARAM_MC_ROLL_P_DEFAULT 6.0f +#define PARAM_MC_ROLLRATE_P_DEFAULT 0.1f +#define PARAM_MC_ROLLRATE_I_DEFAULT 0.0f +#define PARAM_MC_ROLLRATE_D_DEFAULT 0.002f +#define PARAM_MC_PITCH_P_DEFAULT 6.0f +#define PARAM_MC_PITCHRATE_P_DEFAULT 0.1f +#define PARAM_MC_PITCHRATE_I_DEFAULT 0.0f +#define PARAM_MC_PITCHRATE_D_DEFAULT 0.002f +#define PARAM_MC_YAW_P_DEFAULT 2.0f +#define PARAM_MC_YAWRATE_P_DEFAULT 0.3f +#define PARAM_MC_YAWRATE_I_DEFAULT 0.0f +#define PARAM_MC_YAWRATE_D_DEFAULT 0.0f +#define PARAM_MC_YAW_FF_DEFAULT 0.5f +#define PARAM_MC_YAWRATE_MAX_DEFAULT 120.0f +#define PARAM_MC_MAN_R_MAX_DEFAULT 35.0f +#define PARAM_MC_MAN_P_MAX_DEFAULT 35.0f +#define PARAM_MC_MAN_Y_MAX_DEFAULT 120.0f +#define PARAM_MC_ACRO_R_MAX_DEFAULT 35.0f +#define PARAM_MC_ACRO_P_MAX_DEFAULT 35.0f +#define PARAM_MC_ACRO_Y_MAX_DEFAULT 120.0f diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_sim.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_sim.cpp new file mode 100644 index 000000000..d516d7e52 --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_sim.cpp @@ -0,0 +1,142 @@ +/* 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_sim.cpp + * + * MC Attitude Controller Interface for usage in a simulator + * + * @author Roman Bapst + * + */ + +#include "mc_att_control_sim.h" +#include +#include + +#ifdef CONFIG_ARCH_ARM +#else +#include +using namespace std; +#endif + +MulticopterAttitudeControlSim::MulticopterAttitudeControlSim() +{ + /* setup standard gains */ + //XXX: make these configurable + _params.att_p(0) = 5.0; + _params.rate_p(0) = 0.05; + _params.rate_i(0) = 0.0; + _params.rate_d(0) = 0.003; + /* pitch gains */ + _params.att_p(1) = 5.0; + _params.rate_p(1) = 0.05; + _params.rate_i(1) = 0.0; + _params.rate_d(1) = 0.003; + /* yaw gains */ + _params.att_p(2) = 2.8; + _params.rate_p(2) = 0.2; + _params.rate_i(2) = 0.1; + _params.rate_d(2) = 0.0; + _params.yaw_rate_max = 0.5; + _params.yaw_ff = 0.5; + _params.man_roll_max = 0.6; + _params.man_pitch_max = 0.6; + _params.man_yaw_max = 0.6; +} + +MulticopterAttitudeControlSim::~MulticopterAttitudeControlSim() +{ +} + +void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion attitude) +{ + math::Quaternion quat; + quat(0) = (float)attitude.w(); + quat(1) = (float)attitude.x(); + quat(2) = (float)attitude.y(); + quat(3) = (float)attitude.z(); + + _v_att.q[0] = quat(0); + _v_att.q[1] = quat(1); + _v_att.q[2] = quat(2); + _v_att.q[3] = quat(3); + + math::Matrix<3, 3> Rot = quat.to_dcm(); + _v_att.R[0][0] = Rot(0, 0); + _v_att.R[1][0] = Rot(1, 0); + _v_att.R[2][0] = Rot(2, 0); + _v_att.R[0][1] = Rot(0, 1); + _v_att.R[1][1] = Rot(1, 1); + _v_att.R[2][1] = Rot(2, 1); + _v_att.R[0][2] = Rot(0, 2); + _v_att.R[1][2] = Rot(1, 2); + _v_att.R[2][2] = Rot(2, 2); + + _v_att.R_valid = true; +} + +void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d &angular_rate) +{ + // check if this is consistent !!! + _v_att.rollspeed = angular_rate(0); + _v_att.pitchspeed = angular_rate(1); + _v_att.yawspeed = angular_rate(2); +} + +void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference) +{ + _v_att_sp.roll_body = control_attitude_thrust_reference(0); + _v_att_sp.pitch_body = control_attitude_thrust_reference(1); + _v_att_sp.yaw_body = control_attitude_thrust_reference(2); + _v_att_sp.thrust = (control_attitude_thrust_reference(3) - 30) * (-1) / 30; + + // setup rotation matrix + math::Matrix<3, 3> Rot_sp; + Rot_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); + _v_att_sp.R_body[0][0] = Rot_sp(0, 0); + _v_att_sp.R_body[1][0] = Rot_sp(1, 0); + _v_att_sp.R_body[2][0] = Rot_sp(2, 0); + _v_att_sp.R_body[0][1] = Rot_sp(0, 1); + _v_att_sp.R_body[1][1] = Rot_sp(1, 1); + _v_att_sp.R_body[2][1] = Rot_sp(2, 1); + _v_att_sp.R_body[0][2] = Rot_sp(0, 2); + _v_att_sp.R_body[1][2] = Rot_sp(1, 2); + _v_att_sp.R_body[2][2] = Rot_sp(2, 2); +} + +void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d &motor_inputs) +{ + motor_inputs(0) = _actuators.control[0]; + motor_inputs(1) = _actuators.control[1]; + motor_inputs(2) = _actuators.control[2]; + motor_inputs(3) = _actuators.control[3]; +} diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_sim.h b/src/modules/mc_att_control_multiplatform/mc_att_control_sim.h new file mode 100644 index 000000000..a1bf44fc9 --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_sim.h @@ -0,0 +1,97 @@ +#ifndef MC_ATT_CONTROL_BASE_H_ +#define MC_ATT_CONTROL_BASE_H_ + +/* 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_sim.h + * + * MC Attitude Controller Interface for usage in a simulator + * + * @author Roman Bapst + * + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#inlcude "mc_att_control_base.h" + + + +#define YAW_DEADZONE 0.05f +#define MIN_TAKEOFF_THRUST 0.2f +#define RATES_I_LIMIT 0.3f + +class MulticopterAttitudeControlSim : + public MulticopterAttitudeControlBase + +{ +public: + /** + * Constructor + */ + MulticopterAttitudeControlSim(); + + /** + * Destructor + */ + ~MulticopterAttitudeControlSim(); + + /* setters and getters for interface with euroc-gazebo simulator */ + void set_attitude(const Eigen::Quaternion attitude); + void set_attitude_rates(const Eigen::Vector3d &angular_rate); + void set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference); + void get_mixer_input(Eigen::Vector4d &motor_inputs); + +protected: + void vehicle_attitude_setpoint_poll() {}; + + +}; + +#endif /* MC_ATT_CONTROL_BASE_H_ */ diff --git a/src/modules/mc_att_control_multiplatform/module.mk b/src/modules/mc_att_control_multiplatform/module.mk new file mode 100644 index 000000000..7925b8345 --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Multirotor attitude controller (vector based, no Euler singularities) +# + +MODULE_COMMAND = mc_att_control_multiplatform + +SRCS = mc_att_control_main.cpp \ + mc_att_control.cpp \ + mc_att_control_base.cpp \ + mc_att_control_params.c -- cgit v1.2.3 From 683b06321c192bbc76d322b266c62e414e24f4f4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 Jan 2015 16:33:57 +0100 Subject: Revert "uavcan gives compile errors, disable for now" This reverts commit 941ff05720b4ba8c282a84e1f8933469ae7fc39e. --- makefiles/config_px4fmu-v2_default.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index d5c3f4a22..4bdf7722c 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -70,7 +70,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led -# MODULES += modules/uavcan +MODULES += modules/uavcan # # Estimation modules (EKF/ SO3 / other filters) -- cgit v1.2.3 From 017dc424238e168ba63d540010c580e20b6d9dd7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 Jan 2015 16:49:52 +0100 Subject: multiplatform fix errors from werror --- src/platforms/px4_subscriber.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index e7e344201..4b6d5b58f 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -79,7 +79,8 @@ public: /** * Get the last message value */ - virtual const M& get() = 0; + virtual M get() = 0; + /** * Get void pointer to last message value */ @@ -124,7 +125,7 @@ public: /** * Get the last message value */ - const M& get() { return _msg_current; } + M get() { return _msg_current; } /** * Get void pointer to last message value */ @@ -204,7 +205,7 @@ public: /** * Get the last message value */ - const M& get() { return uORB::Subscription::getData(); } + M get() { return uORB::Subscription::getData(); } /** * Get void pointer to last message value */ -- cgit v1.2.3 From 87650e49e14adae8880dfd06e3d4a23bcb7ae3b1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 Jan 2015 16:50:14 +0100 Subject: Revert "turn off werror for now" This reverts commit 1fe70a845ddd5ba9721748ceced81327efa47193. --- makefiles/toolchain_gnu-arm-eabi.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index caaf4ca87..ef70d51f2 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -127,6 +127,7 @@ ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics # ARCHWARNINGS = -Wall \ -Wextra \ + -Werror \ -Wdouble-promotion \ -Wshadow \ -Wfloat-equal \ -- cgit v1.2.3 From f871b777abe841149ed537349ff7a6bd3156e9c9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 Jan 2015 11:09:11 +0100 Subject: destructors for px4_nodehandle --- src/platforms/px4_nodehandle.h | 35 +++++++++++++++++++++++++++++++++-- 1 file changed, 33 insertions(+), 2 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 0e77852f2..3c2bb2d44 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -68,7 +68,8 @@ public: ~NodeHandle() { - //XXX empty lists + _subs.clear(); + _pubs.clear(); }; /** @@ -154,7 +155,36 @@ public: _sub_min_interval(nullptr) {} - ~NodeHandle() {}; + ~NodeHandle() + { + /* Empty subscriptions list */ + uORB::SubscriptionNode *sub = _subs.getHead(); + int count = 0; + while (sub != nullptr) { + if (count++ > kMaxSubscriptions) { + PX4_WARN("exceeded max subscriptions"); + break; + } + + uORB::SubscriptionNode *sib = sub->getSibling(); + delete sub; + sub = sib; + } + + /* Empty publications list */ + uORB::PublicationNode *pub = _pubs.getHead(); + count = 0; + while (pub != nullptr) { + if (count++ > kMaxPublications) { + PX4_WARN("exceeded max publications"); + break; + } + + uORB::PublicationNode *sib = pub->getSibling(); + delete pub; + pub = sib; + } + }; /** * Subscribe with callback to function @@ -254,6 +284,7 @@ public: } private: static const uint16_t kMaxSubscriptions = 100; + static const uint16_t kMaxPublications = 100; List _subs; /**< Subcriptions of node */ List _pubs; /**< Publications of node */ uORB::SubscriptionNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval -- cgit v1.2.3 From 3436abdf0bde16bdda5f4e6600693d6b0f42a01b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 Jan 2015 11:10:52 +0100 Subject: add multiplatform makefile to work around flash size issues --- makefiles/config_px4fmu-v2_default.mk | 5 +- makefiles/config_px4fmu-v2_multiplatform.mk | 167 ++++++++++++++++++++++++++++ 2 files changed, 169 insertions(+), 3 deletions(-) create mode 100644 makefiles/config_px4fmu-v2_multiplatform.mk diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 4bdf7722c..09106c090 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -83,10 +83,9 @@ MODULES += modules/position_estimator_inav # Vehicle Control # #MODULES += modules/segway # XXX Needs GCC 4.7 fix -#MODULES += modules/fw_pos_control_l1 -#MODULES += modules/fw_att_control +MODULES += modules/fw_pos_control_l1 +MODULES += modules/fw_att_control MODULES += modules/mc_att_control -MODULES += modules/mc_att_control_multiplatform MODULES += modules/mc_pos_control MODULES += modules/vtol_att_control diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk new file mode 100644 index 000000000..7e95ed24e --- /dev/null +++ b/makefiles/config_px4fmu-v2_multiplatform.mk @@ -0,0 +1,167 @@ +# +# Makefile for the px4fmu_default configuration +# + +# +# Use the configuration's ROMFS, copy the px4iov2 firmware into +# the ROMFS if it's available +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common +ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/stm32/adc +MODULES += drivers/stm32/tone_alarm +MODULES += drivers/led +MODULES += drivers/px4fmu +MODULES += drivers/px4io +MODULES += drivers/boards/px4fmu-v2 +MODULES += drivers/rgbled +MODULES += drivers/mpu6000 +MODULES += drivers/lsm303d +MODULES += drivers/l3gd20 +MODULES += drivers/hmc5883 +MODULES += drivers/ms5611 +MODULES += drivers/mb12xx +# MODULES += drivers/sf0x +MODULES += drivers/ll40ls +# MODULES += drivers/trone +MODULES += drivers/gps +MODULES += drivers/hil +MODULES += drivers/hott +MODULES += drivers/hott/hott_telemetry +MODULES += drivers/hott/hott_sensors +# MODULES += drivers/blinkm +MODULES += drivers/airspeed +MODULES += drivers/ets_airspeed +MODULES += drivers/meas_airspeed +MODULES += drivers/frsky_telemetry +MODULES += modules/sensors +MODULES += drivers/mkblctrl +MODULES += drivers/px4flow + +# +# System commands +# +MODULES += systemcmds/bl_update +MODULES += systemcmds/boardinfo +MODULES += systemcmds/mixer +MODULES += systemcmds/param +MODULES += systemcmds/perf +MODULES += systemcmds/preflight_check +MODULES += systemcmds/pwm +MODULES += systemcmds/esc_calib +MODULES += systemcmds/reboot +MODULES += systemcmds/top +MODULES += systemcmds/config +MODULES += systemcmds/nshterm +MODULES += systemcmds/mtd +MODULES += systemcmds/dumpfile +MODULES += systemcmds/ver + +# +# General system control +# +MODULES += modules/commander +MODULES += modules/navigator +MODULES += modules/mavlink +MODULES += modules/gpio_led +MODULES += modules/uavcan + +# +# Estimation modules (EKF/ SO3 / other filters) +# +MODULES += modules/attitude_estimator_ekf +MODULES += modules/ekf_att_pos_estimator +MODULES += modules/position_estimator_inav + +# +# Vehicle Control +# +#MODULES += modules/segway # XXX Needs GCC 4.7 fix +#MODULES += modules/fw_pos_control_l1 +#MODULES += modules/fw_att_control +# MODULES += modules/mc_att_control +MODULES += modules/mc_att_control_multiplatform +MODULES += modules/mc_pos_control +MODULES += modules/vtol_att_control + +# +# Logging +# +MODULES += modules/sdlog2 + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/controllib +MODULES += modules/uORB +MODULES += modules/dataman + +# +# Libraries +# +LIBRARIES += lib/mathlib/CMSIS +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter +MODULES += lib/ecl +MODULES += lib/external_lgpl +MODULES += lib/geo +MODULES += lib/geo_lookup +MODULES += lib/conversion +MODULES += lib/launchdetection +MODULES += platforms/nuttx + +# +# OBC challenge +# +MODULES += modules/bottle_drop + +# +# Demo apps +# +#MODULES += examples/math_demo +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/hello_sky +#MODULES += examples/px4_simple_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/daemon +#MODULES += examples/px4_daemon_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/debug_values +#MODULES += examples/px4_mavlink_debug + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control +#MODULES += examples/fixedwing_control + +# Hardware test +#MODULES += examples/hwtest + +# Generate parameter XML file +GEN_PARAM_XML = 1 + +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is ... but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +# command priority stack entrypoint +BUILTIN_COMMANDS := \ + $(call _B, sercon, , 2048, sercon_main ) \ + $(call _B, serdis, , 2048, serdis_main ) -- cgit v1.2.3 From 581d4e111082dbe8cde4a7ae3115f30c7a7417fe Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 Jan 2015 11:58:04 +0100 Subject: travis: apt-get install python-empy --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index fd2a6b6d1..9386ddaa3 100644 --- a/.travis.yml +++ b/.travis.yml @@ -12,7 +12,7 @@ before_script: - sudo apt-get install s3cmd grep zip mailutils # General toolchain dependencies - sudo apt-get install libc6:i386 libgcc1:i386 gcc-4.6-base:i386 libstdc++5:i386 libstdc++6:i386 - - sudo apt-get install python-serial python-argparse + - sudo apt-get install python-serial python-argparse python-empy - sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget cmake - pushd . - cd ~ -- cgit v1.2.3 From 0893f9fd96200c58bec5ed9f2b0965184c4ae45e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 Jan 2015 19:13:39 +0100 Subject: remove uavcan from multiplatform build, problems with linking --- makefiles/config_px4fmu-v2_multiplatform.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk index 7e95ed24e..bed2fa506 100644 --- a/makefiles/config_px4fmu-v2_multiplatform.mk +++ b/makefiles/config_px4fmu-v2_multiplatform.mk @@ -70,7 +70,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led -MODULES += modules/uavcan +# MODULES += modules/uavcan # # Estimation modules (EKF/ SO3 / other filters) -- cgit v1.2.3 From 5056b03ab00a11c3705a97edb9bbfc13a32130dd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 Jan 2015 19:14:01 +0100 Subject: geo: fix include --- src/lib/geo/geo.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 012779646..ca3587b93 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -44,7 +44,7 @@ */ #pragma once -#include +#include __BEGIN_DECLS #include "geo_lookup/geo_mag_declination.h" -- cgit v1.2.3 From ee561947e9100f64a206216104e3ce83e8a304ca Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 Jan 2015 19:14:57 +0100 Subject: makefile: fix order --- Makefile | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/Makefile b/Makefile index b800459cb..37ba35fed 100644 --- a/Makefile +++ b/Makefile @@ -104,7 +104,10 @@ DESIRED_FIRMWARES = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4) STAGED_FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(IMAGE_DIR)$(config).px4) FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4) -all: checksubmodules generateuorbtopicheaders $(DESIRED_FIRMWARES) +all: + $(Q) $(MAKE) checksubmodules + $(Q) $(MAKE) generateuorbtopicheaders + $(Q) $(MAKE) $(DESIRED_FIRMWARES) # # Copy FIRMWARES into the image directory. -- 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(-) 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 213f08ee07c57d4781002cefe797f7ce9e9fd561 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 Jan 2015 23:52:04 +0100 Subject: generate uorb topic headers for tests make target --- Makefile | 1 + 1 file changed, 1 insertion(+) diff --git a/Makefile b/Makefile index 37ba35fed..859eca0ce 100644 --- a/Makefile +++ b/Makefile @@ -255,6 +255,7 @@ testbuild: # unit tests. .PHONY: tests tests: + $(Q) $(MAKE) generateuorbtopicheaders $(Q) (cd $(PX4_BASE)/unittests && $(MAKE) unittests) # -- cgit v1.2.3 From e855e4454cce0a3bdc51b785a44d7660ae5a921d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 7 Jan 2015 00:19:30 +0100 Subject: exclude macro hack for tests target --- src/platforms/px4_defines.h | 2 ++ unittests/CMakeLists.txt | 1 + 2 files changed, 3 insertions(+) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 457b5abe3..28eef7e18 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -177,9 +177,11 @@ typedef param_t px4_param_t; #define PX4_PARAM_GET_BYNAME(_name, _destpt) param_get(param_find(_name), _destpt) /* XXX this is a hack to resolve conflicts with NuttX headers */ +#if !defined(__PX4_TESTS) #define isspace(c) \ ((c) == ' ' || (c) == '\t' || (c) == '\n' || \ (c) == '\r' || (c) == '\f' || c== '\v') +#endif #endif diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index ec3e4104e..0737f6696 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -11,6 +11,7 @@ include_directories(${CMAKE_SOURCE_DIR}/../src/modules) include_directories(${CMAKE_SOURCE_DIR}/../src/lib) add_definitions(-D__EXPORT=) +add_definitions(-D__PX4_TESTS) function(add_gtest) foreach(test_name ${ARGN}) -- cgit v1.2.3 From a01a8f955336e2825d23d5494703ea91f15e3ee1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 7 Jan 2015 08:20:35 +0100 Subject: add missing defines for unittests --- unittests/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 0737f6696..47b84f148 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -12,6 +12,8 @@ include_directories(${CMAKE_SOURCE_DIR}/../src/lib) add_definitions(-D__EXPORT=) add_definitions(-D__PX4_TESTS) +add_definitions(-Dnoreturn_function=) +add_definitions(-Dmain_t=int) function(add_gtest) foreach(test_name ${ARGN}) -- cgit v1.2.3 From 417a82c699c2512ca3b8998c91c9d77f5d826edb Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 7 Jan 2015 13:48:20 +0100 Subject: add ros setup scripts --- Tools/ros/px4_ros_installation_ubuntu.sh | 31 +++++++++++++++++++++++++++++++ Tools/ros/px4_workspace_create.sh | 7 +++++++ Tools/ros/px4_workspace_setup.sh | 23 +++++++++++++++++++++++ 3 files changed, 61 insertions(+) create mode 100755 Tools/ros/px4_ros_installation_ubuntu.sh create mode 100755 Tools/ros/px4_workspace_create.sh create mode 100755 Tools/ros/px4_workspace_setup.sh diff --git a/Tools/ros/px4_ros_installation_ubuntu.sh b/Tools/ros/px4_ros_installation_ubuntu.sh new file mode 100755 index 000000000..c6f27653c --- /dev/null +++ b/Tools/ros/px4_ros_installation_ubuntu.sh @@ -0,0 +1,31 @@ +#!/bin/sh + +# main ROS Setup +# following http://wiki.ros.org/indigo/Installation/Ubuntu +# run this file with . ./px4_ros_setup_ubuntu.sh + +## add ROS repository +sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list' + +## add key +wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | \ + sudo apt-key add - + +## Install main ROS pacakges +sudo apt-get update +sudo apt-get install ros-indigo-desktop-full +sudo rosdep init +rosdep update + +## Setup environment variables +echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc +source ~/.bashrc + +# get rosinstall +sudo apt-get install python-rosinstall + +# Hector packages +sudo apt-get install ros-indigo-hector-quadrotor \ + ros-indigo-octomap-msgs \ + ros-indigo-hector-gazebo + diff --git a/Tools/ros/px4_workspace_create.sh b/Tools/ros/px4_workspace_create.sh new file mode 100755 index 000000000..d8d65c819 --- /dev/null +++ b/Tools/ros/px4_workspace_create.sh @@ -0,0 +1,7 @@ +#!/bin/sh +# this script creates a catkin_ws in the current folder + +mkdir -p catkin_ws/src +cd catkin_ws/src +catkin_init_workspace +cd .. diff --git a/Tools/ros/px4_workspace_setup.sh b/Tools/ros/px4_workspace_setup.sh new file mode 100755 index 000000000..233d21671 --- /dev/null +++ b/Tools/ros/px4_workspace_setup.sh @@ -0,0 +1,23 @@ +#!/bin/sh +# run this script from the root of your catkin_ws +source devel/setup.bash +cd src + +# PX4 Firmware +git clone https://github.com/PX4/Firmware.git +cd Firmware +git checkout ros +cd .. + +# euroc simulator +git clone https://github.com/PX4/euroc_simulator.git +cd euroc_simulator +git checkout px4_nodes +cd .. + +# # mav comm +git clone https://github.com/PX4/mav_comm.git + +cd .. + +catkin_make -- cgit v1.2.3 From 154111d4c011a9fd58812cb88fbbbbc38e30e08d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 7 Jan 2015 17:01:03 +0100 Subject: move checksubmodules and generateuorbtopicheaders dependency --- Makefile | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/Makefile b/Makefile index 2ba2073eb..6adbf765e 100644 --- a/Makefile +++ b/Makefile @@ -104,10 +104,7 @@ DESIRED_FIRMWARES = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4) STAGED_FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(IMAGE_DIR)$(config).px4) FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4) -all: - $(Q) $(MAKE) checksubmodules - $(Q) $(MAKE) generateuorbtopicheaders - $(Q) $(MAKE) $(DESIRED_FIRMWARES) +all: $(DESIRED_FIRMWARES) # # Copy FIRMWARES into the image directory. @@ -127,7 +124,7 @@ $(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4 .PHONY: $(FIRMWARES) $(BUILD_DIR)%.build/firmware.px4: config = $(patsubst $(BUILD_DIR)%.build/firmware.px4,%,$@) $(BUILD_DIR)%.build/firmware.px4: work_dir = $(BUILD_DIR)$(config).build/ -$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: +$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: checksubmodules generateuorbtopicheaders @$(ECHO) %%%% @$(ECHO) %%%% Building $(config) in $(work_dir) @$(ECHO) %%%% -- cgit v1.2.3 From 143ff444e46a20709b1cd50cb732b3c5640d8ecd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 7 Jan 2015 17:28:56 +0100 Subject: fix merge error of tests target change --- Makefile | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/Makefile b/Makefile index 6adbf765e..6480286d2 100644 --- a/Makefile +++ b/Makefile @@ -251,9 +251,7 @@ testbuild: # Unittest targets. Builds and runs the host-level # unit tests. .PHONY: tests -tests: - $(Q) $(MAKE) generateuorbtopicheaders - $(Q) (cd $(PX4_BASE)/unittests && $(MAKE) unittests) +tests: generateuorbtopicheaders $(Q) (mkdir -p $(PX4_BASE)/unittests/build && cd $(PX4_BASE)/unittests/build && cmake .. && $(MAKE) unittests) # -- cgit v1.2.3 From 34b023d0a62bef93feb0ce3131e677f7c8546b45 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 8 Jan 2015 08:08:45 +0100 Subject: revert strange change in nsh Make.defs --- nuttx-configs/px4fmu-v2/nsh/Make.defs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs index e0e868870..99f3b3140 100644 --- a/nuttx-configs/px4fmu-v2/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -90,7 +90,7 @@ else ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) else - # Linux/Cygwin-native toolchain + # Linux/Cygwin-native toolchain MKDEP = $(TOPDIR)/tools/mkdeps.sh ARCHINCLUDES = -I. -isystem $(TOPDIR)/include ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx @@ -117,7 +117,7 @@ ARCHOPTIMIZATION += -g endif ARCHCFLAGS = -std=gnu99 -ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=c++11 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x ARCHWARNINGS = -Wall \ -Wno-sign-compare \ -Wextra \ -- cgit v1.2.3 From c118d17cb576b76df3bc1b765fb38c34421c7be4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 8 Jan 2015 08:15:44 +0100 Subject: fix code style in src/platforms --- src/platforms/px4_defines.h | 6 ++- src/platforms/px4_nodehandle.h | 27 ++++++----- src/platforms/px4_subscriber.h | 24 +++++----- src/platforms/ros/eigen_math.h | 3 +- src/platforms/ros/geo.cpp | 8 ++++ .../attitude_estimator/attitude_estimator.cpp | 24 ++++++---- .../nodes/attitude_estimator/attitude_estimator.h | 7 +-- src/platforms/ros/nodes/commander/commander.cpp | 16 ++++--- src/platforms/ros/nodes/commander/commander.h | 5 +- .../ros/nodes/manual_input/manual_input.cpp | 15 +++--- .../ros/nodes/manual_input/manual_input.h | 9 ++-- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 53 +++++++++++++--------- 12 files changed, 117 insertions(+), 80 deletions(-) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 28eef7e18..6b6a03893 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -84,6 +84,7 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value) if (!ros::param::has(name)) { ros::param::set(name, value); } + return (px4_param_t)name; }; static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) @@ -91,6 +92,7 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) if (!ros::param::has(name)) { ros::param::set(name, value); } + return (px4_param_t)name; }; @@ -179,8 +181,8 @@ typedef param_t px4_param_t; /* XXX this is a hack to resolve conflicts with NuttX headers */ #if !defined(__PX4_TESTS) #define isspace(c) \ - ((c) == ' ' || (c) == '\t' || (c) == '\n' || \ - (c) == '\r' || (c) == '\f' || c== '\v') + ((c) == ' ' || (c) == '\t' || (c) == '\n' || \ + (c) == '\r' || (c) == '\f' || c== '\v') #endif #endif diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 624a466fd..7b14caed9 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -78,11 +78,12 @@ public: * @param fb Callback, executed on receiving a new message */ template - Subscriber *subscribe(const char *topic, void(*fp)(const M&)) + Subscriber *subscribe(const char *topic, void(*fp)(const M &)) { SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS*)sub); - ((SubscriberROS*)sub)->set_ros_sub(ros_sub); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, + (SubscriberROS *)sub); + ((SubscriberROS *)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); return (Subscriber *)sub; } @@ -93,11 +94,12 @@ public: * @param fb Callback, executed on receiving a new message */ template - Subscriber *subscribe(const char *topic, void(T::*fp)(const M&), T *obj) + Subscriber *subscribe(const char *topic, void(T::*fp)(const M &), T *obj) { SubscriberBase *sub = new SubscriberROS(std::bind(fp, obj, std::placeholders::_1)); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS*)sub); - ((SubscriberROS*)sub)->set_ros_sub(ros_sub); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, + (SubscriberROS *)sub); + ((SubscriberROS *)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); return (Subscriber *)sub; } @@ -110,8 +112,9 @@ public: Subscriber *subscribe(const char *topic) { SubscriberBase *sub = new SubscriberROS(); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS*)sub); - ((SubscriberROS*)sub)->set_ros_sub(ros_sub); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, + (SubscriberROS *)sub); + ((SubscriberROS *)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); return (Subscriber *)sub; } @@ -160,6 +163,7 @@ public: /* Empty subscriptions list */ uORB::SubscriptionNode *sub = _subs.getHead(); int count = 0; + while (sub != nullptr) { if (count++ > kMaxSubscriptions) { PX4_WARN("exceeded max subscriptions"); @@ -174,6 +178,7 @@ public: /* Empty publications list */ uORB::PublicationNode *pub = _pubs.getHead(); count = 0; + while (pub != nullptr) { if (count++ > kMaxPublications) { PX4_WARN("exceeded max publications"); @@ -195,8 +200,8 @@ public: template Subscriber *subscribe(const struct orb_metadata *meta, - std::function callback, - unsigned interval) + std::function callback, + unsigned interval) { SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(meta, interval, callback, &_subs); @@ -216,7 +221,7 @@ public: template Subscriber *subscribe(const struct orb_metadata *meta, - unsigned interval) + unsigned interval) { SubscriberUORB *sub_px4 = new SubscriberUORB(meta, interval, &_subs); diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 107c60189..aaacf9e48 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -86,7 +86,7 @@ public: /** * Get void pointer to last message value */ - virtual void * get_void_ptr() = 0; + virtual void *get_void_ptr() = 0; }; #if defined(__PX4_ROS) @@ -97,7 +97,7 @@ template class SubscriberROS : public Subscriber { -friend class NodeHandle; + friend class NodeHandle; public: /** @@ -131,13 +131,14 @@ public: /** * Get void pointer to last message value */ - void * get_void_ptr() { return (void*)&_msg_current; } + void *get_void_ptr() { return (void *)&_msg_current; } protected: /** * Called on topic update, saves the current message and then calls the provided callback function */ - void callback(const M &msg) { + void callback(const M &msg) + { /* Store data */ _msg_current = msg; @@ -151,7 +152,8 @@ protected: /** * Saves the ros subscriber to keep ros subscription alive */ - void set_ros_sub(ros::Subscriber ros_sub) { + void set_ros_sub(ros::Subscriber ros_sub) + { _ros_sub = ros_sub; } @@ -180,8 +182,8 @@ public: * @param list subscriber is added to this list */ SubscriberUORB(const struct orb_metadata *meta, - unsigned interval, - List *list) : + unsigned interval, + List *list) : Subscriber(), uORB::Subscription(meta, interval, list) {} @@ -211,7 +213,7 @@ public: /** * Get void pointer to last message value */ - void * get_void_ptr() { return uORB::Subscription::getDataVoidPtr(); } + void *get_void_ptr() { return uORB::Subscription::getDataVoidPtr(); } }; template @@ -227,9 +229,9 @@ public: * @param list subscriber is added to this list */ SubscriberUORBCallback(const struct orb_metadata *meta, - unsigned interval, - std::function callback, - List *list) : + unsigned interval, + std::function callback, + List *list) : SubscriberUORB(meta, interval, list), _callback(callback) {} diff --git a/src/platforms/ros/eigen_math.h b/src/platforms/ros/eigen_math.h index 60fc9b93c..c7271c157 100755 --- a/src/platforms/ros/eigen_math.h +++ b/src/platforms/ros/eigen_math.h @@ -9,8 +9,7 @@ #define EIGEN_MATH_H_ -struct eigen_matrix_instance -{ +struct eigen_matrix_instance { int numRows; int numCols; float *pData; diff --git a/src/platforms/ros/geo.cpp b/src/platforms/ros/geo.cpp index a7ee6fd84..6fad681c9 100644 --- a/src/platforms/ros/geo.cpp +++ b/src/platforms/ros/geo.cpp @@ -60,6 +60,7 @@ __EXPORT float _wrap_pi(float bearing) } int c = 0; + while (bearing >= M_PI_F) { bearing -= M_TWOPI_F; @@ -69,6 +70,7 @@ __EXPORT float _wrap_pi(float bearing) } c = 0; + while (bearing < -M_PI_F) { bearing += M_TWOPI_F; @@ -88,6 +90,7 @@ __EXPORT float _wrap_2pi(float bearing) } int c = 0; + while (bearing >= M_TWOPI_F) { bearing -= M_TWOPI_F; @@ -97,6 +100,7 @@ __EXPORT float _wrap_2pi(float bearing) } c = 0; + while (bearing < 0.0f) { bearing += M_TWOPI_F; @@ -116,6 +120,7 @@ __EXPORT float _wrap_180(float bearing) } int c = 0; + while (bearing >= 180.0f) { bearing -= 360.0f; @@ -125,6 +130,7 @@ __EXPORT float _wrap_180(float bearing) } c = 0; + while (bearing < -180.0f) { bearing += 360.0f; @@ -144,6 +150,7 @@ __EXPORT float _wrap_360(float bearing) } int c = 0; + while (bearing >= 360.0f) { bearing -= 360.0f; @@ -153,6 +160,7 @@ __EXPORT float _wrap_360(float bearing) } c = 0; + while (bearing < 0.0f) { bearing += 360.0f; diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp index 46c836b36..ce863d10e 100644 --- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp @@ -52,7 +52,7 @@ AttitudeEstimator::AttitudeEstimator() : { } -void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr& msg) +void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg) { px4::vehicle_attitude msg_v_att; @@ -64,8 +64,8 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP int index = 1; quat(0) = (float)msg->pose[index].orientation.w; quat(1) = (float)msg->pose[index].orientation.x; - quat(2) = (float)-msg->pose[index].orientation.y; - quat(3) = (float)-msg->pose[index].orientation.z; + quat(2) = (float) - msg->pose[index].orientation.y; + quat(3) = (float) - msg->pose[index].orientation.z; msg_v_att.q[0] = quat(0); msg_v_att.q[1] = quat(1); @@ -73,11 +73,13 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP msg_v_att.q[3] = quat(3); math::Matrix<3, 3> rot = quat.to_dcm(); + for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { PX4_R(msg_v_att.R, i, j) = rot(i, j); } } + msg_v_att.R_valid = true; math::Vector<3> euler = rot.to_euler(); @@ -93,7 +95,7 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP _vehicle_attitude_pub.publish(msg_v_att); } -void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr& msg) +void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr &msg) { px4::vehicle_attitude msg_v_att; @@ -105,8 +107,8 @@ void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr& msg) int index = 1; quat(0) = (float)msg->orientation.w; quat(1) = (float)msg->orientation.x; - quat(2) = (float)-msg->orientation.y; - quat(3) = (float)-msg->orientation.z; + quat(2) = (float) - msg->orientation.y; + quat(3) = (float) - msg->orientation.z; msg_v_att.q[0] = quat(0); msg_v_att.q[1] = quat(1); @@ -114,11 +116,13 @@ void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr& msg) msg_v_att.q[3] = quat(3); math::Matrix<3, 3> rot = quat.to_dcm(); + for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { PX4_R(msg_v_att.R, i, j) = rot(i, j); } } + msg_v_att.R_valid = true; math::Vector<3> euler = rot.to_euler(); @@ -136,10 +140,10 @@ void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr& msg) int main(int argc, char **argv) { - ros::init(argc, argv, "attitude_estimator"); - AttitudeEstimator m; + ros::init(argc, argv, "attitude_estimator"); + AttitudeEstimator m; - ros::spin(); + ros::spin(); - return 0; + return 0; } diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h index 7c09985f3..f760a39d8 100644 --- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h @@ -42,15 +42,16 @@ #include #include -class AttitudeEstimator { +class AttitudeEstimator +{ public: AttitudeEstimator(); ~AttitudeEstimator() {} protected: - void ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr& msg); - void ImuCallback(const sensor_msgs::ImuConstPtr& msg); + void ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg); + void ImuCallback(const sensor_msgs::ImuConstPtr &msg); ros::NodeHandle _n; ros::Subscriber _sub_modelstates; diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index d8ff739b9..b9fc296f9 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -57,7 +57,7 @@ Commander::Commander() : { } -void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr& msg) +void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg) { px4::vehicle_control_mode msg_vehicle_control_mode; px4::vehicle_status msg_vehicle_status; @@ -72,14 +72,16 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon /* fill actuator armed */ float arm_th = 0.95; _msg_actuator_armed.timestamp = px4::get_time_micros(); + if (_msg_actuator_armed.armed) { /* Check for disarm */ - if (msg->r < -arm_th && msg->z < (1-arm_th)) { + if (msg->r < -arm_th && msg->z < (1 - arm_th)) { _msg_actuator_armed.armed = false; } + } else { /* Check for arm */ - if (msg->r > arm_th && msg->z < (1-arm_th)) { + if (msg->r > arm_th && msg->z < (1 - arm_th)) { _msg_actuator_armed.armed = true; } } @@ -107,8 +109,8 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon int main(int argc, char **argv) { - ros::init(argc, argv, "commander"); - Commander m; - ros::spin(); - return 0; + ros::init(argc, argv, "commander"); + Commander m; + ros::spin(); + return 0; } diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h index f3c2f6f1a..bd4092b3a 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -43,7 +43,8 @@ #include #include -class Commander { +class Commander +{ public: Commander(); @@ -53,7 +54,7 @@ protected: /** * Based on manual control input the status will be set */ - void ManualControlInputCallback(const px4::manual_control_setpointConstPtr& msg); + void ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg); ros::NodeHandle _n; ros::Subscriber _man_ctrl_sp_sub; diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index 475d0c4d2..688df50e0 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.cpp +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -72,7 +72,7 @@ ManualInput::ManualInput() : } -void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg) +void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr &msg) { px4::manual_control_setpoint msg_out; @@ -99,10 +99,11 @@ void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg) _man_ctrl_sp_pub.publish(msg_out); } -void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, double scale, double offset, - double deadzone, float &out) +void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset, + double deadzone, float &out) { double val = msg->axes[map_index]; + if (val + offset > deadzone || val + offset < -deadzone) { out = (float)((val + offset)) * scale; } @@ -110,8 +111,8 @@ void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, do int main(int argc, char **argv) { - ros::init(argc, argv, "manual_input"); - ManualInput m; - ros::spin(); - return 0; + ros::init(argc, argv, "manual_input"); + ManualInput m; + ros::spin(); + return 0; } diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h index fb516d375..93e0abe64 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.h +++ b/src/platforms/ros/nodes/manual_input/manual_input.h @@ -41,7 +41,8 @@ #include "ros/ros.h" #include -class ManualInput { +class ManualInput +{ public: ManualInput(); @@ -51,13 +52,13 @@ protected: /** * Takes ROS joystick message and converts/publishes to px4 manual control setpoint topic */ - void JoyCallback(const sensor_msgs::JoyConstPtr& msg); + void JoyCallback(const sensor_msgs::JoyConstPtr &msg); /** * Helper function to map and scale joystick input */ - void MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, double scale, double offset, double deadzone, - float &out); + void MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset, double deadzone, + float &out); ros::NodeHandle _n; ros::Subscriber _joy_sub; diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 5db180052..e2180af41 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -37,12 +37,13 @@ * * @author Roman Bapst */ - #include - #include - #include - #include +#include +#include +#include +#include - class MultirotorMixer { +class MultirotorMixer +{ public: MultirotorMixer(); @@ -68,11 +69,11 @@ private: struct { float control[6]; - }inputs; + } inputs; struct { float control[6]; - }outputs; + } outputs; bool _armed; ros::Subscriber _sub_actuator_armed; @@ -112,18 +113,22 @@ MultirotorMixer::MultirotorMixer(): _rotor_count(4), _rotors(_config_index[2]) //XXX + eurocconfig hardcoded { - _sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback,this); - _pub = _n.advertise("/mixed_motor_commands",10); + _sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback, this); + _pub = _n.advertise("/mixed_motor_commands", 10); + if (!_n.hasParam("motor_scaling_radps")) { _n.setParam("motor_scaling_radps", 150.0); } + if (!_n.hasParam("motor_offset_radps")) { _n.setParam("motor_offset_radps", 600.0); } - _sub_actuator_armed = _n.subscribe("actuator_armed", 1, &MultirotorMixer::actuatorArmedCallback,this); + + _sub_actuator_armed = _n.subscribe("actuator_armed", 1, &MultirotorMixer::actuatorArmedCallback, this); } -void MultirotorMixer::mix() { +void MultirotorMixer::mix() +{ float roll = math::constrain(inputs.control[0], -1.0f, 1.0f); float pitch = math::constrain(inputs.control[1], -1.0f, 1.0f); float yaw = math::constrain(inputs.control[2], -1.0f, 1.0f); @@ -134,7 +139,7 @@ void MultirotorMixer::mix() { /* perform initial mix pass yielding unbounded outputs, ignore yaw */ for (unsigned i = 0; i < _rotor_count; i++) { float out = roll * _rotors[i].roll_scale - + pitch * _rotors[i].pitch_scale + thrust; + + pitch * _rotors[i].pitch_scale + thrust; /* limit yaw if it causes outputs clipping */ if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) { @@ -145,12 +150,14 @@ void MultirotorMixer::mix() { if (out < min_out) { min_out = out; } + if (out > max_out) { max_out = out; } outputs.control[i] = out; } + /* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */ if (min_out < 0.0f) { float scale_in = thrust / (thrust - min_out); @@ -158,8 +165,8 @@ void MultirotorMixer::mix() { /* mix again with adjusted controls */ for (unsigned i = 0; i < _rotor_count; i++) { outputs.control[i] = scale_in - * (roll * _rotors[i].roll_scale - + pitch * _rotors[i].pitch_scale) + thrust; + * (roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale) + thrust; } } else { @@ -171,6 +178,7 @@ void MultirotorMixer::mix() { /* scale down all outputs if some outputs are too large, reduce total thrust */ float scale_out; + if (max_out > 1.0f) { scale_out = 1.0f / max_out; @@ -184,10 +192,10 @@ void MultirotorMixer::mix() { } } - void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg) +void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg) { // read message - for(int i = 0;i < msg.NUM_ACTUATOR_CONTROLS;i++) { + for (int i = 0; i < msg.NUM_ACTUATOR_CONTROLS; i++) { inputs.control[i] = msg.control[i]; } @@ -200,25 +208,28 @@ void MultirotorMixer::mix() { double offset; _n.getParamCached("motor_scaling_radps", scaling); _n.getParamCached("motor_offset_radps", offset); + if (_armed) { for (int i = 0; i < _rotor_count; i++) { rotor_vel_msg.motor_speed.push_back(outputs.control[i] * scaling + offset); } + } else { for (int i = 0; i < _rotor_count; i++) { rotor_vel_msg.motor_speed.push_back(0.0); } } + _pub.publish(rotor_vel_msg); } - int main(int argc, char **argv) +int main(int argc, char **argv) { - ros::init(argc, argv, "mc_mixer"); - MultirotorMixer mixer; - ros::spin(); + ros::init(argc, argv, "mc_mixer"); + MultirotorMixer mixer; + ros::spin(); - return 0; + return 0; } void MultirotorMixer::actuatorArmedCallback(const PX4_TOPIC_T(actuator_armed) &msg) -- cgit v1.2.3 From 74af4807a1a7adca0a6f6b6c668c48808ab858ec Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 8 Jan 2015 08:26:04 +0100 Subject: Matrix.hpp: remove wrong and correct formatting changes which are not on master for clarity --- src/lib/mathlib/math/Matrix.hpp | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 68f0a777d..f6f4fc5ea 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -83,9 +83,10 @@ public: * trivial ctor * Initializes the elements to zero. */ - MatrixBase() : - data {}, - arm_mat {M, N, &data[0][0]} { + MatrixBase() : + data{}, + arm_mat{M, N, &data[0][0]} + { } virtual ~MatrixBase() {}; @@ -94,17 +95,20 @@ public: * copyt ctor */ MatrixBase(const MatrixBase &m) : - arm_mat {M, N, &data[0][0]} { + arm_mat{M, N, &data[0][0]} + { memcpy(data, m.data, sizeof(data)); } MatrixBase(const float *d) : - arm_mat {M, N, &data[0][0]} { + arm_mat{M, N, &data[0][0]} + { memcpy(data, d, sizeof(data)); } - MatrixBase(const float d[M][N]) : - arm_mat {M, N, &data[0][0]} { + MatrixBase(const float d[M][N]) : + arm_mat{M, N, &data[0][0]} + { memcpy(data, d, sizeof(data)); } @@ -165,9 +169,8 @@ public: bool operator ==(const Matrix &m) const { for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m.data[i][j]) { + if (data[i][j] != m.data[i][j]) return false; - } return true; } -- cgit v1.2.3 From f13f41f704e77ec5259fe2cc8efc440b052db1f7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 8 Jan 2015 09:08:50 +0100 Subject: point uavcan submodule to same commit as master --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index 6a1936489..1efd24427 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 6a193648985adab9665e31a9460be04bc91d8965 +Subproject commit 1efd24427539fa332a15151143466ec760fa5fff -- cgit v1.2.3 From 94ab82ba4074c52c612e5475bbabc57482b24a59 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 8 Jan 2015 09:14:21 +0100 Subject: R_adapted.data is 2d, making this more obvious --- src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index d68f12c8e..f0e02c331 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -619,7 +619,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds att.pitch_sec = euler_angles_sec(1); att.yaw_sec = euler_angles_sec(2); - memcpy(&att.R_sec[0], &R_adapted.data[0], sizeof(att.R_sec)); + memcpy(&att.R_sec[0], &R_adapted.data[0][0], sizeof(att.R_sec)); att.rollspeed_sec = -x_aposteriori[2]; att.pitchspeed_sec = x_aposteriori[1]; -- cgit v1.2.3 From f86c0ed892c0b63af11d23e860c815d1c087ff8e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 8 Jan 2015 09:17:22 +0100 Subject: remove fw_att_control base classes: as long as they are not integrated into fw_att_control_main they are useless --- src/modules/fw_att_control/fw_att_control_base.cpp | 325 --------------------- src/modules/fw_att_control/fw_att_control_base.h | 151 ---------- 2 files changed, 476 deletions(-) delete mode 100644 src/modules/fw_att_control/fw_att_control_base.cpp delete mode 100644 src/modules/fw_att_control/fw_att_control_base.h diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp deleted file mode 100644 index f543c02f9..000000000 --- a/src/modules/fw_att_control/fw_att_control_base.cpp +++ /dev/null @@ -1,325 +0,0 @@ -/* 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_base.cpp - * - * @author Roman Bapst - * - */ - -#include "fw_att_control_base.h" -#include -#include -#include - -using namespace std; - -FixedwingAttitudeControlBase::FixedwingAttitudeControlBase() : - - _task_should_exit(false), _task_running(false), _control_task(-1), - - /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), _nonfinite_input_perf( - perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf( - perf_alloc(PC_COUNT, "fw att control nonfinite output")), - /* states */ - _setpoint_valid(false), _debug(false) { - /* safely initialize structs */ - _att = {}; - _att_sp = {}; - _manual = {}; - _airspeed = {}; - _vcontrol_mode = {}; - _actuators = {}; - _actuators_airframe = {}; - _global_pos = {}; - _vehicle_status = {}; - -} - -FixedwingAttitudeControlBase::~FixedwingAttitudeControlBase() { - -} - -void FixedwingAttitudeControlBase::control_attitude() { - bool lock_integrator = false; - static int loop_counter = 0; - /* scale around tuning airspeed */ - - float airspeed; - - /* if airspeed is not updating, we assume the normal average speed */ - if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) - || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { - airspeed = _parameters.airspeed_trim; - if (nonfinite) { - perf_count(_nonfinite_input_perf); - } - } else { - /* prevent numerical drama by requiring 0.5 m/s minimal speed */ - airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s); - } - - /* - * For scaling our actuators using anything less than the min (close to stall) - * speed doesn't make any sense - its the strongest reasonable deflection we - * want to do in flight and its the baseline a human pilot would choose. - * - * Forcing the scaling to this value allows reasonable handheld tests. - */ - - float airspeed_scaling = _parameters.airspeed_trim - / ((airspeed < _parameters.airspeed_min) ? - _parameters.airspeed_min : airspeed); - - float roll_sp = _parameters.rollsp_offset_rad; - float pitch_sp = _parameters.pitchsp_offset_rad; - float throttle_sp = 0.0f; - - if (_vcontrol_mode.flag_control_velocity_enabled - || _vcontrol_mode.flag_control_position_enabled) { - /* read in attitude setpoint from attitude setpoint uorb topic */ - roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; - pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; - throttle_sp = _att_sp.thrust; - - /* reset integrals where needed */ - if (_att_sp.roll_reset_integral) { - _roll_ctrl.reset_integrator(); - } - if (_att_sp.pitch_reset_integral) { - _pitch_ctrl.reset_integrator(); - } - if (_att_sp.yaw_reset_integral) { - _yaw_ctrl.reset_integrator(); - } - } else { - /* - * Scale down roll and pitch as the setpoints are radians - * and a typical remote can only do around 45 degrees, the mapping is - * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch) - * - * With this mapping the stick angle is a 1:1 representation of - * the commanded attitude. - * - * The trim gets subtracted here from the manual setpoint to get - * the intended attitude setpoint. Later, after the rate control step the - * trim is added again to the control signal. - */ - roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) - + _parameters.rollsp_offset_rad; - pitch_sp = -(_manual.x * _parameters.man_pitch_max - - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; - throttle_sp = _manual.z; - _actuators.control[4] = _manual.flaps; - - /* - * in manual mode no external source should / does emit attitude setpoints. - * emit the manual setpoint here to allow attitude controller tuning - * in attitude control mode. - */ - struct vehicle_attitude_setpoint_s att_sp; - att_sp.timestamp = hrt_absolute_time(); - att_sp.roll_body = roll_sp; - att_sp.pitch_body = pitch_sp; - att_sp.yaw_body = 0.0f - _parameters.trim_yaw; - att_sp.thrust = throttle_sp; - - } - - /* If the aircraft is on ground reset the integrators */ - if (_vehicle_status.condition_landed) { - _roll_ctrl.reset_integrator(); - _pitch_ctrl.reset_integrator(); - _yaw_ctrl.reset_integrator(); - } - - /* Prepare speed_body_u and speed_body_w */ - float speed_body_u = 0.0f; - float speed_body_v = 0.0f; - float speed_body_w = 0.0f; - if (_att.R_valid) { - speed_body_u = _att.R[0][0] * _global_pos.vel_n - + _att.R[1][0] * _global_pos.vel_e - + _att.R[2][0] * _global_pos.vel_d; - speed_body_v = _att.R[0][1] * _global_pos.vel_n - + _att.R[1][1] * _global_pos.vel_e - + _att.R[2][1] * _global_pos.vel_d; - speed_body_w = _att.R[0][2] * _global_pos.vel_n - + _att.R[1][2] * _global_pos.vel_e - + _att.R[2][2] * _global_pos.vel_d; - } else { - if (_debug && loop_counter % 10 == 0) { - warnx("Did not get a valid R\n"); - } - } - - /* Run attitude controllers */ - if (isfinite(roll_sp) && isfinite(pitch_sp)) { - _roll_ctrl.control_attitude(roll_sp, _att.roll); - _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed); - _yaw_ctrl.control_attitude(_att.roll, _att.pitch, speed_body_u, - speed_body_v, speed_body_w, _roll_ctrl.get_desired_rate(), - _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude - - /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ - float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, _att.rollspeed, - _att.yawspeed, _yaw_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, airspeed, - airspeed_scaling, lock_integrator); - _actuators.control[0] = - (isfinite(roll_u)) ? - roll_u + _parameters.trim_roll : _parameters.trim_roll; - if (!isfinite(roll_u)) { - _roll_ctrl.reset_integrator(); - perf_count(_nonfinite_output_perf); - - if (_debug && loop_counter % 10 == 0) { - warnx("roll_u %.4f", (double) roll_u); - } - } - - float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, - _att.pitchspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, airspeed, - airspeed_scaling, lock_integrator); - _actuators.control[1] = - (isfinite(pitch_u)) ? - pitch_u + _parameters.trim_pitch : - _parameters.trim_pitch; - if (!isfinite(pitch_u)) { - _pitch_ctrl.reset_integrator(); - perf_count(_nonfinite_output_perf); - if (_debug && loop_counter % 10 == 0) { - warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," - " airspeed %.4f, airspeed_scaling %.4f," - " roll_sp %.4f, pitch_sp %.4f," - " _roll_ctrl.get_desired_rate() %.4f," - " _pitch_ctrl.get_desired_rate() %.4f" - " att_sp.roll_body %.4f", (double) pitch_u, - (double) _yaw_ctrl.get_desired_rate(), - (double) airspeed, (double) airspeed_scaling, - (double) roll_sp, (double) pitch_sp, - (double) _roll_ctrl.get_desired_rate(), - (double) _pitch_ctrl.get_desired_rate(), - (double) _att_sp.roll_body); - } - } - - float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, - _att.pitchspeed, _att.yawspeed, _pitch_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, airspeed, - airspeed_scaling, lock_integrator); - _actuators.control[2] = - (isfinite(yaw_u)) ? - yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; - if (!isfinite(yaw_u)) { - _yaw_ctrl.reset_integrator(); - perf_count(_nonfinite_output_perf); - if (_debug && loop_counter % 10 == 0) { - warnx("yaw_u %.4f", (double) yaw_u); - } - } - - /* throttle passed through */ - _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; - if (!isfinite(throttle_sp)) { - if (_debug && loop_counter % 10 == 0) { - warnx("throttle_sp %.4f", (double) throttle_sp); - } - } - } else { - perf_count(_nonfinite_input_perf); - if (_debug && loop_counter % 10 == 0) { - warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", - (double) roll_sp, (double) pitch_sp); - } - } - -} - -void FixedwingAttitudeControlBase::set_attitude(const Eigen::Quaternion attitude) { - // watch out, still need to see where we modify attitude for the tailsitter case - math::Quaternion quat; - quat(0) = (float)attitude.w(); - quat(1) = (float)attitude.x(); - quat(2) = (float)attitude.y(); - quat(3) = (float)attitude.z(); - - _att.q[0] = quat(0); - _att.q[1] = quat(1); - _att.q[2] = quat(2); - _att.q[3] = quat(3); - - math::Matrix<3,3> Rot = quat.to_dcm(); - _att.R[0][0] = Rot(0,0); - _att.R[1][0] = Rot(1,0); - _att.R[2][0] = Rot(2,0); - _att.R[0][1] = Rot(0,1); - _att.R[1][1] = Rot(1,1); - _att.R[2][1] = Rot(2,1); - _att.R[0][2] = Rot(0,2); - _att.R[1][2] = Rot(1,2); - _att.R[2][2] = Rot(2,2); - - _att.R_valid = true; -} -void FixedwingAttitudeControlBase::set_attitude_rates(const Eigen::Vector3d& angular_rate) { - _att.rollspeed = angular_rate(0); - _att.pitchspeed = angular_rate(1); - _att.yawspeed = angular_rate(2); -} -void FixedwingAttitudeControlBase::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) { - _att_sp.roll_body = control_attitude_thrust_reference(0); - _att_sp.pitch_body = control_attitude_thrust_reference(1); - _att_sp.yaw_body = control_attitude_thrust_reference(2); - _att_sp.thrust = (control_attitude_thrust_reference(3) -30)*(-1)/30; - - // setup rotation matrix - math::Matrix<3,3> Rot_sp; - Rot_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body); - _att_sp.R_body[0][0] = Rot_sp(0,0); - _att_sp.R_body[1][0] = Rot_sp(1,0); - _att_sp.R_body[2][0] = Rot_sp(2,0); - _att_sp.R_body[0][1] = Rot_sp(0,1); - _att_sp.R_body[1][1] = Rot_sp(1,1); - _att_sp.R_body[2][1] = Rot_sp(2,1); - _att_sp.R_body[0][2] = Rot_sp(0,2); - _att_sp.R_body[1][2] = Rot_sp(1,2); - _att_sp.R_body[2][2] = Rot_sp(2,2); -} -void FixedwingAttitudeControlBase::get_mixer_input(Eigen::Vector4d& motor_inputs) { - motor_inputs(0) = _actuators.control[0]; - motor_inputs(1) = _actuators.control[1]; - motor_inputs(2) = _actuators.control[2]; - motor_inputs(3) = _actuators.control[3]; -} diff --git a/src/modules/fw_att_control/fw_att_control_base.h b/src/modules/fw_att_control/fw_att_control_base.h deleted file mode 100644 index bde1b755f..000000000 --- a/src/modules/fw_att_control/fw_att_control_base.h +++ /dev/null @@ -1,151 +0,0 @@ -#ifndef FW_ATT_CONTROL_BASE_H_ -#define FW_ATT_CONTROL_BASE_H_ - -/* 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 fw_att_control_base.h - * - * @author Roman Bapst - * - */ - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -class FixedwingAttitudeControlBase -{ -public: - /** - * Constructor - */ - FixedwingAttitudeControlBase(); - - /** - * Destructor - */ - ~FixedwingAttitudeControlBase(); - - -protected: - - bool _task_should_exit; /**< if true, sensor task should exit */ - bool _task_running; /**< if true, task is running in its mainloop */ - int _control_task; /**< task handle for sensor task */ - - struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ - struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ - struct actuator_controls_s _actuators; /**< actuator control inputs */ - struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ - struct vehicle_global_position_s _global_pos; /**< global position */ - struct vehicle_status_s _vehicle_status; /**< vehicle status */ - - perf_counter_t _loop_perf; /**< loop performance counter */ - perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ - perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ - - bool _setpoint_valid; /**< flag if the position control setpoint is valid */ - bool _debug; /**< if set to true, print debug output */ - - struct { - float tconst; - float p_p; - float p_d; - float p_i; - float p_ff; - float p_rmax_pos; - float p_rmax_neg; - float p_integrator_max; - float p_roll_feedforward; - float r_p; - float r_d; - float r_i; - float r_ff; - float r_integrator_max; - float r_rmax; - float y_p; - float y_i; - float y_d; - float y_ff; - float y_roll_feedforward; - float y_integrator_max; - float y_coordinated_min_speed; - float y_rmax; - - float airspeed_min; - float airspeed_trim; - float airspeed_max; - - float trim_roll; - float trim_pitch; - float trim_yaw; - float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ - float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ - float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ - float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ - float man_roll_max; /**< Max Roll in rad */ - float man_pitch_max; /**< Max Pitch in rad */ - - } _parameters; /**< local copies of interesting parameters */ - - ECL_RollController _roll_ctrl; - ECL_PitchController _pitch_ctrl; - ECL_YawController _yaw_ctrl; - - void control_attitude(); - - // setters and getters for interface with euroc-gazebo simulator - void set_attitude(const Eigen::Quaternion attitude); - void set_attitude_rates(const Eigen::Vector3d& angular_rate); - void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); - void get_mixer_input(Eigen::Vector4d& motor_inputs); - -}; - -#endif /* FW_ATT_CONTROL_BASE_H_ */ -- cgit v1.2.3 From f960bbf529ba41580e06eb8a771600279f58e3c8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 8 Jan 2015 10:54:09 +0100 Subject: bring back switch_pos_t --- src/modules/sensors/sensors.cpp | 8 ++++---- src/modules/uORB/uORB.h | 1 + 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f6ffedc6b..0a27367d9 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -189,8 +189,8 @@ private: /** * Get switch position for specified function. */ - uint8_t get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv); - uint8_t get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv); + switch_pos_t get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv); + switch_pos_t get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv); /** * Update paramters from RC channels if the functionality is activated and the @@ -1666,7 +1666,7 @@ Sensors::get_rc_value(uint8_t func, float min_value, float max_value) } } -uint8_t +switch_pos_t Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv) { if (_rc.function[func] >= 0) { @@ -1687,7 +1687,7 @@ Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mi } } -uint8_t +switch_pos_t Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv) { if (_rc.function[func] >= 0) { diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 707ff1efd..f19fbba7c 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -314,5 +314,6 @@ typedef uint8_t arming_state_t; typedef uint8_t main_state_t; typedef uint8_t hil_state_t; typedef uint8_t navigation_state_t; +typedef uint8_t switch_pos_t; #endif /* _UORB_UORB_H */ -- cgit v1.2.3 From 42430fb5efae6801e63106df3a33de6150ff5095 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 8 Jan 2015 11:29:16 +0100 Subject: remove .catkin_workspace file --- .catkin_workspace | 1 - 1 file changed, 1 deletion(-) delete mode 100644 .catkin_workspace diff --git a/.catkin_workspace b/.catkin_workspace deleted file mode 100644 index 52fd97e7e..000000000 --- a/.catkin_workspace +++ /dev/null @@ -1 +0,0 @@ -# This file currently only serves to mark the location of a catkin workspace for tool integration -- cgit v1.2.3 From bb0d04212f31051fe4b71848c0a3b8adeb9b6e39 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 8 Jan 2015 11:33:59 +0100 Subject: remove copy of std_msgs --- msg/std_msgs/Bool.msg | 1 - msg/std_msgs/Byte.msg | 1 - msg/std_msgs/ByteMultiArray.msg | 6 ------ msg/std_msgs/Char.msg | 1 - msg/std_msgs/ColorRGBA.msg | 4 ---- msg/std_msgs/Duration.msg | 1 - msg/std_msgs/Empty.msg | 0 msg/std_msgs/Float32.msg | 1 - msg/std_msgs/Float32MultiArray.msg | 6 ------ msg/std_msgs/Float64.msg | 1 - msg/std_msgs/Float64MultiArray.msg | 6 ------ msg/std_msgs/Header.msg | 15 --------------- msg/std_msgs/Int16.msg | 1 - msg/std_msgs/Int16MultiArray.msg | 6 ------ msg/std_msgs/Int32.msg | 1 - msg/std_msgs/Int32MultiArray.msg | 6 ------ msg/std_msgs/Int64.msg | 1 - msg/std_msgs/Int64MultiArray.msg | 6 ------ msg/std_msgs/Int8.msg | 1 - msg/std_msgs/Int8MultiArray.msg | 6 ------ msg/std_msgs/MultiArrayDimension.msg | 3 --- msg/std_msgs/MultiArrayLayout.msg | 26 -------------------------- msg/std_msgs/String.msg | 1 - msg/std_msgs/Time.msg | 1 - msg/std_msgs/UInt16.msg | 1 - msg/std_msgs/UInt16MultiArray.msg | 6 ------ msg/std_msgs/UInt32.msg | 1 - msg/std_msgs/UInt32MultiArray.msg | 6 ------ msg/std_msgs/UInt64.msg | 1 - msg/std_msgs/UInt64MultiArray.msg | 6 ------ msg/std_msgs/UInt8.msg | 1 - msg/std_msgs/UInt8MultiArray.msg | 6 ------ 32 files changed, 130 deletions(-) delete mode 100644 msg/std_msgs/Bool.msg delete mode 100644 msg/std_msgs/Byte.msg delete mode 100644 msg/std_msgs/ByteMultiArray.msg delete mode 100644 msg/std_msgs/Char.msg delete mode 100644 msg/std_msgs/ColorRGBA.msg delete mode 100644 msg/std_msgs/Duration.msg delete mode 100644 msg/std_msgs/Empty.msg delete mode 100644 msg/std_msgs/Float32.msg delete mode 100644 msg/std_msgs/Float32MultiArray.msg delete mode 100644 msg/std_msgs/Float64.msg delete mode 100644 msg/std_msgs/Float64MultiArray.msg delete mode 100644 msg/std_msgs/Header.msg delete mode 100644 msg/std_msgs/Int16.msg delete mode 100644 msg/std_msgs/Int16MultiArray.msg delete mode 100644 msg/std_msgs/Int32.msg delete mode 100644 msg/std_msgs/Int32MultiArray.msg delete mode 100644 msg/std_msgs/Int64.msg delete mode 100644 msg/std_msgs/Int64MultiArray.msg delete mode 100644 msg/std_msgs/Int8.msg delete mode 100644 msg/std_msgs/Int8MultiArray.msg delete mode 100644 msg/std_msgs/MultiArrayDimension.msg delete mode 100644 msg/std_msgs/MultiArrayLayout.msg delete mode 100644 msg/std_msgs/String.msg delete mode 100644 msg/std_msgs/Time.msg delete mode 100644 msg/std_msgs/UInt16.msg delete mode 100644 msg/std_msgs/UInt16MultiArray.msg delete mode 100644 msg/std_msgs/UInt32.msg delete mode 100644 msg/std_msgs/UInt32MultiArray.msg delete mode 100644 msg/std_msgs/UInt64.msg delete mode 100644 msg/std_msgs/UInt64MultiArray.msg delete mode 100644 msg/std_msgs/UInt8.msg delete mode 100644 msg/std_msgs/UInt8MultiArray.msg diff --git a/msg/std_msgs/Bool.msg b/msg/std_msgs/Bool.msg deleted file mode 100644 index f7cabb94f..000000000 --- a/msg/std_msgs/Bool.msg +++ /dev/null @@ -1 +0,0 @@ -bool data \ No newline at end of file diff --git a/msg/std_msgs/Byte.msg b/msg/std_msgs/Byte.msg deleted file mode 100644 index d993b3455..000000000 --- a/msg/std_msgs/Byte.msg +++ /dev/null @@ -1 +0,0 @@ -byte data diff --git a/msg/std_msgs/ByteMultiArray.msg b/msg/std_msgs/ByteMultiArray.msg deleted file mode 100644 index bb00bd348..000000000 --- a/msg/std_msgs/ByteMultiArray.msg +++ /dev/null @@ -1,6 +0,0 @@ -# Please look at the MultiArrayLayout message definition for -# documentation on all multiarrays. - -MultiArrayLayout layout # specification of data layout -byte[] data # array of data - diff --git a/msg/std_msgs/Char.msg b/msg/std_msgs/Char.msg deleted file mode 100644 index 39a1d46a9..000000000 --- a/msg/std_msgs/Char.msg +++ /dev/null @@ -1 +0,0 @@ -char data \ No newline at end of file diff --git a/msg/std_msgs/ColorRGBA.msg b/msg/std_msgs/ColorRGBA.msg deleted file mode 100644 index 182dbc834..000000000 --- a/msg/std_msgs/ColorRGBA.msg +++ /dev/null @@ -1,4 +0,0 @@ -float32 r -float32 g -float32 b -float32 a diff --git a/msg/std_msgs/Duration.msg b/msg/std_msgs/Duration.msg deleted file mode 100644 index f13931ec8..000000000 --- a/msg/std_msgs/Duration.msg +++ /dev/null @@ -1 +0,0 @@ -duration data diff --git a/msg/std_msgs/Empty.msg b/msg/std_msgs/Empty.msg deleted file mode 100644 index e69de29bb..000000000 diff --git a/msg/std_msgs/Float32.msg b/msg/std_msgs/Float32.msg deleted file mode 100644 index e89740534..000000000 --- a/msg/std_msgs/Float32.msg +++ /dev/null @@ -1 +0,0 @@ -float32 data \ No newline at end of file diff --git a/msg/std_msgs/Float32MultiArray.msg b/msg/std_msgs/Float32MultiArray.msg deleted file mode 100644 index 915830846..000000000 --- a/msg/std_msgs/Float32MultiArray.msg +++ /dev/null @@ -1,6 +0,0 @@ -# Please look at the MultiArrayLayout message definition for -# documentation on all multiarrays. - -MultiArrayLayout layout # specification of data layout -float32[] data # array of data - diff --git a/msg/std_msgs/Float64.msg b/msg/std_msgs/Float64.msg deleted file mode 100644 index cd09d39b8..000000000 --- a/msg/std_msgs/Float64.msg +++ /dev/null @@ -1 +0,0 @@ -float64 data \ No newline at end of file diff --git a/msg/std_msgs/Float64MultiArray.msg b/msg/std_msgs/Float64MultiArray.msg deleted file mode 100644 index 0a13b928f..000000000 --- a/msg/std_msgs/Float64MultiArray.msg +++ /dev/null @@ -1,6 +0,0 @@ -# Please look at the MultiArrayLayout message definition for -# documentation on all multiarrays. - -MultiArrayLayout layout # specification of data layout -float64[] data # array of data - diff --git a/msg/std_msgs/Header.msg b/msg/std_msgs/Header.msg deleted file mode 100644 index f90d622ea..000000000 --- a/msg/std_msgs/Header.msg +++ /dev/null @@ -1,15 +0,0 @@ -# Standard metadata for higher-level stamped data types. -# This is generally used to communicate timestamped data -# in a particular coordinate frame. -# -# sequence ID: consecutively increasing ID -uint32 seq -#Two-integer timestamp that is expressed as: -# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') -# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') -# time-handling sugar is provided by the client library -time stamp -#Frame this data is associated with -# 0: no frame -# 1: global frame -string frame_id diff --git a/msg/std_msgs/Int16.msg b/msg/std_msgs/Int16.msg deleted file mode 100644 index c4389faf7..000000000 --- a/msg/std_msgs/Int16.msg +++ /dev/null @@ -1 +0,0 @@ -int16 data diff --git a/msg/std_msgs/Int16MultiArray.msg b/msg/std_msgs/Int16MultiArray.msg deleted file mode 100644 index d2ddea1d1..000000000 --- a/msg/std_msgs/Int16MultiArray.msg +++ /dev/null @@ -1,6 +0,0 @@ -# Please look at the MultiArrayLayout message definition for -# documentation on all multiarrays. - -MultiArrayLayout layout # specification of data layout -int16[] data # array of data - diff --git a/msg/std_msgs/Int32.msg b/msg/std_msgs/Int32.msg deleted file mode 100644 index 0ecfe35f5..000000000 --- a/msg/std_msgs/Int32.msg +++ /dev/null @@ -1 +0,0 @@ -int32 data \ No newline at end of file diff --git a/msg/std_msgs/Int32MultiArray.msg b/msg/std_msgs/Int32MultiArray.msg deleted file mode 100644 index af60abda3..000000000 --- a/msg/std_msgs/Int32MultiArray.msg +++ /dev/null @@ -1,6 +0,0 @@ -# Please look at the MultiArrayLayout message definition for -# documentation on all multiarrays. - -MultiArrayLayout layout # specification of data layout -int32[] data # array of data - diff --git a/msg/std_msgs/Int64.msg b/msg/std_msgs/Int64.msg deleted file mode 100644 index 6961e00f5..000000000 --- a/msg/std_msgs/Int64.msg +++ /dev/null @@ -1 +0,0 @@ -int64 data \ No newline at end of file diff --git a/msg/std_msgs/Int64MultiArray.msg b/msg/std_msgs/Int64MultiArray.msg deleted file mode 100644 index f4f35e171..000000000 --- a/msg/std_msgs/Int64MultiArray.msg +++ /dev/null @@ -1,6 +0,0 @@ -# Please look at the MultiArrayLayout message definition for -# documentation on all multiarrays. - -MultiArrayLayout layout # specification of data layout -int64[] data # array of data - diff --git a/msg/std_msgs/Int8.msg b/msg/std_msgs/Int8.msg deleted file mode 100644 index 1e42e554f..000000000 --- a/msg/std_msgs/Int8.msg +++ /dev/null @@ -1 +0,0 @@ -int8 data diff --git a/msg/std_msgs/Int8MultiArray.msg b/msg/std_msgs/Int8MultiArray.msg deleted file mode 100644 index a59a37259..000000000 --- a/msg/std_msgs/Int8MultiArray.msg +++ /dev/null @@ -1,6 +0,0 @@ -# Please look at the MultiArrayLayout message definition for -# documentation on all multiarrays. - -MultiArrayLayout layout # specification of data layout -int8[] data # array of data - diff --git a/msg/std_msgs/MultiArrayDimension.msg b/msg/std_msgs/MultiArrayDimension.msg deleted file mode 100644 index 08240462c..000000000 --- a/msg/std_msgs/MultiArrayDimension.msg +++ /dev/null @@ -1,3 +0,0 @@ -string label # label of given dimension -uint32 size # size of given dimension (in type units) -uint32 stride # stride of given dimension \ No newline at end of file diff --git a/msg/std_msgs/MultiArrayLayout.msg b/msg/std_msgs/MultiArrayLayout.msg deleted file mode 100644 index 5437f8542..000000000 --- a/msg/std_msgs/MultiArrayLayout.msg +++ /dev/null @@ -1,26 +0,0 @@ -# The multiarray declares a generic multi-dimensional array of a -# particular data type. Dimensions are ordered from outer most -# to inner most. - -MultiArrayDimension[] dim # Array of dimension properties -uint32 data_offset # padding bytes at front of data - -# Accessors should ALWAYS be written in terms of dimension stride -# and specified outer-most dimension first. -# -# multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k] -# -# A standard, 3-channel 640x480 image with interleaved color channels -# would be specified as: -# -# dim[0].label = "height" -# dim[0].size = 480 -# dim[0].stride = 3*640*480 = 921600 (note dim[0] stride is just size of image) -# dim[1].label = "width" -# dim[1].size = 640 -# dim[1].stride = 3*640 = 1920 -# dim[2].label = "channel" -# dim[2].size = 3 -# dim[2].stride = 3 -# -# multiarray(i,j,k) refers to the ith row, jth column, and kth channel. \ No newline at end of file diff --git a/msg/std_msgs/String.msg b/msg/std_msgs/String.msg deleted file mode 100644 index ae721739e..000000000 --- a/msg/std_msgs/String.msg +++ /dev/null @@ -1 +0,0 @@ -string data diff --git a/msg/std_msgs/Time.msg b/msg/std_msgs/Time.msg deleted file mode 100644 index 7f8f72171..000000000 --- a/msg/std_msgs/Time.msg +++ /dev/null @@ -1 +0,0 @@ -time data diff --git a/msg/std_msgs/UInt16.msg b/msg/std_msgs/UInt16.msg deleted file mode 100644 index 87d0c44eb..000000000 --- a/msg/std_msgs/UInt16.msg +++ /dev/null @@ -1 +0,0 @@ -uint16 data diff --git a/msg/std_msgs/UInt16MultiArray.msg b/msg/std_msgs/UInt16MultiArray.msg deleted file mode 100644 index f38970b65..000000000 --- a/msg/std_msgs/UInt16MultiArray.msg +++ /dev/null @@ -1,6 +0,0 @@ -# Please look at the MultiArrayLayout message definition for -# documentation on all multiarrays. - -MultiArrayLayout layout # specification of data layout -uint16[] data # array of data - diff --git a/msg/std_msgs/UInt32.msg b/msg/std_msgs/UInt32.msg deleted file mode 100644 index b6c696b42..000000000 --- a/msg/std_msgs/UInt32.msg +++ /dev/null @@ -1 +0,0 @@ -uint32 data \ No newline at end of file diff --git a/msg/std_msgs/UInt32MultiArray.msg b/msg/std_msgs/UInt32MultiArray.msg deleted file mode 100644 index b2bb0771f..000000000 --- a/msg/std_msgs/UInt32MultiArray.msg +++ /dev/null @@ -1,6 +0,0 @@ -# Please look at the MultiArrayLayout message definition for -# documentation on all multiarrays. - -MultiArrayLayout layout # specification of data layout -uint32[] data # array of data - diff --git a/msg/std_msgs/UInt64.msg b/msg/std_msgs/UInt64.msg deleted file mode 100644 index 2eb1afad3..000000000 --- a/msg/std_msgs/UInt64.msg +++ /dev/null @@ -1 +0,0 @@ -uint64 data \ No newline at end of file diff --git a/msg/std_msgs/UInt64MultiArray.msg b/msg/std_msgs/UInt64MultiArray.msg deleted file mode 100644 index 30d0cd928..000000000 --- a/msg/std_msgs/UInt64MultiArray.msg +++ /dev/null @@ -1,6 +0,0 @@ -# Please look at the MultiArrayLayout message definition for -# documentation on all multiarrays. - -MultiArrayLayout layout # specification of data layout -uint64[] data # array of data - diff --git a/msg/std_msgs/UInt8.msg b/msg/std_msgs/UInt8.msg deleted file mode 100644 index 5eefd870d..000000000 --- a/msg/std_msgs/UInt8.msg +++ /dev/null @@ -1 +0,0 @@ -uint8 data diff --git a/msg/std_msgs/UInt8MultiArray.msg b/msg/std_msgs/UInt8MultiArray.msg deleted file mode 100644 index 31f7d6a21..000000000 --- a/msg/std_msgs/UInt8MultiArray.msg +++ /dev/null @@ -1,6 +0,0 @@ -# Please look at the MultiArrayLayout message definition for -# documentation on all multiarrays. - -MultiArrayLayout layout # specification of data layout -uint8[] data # array of data - -- cgit v1.2.3 From 2b43e0fc3072c246de9cd3dd36e3b803c7b81a9c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 9 Jan 2015 09:15:48 +0100 Subject: autstart for mc_att_control_m --- ROMFS/px4fmu_common/init.d/rc.mc_apps | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 268eb9bba..2248b0bcd 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -8,5 +8,10 @@ attitude_estimator_ekf start #ekf_att_pos_estimator start position_estimator_inav start -mc_att_control start +if mc_att_control start +then +else + # try the multiplatform version + mc_att_control_m start +fi mc_pos_control start -- 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(-) 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 aa8555c02b48ec5864a4dbbec6f38fbae6f73c7c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 9 Jan 2015 16:56:15 +0100 Subject: add house world launch file --- launch/gazebo_multicopter_house_world.launch | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100644 launch/gazebo_multicopter_house_world.launch diff --git a/launch/gazebo_multicopter_house_world.launch b/launch/gazebo_multicopter_house_world.launch new file mode 100644 index 000000000..ce5aca357 --- /dev/null +++ b/launch/gazebo_multicopter_house_world.launch @@ -0,0 +1,6 @@ + + + + + + -- cgit v1.2.3 From 6580d66d45cfb4b251e3b0f32b61161ee0d14ecb Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 10 Jan 2015 19:14:23 +0100 Subject: ros sim: use ardrone model --- launch/gazebo_multicopter.launch | 2 +- launch/multicopter.launch | 6 +++--- src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp | 6 +++--- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 2 +- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/launch/gazebo_multicopter.launch b/launch/gazebo_multicopter.launch index febf7bdc0..8091e3ff2 100644 --- a/launch/gazebo_multicopter.launch +++ b/launch/gazebo_multicopter.launch @@ -1,6 +1,6 @@ - + diff --git a/launch/multicopter.launch b/launch/multicopter.launch index 9956c871d..0a4b8c26d 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -14,9 +14,9 @@ - - - + + + diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp index ce863d10e..920334363 100644 --- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp @@ -47,7 +47,7 @@ AttitudeEstimator::AttitudeEstimator() : _n(), // _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &AttitudeEstimator::ModelStatesCallback, this)), - _sub_imu(_n.subscribe("/vtol/imu", 1, &AttitudeEstimator::ImuCallback, this)), + _sub_imu(_n.subscribe("/ardrone/imu", 1, &AttitudeEstimator::ImuCallback, this)), _vehicle_attitude_pub(_n.advertise("vehicle_attitude", 1)) { } @@ -60,7 +60,7 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP /* Convert quaternion to rotation matrix */ math::Quaternion quat; - //XXX: search for vtol or other (other than 'plane') vehicle here + //XXX: search for ardrone or other (other than 'plane') vehicle here int index = 1; quat(0) = (float)msg->pose[index].orientation.w; quat(1) = (float)msg->pose[index].orientation.x; @@ -103,7 +103,7 @@ void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr &msg) /* Convert quaternion to rotation matrix */ math::Quaternion quat; - //XXX: search for vtol or other (other than 'plane') vehicle here + //XXX: search for ardrone or other (other than 'plane') vehicle here int index = 1; quat(0) = (float)msg->orientation.w; quat(1) = (float)msg->orientation.x; diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index e2180af41..3867d448e 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -111,7 +111,7 @@ const MultirotorMixer::Rotor *_config_index[3] = { MultirotorMixer::MultirotorMixer(): _n(), _rotor_count(4), - _rotors(_config_index[2]) //XXX + eurocconfig hardcoded + _rotors(_config_index[0]) //XXX hardcoded { _sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback, this); _pub = _n.advertise("/mixed_motor_commands", 10); -- cgit v1.2.3 From 8f8daf6594a066919d84fc5e79cd264a23dadafa Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 10 Jan 2015 19:15:01 +0100 Subject: ros: house_world launch file --- launch/gazebo_multicopter_house_world.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/gazebo_multicopter_house_world.launch b/launch/gazebo_multicopter_house_world.launch index ce5aca357..7b6be75ac 100644 --- a/launch/gazebo_multicopter_house_world.launch +++ b/launch/gazebo_multicopter_house_world.launch @@ -1,6 +1,6 @@ - + -- cgit v1.2.3 From 2669c699c7974b9f1be5a0beb678dac724dfc909 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 13 Jan 2015 09:12:49 +0100 Subject: ros attitude estimator dummy: fix topic name --- src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp index 920334363..bcee0b479 100644 --- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp @@ -47,7 +47,7 @@ AttitudeEstimator::AttitudeEstimator() : _n(), // _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &AttitudeEstimator::ModelStatesCallback, this)), - _sub_imu(_n.subscribe("/ardrone/imu", 1, &AttitudeEstimator::ImuCallback, this)), + _sub_imu(_n.subscribe("/px4_multicopter/imu", 1, &AttitudeEstimator::ImuCallback, this)), _vehicle_attitude_pub(_n.advertise("vehicle_attitude", 1)) { } -- cgit v1.2.3 From 1cff86b0b562301020973f354043f27272d29f5b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 13 Jan 2015 09:13:11 +0100 Subject: ros mixer: increase number of controls to default to fix undefined behaviour --- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 3867d448e..1073833c0 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -68,11 +68,11 @@ private: unsigned _rotor_count; struct { - float control[6]; + float control[8]; } inputs; struct { - float control[6]; + float control[8]; } outputs; bool _armed; -- cgit v1.2.3 From 0c82600b83cea4c47ae8810cf1b29c4c805f79cf Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 13 Jan 2015 11:46:50 +0100 Subject: rename launch files and add iris empty world launch file --- launch/gazebo_ardrone_empty_world.launch | 6 ++++++ launch/gazebo_ardrone_house_world.launch | 6 ++++++ launch/gazebo_iris_empty_world.launch | 6 ++++++ launch/gazebo_multicopter.launch | 6 ------ launch/gazebo_multicopter_house_world.launch | 6 ------ 5 files changed, 18 insertions(+), 12 deletions(-) create mode 100644 launch/gazebo_ardrone_empty_world.launch create mode 100644 launch/gazebo_ardrone_house_world.launch create mode 100644 launch/gazebo_iris_empty_world.launch delete mode 100644 launch/gazebo_multicopter.launch delete mode 100644 launch/gazebo_multicopter_house_world.launch diff --git a/launch/gazebo_ardrone_empty_world.launch b/launch/gazebo_ardrone_empty_world.launch new file mode 100644 index 000000000..8091e3ff2 --- /dev/null +++ b/launch/gazebo_ardrone_empty_world.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/launch/gazebo_ardrone_house_world.launch b/launch/gazebo_ardrone_house_world.launch new file mode 100644 index 000000000..7b6be75ac --- /dev/null +++ b/launch/gazebo_ardrone_house_world.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/launch/gazebo_iris_empty_world.launch b/launch/gazebo_iris_empty_world.launch new file mode 100644 index 000000000..20574e00d --- /dev/null +++ b/launch/gazebo_iris_empty_world.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/launch/gazebo_multicopter.launch b/launch/gazebo_multicopter.launch deleted file mode 100644 index 8091e3ff2..000000000 --- a/launch/gazebo_multicopter.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/launch/gazebo_multicopter_house_world.launch b/launch/gazebo_multicopter_house_world.launch deleted file mode 100644 index 7b6be75ac..000000000 --- a/launch/gazebo_multicopter_house_world.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - -- cgit v1.2.3 From dd2d53d7e32044d44ec37bf36b0c0a6949c4f662 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 13 Jan 2015 14:49:38 +0100 Subject: add outdoor launch file --- launch/gazebo_iris_outdoor_world.launch | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100644 launch/gazebo_iris_outdoor_world.launch diff --git a/launch/gazebo_iris_outdoor_world.launch b/launch/gazebo_iris_outdoor_world.launch new file mode 100644 index 000000000..e7fd0b395 --- /dev/null +++ b/launch/gazebo_iris_outdoor_world.launch @@ -0,0 +1,6 @@ + + + + + + -- cgit v1.2.3 From f6c0d2310d538e44c3764f3a13d80789ca34170e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 14 Jan 2015 07:41:14 +0100 Subject: ros mixer node: add w mixer --- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 1073833c0..e66558fe2 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -101,17 +101,24 @@ const MultirotorMixer::Rotor _config_quad_plus_euroc[] = { { 1.000000, 0.000000, -1.00 }, { -1.000000, 0.000000, -1.00 }, }; +const MultirotorMixer::Rotor _config_quad_wide[] = { + { -0.927184, 0.374607, 1.000000 }, + { 0.777146, -0.629320, 1.000000 }, + { 0.927184, 0.374607, -1.000000 }, + { -0.777146, -0.629320, -1.000000 }, +}; -const MultirotorMixer::Rotor *_config_index[3] = { +const MultirotorMixer::Rotor *_config_index[4] = { &_config_x[0], &_config_quad_plus[0], - &_config_quad_plus_euroc[0] + &_config_quad_plus_euroc[0], + &_config_quad_wide[0] }; MultirotorMixer::MultirotorMixer(): _n(), _rotor_count(4), - _rotors(_config_index[0]) //XXX hardcoded + _rotors(_config_index[3]) //XXX hardcoded { _sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback, this); _pub = _n.advertise("/mixed_motor_commands", 10); -- cgit v1.2.3 From 6b0d0aa2a5d2e9623ab2850396818679672715e9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 14 Jan 2015 11:27:32 +0100 Subject: ros: make mixer name a param --- launch/gazebo_ardrone_empty_world.launch | 2 +- launch/gazebo_ardrone_house_world.launch | 2 +- launch/gazebo_iris_empty_world.launch | 2 +- launch/gazebo_iris_outdoor_world.launch | 2 +- launch/multicopter.launch | 1 + launch/multicopter_w.launch | 9 +++++++++ launch/multicopter_x.launch | 9 +++++++++ src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 20 +++++++++++++++++++- 8 files changed, 42 insertions(+), 5 deletions(-) create mode 100644 launch/multicopter_w.launch create mode 100644 launch/multicopter_x.launch diff --git a/launch/gazebo_ardrone_empty_world.launch b/launch/gazebo_ardrone_empty_world.launch index 8091e3ff2..00ef7ab45 100644 --- a/launch/gazebo_ardrone_empty_world.launch +++ b/launch/gazebo_ardrone_empty_world.launch @@ -1,6 +1,6 @@ - + diff --git a/launch/gazebo_ardrone_house_world.launch b/launch/gazebo_ardrone_house_world.launch index 7b6be75ac..f9ac1cac5 100644 --- a/launch/gazebo_ardrone_house_world.launch +++ b/launch/gazebo_ardrone_house_world.launch @@ -1,6 +1,6 @@ - + diff --git a/launch/gazebo_iris_empty_world.launch b/launch/gazebo_iris_empty_world.launch index 20574e00d..22e032978 100644 --- a/launch/gazebo_iris_empty_world.launch +++ b/launch/gazebo_iris_empty_world.launch @@ -1,6 +1,6 @@ - + diff --git a/launch/gazebo_iris_outdoor_world.launch b/launch/gazebo_iris_outdoor_world.launch index e7fd0b395..88960b5ec 100644 --- a/launch/gazebo_iris_outdoor_world.launch +++ b/launch/gazebo_iris_outdoor_world.launch @@ -1,6 +1,6 @@ - + diff --git a/launch/multicopter.launch b/launch/multicopter.launch index 0a4b8c26d..79c3c36d9 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -19,6 +19,7 @@ + diff --git a/launch/multicopter_w.launch b/launch/multicopter_w.launch new file mode 100644 index 000000000..f124082aa --- /dev/null +++ b/launch/multicopter_w.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/launch/multicopter_x.launch b/launch/multicopter_x.launch new file mode 100644 index 000000000..6355b87be --- /dev/null +++ b/launch/multicopter_x.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index e66558fe2..9954692bc 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -41,6 +41,7 @@ #include #include #include +#include class MultirotorMixer { @@ -118,7 +119,7 @@ const MultirotorMixer::Rotor *_config_index[4] = { MultirotorMixer::MultirotorMixer(): _n(), _rotor_count(4), - _rotors(_config_index[3]) //XXX hardcoded + _rotors(_config_index[0]) { _sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback, this); _pub = _n.advertise("/mixed_motor_commands", 10); @@ -131,6 +132,10 @@ MultirotorMixer::MultirotorMixer(): _n.setParam("motor_offset_radps", 600.0); } + if (!_n.hasParam("mixer")) { + _n.setParam("mixer", "x"); + } + _sub_actuator_armed = _n.subscribe("actuator_armed", 1, &MultirotorMixer::actuatorArmedCallback, this); } @@ -206,6 +211,19 @@ void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_contro inputs.control[i] = msg.control[i]; } + // switch mixer if necessary + std::string mixer_name; + _n.getParamCached("mixer", mixer_name); + if (mixer_name == "x") { + _rotors = _config_index[0]; + } else if (mixer_name == "+") { + _rotors = _config_index[1]; + } else if (mixer_name == "e") { + _rotors = _config_index[2]; + } else if (mixer_name == "w") { + _rotors = _config_index[3]; + } + // mix mix(); -- cgit v1.2.3 From 3da1b701ed941effd92e7699e8e435a810300b49 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 14 Jan 2015 11:31:44 +0100 Subject: ros: add type specific use files than can be used by all toplevel launchfiles --- launch/ardrone.launch | 5 +++++ launch/gazebo_ardrone_empty_world.launch | 2 +- launch/gazebo_ardrone_house_world.launch | 2 +- launch/gazebo_iris_empty_world.launch | 2 +- launch/gazebo_iris_outdoor_world.launch | 2 +- launch/iris.launch | 5 +++++ 6 files changed, 14 insertions(+), 4 deletions(-) create mode 100644 launch/ardrone.launch create mode 100644 launch/iris.launch diff --git a/launch/ardrone.launch b/launch/ardrone.launch new file mode 100644 index 000000000..aa89bdd4e --- /dev/null +++ b/launch/ardrone.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/launch/gazebo_ardrone_empty_world.launch b/launch/gazebo_ardrone_empty_world.launch index 00ef7ab45..395e70b00 100644 --- a/launch/gazebo_ardrone_empty_world.launch +++ b/launch/gazebo_ardrone_empty_world.launch @@ -1,6 +1,6 @@ - + diff --git a/launch/gazebo_ardrone_house_world.launch b/launch/gazebo_ardrone_house_world.launch index f9ac1cac5..f43c16b50 100644 --- a/launch/gazebo_ardrone_house_world.launch +++ b/launch/gazebo_ardrone_house_world.launch @@ -1,6 +1,6 @@ - + diff --git a/launch/gazebo_iris_empty_world.launch b/launch/gazebo_iris_empty_world.launch index 22e032978..833aaf07a 100644 --- a/launch/gazebo_iris_empty_world.launch +++ b/launch/gazebo_iris_empty_world.launch @@ -1,6 +1,6 @@ - + diff --git a/launch/gazebo_iris_outdoor_world.launch b/launch/gazebo_iris_outdoor_world.launch index 88960b5ec..c2975a050 100644 --- a/launch/gazebo_iris_outdoor_world.launch +++ b/launch/gazebo_iris_outdoor_world.launch @@ -1,6 +1,6 @@ - + diff --git a/launch/iris.launch b/launch/iris.launch new file mode 100644 index 000000000..d9d08fae0 --- /dev/null +++ b/launch/iris.launch @@ -0,0 +1,5 @@ + + + + + -- cgit v1.2.3 From 68914df4d0638c974e94657b35a8c12f71738d01 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 14 Jan 2015 11:34:55 +0100 Subject: ros: move params to type/frame launch files --- launch/ardrone.launch | 14 ++++++++++++++ launch/iris.launch | 14 ++++++++++++++ launch/multicopter.launch | 13 ------------- 3 files changed, 28 insertions(+), 13 deletions(-) diff --git a/launch/ardrone.launch b/launch/ardrone.launch index aa89bdd4e..495388be5 100644 --- a/launch/ardrone.launch +++ b/launch/ardrone.launch @@ -2,4 +2,18 @@ + + + + + + + + + + + + + + diff --git a/launch/iris.launch b/launch/iris.launch index d9d08fae0..81a137173 100644 --- a/launch/iris.launch +++ b/launch/iris.launch @@ -2,4 +2,18 @@ + + + + + + + + + + + + + + diff --git a/launch/multicopter.launch b/launch/multicopter.launch index 79c3c36d9..96ff3ad99 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -7,19 +7,6 @@ - - - - - - - - - - - - - -- cgit v1.2.3 From 1f26e1f5ebb0d0fffec98644d2a824f88415e9d7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 15 Jan 2015 12:36:30 +0100 Subject: remove hector from install script --- Tools/ros/px4_ros_installation_ubuntu.sh | 6 ------ 1 file changed, 6 deletions(-) diff --git a/Tools/ros/px4_ros_installation_ubuntu.sh b/Tools/ros/px4_ros_installation_ubuntu.sh index c6f27653c..d83039623 100755 --- a/Tools/ros/px4_ros_installation_ubuntu.sh +++ b/Tools/ros/px4_ros_installation_ubuntu.sh @@ -23,9 +23,3 @@ source ~/.bashrc # get rosinstall sudo apt-get install python-rosinstall - -# Hector packages -sudo apt-get install ros-indigo-hector-quadrotor \ - ros-indigo-octomap-msgs \ - ros-indigo-hector-gazebo - -- cgit v1.2.3 From e60c1a842c856e1a19fcdc1b169dfdbc813e9ce2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 15 Jan 2015 12:37:24 +0100 Subject: ros mixer: add iris --- launch/iris.launch | 1 + src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 17 +++++++++++++++-- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/launch/iris.launch b/launch/iris.launch index 81a137173..44a0ae034 100644 --- a/launch/iris.launch +++ b/launch/iris.launch @@ -14,6 +14,7 @@ + diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 9954692bc..54f5fa78b 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -108,12 +108,19 @@ const MultirotorMixer::Rotor _config_quad_wide[] = { { 0.927184, 0.374607, -1.000000 }, { -0.777146, -0.629320, -1.000000 }, }; +const MultirotorMixer::Rotor _config_quad_iris[] = { + { -0.876559, 0.481295, 1.000000 }, + { 0.826590, -0.562805, 1.000000 }, + { 0.876559, 0.481295, -1.000000 }, + { -0.826590, -0.562805, -1.000000 }, +}; -const MultirotorMixer::Rotor *_config_index[4] = { +const MultirotorMixer::Rotor *_config_index[5] = { &_config_x[0], &_config_quad_plus[0], &_config_quad_plus_euroc[0], - &_config_quad_wide[0] + &_config_quad_wide[0], + &_config_quad_iris[0] }; MultirotorMixer::MultirotorMixer(): @@ -216,13 +223,19 @@ void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_contro _n.getParamCached("mixer", mixer_name); if (mixer_name == "x") { _rotors = _config_index[0]; + ROS_WARN("using x"); } else if (mixer_name == "+") { _rotors = _config_index[1]; } else if (mixer_name == "e") { _rotors = _config_index[2]; } else if (mixer_name == "w") { _rotors = _config_index[3]; + ROS_WARN("using w"); + } else if (mixer_name == "i") { + _rotors = _config_index[4]; + ROS_WARN("using i"); } + ROS_WARN("mixer_name %s", mixer_name.c_str()); // mix mix(); -- cgit v1.2.3 From ac76cdbc373cd09210ad0d35db91ff08c39cf872 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 16 Jan 2015 11:42:00 +0100 Subject: added catkin_make --- Tools/ros/px4_workspace_create.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/ros/px4_workspace_create.sh b/Tools/ros/px4_workspace_create.sh index d8d65c819..4055f7320 100755 --- a/Tools/ros/px4_workspace_create.sh +++ b/Tools/ros/px4_workspace_create.sh @@ -5,3 +5,4 @@ mkdir -p catkin_ws/src cd catkin_ws/src catkin_init_workspace cd .. +catkin_make -- cgit v1.2.3 From 18b4e1583663c3066d8fd4496ce74f2f8bf23103 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 16 Jan 2015 12:10:33 +0100 Subject: added needed dependencies because or recent changes in ROS --- Tools/ros/px4_ros_installation_ubuntu.sh | 4 ++++ Tools/ros/px4_workspace_setup.sh | 8 +++++++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/Tools/ros/px4_ros_installation_ubuntu.sh b/Tools/ros/px4_ros_installation_ubuntu.sh index d83039623..13532049f 100755 --- a/Tools/ros/px4_ros_installation_ubuntu.sh +++ b/Tools/ros/px4_ros_installation_ubuntu.sh @@ -23,3 +23,7 @@ source ~/.bashrc # get rosinstall sudo apt-get install python-rosinstall + +# additional dependencies necessary +sudo apt-get install ros-indigo-octomap-msgs + diff --git a/Tools/ros/px4_workspace_setup.sh b/Tools/ros/px4_workspace_setup.sh index 233d21671..0a7e1742c 100755 --- a/Tools/ros/px4_workspace_setup.sh +++ b/Tools/ros/px4_workspace_setup.sh @@ -15,9 +15,15 @@ cd euroc_simulator git checkout px4_nodes cd .. -# # mav comm +# mav comm git clone https://github.com/PX4/mav_comm.git +# glog catkin +git clone https://github.com/ethz-asl/glog_catkin.git + +# catkin simple +git clone https://github.com/catkin/catkin_simple.git + cd .. catkin_make -- cgit v1.2.3 From dfba2f3cb00e65bb240975e4a9cbfa03e6189e96 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 16 Jan 2015 15:18:53 +0100 Subject: add drcsim to workspace setup script --- Tools/ros/px4_workspace_setup.sh | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/Tools/ros/px4_workspace_setup.sh b/Tools/ros/px4_workspace_setup.sh index 0a7e1742c..7c8af6e8f 100755 --- a/Tools/ros/px4_workspace_setup.sh +++ b/Tools/ros/px4_workspace_setup.sh @@ -18,12 +18,17 @@ cd .. # mav comm git clone https://github.com/PX4/mav_comm.git -# glog catkin +# glog catkin git clone https://github.com/ethz-asl/glog_catkin.git # catkin simple git clone https://github.com/catkin/catkin_simple.git +# drcsim (for scenery and models) +hg clone https://bitbucket.org/osrf/osrf-common +hg clone https://bitbucket.org/osrf/sandia-hand +hg clone https://bitbucket.org/osrf/drcsim + cd .. catkin_make -- cgit v1.2.3 From 94091a1ce7439ca6744137fae11d07442e8f3622 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 17 Jan 2015 16:46:20 +0100 Subject: fix dependencies in CMakeLists.txt --- CMakeLists.txt | 30 ++++++++++-------------------- 1 file changed, 10 insertions(+), 20 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index eca28b15e..2de236ff6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -103,9 +103,8 @@ generate_messages( catkin_package( INCLUDE_DIRS src/include LIBRARIES px4 - CATKIN_DEPENDS roscpp rospy std_msgs + CATKIN_DEPENDS message_runtime roscpp rospy std_msgs DEPENDS system_lib - CATKIN_DEPENDS message_runtime ) ########### @@ -132,6 +131,7 @@ add_library(px4 src/lib/mathlib/math/Limits.cpp src/modules/systemlib/circuit_breaker.cpp ) +add_dependencies(px4 ${PROJECT_NAME}_generate_messages_cpp) target_link_libraries(px4 ${catkin_LIBRARIES} @@ -141,12 +141,7 @@ target_link_libraries(px4 add_executable(publisher src/examples/publisher/publisher_main.cpp src/examples/publisher/publisher_example.cpp) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -add_dependencies(publisher px4_generate_messages_cpp) - -## Specify libraries to link a library or executable target against +add_dependencies(publisher ${PROJECT_NAME}_generate_messages_cpp) target_link_libraries(publisher ${catkin_LIBRARIES} px4 @@ -156,12 +151,7 @@ target_link_libraries(publisher add_executable(subscriber src/examples/subscriber/subscriber_main.cpp src/examples/subscriber/subscriber_example.cpp) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -add_dependencies(subscriber px4_generate_messages_cpp) - -## Specify libraries to link a library or executable target against +add_dependencies(subscriber ${PROJECT_NAME}_generate_messages_cpp) target_link_libraries(subscriber ${catkin_LIBRARIES} px4 @@ -172,7 +162,7 @@ add_executable(mc_att_control src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp src/modules/mc_att_control_multiplatform/mc_att_control.cpp src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp) -add_dependencies(mc_att_control px4_generate_messages_cpp) +add_dependencies(mc_att_control ${PROJECT_NAME}_generate_messages_cpp_cpp) target_link_libraries(mc_att_control ${catkin_LIBRARIES} px4 @@ -181,7 +171,7 @@ target_link_libraries(mc_att_control ## Attitude Estimator dummy add_executable(attitude_estimator src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp) -add_dependencies(attitude_estimator px4_generate_messages_cpp) +add_dependencies(attitude_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp) target_link_libraries(attitude_estimator ${catkin_LIBRARIES} px4 @@ -190,7 +180,7 @@ target_link_libraries(attitude_estimator ## Manual input add_executable(manual_input src/platforms/ros/nodes/manual_input/manual_input.cpp) -add_dependencies(manual_input px4_generate_messages_cpp) +add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) target_link_libraries(manual_input ${catkin_LIBRARIES} px4 @@ -199,7 +189,7 @@ target_link_libraries(manual_input ## Multicopter Mixer dummy add_executable(mc_mixer src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp) -add_dependencies(mc_mixer px4_generate_messages_cpp) +add_dependencies(mc_mixer ${PROJECT_NAME}_generate_messages_cpp_cpp) target_link_libraries(mc_mixer ${catkin_LIBRARIES} px4 @@ -208,7 +198,7 @@ target_link_libraries(mc_mixer ## Commander add_executable(commander src/platforms/ros/nodes/commander/commander.cpp) -add_dependencies(manual_input px4_generate_messages_cpp) +add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) target_link_libraries(commander ${catkin_LIBRARIES} px4 @@ -229,7 +219,7 @@ target_link_libraries(commander # ) ## Mark executables and/or libraries for installation -install(TARGETS px4 publisher subscriber +install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -- cgit v1.2.3 From 93173190fd1f20b9c9a939d423d8cfe6cf13f562 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 16 Jan 2015 18:10:16 +0100 Subject: removed drcsim, set bash as executing shell --- Tools/ros/px4_workspace_setup.sh | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/Tools/ros/px4_workspace_setup.sh b/Tools/ros/px4_workspace_setup.sh index 7c8af6e8f..8c50579b6 100755 --- a/Tools/ros/px4_workspace_setup.sh +++ b/Tools/ros/px4_workspace_setup.sh @@ -1,4 +1,4 @@ -#!/bin/sh +#!/bin/bash # run this script from the root of your catkin_ws source devel/setup.bash cd src @@ -27,7 +27,6 @@ git clone https://github.com/catkin/catkin_simple.git # drcsim (for scenery and models) hg clone https://bitbucket.org/osrf/osrf-common hg clone https://bitbucket.org/osrf/sandia-hand -hg clone https://bitbucket.org/osrf/drcsim cd .. -- cgit v1.2.3 From 43f3e3ac789d7af15b41ba5f857cc8596b70999d Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 16 Jan 2015 18:21:06 +0100 Subject: adding drcsim binary installation --- Tools/ros/px4_ros_installation_ubuntu.sh | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Tools/ros/px4_ros_installation_ubuntu.sh b/Tools/ros/px4_ros_installation_ubuntu.sh index 13532049f..80f684590 100755 --- a/Tools/ros/px4_ros_installation_ubuntu.sh +++ b/Tools/ros/px4_ros_installation_ubuntu.sh @@ -2,14 +2,17 @@ # main ROS Setup # following http://wiki.ros.org/indigo/Installation/Ubuntu +# also adding dependencies for gazebo http://gazebosim.org/tutorials?tut=drcsim_install # run this file with . ./px4_ros_setup_ubuntu.sh ## add ROS repository sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list' +sudo sh -c 'echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" > /etc/apt/sources.list.d/drc-latest.list' ## add key wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | \ sudo apt-key add - +wget http://packages.osrfoundation.org/drc.key -O - | sudo apt-key add - ## Install main ROS pacakges sudo apt-get update @@ -26,4 +29,5 @@ sudo apt-get install python-rosinstall # additional dependencies necessary sudo apt-get install ros-indigo-octomap-msgs +sudo apt-get install drcsim -- cgit v1.2.3 From 4c9cc4175b692c1074a64d16ef02b21dc44594d8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 19 Jan 2015 07:47:12 +0100 Subject: ros: update ubuntu install scripts --- Tools/ros/px4_ros_installation_ubuntu.sh | 18 ++++++++++++------ Tools/ros/px4_workspace_setup.sh | 2 -- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/Tools/ros/px4_ros_installation_ubuntu.sh b/Tools/ros/px4_ros_installation_ubuntu.sh index 80f684590..5bca6dfc4 100755 --- a/Tools/ros/px4_ros_installation_ubuntu.sh +++ b/Tools/ros/px4_ros_installation_ubuntu.sh @@ -2,17 +2,15 @@ # main ROS Setup # following http://wiki.ros.org/indigo/Installation/Ubuntu -# also adding dependencies for gazebo http://gazebosim.org/tutorials?tut=drcsim_install +# also adding drcsim http://gazebosim.org/tutorials?tut=drcsim_install # run this file with . ./px4_ros_setup_ubuntu.sh ## add ROS repository sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list' -sudo sh -c 'echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" > /etc/apt/sources.list.d/drc-latest.list' ## add key wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | \ sudo apt-key add - -wget http://packages.osrfoundation.org/drc.key -O - | sudo apt-key add - ## Install main ROS pacakges sudo apt-get update @@ -22,12 +20,20 @@ rosdep update ## Setup environment variables echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc -source ~/.bashrc +. ~/.bashrc # get rosinstall sudo apt-get install python-rosinstall -# additional dependencies necessary +# additional dependencies sudo apt-get install ros-indigo-octomap-msgs -sudo apt-get install drcsim +## drcsim setup (for models) +### add osrf repository +sudo sh -c 'echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" > /etc/apt/sources.list.d/drc-latest.list' + +### add key +wget http://packages.osrfoundation.org/drc.key -O - | sudo apt-key add - + +### install drcsim +sudo apt-get install drcsim diff --git a/Tools/ros/px4_workspace_setup.sh b/Tools/ros/px4_workspace_setup.sh index 8c50579b6..1675e54f3 100755 --- a/Tools/ros/px4_workspace_setup.sh +++ b/Tools/ros/px4_workspace_setup.sh @@ -25,8 +25,6 @@ git clone https://github.com/ethz-asl/glog_catkin.git git clone https://github.com/catkin/catkin_simple.git # drcsim (for scenery and models) -hg clone https://bitbucket.org/osrf/osrf-common -hg clone https://bitbucket.org/osrf/sandia-hand cd .. -- cgit v1.2.3 From a5caf1c99be599401379743b675862b0601fed6e Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 19 Jan 2015 10:17:58 +0100 Subject: adding '-y' option to apt-get and missing update before drcsim installation --- Tools/ros/px4_ros_installation_ubuntu.sh | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/Tools/ros/px4_ros_installation_ubuntu.sh b/Tools/ros/px4_ros_installation_ubuntu.sh index 5bca6dfc4..72b4f9468 100755 --- a/Tools/ros/px4_ros_installation_ubuntu.sh +++ b/Tools/ros/px4_ros_installation_ubuntu.sh @@ -14,7 +14,7 @@ wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | \ ## Install main ROS pacakges sudo apt-get update -sudo apt-get install ros-indigo-desktop-full +sudo apt-get -y install ros-indigo-desktop-full sudo rosdep init rosdep update @@ -23,10 +23,10 @@ echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc . ~/.bashrc # get rosinstall -sudo apt-get install python-rosinstall +sudo apt-get -y install python-rosinstall # additional dependencies -sudo apt-get install ros-indigo-octomap-msgs +sudo apt-get -y install ros-indigo-octomap-msgs ## drcsim setup (for models) ### add osrf repository @@ -36,4 +36,5 @@ sudo sh -c 'echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" wget http://packages.osrfoundation.org/drc.key -O - | sudo apt-key add - ### install drcsim -sudo apt-get install drcsim +sudo apt-get update +sudo apt-get -y install drcsim -- cgit v1.2.3 From f176147d2af34d83d2e0c27cae8b98dfe995d812 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 11:04:56 +0100 Subject: Allow GCC 4.9.3 --- makefiles/toolchain_gnu-arm-eabi.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index ef70d51f2..396980453 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -50,7 +50,7 @@ OBJDUMP = $(CROSSDEV)objdump # Check if the right version of the toolchain is available # -CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.4 +CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.4 4.9.3 CROSSDEV_VER_FOUND = $(shell $(CC) -dumpversion) ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED))) -- cgit v1.2.3 From 6574692bd7a4101113cf6a80bcfae968d5a6e4f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 11:01:25 +0100 Subject: uORB: Remove unused function --- src/modules/uORB/uORB.cpp | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 149b8f6d2..2fbb05fee 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -817,18 +817,6 @@ uorb_main(int argc, char *argv[]) namespace { -void debug(const char *fmt, ...) -{ - va_list ap; - - va_start(ap, fmt); - vfprintf(stderr, fmt, ap); - va_end(ap); - fprintf(stderr, "\n"); - fflush(stderr); - usleep(100000); -} - /** * Advertise a node; don't consider it an error if the node has * already been advertised. -- cgit v1.2.3 From fd275147e4ae2efe2803e0bdc8f2c2929db70b5d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 13:39:13 +0100 Subject: Add hackery on NuttX header, to be removed during rebase -i --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index dbcccb245..3d8171f6e 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit dbcccb2455d759b789d549d25e1fbf489b2d3c83 +Subproject commit 3d8171f6ea88297d8595525c8222d61e9cf20fd0 -- cgit v1.2.3 From 85b6907e1db1a6af88fe469e8e08dbd0a9d7a2a7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 13:39:36 +0100 Subject: Fixes to make GCC 4.9 link --- makefiles/toolchain_gnu-arm-eabi.mk | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 396980453..a8b4f1811 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -110,9 +110,7 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ -fno-strength-reduce \ -fomit-frame-pointer \ -funsafe-math-optimizations \ - -fno-builtin-printf \ - -ffunction-sections \ - -fdata-sections + -fno-builtin-printf # enable precise stack overflow tracking # note - requires corresponding support in NuttX @@ -166,7 +164,8 @@ ARCHWARNINGSXX = $(ARCHWARNINGS) \ # pull in *just* libm from the toolchain ... this is grody LIBM := $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a) -EXTRA_LIBS += $(LIBM) +LIBC := $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libc.a) +EXTRA_LIBS += $(LIBM) $(LIBC) # Flags we pass to the C compiler # -- cgit v1.2.3 From d1eac3510bdee3aa133a9eea248f882758dd29d0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 13:40:10 +0100 Subject: DELETE DURING REBASE: Remove some apps to make space --- makefiles/config_px4fmu-v2_default.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index a5f996bb3..2ba07821f 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -31,7 +31,7 @@ MODULES += drivers/mb12xx MODULES += drivers/ll40ls # MODULES += drivers/trone MODULES += drivers/gps -MODULES += drivers/hil +# MODULES += drivers/hil # MODULES += drivers/hott # MODULES += drivers/hott/hott_telemetry # MODULES += drivers/hott/hott_sensors @@ -76,7 +76,7 @@ MODULES += modules/uavcan # Estimation modules (EKF/ SO3 / other filters) # MODULES += modules/attitude_estimator_ekf -MODULES += modules/ekf_att_pos_estimator +# MODULES += modules/ekf_att_pos_estimator MODULES += modules/position_estimator_inav # -- cgit v1.2.3 From 7c3223b8609ee418b520d19cae7e52d2a7a85e99 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 18 Jan 2015 18:43:45 +0100 Subject: added a messageplayer prototype for ros --- CMakeLists.txt | 85 +++++----- makefiles/config_px4fmu-v2_multiplatform.mk | 3 +- makefiles/config_px4fmu-v2_test.mk | 2 +- msg/templates/msg.h.template | 74 +++++++-- src/examples/publisher/publisher_example.cpp | 19 ++- src/examples/publisher/publisher_example.h | 1 + src/examples/subscriber/subscriber_example.cpp | 14 +- src/examples/subscriber/subscriber_example.h | 3 +- src/platforms/nuttx/px4_messages/px4_rc_channels.h | 26 +++ src/platforms/px4_includes.h | 4 + src/platforms/px4_message.h | 77 +++++++++ src/platforms/px4_nodehandle.h | 180 ++++++++++++++------- src/platforms/px4_publisher.h | 41 +++-- src/platforms/px4_subscriber.h | 114 +++++++++---- src/platforms/ros/px4_messages/px4_rc_channels.h | 25 +++ 15 files changed, 496 insertions(+), 172 deletions(-) create mode 100644 src/platforms/nuttx/px4_messages/px4_rc_channels.h create mode 100644 src/platforms/px4_message.h create mode 100644 src/platforms/ros/px4_messages/px4_rc_channels.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 2de236ff6..0ac9912df 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -116,6 +116,7 @@ catkin_package( include_directories( ${catkin_INCLUDE_DIRS} src/platforms + src/platforms/ros/px4_messages src/include src/modules src/ @@ -157,52 +158,52 @@ target_link_libraries(subscriber px4 ) -## MC Attitude Control -add_executable(mc_att_control - src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp - src/modules/mc_att_control_multiplatform/mc_att_control.cpp - src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp) -add_dependencies(mc_att_control ${PROJECT_NAME}_generate_messages_cpp_cpp) -target_link_libraries(mc_att_control - ${catkin_LIBRARIES} - px4 -) +# ## MC Attitude Control +# add_executable(mc_att_control + # src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp + # src/modules/mc_att_control_multiplatform/mc_att_control.cpp + # src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp) +# add_dependencies(mc_att_control ${PROJECT_NAME}_generate_messages_cpp_cpp) +# target_link_libraries(mc_att_control + # ${catkin_LIBRARIES} + # px4 +# ) -## Attitude Estimator dummy -add_executable(attitude_estimator - src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp) -add_dependencies(attitude_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp) -target_link_libraries(attitude_estimator - ${catkin_LIBRARIES} - px4 -) +# ## Attitude Estimator dummy +# add_executable(attitude_estimator + # src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp) +# add_dependencies(attitude_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp) +# target_link_libraries(attitude_estimator + # ${catkin_LIBRARIES} + # px4 +# ) -## Manual input -add_executable(manual_input - src/platforms/ros/nodes/manual_input/manual_input.cpp) -add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) -target_link_libraries(manual_input - ${catkin_LIBRARIES} - px4 -) +# ## Manual input +# add_executable(manual_input + # src/platforms/ros/nodes/manual_input/manual_input.cpp) +# add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) +# target_link_libraries(manual_input + # ${catkin_LIBRARIES} + # px4 +# ) -## Multicopter Mixer dummy -add_executable(mc_mixer - src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp) -add_dependencies(mc_mixer ${PROJECT_NAME}_generate_messages_cpp_cpp) -target_link_libraries(mc_mixer - ${catkin_LIBRARIES} - px4 -) +# ## Multicopter Mixer dummy +# add_executable(mc_mixer + # src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp) +# add_dependencies(mc_mixer ${PROJECT_NAME}_generate_messages_cpp_cpp) +# target_link_libraries(mc_mixer + # ${catkin_LIBRARIES} + # px4 +# ) -## Commander -add_executable(commander - src/platforms/ros/nodes/commander/commander.cpp) -add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) -target_link_libraries(commander - ${catkin_LIBRARIES} - px4 -) +# ## Commander +# add_executable(commander + # src/platforms/ros/nodes/commander/commander.cpp) +# add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) +# target_link_libraries(commander + # ${catkin_LIBRARIES} + # px4 +# ) ############# ## Install ## diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk index bed2fa506..bfa26f83c 100644 --- a/makefiles/config_px4fmu-v2_multiplatform.mk +++ b/makefiles/config_px4fmu-v2_multiplatform.mk @@ -86,7 +86,8 @@ MODULES += modules/position_estimator_inav #MODULES += modules/fw_pos_control_l1 #MODULES += modules/fw_att_control # MODULES += modules/mc_att_control -MODULES += modules/mc_att_control_multiplatform +# MODULES += modules/mc_att_control_multiplatform +MODULES += examples/publisher MODULES += modules/mc_pos_control MODULES += modules/vtol_att_control diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index fa2c83262..4defe49e7 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -56,7 +56,7 @@ MODULES += systemcmds/ver # Example modules # MODULES += examples/matlab_csv_serial -MODULES += examples/subscriber +#MODULES += examples/subscriber MODULES += examples/publisher # diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template index 19068a3a1..c27daed83 100644 --- a/msg/templates/msg.h.template +++ b/msg/templates/msg.h.template @@ -95,17 +95,17 @@ for field in spec.parsed_fields(): @############################## @{ -type_map = {'int8': 'int8_t', - 'int16': 'int16_t', - 'int32': 'int32_t', - 'int64': 'int64_t', - 'uint8': 'uint8_t', - 'uint16': 'uint16_t', - 'uint32': 'uint32_t', - 'uint64': 'uint64_t', - 'float32': 'float', - 'bool': 'bool', - 'fence_vertex': 'fence_vertex'} +type_map = {'int8': ['int8_t', '0'], + 'int16': ['int16_t', '0'], + 'int32': ['int32_t', '0'], + 'int64': ['int64_t', '0'], + 'uint8': ['uint8_t', '0'], + 'uint16': ['uint16_t', '0'], + 'uint32': ['uint32_t', '0'], + 'uint64': ['uint64_t', '0'], + 'float32': ['float', '0.0f'], + 'bool': ['bool', 'false'], + 'fence_vertex': ['fence_vertex', '']} # Function to print a standard ros type def print_field_def(field): @@ -129,20 +129,70 @@ def print_field_def(field): if type in type_map: # need to add _t: int8 --> int8_t - type_px4 = type_map[type] + type_px4 = type_map[type][0] else: raise Exception("Type {0} not supported, add to to template file!".format(type)) print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size)) +# Function to init fields +def get_field_init(field): + type = field.type + # detect embedded types + sl_pos = type.find('/') + type_appendix = '' + type_prefix = '' + if (sl_pos >= 0): + type = type[sl_pos + 1:] + type_prefix = 'struct ' + type_appendix = '_s' + + # detect arrays + a_pos = type.find('[') + array_size = '' + if (a_pos >= 0): + # field is array + array_size = type[a_pos:] + type = type[:a_pos] + return '\n\t%s{},'%(field.name) + + if type in type_map: + # need to add _t: int8 --> int8_t + type_px4 = type_map[type][0] + init_value = type_map[type][1] + else: + raise Exception("Type {0} not supported, add to to template file!".format(type)) + + return '\n\t%s(%s),'%(field.name, init_value) } +@#ifdef __cplusplus +@#class @(spec.short_name)_s { +@#public: +@#else struct @(spec.short_name)_s { +@#endif @{ # loop over all fields and print the type and name for field in spec.parsed_fields(): if (not field.is_header): print_field_def(field) }@ + +@##ifdef __cplusplus +@#@(spec.short_name)_s() : +@#@{ +@#field_init = '' +@## loop over all fields and init +@#for field in spec.parsed_fields(): +@# if (not field.is_header): +@# field_init += get_field_init(field) +@# +@#print(field_init[:-1]) +@#}@ +@#{} +@#virtual ~@(spec.short_name)_s() {} +@##endif + }; /** diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp index 2e5779ebe..e85e42b38 100644 --- a/src/examples/publisher/publisher_example.cpp +++ b/src/examples/publisher/publisher_example.cpp @@ -45,7 +45,8 @@ using namespace px4; PublisherExample::PublisherExample() : _n(), - _rc_channels_pub(PX4_ADVERTISE(_n, rc_channels)) + _rc_channels_pub(_n.advertise()) + // _rc_channels_pub(PX4_ADVERTISE(_n, rc_channels)) { } @@ -55,11 +56,19 @@ int PublisherExample::main() px4::Rate loop_rate(10); while (px4::ok()) { - PX4_TOPIC_T(rc_channels) msg; - msg.timestamp_last_valid = px4::get_time_micros(); - PX4_INFO("%llu", msg.timestamp_last_valid); + // PX4_TOPIC_T(rc_channels) msg; + // msg.timestamp_last_valid = px4::get_time_micros(); + // PX4_INFO("%llu", msg.timestamp_last_valid); + + // _rc_channels_pub->publish(msg); + + //XXX + px4_rc_channels msg2; + msg2.data().timestamp_last_valid = px4::get_time_micros(); + PX4_INFO("%llu", msg2.data().timestamp_last_valid); + // msg2.pub->publish2(); + _rc_channels_pub->publish(msg2); - _rc_channels_pub->publish(msg); _n.spinOnce(); loop_rate.sleep(); diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h index 304ecef47..86586d0d3 100644 --- a/src/examples/publisher/publisher_example.h +++ b/src/examples/publisher/publisher_example.h @@ -48,5 +48,6 @@ public: int main(); protected: px4::NodeHandle _n; + // px4::Publisher * _rc_channels_pub; px4::Publisher * _rc_channels_pub; }; diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 3c80561ca..1c8f4c62b 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -62,11 +62,13 @@ SubscriberExample::SubscriberExample() : /* Do some subscriptions */ /* Function */ - PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval); - /* Class Method */ - PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000); - /* No callback */ - _sub_rc_chan = PX4_SUBSCRIBE(_n, rc_channels, 500); + // PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval); + _n.subscribe(rc_channels_callback_function); + + // [> Class Method <] + // PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000); + // [> No callback <] + // _sub_rc_chan = PX4_SUBSCRIBE(_n, rc_channels, 500); PX4_INFO("subscribed"); } @@ -78,5 +80,5 @@ SubscriberExample::SubscriberExample() : void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", msg.timestamp_last_valid, - _sub_rc_chan->get().timestamp_last_valid); + _sub_rc_chan->get().data().timestamp_last_valid); } diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index eb2c956e0..b52ae991b 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -56,7 +56,8 @@ protected: int32_t _interval; px4_param_t _p_test_float; float _test_float; - px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan; + // px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan; + px4::Subscriber * _sub_rc_chan; void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg); diff --git a/src/platforms/nuttx/px4_messages/px4_rc_channels.h b/src/platforms/nuttx/px4_messages/px4_rc_channels.h new file mode 100644 index 000000000..bfca48469 --- /dev/null +++ b/src/platforms/nuttx/px4_messages/px4_rc_channels.h @@ -0,0 +1,26 @@ +#include +#include +#include "platforms/px4_message.h" + +#pragma once +namespace px4 +{ + +class px4_rc_channels : + public PX4Message +{ +public: + px4_rc_channels() : + PX4Message() + {} + + px4_rc_channels(rc_channels_s msg) : + PX4Message(msg) + {} + + ~px4_rc_channels() {} + + PX4TopicHandle handle() {return (PX4TopicHandle)ORB_ID(rc_channels);} +}; + +} diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index fc31162c8..bdb2b8de9 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -49,6 +49,7 @@ #ifdef __cplusplus #include "ros/ros.h" #include "px4/rc_channels.h" +#include "px4_rc_channels.h" //XXX #include "px4/vehicle_attitude.h" #include #include @@ -71,6 +72,9 @@ #include #include #include +#ifdef __cplusplus +#include //XXX +#endif #include #include #include diff --git a/src/platforms/px4_message.h b/src/platforms/px4_message.h new file mode 100644 index 000000000..05fcf1140 --- /dev/null +++ b/src/platforms/px4_message.h @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * 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 px4_message.h + * + * Defines the message base types + */ +#pragma once + +#if defined(__PX4_ROS) +typedef const char* PX4TopicHandle; +#else +#include +typedef const struct orb_metatdata* PX4TopicHandle; +#endif + +namespace px4 +{ + +template +class PX4Message +{ + // friend class NodeHandle; +// #if defined(__PX4_ROS) + // template + // friend class SubscriberROS; +// #endif + +public: + PX4Message() : + _data() + {} + + PX4Message(M msg) : + _data(msg) + {} + + virtual ~PX4Message() {}; + + virtual M& data() {return _data;} + virtual PX4TopicHandle handle() = 0; +private: + M _data; +}; + +} diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 7b14caed9..a40581239 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -48,6 +48,7 @@ #include "ros/ros.h" #include #include +#include #else /* includes when building for NuttX */ #include @@ -77,15 +78,25 @@ public: * @param topic Name of the topic * @param fb Callback, executed on receiving a new message */ - template - Subscriber *subscribe(const char *topic, void(*fp)(const M &)) + // template + // Subscriber *subscribe(const char *topic, void(*fp)(const M &)) + // { + // SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); + // ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, + // (SubscriberROS *)sub); + // ((SubscriberROS *)sub)->set_ros_sub(ros_sub); + // _subs.push_back(sub); + // return (Subscriber *)sub; + // } + template + Subscriber *subscribe(void(*fp)(const typename std::remove_referencedata())>::type &)) { - SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, - (SubscriberROS *)sub); - ((SubscriberROS *)sub)->set_ros_sub(ros_sub); + SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe((new T())->handle(), kQueueSizeDefault, + &SubscriberROS::callback, (SubscriberROS *)sub); + ((SubscriberROS *)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); - return (Subscriber *)sub; + return (Subscriber *)sub; } /** @@ -93,40 +104,49 @@ public: * @param topic Name of the topic * @param fb Callback, executed on receiving a new message */ - template - Subscriber *subscribe(const char *topic, void(T::*fp)(const M &), T *obj) - { - SubscriberBase *sub = new SubscriberROS(std::bind(fp, obj, std::placeholders::_1)); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, - (SubscriberROS *)sub); - ((SubscriberROS *)sub)->set_ros_sub(ros_sub); - _subs.push_back(sub); - return (Subscriber *)sub; - } + // template + // Subscriber *subscribe(const char *topic, void(T::*fp)(const M &), T *obj) + // { + // SubscriberBase *sub = new SubscriberROS(std::bind(fp, obj, std::placeholders::_1)); + // ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, + // (SubscriberROS *)sub); + // ((SubscriberROS *)sub)->set_ros_sub(ros_sub); + // _subs.push_back(sub); + // return (Subscriber *)sub; + // } /** * Subscribe with no callback, just the latest value is stored on updates * @param topic Name of the topic */ - template - Subscriber *subscribe(const char *topic) - { - SubscriberBase *sub = new SubscriberROS(); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, - (SubscriberROS *)sub); - ((SubscriberROS *)sub)->set_ros_sub(ros_sub); - _subs.push_back(sub); - return (Subscriber *)sub; - } + // template + // Subscriber *subscribe(const char *topic) + // { + // SubscriberBase *sub = new SubscriberROS(); + // ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, + // (SubscriberROS *)sub); + // ((SubscriberROS *)sub)->set_ros_sub(ros_sub); + // _subs.push_back(sub); + // return (Subscriber *)sub; + // } /** * Advertise topic * @param topic Name of the topic */ - template - Publisher *advertise(const char *topic) + // template + // Publisher *advertise(const char *topic) + // { + // ros::Publisher ros_pub = ros::NodeHandle::advertise(topic, kQueueSizeDefault); + // Publisher *pub = new Publisher(ros_pub); + // _pubs.push_back(pub); + // return pub; + // } + template + Publisher* advertise() + // Publisher *advertise() { - ros::Publisher ros_pub = ros::NodeHandle::advertise(topic, kQueueSizeDefault); + ros::Publisher ros_pub = ros::NodeHandle::advertisedata())>::type &>((new T())->handle(), kQueueSizeDefault); Publisher *pub = new Publisher(ros_pub); _pubs.push_back(pub); return pub; @@ -161,7 +181,7 @@ public: ~NodeHandle() { /* Empty subscriptions list */ - uORB::SubscriptionNode *sub = _subs.getHead(); + SubscriberNode *sub = _subs.getHead(); int count = 0; while (sub != nullptr) { @@ -170,13 +190,13 @@ public: break; } - uORB::SubscriptionNode *sib = sub->getSibling(); + SubscriberNode *sib = sub->getSibling(); delete sub; sub = sib; } /* Empty publications list */ - uORB::PublicationNode *pub = _pubs.getHead(); + Publisher *pub = _pubs.getHead(); count = 0; while (pub != nullptr) { @@ -185,7 +205,7 @@ public: break; } - uORB::PublicationNode *sib = pub->getSibling(); + Publisher *sib = pub->getSibling(); delete pub; pub = sib; } @@ -198,19 +218,41 @@ public: * @param interval Minimal interval between calls to callback */ - template - Subscriber *subscribe(const struct orb_metadata *meta, - std::function callback, - unsigned interval) + // template + // Subscriber *subscribe(const struct orb_metadata *meta, + // std::function callback, + // unsigned interval) + // { + // SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(meta, interval, callback, &_subs); + + // [> Check if this is the smallest interval so far and update _sub_min_interval <] + // if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { + // _sub_min_interval = sub_px4; + // } + + // return (Subscriber *)sub_px4; + // } + /** + * Subscribe with callback to function + * @param meta Describes the topic which nodehande should subscribe to + * @param callback Callback, executed on receiving a new message + * @param interval Minimal interval between calls to callback + */ + + template + Subscriber *subscribe(std::functiondata())>::type &)> callback, unsigned interval=10) //XXX interval { - SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(meta, interval, callback, &_subs); + const struct orb_metadata * meta = NULL; + uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(meta, interval); + SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, callback); /* Check if this is the smallest interval so far and update _sub_min_interval */ - if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { + if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { _sub_min_interval = sub_px4; } + _subs.add((SubscriberNode *)sub_px4); - return (Subscriber *)sub_px4; + return (Subscriber *)sub_px4; } /** @@ -219,29 +261,47 @@ public: * @param interval Minimal interval between data fetches from orb */ - template - Subscriber *subscribe(const struct orb_metadata *meta, - unsigned interval) - { - SubscriberUORB *sub_px4 = new SubscriberUORB(meta, interval, &_subs); + // template + // Subscriber *subscribe(const struct orb_metadata *meta, + // unsigned interval) + // { + // SubscriberUORB *sub_px4 = new SubscriberUORB(meta, interval, &_subs); - /* Check if this is the smallest interval so far and update _sub_min_interval */ - if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { - _sub_min_interval = sub_px4; - } + // [> Check if this is the smallest interval so far and update _sub_min_interval <] + // if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { + // _sub_min_interval = sub_px4; + // } - return (Subscriber *)sub_px4; - } + // return (Subscriber *)sub_px4; + // } /** * Advertise topic * @param meta Describes the topic which is advertised */ - template - Publisher *advertise(const struct orb_metadata *meta) + // template + // Publisher *advertise(const struct orb_metadata *meta) + // { + // //XXX + // Publisher *pub = new Publisher(meta, &_pubs); + // return pub; + // } + + /** + * Advertise topic + * @param meta Describes the topic which is advertised + */ + template + Publisher *advertise() { //XXX - Publisher *pub = new Publisher(meta, &_pubs); + // uORB::PublicationBase * uorb_pub = new uORB::PublicationBase((new T())->handle()); + const struct orb_metadata * meta = NULL; + uORB::PublicationBase * uorb_pub = new uORB::PublicationBase(meta); + Publisher *pub = new Publisher(uorb_pub); + + _pubs.add(pub); + return pub; } @@ -251,7 +311,7 @@ public: void spinOnce() { /* Loop through subscriptions, call callback for updated subscriptions */ - uORB::SubscriptionNode *sub = _subs.getHead(); + SubscriberNode *sub = _subs.getHead(); int count = 0; while (sub != nullptr) { @@ -281,7 +341,7 @@ public: /* Poll fd with smallest interval */ struct pollfd pfd; - pfd.fd = _sub_min_interval->getHandle(); + pfd.fd = _sub_min_interval->getUORBHandle(); pfd.events = POLLIN; poll(&pfd, 1, timeout_ms); spinOnce(); @@ -290,9 +350,9 @@ public: private: static const uint16_t kMaxSubscriptions = 100; static const uint16_t kMaxPublications = 100; - List _subs; /**< Subcriptions of node */ - List _pubs; /**< Publications of node */ - uORB::SubscriptionNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval + List _subs; /**< Subcriptions of node */ + List _pubs; /**< Publications of node */ + SubscriberNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval of all Subscriptions in _subs*/ }; #endif diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index c6f3d6108..aff045d23 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -46,6 +46,8 @@ #include #endif +#include + namespace px4 { @@ -58,11 +60,19 @@ public: PublisherBase() {}; ~PublisherBase() {}; + /** Publishes msg + * @param msg the message which is published to the topic + */ + // virtual int publish2(const PX4Message * const msg) = 0; + }; #if defined(__PX4_ROS) +// template class Publisher : public PublisherBase + // public PublisherBase, + // public T { public: /** @@ -71,6 +81,7 @@ public: */ Publisher(ros::Publisher ros_pub) : PublisherBase(), + // T(), _ros_pub(ros_pub) {} @@ -79,15 +90,20 @@ public: /** Publishes msg * @param msg the message which is published to the topic */ - template - int publish(const M &msg) { _ros_pub.publish(msg); return 0; } + // int publish(const M &msg) { _ros_pub.publish(msg); return 0; } + template + int publish(T &msg) { + _ros_pub.publish(msg.data()); + return 0;} private: ros::Publisher _ros_pub; /**< Handle to the ros publisher */ }; #else class Publisher : - public uORB::PublicationNode, - public PublisherBase + // public uORB::PublicationNode, + public PublisherBase, + public ListNode + { public: /** @@ -95,10 +111,14 @@ public: * @param meta orb metadata for the topic which is used * @param list publisher is added to this list */ - Publisher(const struct orb_metadata *meta, - List *list) : - uORB::PublicationNode(meta, list), - PublisherBase() + // Publisher(const struct orb_metadata *meta, + // List *list) : + // uORB::PublicationNode(meta, list), + // PublisherBase() + // {} + Publisher(uORB::PublicationBase * uorb_pub) : + PublisherBase(), + _uorb_pub(uorb_pub) {} ~Publisher() {}; @@ -109,7 +129,7 @@ public: template int publish(const M &msg) { - uORB::PublicationBase::update((void *)&msg); + _uorb_pub->update((void *)&msg); return 0; } @@ -117,6 +137,9 @@ public: * Empty callback for list traversal */ void update() {} ; +private: + uORB::PublicationBase * _uorb_pub; /**< Handle to the publisher */ + }; #endif } diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index aaacf9e48..ef03922ad 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -39,6 +39,7 @@ #pragma once #include +#include #if defined(__PX4_ROS) /* includes when building for ros */ @@ -67,7 +68,7 @@ public: /** * Subscriber class which is used by nodehandle */ -template +template class Subscriber : public SubscriberBase { @@ -81,7 +82,7 @@ public: /** * Get the last message value */ - virtual M get() = 0; + virtual T get() = 0; /** * Get void pointer to last message value @@ -93,9 +94,9 @@ public: /** * Subscriber class that is templated with the ros n message type */ -template +template class SubscriberROS : - public Subscriber + public Subscriber { friend class NodeHandle; @@ -103,8 +104,8 @@ public: /** * Construct Subscriber by providing a callback function */ - SubscriberROS(std::function cbf) : - Subscriber(), + SubscriberROS(std::functiondata())>::type &)> cbf) : + Subscriber(), _ros_sub(), _cbf(cbf), _msg_current() @@ -114,7 +115,7 @@ public: * Construct Subscriber without a callback function */ SubscriberROS() : - Subscriber(), + Subscriber(), _ros_sub(), _cbf(NULL), _msg_current() @@ -127,7 +128,7 @@ public: /** * Get the last message value */ - M get() { return _msg_current; } + T get() { return _msg_current; } /** * Get void pointer to last message value */ @@ -137,10 +138,10 @@ protected: /** * Called on topic update, saves the current message and then calls the provided callback function */ - void callback(const M &msg) + void callback(const typename std::remove_referencedata())>::type &msg) { /* Store data */ - _msg_current = msg; + _msg_current = (T)msg; /* Call callback */ if (_cbf != NULL) { @@ -158,20 +159,47 @@ protected: } ros::Subscriber _ros_sub; /**< Handle to ros subscriber */ - std::function _cbf; /**< Callback that the user provided on the subscription */ - M _msg_current; /**< Current Message value */ + std::functiondata())>::type &)> _cbf; /**< Callback that the user provided on the subscription */ + T _msg_current; /**< Current Message value */ }; #else // Building for NuttX + +/** + * Because we maintain a list of subscribers we need a node class + */ +class SubscriberNode : + public ListNode +{ +public: + SubscriberNode(unsigned interval) : + ListNode(), + _interval(interval) + {} + + virtual ~SubscriberNode() {} + + virtual void update() = 0; + + virtual int getUORBHandle() = 0; + + unsigned get_interval() { return _interval; } + +protected: + unsigned _interval; + +}; + + /** * Subscriber class that is templated with the uorb subscription message type */ -template +template class SubscriberUORB : - public Subscriber, - public uORB::Subscription + public Subscriber, + public SubscriberNode { public: @@ -181,11 +209,15 @@ public: * @param interval Minimal interval between calls to callback * @param list subscriber is added to this list */ - SubscriberUORB(const struct orb_metadata *meta, - unsigned interval, - List *list) : - Subscriber(), - uORB::Subscription(meta, interval, list) + // SubscriberUORB(const struct orb_metadata *meta, + // unsigned interval, + // List *list) : + // Subscriber(), + // uORB::Subscription(meta, interval, list) + // {} + SubscriberUORB(uORB::SubscriptionBase * uorb_sub, unsigned interval) : + SubscriberNode(interval), + _uorb_sub(uorb_sub) {} ~SubscriberUORB() {}; @@ -196,29 +228,36 @@ public: */ virtual void update() { - if (!uORB::Subscription::updated()) { + if (!_uorb_sub->updated()) { /* Topic not updated */ return; } /* get latest data */ - uORB::Subscription::update(); + _uorb_sub->update(get_void_ptr()); }; /* Accessors*/ /** * Get the last message value */ - M get() { return uORB::Subscription::getData(); } + T get() { return (T)(typename std::remove_referencedata())>::type)*_uorb_sub; } /** * Get void pointer to last message value */ - void *get_void_ptr() { return uORB::Subscription::getDataVoidPtr(); } + void *get_void_ptr() { return (void *)(typename std::remove_referencedata())>::type*)_uorb_sub; } + + int getUORBHandle() { return _uorb_sub->getHandle(); } + +protected: + uORB::SubscriptionBase * _uorb_sub; /**< Handle to the subscription */ + typename std::remove_referencedata())>::type getUORBData() { return (typename std::remove_referencedata())>::type)*_uorb_sub; } }; -template +//XXX reduce to one class with overloaded constructor? +template class SubscriberUORBCallback : - public SubscriberUORB + public SubscriberUORB { public: /** @@ -228,11 +267,16 @@ public: * @param interval Minimal interval between calls to callback * @param list subscriber is added to this list */ - SubscriberUORBCallback(const struct orb_metadata *meta, - unsigned interval, - std::function callback, - List *list) : - SubscriberUORB(meta, interval, list), + // SubscriberUORBCallback(const struct orb_metadata *meta, + // unsigned interval, + // std::function callback, + // List *list) : + // SubscriberUORB(meta, interval, list), + // _callback(callback) + // {} + SubscriberUORBCallback(uORB::SubscriptionBase * uorb_sub, + std::functiondata())>::type &)> callback) : + SubscriberUORB(uorb_sub), _callback(callback) {} @@ -245,13 +289,13 @@ public: */ virtual void update() { - if (!uORB::Subscription::updated()) { + if (!SubscriberUORB::_uorb_sub->updated()) { /* Topic not updated, do not call callback */ return; } /* get latest data */ - uORB::Subscription::update(); + SubscriberUORB::_uorb_sub->update(); /* Check if there is a callback */ @@ -260,12 +304,12 @@ public: } /* Call callback which performs actions based on this data */ - _callback(uORB::Subscription::getData()); + _callback(SubscriberUORB::getUORBData()); }; protected: - std::function _callback; /**< Callback handle, + std::functiondata())>::type &)> _callback; /**< Callback handle, called when new data is available */ }; #endif diff --git a/src/platforms/ros/px4_messages/px4_rc_channels.h b/src/platforms/ros/px4_messages/px4_rc_channels.h new file mode 100644 index 000000000..a5a5ee202 --- /dev/null +++ b/src/platforms/ros/px4_messages/px4_rc_channels.h @@ -0,0 +1,25 @@ +#include "px4/rc_channels.h" +#include "platforms/px4_message.h" + +#pragma once +namespace px4 +{ + +class px4_rc_channels : + public PX4Message +{ +public: + px4_rc_channels() : + PX4Message() + {} + + px4_rc_channels(rc_channels msg) : + PX4Message(msg) + {} + + ~px4_rc_channels() {} + + PX4TopicHandle handle() {return "rc_channels";} +}; + +} -- cgit v1.2.3 From cadcad6ffbdfbe9b92a5936f4d894138f912b4ff Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 20 Jan 2015 18:27:05 +0100 Subject: messagelayer prototype for nuttx --- makefiles/config_px4fmu-v2_test.mk | 2 +- msg/templates/msg.h.template | 7 +- src/examples/subscriber/subscriber_example.cpp | 19 +++-- src/examples/subscriber/subscriber_example.h | 2 +- src/platforms/nuttx/px4_messages/px4_rc_channels.h | 2 +- src/platforms/px4_message.h | 3 +- src/platforms/px4_nodehandle.h | 20 +++-- src/platforms/px4_publisher.h | 4 +- src/platforms/px4_subscriber.h | 93 ++++++++++------------ 9 files changed, 78 insertions(+), 74 deletions(-) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 4defe49e7..fa2c83262 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -56,7 +56,7 @@ MODULES += systemcmds/ver # Example modules # MODULES += examples/matlab_csv_serial -#MODULES += examples/subscriber +MODULES += examples/subscriber MODULES += examples/publisher # diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template index c27daed83..826418d11 100644 --- a/msg/templates/msg.h.template +++ b/msg/templates/msg.h.template @@ -165,12 +165,13 @@ def get_field_init(field): return '\n\t%s(%s),'%(field.name, init_value) } -@#ifdef __cplusplus +#ifdef __cplusplus @#class @(spec.short_name)_s { +struct __EXPORT @(spec.short_name)_s { @#public: -@#else +#else struct @(spec.short_name)_s { -@#endif +#endif @{ # loop over all fields and print the type and name for field in spec.parsed_fields(): diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 1c8f4c62b..c662a07ad 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -43,8 +43,10 @@ using namespace px4; -void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid); +// void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) { +void rc_channels_callback_function(const px4_rc_channels &msg); +void rc_channels_callback_function(const px4_rc_channels &msg) { + PX4_INFO("I heard: [%llu]", msg.data().timestamp_last_valid); } SubscriberExample::SubscriberExample() : @@ -63,7 +65,8 @@ SubscriberExample::SubscriberExample() : /* Do some subscriptions */ /* Function */ // PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval); - _n.subscribe(rc_channels_callback_function); + _n.subscribe(rc_channels_callback_function); //ROS version + // _n.subscribe(std::bind(&rc_channels_callback_function, std::placeholders::_1)); UORB version // [> Class Method <] // PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000); @@ -77,8 +80,8 @@ SubscriberExample::SubscriberExample() : * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. * Also the current value of the _sub_rc_chan subscription is printed */ -void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", - msg.timestamp_last_valid, - _sub_rc_chan->get().data().timestamp_last_valid); -} +// void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { + // PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", + // msg.timestamp_last_valid, + // _sub_rc_chan->get().data().timestamp_last_valid); +// } diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index b52ae991b..43dafe010 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -59,7 +59,7 @@ protected: // px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan; px4::Subscriber * _sub_rc_chan; - void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg); + // void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg); }; diff --git a/src/platforms/nuttx/px4_messages/px4_rc_channels.h b/src/platforms/nuttx/px4_messages/px4_rc_channels.h index bfca48469..cb1135eb9 100644 --- a/src/platforms/nuttx/px4_messages/px4_rc_channels.h +++ b/src/platforms/nuttx/px4_messages/px4_rc_channels.h @@ -6,7 +6,7 @@ namespace px4 { -class px4_rc_channels : +class __EXPORT px4_rc_channels : public PX4Message { public: diff --git a/src/platforms/px4_message.h b/src/platforms/px4_message.h index 05fcf1140..c908f6fc6 100644 --- a/src/platforms/px4_message.h +++ b/src/platforms/px4_message.h @@ -49,7 +49,7 @@ namespace px4 { template -class PX4Message +class __EXPORT PX4Message { // friend class NodeHandle; // #if defined(__PX4_ROS) @@ -69,6 +69,7 @@ public: virtual ~PX4Message() {}; virtual M& data() {return _data;} + virtual const M& data() const {return _data;} virtual PX4TopicHandle handle() = 0; private: M _data; diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index a40581239..2406a4a77 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -89,7 +89,8 @@ public: // return (Subscriber *)sub; // } template - Subscriber *subscribe(void(*fp)(const typename std::remove_referencedata())>::type &)) + // Subscriber *subscribe(void(*fp)(const typename std::remove_referencedata())>::type &)) + Subscriber *subscribe(void(*fp)(const T &)) { SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); ros::Subscriber ros_sub = ros::NodeHandle::subscribe((new T())->handle(), kQueueSizeDefault, @@ -240,17 +241,20 @@ public: */ template - Subscriber *subscribe(std::functiondata())>::type &)> callback, unsigned interval=10) //XXX interval + // Subscriber *subscribe(std::functiondata())>::type &)> callback, unsigned interval=10) //XXX interval + // Subscriber *subscribe(void(*fp)(const typename std::remove_referencedata())>::type &), unsigned interval=10) //XXX interval + Subscriber *subscribe(void(*fp)(const T &), unsigned interval=10) //XXX interval { const struct orb_metadata * meta = NULL; uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(meta, interval); - SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, callback); + // SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, callback); + SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, std::bind(fp, std::placeholders::_1)); - /* Check if this is the smallest interval so far and update _sub_min_interval */ - if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { - _sub_min_interval = sub_px4; - } - _subs.add((SubscriberNode *)sub_px4); + // [> Check if this is the smallest interval so far and update _sub_min_interval <] + // if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { + // _sub_min_interval = sub_px4; + // } + // _subs.add((SubscriberNode *)sub_px4); return (Subscriber *)sub_px4; } diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index aff045d23..6d75e28fc 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -54,7 +54,7 @@ namespace px4 /** * Untemplated publisher base class * */ -class PublisherBase +class __EXPORT PublisherBase { public: PublisherBase() {}; @@ -99,7 +99,7 @@ private: ros::Publisher _ros_pub; /**< Handle to the ros publisher */ }; #else -class Publisher : +class __EXPORT Publisher : // public uORB::PublicationNode, public PublisherBase, public ListNode diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index ef03922ad..88cc86ab8 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -57,11 +57,11 @@ namespace px4 /** * Untemplated subscriber base class * */ -class SubscriberBase +class __EXPORT SubscriberBase { public: SubscriberBase() {}; - ~SubscriberBase() {}; + virtual ~SubscriberBase() {}; }; @@ -69,25 +69,25 @@ public: * Subscriber class which is used by nodehandle */ template -class Subscriber : +class __EXPORT Subscriber : public SubscriberBase { public: Subscriber() : - SubscriberBase() + SubscriberBase(), + _msg_current() {}; - ~Subscriber() {}; + + virtual ~Subscriber() {} /* Accessors*/ /** * Get the last message value */ - virtual T get() = 0; + virtual T get() {return _msg_current;} - /** - * Get void pointer to last message value - */ - virtual void *get_void_ptr() = 0; +protected: + T _msg_current; /**< Current Message value */ }; #if defined(__PX4_ROS) @@ -104,11 +104,11 @@ public: /** * Construct Subscriber by providing a callback function */ - SubscriberROS(std::functiondata())>::type &)> cbf) : + // SubscriberROS(std::functiondata())>::type &)> cbf) : + SubscriberROS(std::function cbf) : Subscriber(), _ros_sub(), - _cbf(cbf), - _msg_current() + _cbf(cbf) {} /** @@ -117,35 +117,26 @@ public: SubscriberROS() : Subscriber(), _ros_sub(), - _cbf(NULL), - _msg_current() + _cbf(NULL) {} - ~SubscriberROS() {}; - - /* Accessors*/ - /** - * Get the last message value - */ - T get() { return _msg_current; } - /** - * Get void pointer to last message value - */ - void *get_void_ptr() { return (void *)&_msg_current; } + virtual ~SubscriberROS() {}; protected: /** * Called on topic update, saves the current message and then calls the provided callback function + * needs to use the native type as it is called by ROS */ void callback(const typename std::remove_referencedata())>::type &msg) { /* Store data */ - _msg_current = (T)msg; + this->_msg_current = (T)msg; /* Call callback */ if (_cbf != NULL) { - _cbf(msg); + // _cbf(_msg_current); + _cbf(this->get()); } } @@ -159,8 +150,7 @@ protected: } ros::Subscriber _ros_sub; /**< Handle to ros subscriber */ - std::functiondata())>::type &)> _cbf; /**< Callback that the user provided on the subscription */ - T _msg_current; /**< Current Message value */ + std::function _cbf; /**< Callback that the user provided on the subscription */ }; @@ -170,7 +160,7 @@ protected: /** * Because we maintain a list of subscribers we need a node class */ -class SubscriberNode : +class __EXPORT SubscriberNode : public ListNode { public: @@ -197,7 +187,7 @@ protected: * Subscriber class that is templated with the uorb subscription message type */ template -class SubscriberUORB : +class __EXPORT SubscriberUORB : public Subscriber, public SubscriberNode { @@ -220,7 +210,7 @@ public: _uorb_sub(uorb_sub) {} - ~SubscriberUORB() {}; + virtual ~SubscriberUORB() {}; /** * Update Subscription @@ -229,11 +219,10 @@ public: virtual void update() { if (!_uorb_sub->updated()) { - /* Topic not updated */ + /* Topic not updated, do not call callback */ return; } - /* get latest data */ _uorb_sub->update(get_void_ptr()); }; @@ -241,11 +230,16 @@ public: /** * Get the last message value */ - T get() { return (T)(typename std::remove_referencedata())>::type)*_uorb_sub; } + // T get() { return (T)(typename std::remove_referencedata())>::type)*_uorb_sub; } + // T get() { + // typename std::remove_referencedata())>::type msg = (typename std::remove_referencedata())>::type)*_uorb_sub; + // return (T)msg; + // } + /** * Get void pointer to last message value */ - void *get_void_ptr() { return (void *)(typename std::remove_referencedata())>::type*)_uorb_sub; } + void *get_void_ptr() { return (void *)&(this->_msg_current.data()); } int getUORBHandle() { return _uorb_sub->getHandle(); } @@ -256,7 +250,7 @@ protected: //XXX reduce to one class with overloaded constructor? template -class SubscriberUORBCallback : +class __EXPORT SubscriberUORBCallback : public SubscriberUORB { public: @@ -275,12 +269,13 @@ public: // _callback(callback) // {} SubscriberUORBCallback(uORB::SubscriptionBase * uorb_sub, - std::functiondata())>::type &)> callback) : - SubscriberUORB(uorb_sub), - _callback(callback) + unsigned interval, + std::function cbf) : + SubscriberUORB(uorb_sub, interval), + _cbf(cbf) {} - ~SubscriberUORBCallback() {}; + virtual ~SubscriberUORBCallback() {}; /** * Update Subscription @@ -289,28 +284,28 @@ public: */ virtual void update() { - if (!SubscriberUORB::_uorb_sub->updated()) { + if (!this->_uorb_sub->updated()) { /* Topic not updated, do not call callback */ return; - } /* get latest data */ - SubscriberUORB::_uorb_sub->update(); + this->_uorb_sub->update(this->get_void_ptr()); /* Check if there is a callback */ - if (_callback == nullptr) { + if (_cbf == nullptr) { return; } /* Call callback which performs actions based on this data */ - _callback(SubscriberUORB::getUORBData()); + _cbf(Subscriber::get()); + } }; protected: - std::functiondata())>::type &)> _callback; /**< Callback handle, - called when new data is available */ + // std::functiondata())>::type &)> _callback; [>*< Callback handle, called when new data is available */ + std::function _cbf; /**< Callback that the user provided on the subscription */ }; #endif -- cgit v1.2.3 From 632a0866ef16723dd6e1f0a2f7c575706b9e10cc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 14:34:05 +0100 Subject: remove unneeded functionality in template --- msg/templates/msg.h.template | 69 ++++++++------------------------------------ 1 file changed, 12 insertions(+), 57 deletions(-) diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template index 826418d11..cc128c1ea 100644 --- a/msg/templates/msg.h.template +++ b/msg/templates/msg.h.template @@ -95,17 +95,17 @@ for field in spec.parsed_fields(): @############################## @{ -type_map = {'int8': ['int8_t', '0'], - 'int16': ['int16_t', '0'], - 'int32': ['int32_t', '0'], - 'int64': ['int64_t', '0'], - 'uint8': ['uint8_t', '0'], - 'uint16': ['uint16_t', '0'], - 'uint32': ['uint32_t', '0'], - 'uint64': ['uint64_t', '0'], - 'float32': ['float', '0.0f'], - 'bool': ['bool', 'false'], - 'fence_vertex': ['fence_vertex', '']} +type_map = {'int8': 'int8_t', + 'int16': 'int16_t', + 'int32': 'int32_t', + 'int64': 'int64_t', + 'uint8': 'uint8_t', + 'uint16': 'uint16_t', + 'uint32': 'uint32_t', + 'uint64': 'uint64_t', + 'float32': 'float', + 'bool': 'bool', + 'fence_vertex': 'fence_vertex'} # Function to print a standard ros type def print_field_def(field): @@ -129,41 +129,12 @@ def print_field_def(field): if type in type_map: # need to add _t: int8 --> int8_t - type_px4 = type_map[type][0] + type_px4 = type_map[type] else: raise Exception("Type {0} not supported, add to to template file!".format(type)) print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size)) -# Function to init fields -def get_field_init(field): - type = field.type - # detect embedded types - sl_pos = type.find('/') - type_appendix = '' - type_prefix = '' - if (sl_pos >= 0): - type = type[sl_pos + 1:] - type_prefix = 'struct ' - type_appendix = '_s' - - # detect arrays - a_pos = type.find('[') - array_size = '' - if (a_pos >= 0): - # field is array - array_size = type[a_pos:] - type = type[:a_pos] - return '\n\t%s{},'%(field.name) - - if type in type_map: - # need to add _t: int8 --> int8_t - type_px4 = type_map[type][0] - init_value = type_map[type][1] - else: - raise Exception("Type {0} not supported, add to to template file!".format(type)) - - return '\n\t%s(%s),'%(field.name, init_value) } #ifdef __cplusplus @#class @(spec.short_name)_s { @@ -178,22 +149,6 @@ for field in spec.parsed_fields(): if (not field.is_header): print_field_def(field) }@ - -@##ifdef __cplusplus -@#@(spec.short_name)_s() : -@#@{ -@#field_init = '' -@## loop over all fields and init -@#for field in spec.parsed_fields(): -@# if (not field.is_header): -@# field_init += get_field_init(field) -@# -@#print(field_init[:-1]) -@#}@ -@#{} -@#virtual ~@(spec.short_name)_s() {} -@##endif - }; /** -- cgit v1.2.3 From 1f706eeb2fd210e7059e2b58f14432c987d76abf Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 14:34:34 +0100 Subject: small cleanup --- src/examples/publisher/publisher_example.h | 1 - src/examples/subscriber/subscriber_example.cpp | 3 +-- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h index 86586d0d3..304ecef47 100644 --- a/src/examples/publisher/publisher_example.h +++ b/src/examples/publisher/publisher_example.h @@ -48,6 +48,5 @@ public: int main(); protected: px4::NodeHandle _n; - // px4::Publisher * _rc_channels_pub; px4::Publisher * _rc_channels_pub; }; diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index c662a07ad..fd462cd2d 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -66,8 +66,7 @@ SubscriberExample::SubscriberExample() : /* Function */ // PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval); _n.subscribe(rc_channels_callback_function); //ROS version - // _n.subscribe(std::bind(&rc_channels_callback_function, std::placeholders::_1)); UORB version - + // [> Class Method <] // PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000); // [> No callback <] -- cgit v1.2.3 From 3a38b0fe359296aa19ec43ab82743aebeadb335c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 14:35:36 +0100 Subject: define __EXPORT for ROS --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0ac9912df..5488dbe9d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,6 +2,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) +add_definitions(-D__EXPORT=) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -- cgit v1.2.3 From 779400aad3f3a97057e4377d51ff362756938241 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 13:39:13 +0100 Subject: Add hackery on NuttX header, to be removed during rebase -i Conflicts: NuttX --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 574bac488..3d8171f6e 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 574bac488f384ddaa344378e25653c27124a2b69 +Subproject commit 3d8171f6ea88297d8595525c8222d61e9cf20fd0 -- cgit v1.2.3 From 1628999361f5bd939ffd6312cae1d9e67698f3d4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 15:44:15 +0100 Subject: Revert "Fixes to make GCC 4.9 link" This reverts commit 85b6907e1db1a6af88fe469e8e08dbd0a9d7a2a7. --- makefiles/toolchain_gnu-arm-eabi.mk | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index a8b4f1811..396980453 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -110,7 +110,9 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ -fno-strength-reduce \ -fomit-frame-pointer \ -funsafe-math-optimizations \ - -fno-builtin-printf + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections # enable precise stack overflow tracking # note - requires corresponding support in NuttX @@ -164,8 +166,7 @@ ARCHWARNINGSXX = $(ARCHWARNINGS) \ # pull in *just* libm from the toolchain ... this is grody LIBM := $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a) -LIBC := $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libc.a) -EXTRA_LIBS += $(LIBM) $(LIBC) +EXTRA_LIBS += $(LIBM) # Flags we pass to the C compiler # -- cgit v1.2.3 From 1511fd7b2dd19e0ae4c63553cbaf00e2a753148f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 16:33:19 +0100 Subject: make handle() static --- src/platforms/nuttx/px4_messages/px4_rc_channels.h | 2 +- src/platforms/px4_message.h | 3 +-- src/platforms/px4_nodehandle.h | 10 ++++------ src/platforms/ros/px4_messages/px4_rc_channels.h | 2 +- 4 files changed, 7 insertions(+), 10 deletions(-) diff --git a/src/platforms/nuttx/px4_messages/px4_rc_channels.h b/src/platforms/nuttx/px4_messages/px4_rc_channels.h index cb1135eb9..2ce7bfc80 100644 --- a/src/platforms/nuttx/px4_messages/px4_rc_channels.h +++ b/src/platforms/nuttx/px4_messages/px4_rc_channels.h @@ -20,7 +20,7 @@ public: ~px4_rc_channels() {} - PX4TopicHandle handle() {return (PX4TopicHandle)ORB_ID(rc_channels);} + static PX4TopicHandle handle() {return ORB_ID(rc_channels);} }; } diff --git a/src/platforms/px4_message.h b/src/platforms/px4_message.h index c908f6fc6..bff7aa313 100644 --- a/src/platforms/px4_message.h +++ b/src/platforms/px4_message.h @@ -42,7 +42,7 @@ typedef const char* PX4TopicHandle; #else #include -typedef const struct orb_metatdata* PX4TopicHandle; +typedef orb_id_t PX4TopicHandle; #endif namespace px4 @@ -70,7 +70,6 @@ public: virtual M& data() {return _data;} virtual const M& data() const {return _data;} - virtual PX4TopicHandle handle() = 0; private: M _data; }; diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 2406a4a77..5ae4f9325 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -93,7 +93,7 @@ public: Subscriber *subscribe(void(*fp)(const T &)) { SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe((new T())->handle(), kQueueSizeDefault, + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS *)sub); ((SubscriberROS *)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); @@ -147,7 +147,7 @@ public: Publisher* advertise() // Publisher *advertise() { - ros::Publisher ros_pub = ros::NodeHandle::advertisedata())>::type &>((new T())->handle(), kQueueSizeDefault); + ros::Publisher ros_pub = ros::NodeHandle::advertisedata())>::type &>(T::handle(), kQueueSizeDefault); Publisher *pub = new Publisher(ros_pub); _pubs.push_back(pub); return pub; @@ -245,8 +245,7 @@ public: // Subscriber *subscribe(void(*fp)(const typename std::remove_referencedata())>::type &), unsigned interval=10) //XXX interval Subscriber *subscribe(void(*fp)(const T &), unsigned interval=10) //XXX interval { - const struct orb_metadata * meta = NULL; - uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(meta, interval); + uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval); // SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, callback); SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, std::bind(fp, std::placeholders::_1)); @@ -300,8 +299,7 @@ public: { //XXX // uORB::PublicationBase * uorb_pub = new uORB::PublicationBase((new T())->handle()); - const struct orb_metadata * meta = NULL; - uORB::PublicationBase * uorb_pub = new uORB::PublicationBase(meta); + uORB::PublicationBase * uorb_pub = new uORB::PublicationBase(T::handle()); Publisher *pub = new Publisher(uorb_pub); _pubs.add(pub); diff --git a/src/platforms/ros/px4_messages/px4_rc_channels.h b/src/platforms/ros/px4_messages/px4_rc_channels.h index a5a5ee202..5e9dc72cd 100644 --- a/src/platforms/ros/px4_messages/px4_rc_channels.h +++ b/src/platforms/ros/px4_messages/px4_rc_channels.h @@ -19,7 +19,7 @@ public: ~px4_rc_channels() {} - PX4TopicHandle handle() {return "rc_channels";} + static PX4TopicHandle handle() {return "rc_channels";} }; } -- cgit v1.2.3 From 2a2594a1717a5067ac1f209974851f9512e46415 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 17:40:42 +0100 Subject: re-enable adding of subscriber to list --- src/platforms/px4_nodehandle.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 5ae4f9325..4a1e70075 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -249,11 +249,11 @@ public: // SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, callback); SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, std::bind(fp, std::placeholders::_1)); - // [> Check if this is the smallest interval so far and update _sub_min_interval <] - // if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { - // _sub_min_interval = sub_px4; - // } - // _subs.add((SubscriberNode *)sub_px4); + /* Check if this is the smallest interval so far and update _sub_min_interval */ + if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { + _sub_min_interval = sub_px4; + } + _subs.add((SubscriberNode *)sub_px4); return (Subscriber *)sub_px4; } -- cgit v1.2.3 From 02fdd48a477853a2f54c7954478a3ce5b5b3f497 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 17:41:13 +0100 Subject: publisher: use wrapper message type --- src/platforms/px4_publisher.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index 6d75e28fc..0c8dd62ba 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -126,10 +126,10 @@ public: /** Publishes msg * @param msg the message which is published to the topic */ - template - int publish(const M &msg) + template + int publish(const T &msg) { - _uorb_pub->update((void *)&msg); + _uorb_pub->update((void *)&(msg.data())); return 0; } -- cgit v1.2.3 From b04fcad525c8f76419d468f43a4b159bc5200fbe Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 17:41:56 +0100 Subject: fix bracket position --- src/platforms/px4_subscriber.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 88cc86ab8..8e8b786b1 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -287,6 +287,7 @@ public: if (!this->_uorb_sub->updated()) { /* Topic not updated, do not call callback */ return; + } /* get latest data */ this->_uorb_sub->update(this->get_void_ptr()); @@ -299,7 +300,6 @@ public: /* Call callback which performs actions based on this data */ _cbf(Subscriber::get()); - } }; -- cgit v1.2.3 From ed526173bb4f6a809e725aeb8214cf24042edd76 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 17:57:37 +0100 Subject: clean up publisher example --- src/examples/publisher/publisher_example.cpp | 19 ++++--------------- 1 file changed, 4 insertions(+), 15 deletions(-) diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp index e85e42b38..6b30c2fe3 100644 --- a/src/examples/publisher/publisher_example.cpp +++ b/src/examples/publisher/publisher_example.cpp @@ -46,9 +46,7 @@ using namespace px4; PublisherExample::PublisherExample() : _n(), _rc_channels_pub(_n.advertise()) - // _rc_channels_pub(PX4_ADVERTISE(_n, rc_channels)) { - } int PublisherExample::main() @@ -56,19 +54,10 @@ int PublisherExample::main() px4::Rate loop_rate(10); while (px4::ok()) { - // PX4_TOPIC_T(rc_channels) msg; - // msg.timestamp_last_valid = px4::get_time_micros(); - // PX4_INFO("%llu", msg.timestamp_last_valid); - - // _rc_channels_pub->publish(msg); - - //XXX - px4_rc_channels msg2; - msg2.data().timestamp_last_valid = px4::get_time_micros(); - PX4_INFO("%llu", msg2.data().timestamp_last_valid); - // msg2.pub->publish2(); - _rc_channels_pub->publish(msg2); - + px4_rc_channels msg; + msg.data().timestamp_last_valid = px4::get_time_micros(); + PX4_INFO("%llu", msg.data().timestamp_last_valid); + _rc_channels_pub->publish(msg); _n.spinOnce(); loop_rate.sleep(); -- cgit v1.2.3 From f60e65b38fd051595b465f619a19effa9c13efee Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 18:05:31 +0100 Subject: clean up subscriber example --- src/examples/subscriber/subscriber_example.cpp | 5 +---- src/examples/subscriber/subscriber_example.h | 3 +-- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index fd462cd2d..18f1a9eb9 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -43,8 +43,6 @@ using namespace px4; -// void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) { -void rc_channels_callback_function(const px4_rc_channels &msg); void rc_channels_callback_function(const px4_rc_channels &msg) { PX4_INFO("I heard: [%llu]", msg.data().timestamp_last_valid); } @@ -64,9 +62,8 @@ SubscriberExample::SubscriberExample() : /* Do some subscriptions */ /* Function */ - // PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval); _n.subscribe(rc_channels_callback_function); //ROS version - + // [> Class Method <] // PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000); // [> No callback <] diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index 43dafe010..883d83be7 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -41,7 +41,7 @@ using namespace px4; -void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg); +void rc_channels_callback_function(const px4_rc_channels &msg); class SubscriberExample { public: @@ -56,7 +56,6 @@ protected: int32_t _interval; px4_param_t _p_test_float; float _test_float; - // px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan; px4::Subscriber * _sub_rc_chan; // void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg); -- cgit v1.2.3 From a761df4ffa0e77e1bd34a6e02ba621ca71523389 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 18:14:20 +0100 Subject: clean up px4_publisher --- src/platforms/px4_publisher.h | 19 ------------------- 1 file changed, 19 deletions(-) diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index 0c8dd62ba..911554503 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -59,20 +59,11 @@ class __EXPORT PublisherBase public: PublisherBase() {}; ~PublisherBase() {}; - - /** Publishes msg - * @param msg the message which is published to the topic - */ - // virtual int publish2(const PX4Message * const msg) = 0; - }; #if defined(__PX4_ROS) -// template class Publisher : public PublisherBase - // public PublisherBase, - // public T { public: /** @@ -81,7 +72,6 @@ public: */ Publisher(ros::Publisher ros_pub) : PublisherBase(), - // T(), _ros_pub(ros_pub) {} @@ -90,7 +80,6 @@ public: /** Publishes msg * @param msg the message which is published to the topic */ - // int publish(const M &msg) { _ros_pub.publish(msg); return 0; } template int publish(T &msg) { _ros_pub.publish(msg.data()); @@ -100,7 +89,6 @@ private: }; #else class __EXPORT Publisher : - // public uORB::PublicationNode, public PublisherBase, public ListNode @@ -108,14 +96,7 @@ class __EXPORT Publisher : public: /** * Construct Publisher by providing orb meta data - * @param meta orb metadata for the topic which is used - * @param list publisher is added to this list */ - // Publisher(const struct orb_metadata *meta, - // List *list) : - // uORB::PublicationNode(meta, list), - // PublisherBase() - // {} Publisher(uORB::PublicationBase * uorb_pub) : PublisherBase(), _uorb_pub(uorb_pub) -- cgit v1.2.3 From 358c91932575c191c7717d9c083611d651721476 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 18:23:18 +0100 Subject: clean up px4_subscriber --- src/platforms/px4_subscriber.h | 43 ++++++++++-------------------------------- 1 file changed, 10 insertions(+), 33 deletions(-) diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 8e8b786b1..2b289771b 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -104,7 +104,6 @@ public: /** * Construct Subscriber by providing a callback function */ - // SubscriberROS(std::functiondata())>::type &)> cbf) : SubscriberROS(std::function cbf) : Subscriber(), _ros_sub(), @@ -195,16 +194,8 @@ public: /** * Construct SubscriberUORB by providing orb meta data without callback - * @param meta orb metadata for the topic which is used * @param interval Minimal interval between calls to callback - * @param list subscriber is added to this list */ - // SubscriberUORB(const struct orb_metadata *meta, - // unsigned interval, - // List *list) : - // Subscriber(), - // uORB::Subscription(meta, interval, list) - // {} SubscriberUORB(uORB::SubscriptionBase * uorb_sub, unsigned interval) : SubscriberNode(interval), _uorb_sub(uorb_sub) @@ -227,25 +218,21 @@ public: }; /* Accessors*/ - /** - * Get the last message value - */ - // T get() { return (T)(typename std::remove_referencedata())>::type)*_uorb_sub; } - // T get() { - // typename std::remove_referencedata())>::type msg = (typename std::remove_referencedata())>::type)*_uorb_sub; - // return (T)msg; - // } + int getUORBHandle() { return _uorb_sub->getHandle(); } + +protected: + uORB::SubscriptionBase * _uorb_sub; /**< Handle to the subscription */ + + typename std::remove_referencedata())>::type getUORBData() + { + return (typename std::remove_referencedata())>::type)*_uorb_sub; + } /** * Get void pointer to last message value */ void *get_void_ptr() { return (void *)&(this->_msg_current.data()); } - int getUORBHandle() { return _uorb_sub->getHandle(); } - -protected: - uORB::SubscriptionBase * _uorb_sub; /**< Handle to the subscription */ - typename std::remove_referencedata())>::type getUORBData() { return (typename std::remove_referencedata())>::type)*_uorb_sub; } }; //XXX reduce to one class with overloaded constructor? @@ -256,18 +243,9 @@ class __EXPORT SubscriberUORBCallback : public: /** * Construct SubscriberUORBCallback by providing orb meta data - * @param meta orb metadata for the topic which is used - * @param callback Callback, executed on receiving a new message + * @param cbf Callback, executed on receiving a new message * @param interval Minimal interval between calls to callback - * @param list subscriber is added to this list */ - // SubscriberUORBCallback(const struct orb_metadata *meta, - // unsigned interval, - // std::function callback, - // List *list) : - // SubscriberUORB(meta, interval, list), - // _callback(callback) - // {} SubscriberUORBCallback(uORB::SubscriptionBase * uorb_sub, unsigned interval, std::function cbf) : @@ -304,7 +282,6 @@ public: }; protected: - // std::functiondata())>::type &)> _callback; [>*< Callback handle, called when new data is available */ std::function _cbf; /**< Callback that the user provided on the subscription */ }; #endif -- cgit v1.2.3 From 2b103d319c8547d0d6e9ae14a6413a787051e205 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 18:29:22 +0100 Subject: clean up px4_nodehandle --- src/platforms/px4_nodehandle.h | 61 +----------------------------------------- 1 file changed, 1 insertion(+), 60 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 4a1e70075..634e5e5db 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -78,18 +78,7 @@ public: * @param topic Name of the topic * @param fb Callback, executed on receiving a new message */ - // template - // Subscriber *subscribe(const char *topic, void(*fp)(const M &)) - // { - // SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); - // ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, - // (SubscriberROS *)sub); - // ((SubscriberROS *)sub)->set_ros_sub(ros_sub); - // _subs.push_back(sub); - // return (Subscriber *)sub; - // } template - // Subscriber *subscribe(void(*fp)(const typename std::remove_referencedata())>::type &)) Subscriber *subscribe(void(*fp)(const T &)) { SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); @@ -133,19 +122,9 @@ public: /** * Advertise topic - * @param topic Name of the topic */ - // template - // Publisher *advertise(const char *topic) - // { - // ros::Publisher ros_pub = ros::NodeHandle::advertise(topic, kQueueSizeDefault); - // Publisher *pub = new Publisher(ros_pub); - // _pubs.push_back(pub); - // return pub; - // } template Publisher* advertise() - // Publisher *advertise() { ros::Publisher ros_pub = ros::NodeHandle::advertisedata())>::type &>(T::handle(), kQueueSizeDefault); Publisher *pub = new Publisher(ros_pub); @@ -214,39 +193,14 @@ public: /** * Subscribe with callback to function - * @param meta Describes the topic which nodehande should subscribe to - * @param callback Callback, executed on receiving a new message - * @param interval Minimal interval between calls to callback - */ - - // template - // Subscriber *subscribe(const struct orb_metadata *meta, - // std::function callback, - // unsigned interval) - // { - // SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(meta, interval, callback, &_subs); - - // [> Check if this is the smallest interval so far and update _sub_min_interval <] - // if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { - // _sub_min_interval = sub_px4; - // } - - // return (Subscriber *)sub_px4; - // } - /** - * Subscribe with callback to function - * @param meta Describes the topic which nodehande should subscribe to - * @param callback Callback, executed on receiving a new message + * @param fp Callback, executed on receiving a new message * @param interval Minimal interval between calls to callback */ template - // Subscriber *subscribe(std::functiondata())>::type &)> callback, unsigned interval=10) //XXX interval - // Subscriber *subscribe(void(*fp)(const typename std::remove_referencedata())>::type &), unsigned interval=10) //XXX interval Subscriber *subscribe(void(*fp)(const T &), unsigned interval=10) //XXX interval { uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval); - // SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, callback); SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, std::bind(fp, std::placeholders::_1)); /* Check if this is the smallest interval so far and update _sub_min_interval */ @@ -280,19 +234,6 @@ public: /** * Advertise topic - * @param meta Describes the topic which is advertised - */ - // template - // Publisher *advertise(const struct orb_metadata *meta) - // { - // //XXX - // Publisher *pub = new Publisher(meta, &_pubs); - // return pub; - // } - - /** - * Advertise topic - * @param meta Describes the topic which is advertised */ template Publisher *advertise() -- cgit v1.2.3 From 2af44f5995dd121a7ce2aefd3ab1c7d8dcf3fb8d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 22 Jan 2015 08:10:08 +0100 Subject: multiplatform: introduce PublisherNode class for uorb for more consistency --- src/platforms/px4_nodehandle.h | 6 +++--- src/platforms/px4_publisher.h | 18 +++++++++++++++++- src/platforms/px4_subscriber.h | 3 --- 3 files changed, 20 insertions(+), 7 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 634e5e5db..8fafa168a 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -176,7 +176,7 @@ public: } /* Empty publications list */ - Publisher *pub = _pubs.getHead(); + PublisherNode *pub = _pubs.getHead(); count = 0; while (pub != nullptr) { @@ -185,7 +185,7 @@ public: break; } - Publisher *sib = pub->getSibling(); + PublisherNode *sib = pub->getSibling(); delete pub; pub = sib; } @@ -294,7 +294,7 @@ private: static const uint16_t kMaxSubscriptions = 100; static const uint16_t kMaxPublications = 100; List _subs; /**< Subcriptions of node */ - List _pubs; /**< Publications of node */ + List _pubs; /**< Publications of node */ SubscriberNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval of all Subscriptions in _subs*/ }; diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index 911554503..7195777f5 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -88,9 +88,25 @@ private: ros::Publisher _ros_pub; /**< Handle to the ros publisher */ }; #else +/** + * Because we maintain a list of publishers we need a node class + */ +class __EXPORT PublisherNode : + public ListNode +{ +public: + PublisherNode() : + ListNode() + {} + + virtual ~PublisherNode() {} + + virtual void update() = 0; +}; + class __EXPORT Publisher : public PublisherBase, - public ListNode + public PublisherNode { public: diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 2b289771b..d03b3edee 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -154,8 +154,6 @@ protected: }; #else // Building for NuttX - - /** * Because we maintain a list of subscribers we need a node class */ @@ -181,7 +179,6 @@ protected: }; - /** * Subscriber class that is templated with the uorb subscription message type */ -- cgit v1.2.3 From 8c4fce3654bbf4cb31314f1fb596f4fd17772589 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 22 Jan 2015 09:30:43 +0100 Subject: multiplatform: better publisher base class --- makefiles/toolchain_gnu-arm-eabi.mk | 40 +++++++++++++-------------- src/examples/publisher/publisher_example.h | 3 +- src/platforms/px4_nodehandle.h | 14 +++++----- src/platforms/px4_publisher.h | 44 ++++++++++++++++++++---------- 4 files changed, 59 insertions(+), 42 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 396980453..d38ef645f 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -125,26 +125,26 @@ ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics # Generic warnings # -ARCHWARNINGS = -Wall \ - -Wextra \ - -Werror \ - -Wdouble-promotion \ - -Wshadow \ - -Wfloat-equal \ - -Wframe-larger-than=1024 \ - -Wpointer-arith \ - -Wlogical-op \ - -Wmissing-declarations \ - -Wpacked \ - -Wno-unused-parameter \ - -Werror=format-security \ - -Werror=array-bounds \ - -Wfatal-errors \ - -Wformat=1 \ - -Werror=unused-but-set-variable \ - -Werror=unused-variable \ - -Werror=double-promotion \ - -Werror=reorder +# ARCHWARNINGS = -Wall \ + # -Wextra \ + # -Werror \ + # -Wdouble-promotion \ + # -Wshadow \ + # -Wfloat-equal \ + # -Wframe-larger-than=1024 \ + # -Wpointer-arith \ + # -Wlogical-op \ + # -Wmissing-declarations \ + # -Wpacked \ + # -Wno-unused-parameter \ + # -Werror=format-security \ + # -Werror=array-bounds \ + # -Wfatal-errors \ + # -Wformat=1 \ + # -Werror=unused-but-set-variable \ + # -Werror=unused-variable \ + # -Werror=double-promotion \ + # -Werror=reorder # -Wcast-qual - generates spurious noreturn attribute warnings, try again later # -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code # -Wcast-align - would help catch bad casts in some cases, but generates too many false positives diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h index 304ecef47..4ff59a226 100644 --- a/src/examples/publisher/publisher_example.h +++ b/src/examples/publisher/publisher_example.h @@ -37,6 +37,7 @@ * * @author Thomas Gubler */ +#pragma once #include class PublisherExample { @@ -48,5 +49,5 @@ public: int main(); protected: px4::NodeHandle _n; - px4::Publisher * _rc_channels_pub; + px4::Publisher * _rc_channels_pub; }; diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 8fafa168a..a25d57845 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -124,12 +124,12 @@ public: * Advertise topic */ template - Publisher* advertise() + Publisher* advertise() { ros::Publisher ros_pub = ros::NodeHandle::advertisedata())>::type &>(T::handle(), kQueueSizeDefault); - Publisher *pub = new Publisher(ros_pub); - _pubs.push_back(pub); - return pub; + PublisherROS *pub = new PublisherROS(ros_pub); + _pubs.push_back((PublisherBase*)pub); + return (Publisher*)pub; } /** @@ -236,16 +236,16 @@ public: * Advertise topic */ template - Publisher *advertise() + Publisher *advertise() { //XXX // uORB::PublicationBase * uorb_pub = new uORB::PublicationBase((new T())->handle()); uORB::PublicationBase * uorb_pub = new uORB::PublicationBase(T::handle()); - Publisher *pub = new Publisher(uorb_pub); + PublisherUORB *pub = new PublisherUORB(uorb_pub); _pubs.add(pub); - return pub; + return (Publisher*)pub; } /** diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index 7195777f5..afedbbee7 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -61,29 +61,44 @@ public: ~PublisherBase() {}; }; +/** + * Publisher base class, templated with the message type + * */ +template +class __EXPORT Publisher +{ +public: + Publisher() {}; + ~Publisher() {}; + + virtual int publish(const T &msg) = 0; +}; + #if defined(__PX4_ROS) -class Publisher : - public PublisherBase +template +class PublisherROS : + public Publisher { public: /** * Construct Publisher by providing a ros::Publisher * @param ros_pub the ros publisher which will be used to perform the publications */ - Publisher(ros::Publisher ros_pub) : - PublisherBase(), + PublisherROS(ros::Publisher ros_pub) : + Publisher(), _ros_pub(ros_pub) {} - ~Publisher() {}; + ~PublisherROS() {}; /** Publishes msg * @param msg the message which is published to the topic */ - template - int publish(T &msg) { + int publish(const T &msg) + { _ros_pub.publish(msg.data()); - return 0;} + return 0; + } private: ros::Publisher _ros_pub; /**< Handle to the ros publisher */ }; @@ -104,8 +119,9 @@ public: virtual void update() = 0; }; -class __EXPORT Publisher : - public PublisherBase, +template +class __EXPORT PublisherUORB : + public Publisher, public PublisherNode { @@ -113,17 +129,17 @@ public: /** * Construct Publisher by providing orb meta data */ - Publisher(uORB::PublicationBase * uorb_pub) : - PublisherBase(), + PublisherUORB(uORB::PublicationBase * uorb_pub) : + Publisher(), + PublisherNode(), _uorb_pub(uorb_pub) {} - ~Publisher() {}; + ~PublisherUORB() {}; /** Publishes msg * @param msg the message which is published to the topic */ - template int publish(const T &msg) { _uorb_pub->update((void *)&(msg.data())); -- cgit v1.2.3 From 67b465d800d03c6e85504b7b3f2fcc5a29497352 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 22 Jan 2015 12:31:07 +0100 Subject: add iris house world launch file --- launch/gazebo_iris_house_world.launch | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100644 launch/gazebo_iris_house_world.launch diff --git a/launch/gazebo_iris_house_world.launch b/launch/gazebo_iris_house_world.launch new file mode 100644 index 000000000..bfdf9fe31 --- /dev/null +++ b/launch/gazebo_iris_house_world.launch @@ -0,0 +1,6 @@ + + + + + + -- cgit v1.2.3 From 8e15a5b9d0400707f539703d01f702bee03a10db Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 22 Jan 2015 15:07:06 +0100 Subject: install script: add missing joy dependency --- Tools/ros/px4_ros_installation_ubuntu.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/ros/px4_ros_installation_ubuntu.sh b/Tools/ros/px4_ros_installation_ubuntu.sh index 72b4f9468..3b8b29381 100755 --- a/Tools/ros/px4_ros_installation_ubuntu.sh +++ b/Tools/ros/px4_ros_installation_ubuntu.sh @@ -26,7 +26,7 @@ echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc sudo apt-get -y install python-rosinstall # additional dependencies -sudo apt-get -y install ros-indigo-octomap-msgs +sudo apt-get -y install ros-indigo-octomap-msgs ros-indigo-joy ## drcsim setup (for models) ### add osrf repository -- cgit v1.2.3 From 49d41773fc990a6b878543fac2c5cda328bf7d78 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 22 Jan 2015 16:47:28 +0100 Subject: ros wrapper port callback to methods and subscriber without callback --- src/examples/subscriber/subscriber_example.cpp | 20 ++++++------ src/examples/subscriber/subscriber_example.h | 2 +- src/platforms/px4_nodehandle.h | 43 +++++++++++++------------- src/platforms/px4_subscriber.h | 3 +- 4 files changed, 34 insertions(+), 34 deletions(-) diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 18f1a9eb9..217e4d48f 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -64,10 +64,11 @@ SubscriberExample::SubscriberExample() : /* Function */ _n.subscribe(rc_channels_callback_function); //ROS version - // [> Class Method <] - // PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000); - // [> No callback <] - // _sub_rc_chan = PX4_SUBSCRIBE(_n, rc_channels, 500); + /* No callback */ + _sub_rc_chan = _n.subscribe(); + + /* Class Method */ + _n.subscribe(&SubscriberExample::rc_channels_callback, this); PX4_INFO("subscribed"); } @@ -76,8 +77,9 @@ SubscriberExample::SubscriberExample() : * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. * Also the current value of the _sub_rc_chan subscription is printed */ -// void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { - // PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", - // msg.timestamp_last_valid, - // _sub_rc_chan->get().data().timestamp_last_valid); -// } +void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) { + PX4_INFO("Callback (method): [%llu]", + msg.data().timestamp_last_valid); + PX4_INFO("Callback (method): value of _sub_rc_chan: [%llu]", + _sub_rc_chan->get().data().timestamp_last_valid); +} diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index 883d83be7..8da3df438 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -58,7 +58,7 @@ protected: float _test_float; px4::Subscriber * _sub_rc_chan; - // void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg); + void rc_channels_callback(const px4_rc_channels &msg); }; diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index a25d57845..e619623b9 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -91,34 +91,33 @@ public: /** * Subscribe with callback to class method - * @param topic Name of the topic * @param fb Callback, executed on receiving a new message + * @param obj pointer class instance */ - // template - // Subscriber *subscribe(const char *topic, void(T::*fp)(const M &), T *obj) - // { - // SubscriberBase *sub = new SubscriberROS(std::bind(fp, obj, std::placeholders::_1)); - // ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, - // (SubscriberROS *)sub); - // ((SubscriberROS *)sub)->set_ros_sub(ros_sub); - // _subs.push_back(sub); - // return (Subscriber *)sub; - // } + template + Subscriber *subscribe(void(C::*fp)(const T &), C *obj) + { + SubscriberBase *sub = new SubscriberROS(std::bind(fp, obj, std::placeholders::_1)); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault, + &SubscriberROS::callback, (SubscriberROS *)sub); + ((SubscriberROS *)sub)->set_ros_sub(ros_sub); + _subs.push_back(sub); + return (Subscriber *)sub; + } /** * Subscribe with no callback, just the latest value is stored on updates - * @param topic Name of the topic */ - // template - // Subscriber *subscribe(const char *topic) - // { - // SubscriberBase *sub = new SubscriberROS(); - // ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, - // (SubscriberROS *)sub); - // ((SubscriberROS *)sub)->set_ros_sub(ros_sub); - // _subs.push_back(sub); - // return (Subscriber *)sub; - // } + template + Subscriber *subscribe() + { + SubscriberBase *sub = new SubscriberROS(); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault, + &SubscriberROS::callback, (SubscriberROS *)sub); + ((SubscriberROS *)sub)->set_ros_sub(ros_sub); + _subs.push_back(sub); + return (Subscriber *)sub; + } /** * Advertise topic diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index d03b3edee..c499712a9 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -130,11 +130,10 @@ protected: void callback(const typename std::remove_referencedata())>::type &msg) { /* Store data */ - this->_msg_current = (T)msg; + this->_msg_current.data() = msg; /* Call callback */ if (_cbf != NULL) { - // _cbf(_msg_current); _cbf(this->get()); } -- cgit v1.2.3 From b2a911b88d6a541218ef6e633be0d6693de31c8e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 22 Jan 2015 17:10:20 +0100 Subject: uorb wrapper port callback to methods and subscriber without callback --- src/platforms/px4_nodehandle.h | 53 +++++++++++++++++++++++++++++------------- 1 file changed, 37 insertions(+), 16 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index e619623b9..d78c865de 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -81,8 +81,7 @@ public: template Subscriber *subscribe(void(*fp)(const T &)) { - SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault, + SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS *)sub); ((SubscriberROS *)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); @@ -99,7 +98,8 @@ public: { SubscriberBase *sub = new SubscriberROS(std::bind(fp, obj, std::placeholders::_1)); ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault, - &SubscriberROS::callback, (SubscriberROS *)sub); + &SubscriberROS::callback, (SubscriberROS *)sub);//XXX needs cleanup in destructor ore move into class + ((SubscriberROS *)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); return (Subscriber *)sub; @@ -199,7 +199,7 @@ public: template Subscriber *subscribe(void(*fp)(const T &), unsigned interval=10) //XXX interval { - uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval); + uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval);//XXX needs cleanup in destructor ore move into class SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, std::bind(fp, std::placeholders::_1)); /* Check if this is the smallest interval so far and update _sub_min_interval */ @@ -211,25 +211,46 @@ public: return (Subscriber *)sub_px4; } + /** + * Subscribe with callback to class method + * @param fb Callback, executed on receiving a new message + * @param obj pointer class instance + */ + template + Subscriber *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval=10) + { + uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval);//XXX needs cleanup in destructor ore move into class + SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, std::bind(fp, obj, std::placeholders::_1)); + + /* Check if this is the smallest interval so far and update _sub_min_interval */ + if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { + _sub_min_interval = sub_px4; + } + _subs.add((SubscriberNode *)sub_px4); + + return (Subscriber *)sub_px4; + } + /** * Subscribe without callback to function - * @param meta Describes the topic which nodehande should subscribe to * @param interval Minimal interval between data fetches from orb */ - // template - // Subscriber *subscribe(const struct orb_metadata *meta, - // unsigned interval) - // { - // SubscriberUORB *sub_px4 = new SubscriberUORB(meta, interval, &_subs); + template + Subscriber *subscribe(unsigned interval=10) //XXX interval + { + uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval);//XXX needs cleanup in destructor ore move into class - // [> Check if this is the smallest interval so far and update _sub_min_interval <] - // if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { - // _sub_min_interval = sub_px4; - // } + SubscriberUORB *sub_px4 = new SubscriberUORB(uorb_sub, interval); - // return (Subscriber *)sub_px4; - // } + /* Check if this is the smallest interval so far and update _sub_min_interval */ + if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { + _sub_min_interval = sub_px4; + } + _subs.add((SubscriberNode *)sub_px4); + + return (Subscriber *)sub_px4; + } /** * Advertise topic -- cgit v1.2.3 From 1e504478a00f08c4d7ab381aa9bec5cdbee5513f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 07:06:02 +0100 Subject: move ros::subscriber into constructor of subscriber --- src/platforms/px4_nodehandle.h | 15 +++------------ src/platforms/px4_subscriber.h | 29 ++++++++++------------------- 2 files changed, 13 insertions(+), 31 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index d78c865de..40604aa86 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -81,9 +81,7 @@ public: template Subscriber *subscribe(void(*fp)(const T &)) { - SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault, - &SubscriberROS::callback, (SubscriberROS *)sub); - ((SubscriberROS *)sub)->set_ros_sub(ros_sub); + SubscriberBase *sub = new SubscriberROS((ros::NodeHandle*)this, std::bind(fp, std::placeholders::_1)); _subs.push_back(sub); return (Subscriber *)sub; } @@ -96,11 +94,7 @@ public: template Subscriber *subscribe(void(C::*fp)(const T &), C *obj) { - SubscriberBase *sub = new SubscriberROS(std::bind(fp, obj, std::placeholders::_1)); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault, - &SubscriberROS::callback, (SubscriberROS *)sub);//XXX needs cleanup in destructor ore move into class - - ((SubscriberROS *)sub)->set_ros_sub(ros_sub); + SubscriberBase *sub = new SubscriberROS((ros::NodeHandle*)this, std::bind(fp, obj, std::placeholders::_1)); _subs.push_back(sub); return (Subscriber *)sub; } @@ -111,10 +105,7 @@ public: template Subscriber *subscribe() { - SubscriberBase *sub = new SubscriberROS(); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault, - &SubscriberROS::callback, (SubscriberROS *)sub); - ((SubscriberROS *)sub)->set_ros_sub(ros_sub); + SubscriberBase *sub = new SubscriberROS((ros::NodeHandle*)this); _subs.push_back(sub); return (Subscriber *)sub; } diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index c499712a9..a54b8eb08 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -102,24 +102,23 @@ class SubscriberROS : public: /** - * Construct Subscriber by providing a callback function + * Construct Subscriber without a callback function */ - SubscriberROS(std::function cbf) : - Subscriber(), - _ros_sub(), - _cbf(cbf) + SubscriberROS(ros::NodeHandle *rnh) : + px4::Subscriber(), + _cbf(NULL), + _ros_sub(rnh->subscribe(T::handle(), 1, &SubscriberROS::callback, this)) {} /** - * Construct Subscriber without a callback function + * Construct Subscriber by providing a callback function */ - SubscriberROS() : - Subscriber(), - _ros_sub(), - _cbf(NULL) + //XXX queue default + SubscriberROS(ros::NodeHandle *rnh, std::function cbf) : + _cbf(cbf), + _ros_sub(rnh->subscribe(T::handle(), 1, &SubscriberROS::callback, this)) {} - virtual ~SubscriberROS() {}; protected: @@ -139,14 +138,6 @@ protected: } - /** - * Saves the ros subscriber to keep ros subscription alive - */ - void set_ros_sub(ros::Subscriber ros_sub) - { - _ros_sub = ros_sub; - } - ros::Subscriber _ros_sub; /**< Handle to ros subscriber */ std::function _cbf; /**< Callback that the user provided on the subscription */ -- cgit v1.2.3 From 1ad6e00234e990d44f2a9ca93bcc50696c4c530c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 07:16:07 +0100 Subject: re-enable warnings/errors --- makefiles/toolchain_gnu-arm-eabi.mk | 40 ++++++++++++++++++------------------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index d38ef645f..396980453 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -125,26 +125,26 @@ ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics # Generic warnings # -# ARCHWARNINGS = -Wall \ - # -Wextra \ - # -Werror \ - # -Wdouble-promotion \ - # -Wshadow \ - # -Wfloat-equal \ - # -Wframe-larger-than=1024 \ - # -Wpointer-arith \ - # -Wlogical-op \ - # -Wmissing-declarations \ - # -Wpacked \ - # -Wno-unused-parameter \ - # -Werror=format-security \ - # -Werror=array-bounds \ - # -Wfatal-errors \ - # -Wformat=1 \ - # -Werror=unused-but-set-variable \ - # -Werror=unused-variable \ - # -Werror=double-promotion \ - # -Werror=reorder +ARCHWARNINGS = -Wall \ + -Wextra \ + -Werror \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter \ + -Werror=format-security \ + -Werror=array-bounds \ + -Wfatal-errors \ + -Wformat=1 \ + -Werror=unused-but-set-variable \ + -Werror=unused-variable \ + -Werror=double-promotion \ + -Werror=reorder # -Wcast-qual - generates spurious noreturn attribute warnings, try again later # -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code # -Wcast-align - would help catch bad casts in some cases, but generates too many false positives -- cgit v1.2.3 From 2dfd30c25e2839b762ca127fd4af696284df34b9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 07:17:06 +0100 Subject: move uorb::subscriptionbase into constructor of subscriber --- src/platforms/px4_nodehandle.h | 10 +++------- src/platforms/px4_subscriber.h | 13 +++++++------ 2 files changed, 10 insertions(+), 13 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 40604aa86..f2d09c15c 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -190,8 +190,7 @@ public: template Subscriber *subscribe(void(*fp)(const T &), unsigned interval=10) //XXX interval { - uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval);//XXX needs cleanup in destructor ore move into class - SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, std::bind(fp, std::placeholders::_1)); + SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(interval, std::bind(fp, std::placeholders::_1)); /* Check if this is the smallest interval so far and update _sub_min_interval */ if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { @@ -210,8 +209,7 @@ public: template Subscriber *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval=10) { - uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval);//XXX needs cleanup in destructor ore move into class - SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, std::bind(fp, obj, std::placeholders::_1)); + SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(interval, std::bind(fp, obj, std::placeholders::_1)); /* Check if this is the smallest interval so far and update _sub_min_interval */ if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { @@ -230,9 +228,7 @@ public: template Subscriber *subscribe(unsigned interval=10) //XXX interval { - uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval);//XXX needs cleanup in destructor ore move into class - - SubscriberUORB *sub_px4 = new SubscriberUORB(uorb_sub, interval); + SubscriberUORB *sub_px4 = new SubscriberUORB(interval); /* Check if this is the smallest interval so far and update _sub_min_interval */ if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index a54b8eb08..e086cd4c2 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -183,12 +183,14 @@ public: * Construct SubscriberUORB by providing orb meta data without callback * @param interval Minimal interval between calls to callback */ - SubscriberUORB(uORB::SubscriptionBase * uorb_sub, unsigned interval) : + SubscriberUORB(unsigned interval) : SubscriberNode(interval), - _uorb_sub(uorb_sub) + _uorb_sub(new uORB::SubscriptionBase(T::handle(), interval)) {} - virtual ~SubscriberUORB() {}; + virtual ~SubscriberUORB() { + delete _uorb_sub; + }; /** * Update Subscription @@ -233,10 +235,9 @@ public: * @param cbf Callback, executed on receiving a new message * @param interval Minimal interval between calls to callback */ - SubscriberUORBCallback(uORB::SubscriptionBase * uorb_sub, - unsigned interval, + SubscriberUORBCallback(unsigned interval, std::function cbf) : - SubscriberUORB(uorb_sub, interval), + SubscriberUORB(interval), _cbf(cbf) {} -- cgit v1.2.3 From 59f05a7195ae4a2d57e276c31e12bcf6af477408 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 08:03:07 +0100 Subject: move ros::publisher into constructor of publisher --- src/platforms/px4_nodehandle.h | 4 +--- src/platforms/px4_publisher.h | 7 ++++--- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index f2d09c15c..c85b8118b 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -116,8 +116,7 @@ public: template Publisher* advertise() { - ros::Publisher ros_pub = ros::NodeHandle::advertisedata())>::type &>(T::handle(), kQueueSizeDefault); - PublisherROS *pub = new PublisherROS(ros_pub); + PublisherROS *pub = new PublisherROS((ros::NodeHandle*)this); _pubs.push_back((PublisherBase*)pub); return (Publisher*)pub; } @@ -134,7 +133,6 @@ public: private: - static const uint32_t kQueueSizeDefault = 1; /**< Size of queue for ROS */ std::list _subs; /**< Subcriptions of node */ std::list _pubs; /**< Publications of node */ }; diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index afedbbee7..ea675e67b 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -84,9 +84,9 @@ public: * Construct Publisher by providing a ros::Publisher * @param ros_pub the ros publisher which will be used to perform the publications */ - PublisherROS(ros::Publisher ros_pub) : + PublisherROS(ros::NodeHandle *rnh) : Publisher(), - _ros_pub(ros_pub) + _ros_pub(rnh->advertisedata())>::type &>(T::handle(), kQueueSizeDefault)) {} ~PublisherROS() {}; @@ -99,7 +99,8 @@ public: _ros_pub.publish(msg.data()); return 0; } -private: +protected: + static const uint32_t kQueueSizeDefault = 1; /**< Size of queue for ROS */ ros::Publisher _ros_pub; /**< Handle to the ros publisher */ }; #else -- cgit v1.2.3 From 16c85c6d1870f6f410cf75fc0fe3cb386eb9f6ee Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 08:03:26 +0100 Subject: move uorb::publisherbase into constructor of publisher --- src/platforms/px4_nodehandle.h | 7 +------ src/platforms/px4_publisher.h | 4 ++-- src/platforms/px4_subscriber.h | 7 ++++--- 3 files changed, 7 insertions(+), 11 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index c85b8118b..fd1647051 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -243,13 +243,8 @@ public: template Publisher *advertise() { - //XXX - // uORB::PublicationBase * uorb_pub = new uORB::PublicationBase((new T())->handle()); - uORB::PublicationBase * uorb_pub = new uORB::PublicationBase(T::handle()); - PublisherUORB *pub = new PublisherUORB(uorb_pub); - + PublisherUORB *pub = new PublisherUORB(); _pubs.add(pub); - return (Publisher*)pub; } diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index ea675e67b..a9e40e8d6 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -130,10 +130,10 @@ public: /** * Construct Publisher by providing orb meta data */ - PublisherUORB(uORB::PublicationBase * uorb_pub) : + PublisherUORB() : Publisher(), PublisherNode(), - _uorb_pub(uorb_pub) + _uorb_pub(new uORB::PublicationBase(T::handle())) {} ~PublisherUORB() {}; diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index e086cd4c2..17a95f12d 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -107,21 +107,22 @@ public: SubscriberROS(ros::NodeHandle *rnh) : px4::Subscriber(), _cbf(NULL), - _ros_sub(rnh->subscribe(T::handle(), 1, &SubscriberROS::callback, this)) + _ros_sub(rnh->subscribe(T::handle(), kQueueSizeDefault, &SubscriberROS::callback, this)) {} /** * Construct Subscriber by providing a callback function */ - //XXX queue default SubscriberROS(ros::NodeHandle *rnh, std::function cbf) : _cbf(cbf), - _ros_sub(rnh->subscribe(T::handle(), 1, &SubscriberROS::callback, this)) + _ros_sub(rnh->subscribe(T::handle(), kQueueSizeDefault, &SubscriberROS::callback, this)) {} virtual ~SubscriberROS() {}; protected: + static const uint32_t kQueueSizeDefault = 1; /**< Size of queue for ROS */ + /** * Called on topic update, saves the current message and then calls the provided callback function * needs to use the native type as it is called by ROS -- cgit v1.2.3 From 57569482ad5739bb4471826014818c92bdb33c6d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 08:16:07 +0100 Subject: remove unneeded friend --- src/platforms/px4_subscriber.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 17a95f12d..6a81ef8d2 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -98,8 +98,6 @@ template class SubscriberROS : public Subscriber { - friend class NodeHandle; - public: /** * Construct Subscriber without a callback function -- cgit v1.2.3 From af943cf16adb79ab33097bd560a85fee203a5215 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 08:27:06 +0100 Subject: add update_sub_min_interval function --- src/platforms/px4_nodehandle.h | 36 ++++++++++++++++-------------------- 1 file changed, 16 insertions(+), 20 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index fd1647051..2f92ead5a 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -132,7 +132,7 @@ public: void spin() { ros::spin(); } -private: +protected: std::list _subs; /**< Subcriptions of node */ std::list _pubs; /**< Publications of node */ }; @@ -189,13 +189,8 @@ public: Subscriber *subscribe(void(*fp)(const T &), unsigned interval=10) //XXX interval { SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(interval, std::bind(fp, std::placeholders::_1)); - - /* Check if this is the smallest interval so far and update _sub_min_interval */ - if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { - _sub_min_interval = sub_px4; - } + update_sub_min_interval(interval, sub_px4); _subs.add((SubscriberNode *)sub_px4); - return (Subscriber *)sub_px4; } @@ -208,13 +203,8 @@ public: Subscriber *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval=10) { SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(interval, std::bind(fp, obj, std::placeholders::_1)); - - /* Check if this is the smallest interval so far and update _sub_min_interval */ - if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { - _sub_min_interval = sub_px4; - } + update_sub_min_interval(interval, sub_px4); _subs.add((SubscriberNode *)sub_px4); - return (Subscriber *)sub_px4; } @@ -227,13 +217,8 @@ public: Subscriber *subscribe(unsigned interval=10) //XXX interval { SubscriberUORB *sub_px4 = new SubscriberUORB(interval); - - /* Check if this is the smallest interval so far and update _sub_min_interval */ - if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { - _sub_min_interval = sub_px4; - } + update_sub_min_interval(interval, sub_px4); _subs.add((SubscriberNode *)sub_px4); - return (Subscriber *)sub_px4; } @@ -290,13 +275,24 @@ public: spinOnce(); } } -private: +protected: static const uint16_t kMaxSubscriptions = 100; static const uint16_t kMaxPublications = 100; List _subs; /**< Subcriptions of node */ List _pubs; /**< Publications of node */ SubscriberNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval of all Subscriptions in _subs*/ + + /** + * Check if this is the smallest interval so far and update _sub_min_interval + */ + template + void update_sub_min_interval(unsigned interval, SubscriberUORB *sub) + { + if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { + _sub_min_interval = sub; + } + } }; #endif } -- cgit v1.2.3 From da7ad9a3329db43144b5ca3e60c6c56d22fdc3d4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 09:02:04 +0100 Subject: multiplatform: re-enable interval functionality (for uorb) --- src/examples/subscriber/subscriber_example.cpp | 6 +++--- src/platforms/px4_nodehandle.h | 15 +++++++++------ 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 217e4d48f..215336c17 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -62,13 +62,13 @@ SubscriberExample::SubscriberExample() : /* Do some subscriptions */ /* Function */ - _n.subscribe(rc_channels_callback_function); //ROS version + _n.subscribe(rc_channels_callback_function, _interval); /* No callback */ - _sub_rc_chan = _n.subscribe(); + _sub_rc_chan = _n.subscribe(500); /* Class Method */ - _n.subscribe(&SubscriberExample::rc_channels_callback, this); + _n.subscribe(&SubscriberExample::rc_channels_callback, this, 1000); PX4_INFO("subscribed"); } diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 2f92ead5a..80be9ec52 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -79,7 +79,7 @@ public: * @param fb Callback, executed on receiving a new message */ template - Subscriber *subscribe(void(*fp)(const T &)) + Subscriber *subscribe(void(*fp)(const T &), unsigned interval) { SubscriberBase *sub = new SubscriberROS((ros::NodeHandle*)this, std::bind(fp, std::placeholders::_1)); _subs.push_back(sub); @@ -92,7 +92,7 @@ public: * @param obj pointer class instance */ template - Subscriber *subscribe(void(C::*fp)(const T &), C *obj) + Subscriber *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval) { SubscriberBase *sub = new SubscriberROS((ros::NodeHandle*)this, std::bind(fp, obj, std::placeholders::_1)); _subs.push_back(sub); @@ -103,7 +103,7 @@ public: * Subscribe with no callback, just the latest value is stored on updates */ template - Subscriber *subscribe() + Subscriber *subscribe(unsigned interval) { SubscriberBase *sub = new SubscriberROS((ros::NodeHandle*)this); _subs.push_back(sub); @@ -186,8 +186,9 @@ public: */ template - Subscriber *subscribe(void(*fp)(const T &), unsigned interval=10) //XXX interval + Subscriber *subscribe(void(*fp)(const T &), unsigned interval) { + (void)interval; SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(interval, std::bind(fp, std::placeholders::_1)); update_sub_min_interval(interval, sub_px4); _subs.add((SubscriberNode *)sub_px4); @@ -200,8 +201,9 @@ public: * @param obj pointer class instance */ template - Subscriber *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval=10) + Subscriber *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval) { + (void)interval; SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(interval, std::bind(fp, obj, std::placeholders::_1)); update_sub_min_interval(interval, sub_px4); _subs.add((SubscriberNode *)sub_px4); @@ -214,8 +216,9 @@ public: */ template - Subscriber *subscribe(unsigned interval=10) //XXX interval + Subscriber *subscribe(unsigned interval) { + (void)interval; SubscriberUORB *sub_px4 = new SubscriberUORB(interval); update_sub_min_interval(interval, sub_px4); _subs.add((SubscriberNode *)sub_px4); -- cgit v1.2.3 From c33173cd5de81a9fb32757c3a3c662716f76b300 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 09:23:29 +0100 Subject: uorb pub: cleanup objects --- src/platforms/px4_publisher.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index a9e40e8d6..d9cd7a3c1 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -136,7 +136,9 @@ public: _uorb_pub(new uORB::PublicationBase(T::handle())) {} - ~PublisherUORB() {}; + ~PublisherUORB() { + delete _uorb_pub; + }; /** Publishes msg * @param msg the message which is published to the topic -- cgit v1.2.3 From 6f7fa3b4e7e6fd387530a60c800992c5d7bab87b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 10:33:18 +0100 Subject: header generation script: add option to set output filename prefix --- Tools/px_generate_uorb_topic_headers.py | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py index 2ddbd6984..54430cc38 100755 --- a/Tools/px_generate_uorb_topic_headers.py +++ b/Tools/px_generate_uorb_topic_headers.py @@ -60,7 +60,7 @@ def convert_file(filename, outputdir, templatedir, includepath): """ Converts a single .msg file to a uorb header """ - print("Generating uORB headers from {0}".format(filename)) + print("Generating headers from {0}".format(filename)) genmsg.template_tools.generate_from_file(filename, package, outputdir, @@ -85,7 +85,7 @@ def convert_dir(inputdir, outputdir, templatedir): includepath) -def copy_changed(inputdir, outputdir): +def copy_changed(inputdir, outputdir, prefix=''): """ Copies files from inputdir to outputdir if they don't exist in ouputdir or if their content changed @@ -94,7 +94,7 @@ def copy_changed(inputdir, outputdir): fni = os.path.join(inputdir, f) if os.path.isfile(fni): # Check if f exists in outpoutdir, copy the file if not - fno = os.path.join(outputdir, f) + fno = os.path.join(outputdir, prefix + f) if not os.path.isfile(fno): shutil.copy(fni, fno) print("{0}: new header file".format(f)) @@ -108,7 +108,8 @@ def copy_changed(inputdir, outputdir): print("{0}: unchanged".format(f)) -def convert_dir_save(inputdir, outputdir, templatedir, temporarydir): + +def convert_dir_save(inputdir, outputdir, templatedir, temporarydir, prefix): """ Converts all .msg files in inputdir to uORB header files Unchanged existing files are not overwritten. @@ -117,7 +118,7 @@ def convert_dir_save(inputdir, outputdir, templatedir, temporarydir): convert_dir(inputdir, temporarydir, templatedir) # Copy changed headers from temporary dir to output dir - copy_changed(temporarydir, outputdir) + copy_changed(temporarydir, outputdir, prefix) if __name__ == "__main__": parser = argparse.ArgumentParser( @@ -132,6 +133,9 @@ if __name__ == "__main__": help='output directory for header files') parser.add_argument('-t', dest='temporarydir', help='temporary directory') + parser.add_argument('-p', dest='prefix', default='', + help='string added as prefix to the output file ' + ' name when converting directories') args = parser.parse_args() if args.file is not None: @@ -146,4 +150,5 @@ if __name__ == "__main__": args.dir, args.outputdir, args.templatedir, - args.temporarydir) + args.temporarydir, + args.prefix) -- cgit v1.2.3 From 738f65a705c5dd2e664fedf54fd1064f685ff5c7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 10:34:04 +0100 Subject: generate wrapper headers for uorb --- .gitignore | 1 + Makefile | 15 +- msg/templates/px4/msg.h.template | 95 ++++++++++++ msg/templates/uorb/msg.h.template | 159 +++++++++++++++++++++ src/platforms/nuttx/px4_messages/px4_rc_channels.h | 26 ---- 5 files changed, 266 insertions(+), 30 deletions(-) create mode 100644 msg/templates/px4/msg.h.template create mode 100644 msg/templates/uorb/msg.h.template delete mode 100644 src/platforms/nuttx/px4_messages/px4_rc_channels.h diff --git a/.gitignore b/.gitignore index 1c4f49fde..764be0029 100644 --- a/.gitignore +++ b/.gitignore @@ -40,6 +40,7 @@ tags .ropeproject *.orig src/modules/uORB/topics/* +src/platforms/nuttx/px4_messages/* Firmware.zip unittests/build *.generated.h diff --git a/Makefile b/Makefile index 6480286d2..bffba69ae 100644 --- a/Makefile +++ b/Makefile @@ -225,9 +225,12 @@ updatesubmodules: $(Q) (git submodule update) MSG_DIR = $(PX4_BASE)msg -MSG_TEMPLATE_DIR = $(PX4_BASE)msg/templates +UORB_TEMPLATE_DIR = $(PX4_BASE)msg/templates/uorb +MULTIPLATFORM_TEMPLATE_DIR = $(PX4_BASE)msg/templates/px4 TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics -TOPICS_TEMPORARY_DIR = $(BUILD_DIR)topics_temporary +MULTIPLATFORM_HEADER_DIR = $(PX4_BASE)src/platforms/nuttx/px4_messages +MULTIPLATFORM_PREFIX = px4_ +TOPICHEADER_TEMP_DIR = $(BUILD_DIR)topics_temporary GENMSG_PYTHONPATH = $(PX4_BASE)/Tools/genmsg/src GENCPP_PYTHONPATH = $(PX4_BASE)/Tools/gencpp/src @@ -236,9 +239,13 @@ generateuorbtopicheaders: @$(ECHO) "Generating uORB topic headers" $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH) $(PYTHON) \ $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ - -d $(MSG_DIR) -o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR) -t $(TOPICS_TEMPORARY_DIR)) + -d $(MSG_DIR) -o $(TOPICS_DIR) -e $(UORB_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR)) + @$(ECHO) "Generating multiplatform uORB topic wrapper headers" + $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH) $(PYTHON) \ + $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ + -d $(MSG_DIR) -o $(MULTIPLATFORM_HEADER_DIR) -e $(MULTIPLATFORM_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR) -p $(MULTIPLATFORM_PREFIX)) # clean up temporary files - $(Q) (rm -r $(TOPICS_TEMPORARY_DIR)) + $(Q) (rm -r $(TOPICHEADER_TEMP_DIR)) # # Testing targets diff --git a/msg/templates/px4/msg.h.template b/msg/templates/px4/msg.h.template new file mode 100644 index 000000000..ba0fbd4bf --- /dev/null +++ b/msg/templates/px4/msg.h.template @@ -0,0 +1,95 @@ +@############################################### +@# +@# PX4 ROS compatible message source code +@# generation for C++ +@# +@# This file generates the multiplatform wrapper +@# +@# EmPy template for generating .h files +@# Based on the original template for ROS +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - file_name_in (String) Source file +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@# - md5sum (String) MD5Sum of the .msg specification +@############################################### +/**************************************************************************** + * + * Copyright (C) 2013-2015 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. + * + ****************************************************************************/ + +/* Auto-generated by genmsg_cpp from file @file_name_in */ + +@{ +import genmsg.msgs +import gencpp + +cpp_class = 'px4_%s'%spec.short_name +uorb_struct = '%s_s'%spec.short_name +topic_name = spec.short_name +}@ + +#pragma once + +@############################## +@# Generic Includes +@############################## +#include +#include "platforms/px4_message.h" + +@############################## +@# Class +@############################## + +namespace px4 +{ + +class __EXPORT @(cpp_class) : + public PX4Message<@(uorb_struct)> +{ +public: + @(cpp_class)() : + PX4Message<@(uorb_struct)>() + {} + + @(cpp_class)(@(uorb_struct) msg) : + PX4Message<@(uorb_struct)>(msg) + {} + + ~@(cpp_class)() {} + + static PX4TopicHandle handle() {return ORB_ID(@(topic_name));} +}; + +}; diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template new file mode 100644 index 000000000..11cb1ea2a --- /dev/null +++ b/msg/templates/uorb/msg.h.template @@ -0,0 +1,159 @@ +@############################################### +@# +@# PX4 ROS compatible message source code +@# generation for C++ +@# +@# EmPy template for generating .h files +@# Based on the original template for ROS +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - file_name_in (String) Source file +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@# - md5sum (String) MD5Sum of the .msg specification +@############################################### +/**************************************************************************** + * + * Copyright (C) 2013-2015 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. + * + ****************************************************************************/ + + /* Auto-generated by genmsg_cpp from file @file_name_in */ + +@{ +import genmsg.msgs +import gencpp + +cpp_namespace = '::%s::'%(spec.package) # TODO handle nested namespace +cpp_class = '%s_'%spec.short_name +cpp_full_name = '%s%s'%(cpp_namespace,cpp_class) +cpp_full_name_with_alloc = '%s'%(cpp_full_name) +cpp_msg_definition = gencpp.escape_message_definition(msg_definition) +}@ + +#pragma once + +@############################## +@# Generic Includes +@############################## +#include +#include + +@############################## +@# Includes for dependencies +@############################## +@{ +for field in spec.parsed_fields(): + if (not field.is_builtin): + if (not field.is_header): + (package, name) = genmsg.names.package_resource_name(field.base_type) + package = package or spec.package # convert '' to package + print('#include '%(name)) + +}@ +@# Constants +@[for constant in spec.constants]@ +#define @(constant.name) @(int(constant.val)) +@[end for] + +/** + * @@addtogroup topics + * @@{ + */ + +@############################## +@# Main struct of message +@############################## +@{ + +type_map = {'int8': 'int8_t', + 'int16': 'int16_t', + 'int32': 'int32_t', + 'int64': 'int64_t', + 'uint8': 'uint8_t', + 'uint16': 'uint16_t', + 'uint32': 'uint32_t', + 'uint64': 'uint64_t', + 'float32': 'float', + 'bool': 'bool', + 'fence_vertex': 'fence_vertex'} + +# Function to print a standard ros type +def print_field_def(field): + type = field.type + # detect embedded types + sl_pos = type.find('/') + type_appendix = '' + type_prefix = '' + if (sl_pos >= 0): + type = type[sl_pos + 1:] + type_prefix = 'struct ' + type_appendix = '_s' + + # detect arrays + a_pos = type.find('[') + array_size = '' + if (a_pos >= 0): + # field is array + array_size = type[a_pos:] + type = type[:a_pos] + + if type in type_map: + # need to add _t: int8 --> int8_t + type_px4 = type_map[type] + else: + raise Exception("Type {0} not supported, add to to template file!".format(type)) + + print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size)) + +} +#ifdef __cplusplus +@#class @(spec.short_name)_s { +struct __EXPORT @(spec.short_name)_s { +@#public: +#else +struct @(spec.short_name)_s { +#endif +@{ +# loop over all fields and print the type and name +for field in spec.parsed_fields(): + if (not field.is_header): + print_field_def(field) +}@ +}; + +/** + * @@} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(@(spec.short_name)); diff --git a/src/platforms/nuttx/px4_messages/px4_rc_channels.h b/src/platforms/nuttx/px4_messages/px4_rc_channels.h deleted file mode 100644 index 2ce7bfc80..000000000 --- a/src/platforms/nuttx/px4_messages/px4_rc_channels.h +++ /dev/null @@ -1,26 +0,0 @@ -#include -#include -#include "platforms/px4_message.h" - -#pragma once -namespace px4 -{ - -class __EXPORT px4_rc_channels : - public PX4Message -{ -public: - px4_rc_channels() : - PX4Message() - {} - - px4_rc_channels(rc_channels_s msg) : - PX4Message(msg) - {} - - ~px4_rc_channels() {} - - static PX4TopicHandle handle() {return ORB_ID(rc_channels);} -}; - -} -- cgit v1.2.3 From 5e9bef6f4b710aae319bd60068107bff41003fd8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 10:37:17 +0100 Subject: cleanup template for normal uorb headers --- msg/templates/uorb/msg.h.template | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template index 11cb1ea2a..622641617 100644 --- a/msg/templates/uorb/msg.h.template +++ b/msg/templates/uorb/msg.h.template @@ -47,17 +47,14 @@ * ****************************************************************************/ - /* Auto-generated by genmsg_cpp from file @file_name_in */ +/* Auto-generated by genmsg_cpp from file @file_name_in */ @{ import genmsg.msgs import gencpp -cpp_namespace = '::%s::'%(spec.package) # TODO handle nested namespace -cpp_class = '%s_'%spec.short_name -cpp_full_name = '%s%s'%(cpp_namespace,cpp_class) -cpp_full_name_with_alloc = '%s'%(cpp_full_name) -cpp_msg_definition = gencpp.escape_message_definition(msg_definition) +uorb_struct = '%s_s'%spec.short_name +topic_name = spec.short_name }@ #pragma once @@ -137,11 +134,11 @@ def print_field_def(field): } #ifdef __cplusplus -@#class @(spec.short_name)_s { -struct __EXPORT @(spec.short_name)_s { +@#class @(uorb_struct) { +struct __EXPORT @(uorb_struct) { @#public: #else -struct @(spec.short_name)_s { +struct @(uorb_struct) { #endif @{ # loop over all fields and print the type and name @@ -156,4 +153,4 @@ for field in spec.parsed_fields(): */ /* register this as object request broker structure */ -ORB_DECLARE(@(spec.short_name)); +ORB_DECLARE(@(topic_name)); -- cgit v1.2.3 From 4ba57ad285884a2b01ebf8aac2c710ed63f7ffd3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 11:39:10 +0100 Subject: get rid of unnecessary defines --- src/platforms/px4_defines.h | 1 - src/platforms/px4_middleware.h | 4 ---- 2 files changed, 5 deletions(-) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 6b6a03893..325832f0f 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -50,7 +50,6 @@ /* * Building for running within the ROS environment */ -#define __EXPORT #define noreturn_function #ifdef __cplusplus #include "ros/ros.h" diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index cd52fd650..735d34234 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -42,10 +42,6 @@ #include #include -#if defined(__PX4_ROS) -#define __EXPORT -#endif - namespace px4 { -- cgit v1.2.3 From d7e57061c976ba18c69d8be9949660e85f645126 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 11:40:23 +0100 Subject: generate message wrapper headers on ros --- .gitignore | 1 + CMakeLists.txt | 16 +++- Makefile | 6 +- msg/templates/px4/msg.h.template | 95 ------------------------ msg/templates/px4/ros/msg.h.template | 95 ++++++++++++++++++++++++ msg/templates/px4/uorb/msg.h.template | 95 ++++++++++++++++++++++++ src/platforms/ros/px4_messages/px4_rc_channels.h | 25 ------- 7 files changed, 207 insertions(+), 126 deletions(-) delete mode 100644 msg/templates/px4/msg.h.template create mode 100644 msg/templates/px4/ros/msg.h.template create mode 100644 msg/templates/px4/uorb/msg.h.template delete mode 100644 src/platforms/ros/px4_messages/px4_rc_channels.h diff --git a/.gitignore b/.gitignore index 764be0029..1c3f8fe04 100644 --- a/.gitignore +++ b/.gitignore @@ -41,6 +41,7 @@ tags *.orig src/modules/uORB/topics/* src/platforms/nuttx/px4_messages/* +src/platforms/ros/px4_messages/* Firmware.zip unittests/build *.generated.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 5488dbe9d..25822d719 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -125,6 +125,16 @@ include_directories( ${EIGEN_INCLUDE_DIRS} ) +## generate multiplatform wrapper headers +## note that the message header files are generated as in any ROS project with generate_messages() +set(MULTIPLATFORM_HEADER_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src/platforms/ros/px4_messages) +set(MULTIPLATFORM_TEMPLATE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/msg/templates/px4/ros) +set(TOPICHEADER_TEMP_DIR ${CMAKE_BINARY_DIR}/topics_temporary) +set(MULTIPLATFORM_PREFIX px4_) +add_custom_target(multiplatform_message_headers ALL ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/Tools/px_generate_uorb_topic_headers.py + -d ${CMAKE_CURRENT_SOURCE_DIR}/msg -o ${MULTIPLATFORM_HEADER_DIR} -e ${MULTIPLATFORM_TEMPLATE_DIR} + -t ${TOPICHEADER_TEMP_DIR} -p ${MULTIPLATFORM_PREFIX}) + ## Declare a cpp library add_library(px4 src/platforms/ros/px4_ros_impl.cpp @@ -133,7 +143,7 @@ add_library(px4 src/lib/mathlib/math/Limits.cpp src/modules/systemlib/circuit_breaker.cpp ) -add_dependencies(px4 ${PROJECT_NAME}_generate_messages_cpp) +add_dependencies(px4 ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers) target_link_libraries(px4 ${catkin_LIBRARIES} @@ -143,7 +153,7 @@ target_link_libraries(px4 add_executable(publisher src/examples/publisher/publisher_main.cpp src/examples/publisher/publisher_example.cpp) -add_dependencies(publisher ${PROJECT_NAME}_generate_messages_cpp) +add_dependencies(publisher ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers) target_link_libraries(publisher ${catkin_LIBRARIES} px4 @@ -153,7 +163,7 @@ target_link_libraries(publisher add_executable(subscriber src/examples/subscriber/subscriber_main.cpp src/examples/subscriber/subscriber_example.cpp) -add_dependencies(subscriber ${PROJECT_NAME}_generate_messages_cpp) +add_dependencies(subscriber ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers) target_link_libraries(subscriber ${catkin_LIBRARIES} px4 diff --git a/Makefile b/Makefile index bffba69ae..c727c9a69 100644 --- a/Makefile +++ b/Makefile @@ -226,13 +226,13 @@ updatesubmodules: MSG_DIR = $(PX4_BASE)msg UORB_TEMPLATE_DIR = $(PX4_BASE)msg/templates/uorb -MULTIPLATFORM_TEMPLATE_DIR = $(PX4_BASE)msg/templates/px4 +MULTIPLATFORM_TEMPLATE_DIR = $(PX4_BASE)msg/templates/px4/uorb TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics MULTIPLATFORM_HEADER_DIR = $(PX4_BASE)src/platforms/nuttx/px4_messages MULTIPLATFORM_PREFIX = px4_ TOPICHEADER_TEMP_DIR = $(BUILD_DIR)topics_temporary -GENMSG_PYTHONPATH = $(PX4_BASE)/Tools/genmsg/src -GENCPP_PYTHONPATH = $(PX4_BASE)/Tools/gencpp/src +GENMSG_PYTHONPATH = $(PX4_BASE)Tools/genmsg/src +GENCPP_PYTHONPATH = $(PX4_BASE)Tools/gencpp/src .PHONY: generateuorbtopicheaders generateuorbtopicheaders: diff --git a/msg/templates/px4/msg.h.template b/msg/templates/px4/msg.h.template deleted file mode 100644 index ba0fbd4bf..000000000 --- a/msg/templates/px4/msg.h.template +++ /dev/null @@ -1,95 +0,0 @@ -@############################################### -@# -@# PX4 ROS compatible message source code -@# generation for C++ -@# -@# This file generates the multiplatform wrapper -@# -@# EmPy template for generating .h files -@# Based on the original template for ROS -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - file_name_in (String) Source file -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@# - md5sum (String) MD5Sum of the .msg specification -@############################################### -/**************************************************************************** - * - * Copyright (C) 2013-2015 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. - * - ****************************************************************************/ - -/* Auto-generated by genmsg_cpp from file @file_name_in */ - -@{ -import genmsg.msgs -import gencpp - -cpp_class = 'px4_%s'%spec.short_name -uorb_struct = '%s_s'%spec.short_name -topic_name = spec.short_name -}@ - -#pragma once - -@############################## -@# Generic Includes -@############################## -#include -#include "platforms/px4_message.h" - -@############################## -@# Class -@############################## - -namespace px4 -{ - -class __EXPORT @(cpp_class) : - public PX4Message<@(uorb_struct)> -{ -public: - @(cpp_class)() : - PX4Message<@(uorb_struct)>() - {} - - @(cpp_class)(@(uorb_struct) msg) : - PX4Message<@(uorb_struct)>(msg) - {} - - ~@(cpp_class)() {} - - static PX4TopicHandle handle() {return ORB_ID(@(topic_name));} -}; - -}; diff --git a/msg/templates/px4/ros/msg.h.template b/msg/templates/px4/ros/msg.h.template new file mode 100644 index 000000000..176be0d09 --- /dev/null +++ b/msg/templates/px4/ros/msg.h.template @@ -0,0 +1,95 @@ +@############################################### +@# +@# PX4 ROS compatible message source code +@# generation for C++ +@# +@# This file generates the multiplatform wrapper +@# +@# EmPy template for generating .h files +@# Based on the original template for ROS +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - file_name_in (String) Source file +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@# - md5sum (String) MD5Sum of the .msg specification +@############################################### +/**************************************************************************** + * + * Copyright (C) 2013-2015 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. + * + ****************************************************************************/ + +/* Auto-generated by genmsg_cpp from file @file_name_in */ + +@{ +import genmsg.msgs +import gencpp + +cpp_class = 'px4_%s'%spec.short_name +native_type = spec.short_name +topic_name = spec.short_name +}@ + +#pragma once + +@############################## +@# Generic Includes +@############################## +#include +#include "platforms/px4_message.h" + +@############################## +@# Class +@############################## + +namespace px4 +{ + +class @(cpp_class) : + public PX4Message<@(native_type)> +{ +public: + @(cpp_class)() : + PX4Message<@(native_type)>() + {} + + @(cpp_class)(@(native_type) msg) : + PX4Message<@(native_type)>(msg) + {} + + ~@(cpp_class)() {} + + static PX4TopicHandle handle() {return "@(topic_name)";} +}; + +}; diff --git a/msg/templates/px4/uorb/msg.h.template b/msg/templates/px4/uorb/msg.h.template new file mode 100644 index 000000000..2d4251107 --- /dev/null +++ b/msg/templates/px4/uorb/msg.h.template @@ -0,0 +1,95 @@ +@############################################### +@# +@# PX4 ROS compatible message source code +@# generation for C++ +@# +@# This file generates the multiplatform wrapper +@# +@# EmPy template for generating .h files +@# Based on the original template for ROS +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - file_name_in (String) Source file +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@# - md5sum (String) MD5Sum of the .msg specification +@############################################### +/**************************************************************************** + * + * Copyright (C) 2013-2015 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. + * + ****************************************************************************/ + +/* Auto-generated by genmsg_cpp from file @file_name_in */ + +@{ +import genmsg.msgs +import gencpp + +cpp_class = 'px4_%s'%spec.short_name +native_type = '%s_s'%spec.short_name +topic_name = spec.short_name +}@ + +#pragma once + +@############################## +@# Generic Includes +@############################## +#include +#include "platforms/px4_message.h" + +@############################## +@# Class +@############################## + +namespace px4 +{ + +class __EXPORT @(cpp_class) : + public PX4Message<@(native_type)> +{ +public: + @(cpp_class)() : + PX4Message<@(native_type)>() + {} + + @(cpp_class)(@(native_type) msg) : + PX4Message<@(native_type)>(msg) + {} + + ~@(cpp_class)() {} + + static PX4TopicHandle handle() {return ORB_ID(@(topic_name));} +}; + +}; diff --git a/src/platforms/ros/px4_messages/px4_rc_channels.h b/src/platforms/ros/px4_messages/px4_rc_channels.h deleted file mode 100644 index 5e9dc72cd..000000000 --- a/src/platforms/ros/px4_messages/px4_rc_channels.h +++ /dev/null @@ -1,25 +0,0 @@ -#include "px4/rc_channels.h" -#include "platforms/px4_message.h" - -#pragma once -namespace px4 -{ - -class px4_rc_channels : - public PX4Message -{ -public: - px4_rc_channels() : - PX4Message() - {} - - px4_rc_channels(rc_channels msg) : - PX4Message(msg) - {} - - ~px4_rc_channels() {} - - static PX4TopicHandle handle() {return "rc_channels";} -}; - -} -- cgit v1.2.3 From f55832a37066c2cb0e6db64d95710b6fa0017fb1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 14:38:39 +0100 Subject: multiplatform: update topic includes --- src/platforms/px4_includes.h | 49 ++++++++++++++++++++++---------------------- 1 file changed, 24 insertions(+), 25 deletions(-) diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index bdb2b8de9..90ec78996 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -48,21 +48,20 @@ #ifdef __cplusplus #include "ros/ros.h" -#include "px4/rc_channels.h" -#include "px4_rc_channels.h" //XXX -#include "px4/vehicle_attitude.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #endif #else @@ -73,17 +72,17 @@ #include #include #ifdef __cplusplus -#include //XXX +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #endif -#include -#include -#include -#include -#include -#include -#include -#include -#include #include #include #include -- cgit v1.2.3 From 1ab9ec5811a5b945ca49a8e6aeef59557dd2970c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 14:38:53 +0100 Subject: fix ros header template --- msg/templates/px4/ros/msg.h.template | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/msg/templates/px4/ros/msg.h.template b/msg/templates/px4/ros/msg.h.template index 176be0d09..76abab163 100644 --- a/msg/templates/px4/ros/msg.h.template +++ b/msg/templates/px4/ros/msg.h.template @@ -65,7 +65,7 @@ topic_name = spec.short_name @############################## @# Generic Includes @############################## -#include +#include "px4/@(topic_name).h" #include "platforms/px4_message.h" @############################## -- cgit v1.2.3 From 73f342616e275ca726d70807212a733f9971bf66 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 15:16:13 +0100 Subject: add better accessor --- src/examples/subscriber/subscriber_example.cpp | 2 +- src/platforms/px4_subscriber.h | 10 +++++++++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 215336c17..781dde486 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -81,5 +81,5 @@ void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) { PX4_INFO("Callback (method): [%llu]", msg.data().timestamp_last_valid); PX4_INFO("Callback (method): value of _sub_rc_chan: [%llu]", - _sub_rc_chan->get().data().timestamp_last_valid); + _sub_rc_chan->data().timestamp_last_valid); } diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 6a81ef8d2..6ca35b173 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -84,7 +84,15 @@ public: /** * Get the last message value */ - virtual T get() {return _msg_current;} + virtual T& get() {return _msg_current;} + + /** + * Get the last native message value + */ + virtual decltype(((T*)nullptr)->data()) data() + { + return _msg_current.data(); + } protected: T _msg_current; /**< Current Message value */ -- cgit v1.2.3 From 6c0b5cf1259a0560a5adc966c4ca306f998c2c86 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Jan 2015 15:24:18 +0100 Subject: Update NuttX version as required --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 3d8171f6e..e4c914e26 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 3d8171f6ea88297d8595525c8222d61e9cf20fd0 +Subproject commit e4c914e261d2647e44d05222afa7aa3cc90d3c67 -- cgit v1.2.3 From 6357fa75976a1d215d263b316d7fe495b82cfff4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 15:39:11 +0100 Subject: header generation script: create dir if it does not exist --- Tools/px_generate_uorb_topic_headers.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py index 54430cc38..4bcab4d54 100755 --- a/Tools/px_generate_uorb_topic_headers.py +++ b/Tools/px_generate_uorb_topic_headers.py @@ -90,6 +90,11 @@ def copy_changed(inputdir, outputdir, prefix=''): Copies files from inputdir to outputdir if they don't exist in ouputdir or if their content changed """ + + # Make sure output directory exists: + if not os.path.isdir(outputdir): + os.makedirs(outputdir) + for f in os.listdir(inputdir): fni = os.path.join(inputdir, f) if os.path.isfile(fni): -- cgit v1.2.3 From dfd078651f44533b2f5998b0edce781b11507275 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 18:00:59 +0100 Subject: subscriber: move nuttx startup handling --- src/examples/subscriber/module.mk | 1 + src/examples/subscriber/subscriber_main.cpp | 63 +------------- src/examples/subscriber/subscriber_start_nuttx.cpp | 99 ++++++++++++++++++++++ 3 files changed, 102 insertions(+), 61 deletions(-) create mode 100644 src/examples/subscriber/subscriber_start_nuttx.cpp diff --git a/src/examples/subscriber/module.mk b/src/examples/subscriber/module.mk index 3156ebb30..0e02c65b4 100644 --- a/src/examples/subscriber/module.mk +++ b/src/examples/subscriber/module.mk @@ -38,6 +38,7 @@ MODULE_COMMAND = subscriber SRCS = subscriber_main.cpp \ + subscriber_start_nuttx.cpp \ subscriber_example.cpp \ subscriber_params.c diff --git a/src/examples/subscriber/subscriber_main.cpp b/src/examples/subscriber/subscriber_main.cpp index 906921e01..9752bc3cc 100644 --- a/src/examples/subscriber/subscriber_main.cpp +++ b/src/examples/subscriber/subscriber_main.cpp @@ -37,70 +37,11 @@ * * @author Thomas Gubler */ -#include -#include #include "subscriber_example.h" +bool thread_running = false; /**< Deamon status flag */ -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(subscriber); - -#if !defined(__PX4_ROS) -extern "C" __EXPORT int subscriber_main(int argc, char *argv[]); -int subscriber_main(int argc, char *argv[]) -{ - if (argc < 1) { - errx(1, "usage: subscriber {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("subscriber", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - subscriber_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(subscriber) +int main(int argc, char **argv) { px4::init(argc, argv, "subscriber"); diff --git a/src/examples/subscriber/subscriber_start_nuttx.cpp b/src/examples/subscriber/subscriber_start_nuttx.cpp new file mode 100644 index 000000000..6129b19ac --- /dev/null +++ b/src/examples/subscriber/subscriber_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 subscriber_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 subscriber_main(int argc, char *argv[]); +int subscriber_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "usage: subscriber {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("subscriber", + 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 a36d0dce85868f35ad9c4e097f5b788e84d7d18f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 18:01:16 +0100 Subject: publisher: move nuttx startup handling --- src/examples/publisher/module.mk | 1 + src/examples/publisher/publisher_main.cpp | 61 +-------------- src/examples/publisher/publisher_start_nuttx.cpp | 99 ++++++++++++++++++++++++ 3 files changed, 102 insertions(+), 59 deletions(-) create mode 100644 src/examples/publisher/publisher_start_nuttx.cpp diff --git a/src/examples/publisher/module.mk b/src/examples/publisher/module.mk index 4f941cd43..62a5f8dd1 100644 --- a/src/examples/publisher/module.mk +++ b/src/examples/publisher/module.mk @@ -38,6 +38,7 @@ MODULE_COMMAND = publisher SRCS = publisher_main.cpp \ + publisher_start_nuttx.cpp \ publisher_example.cpp MODULE_STACKSIZE = 1200 diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp index 81439a8be..85d24d4ef 100644 --- a/src/examples/publisher/publisher_main.cpp +++ b/src/examples/publisher/publisher_main.cpp @@ -41,66 +41,9 @@ #include #include "publisher_example.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(publisher); - -#if !defined(__PX4_ROS) -extern "C" __EXPORT int publisher_main(int argc, char *argv[]); -int publisher_main(int argc, char *argv[]) -{ - if (argc < 1) { - errx(1, "usage: publisher {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("publisher", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - publisher_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 +bool thread_running = false; /**< Deamon status flag */ -PX4_MAIN_FUNCTION(publisher) +int main(int argc, char **argv) { px4::init(argc, argv, "publisher"); diff --git a/src/examples/publisher/publisher_start_nuttx.cpp b/src/examples/publisher/publisher_start_nuttx.cpp new file mode 100644 index 000000000..db9d85269 --- /dev/null +++ b/src/examples/publisher/publisher_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 publisher_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 publisher_main(int argc, char *argv[]); +int publisher_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "usage: publisher {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("publisher", + 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 aa7e5ddb388e5d382b3dc214de6a1581c2a2fe71 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 18:11:44 +0100 Subject: move position of spin_once in publisher example --- src/examples/publisher/publisher_example.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp index 6b30c2fe3..9952a16e4 100644 --- a/src/examples/publisher/publisher_example.cpp +++ b/src/examples/publisher/publisher_example.cpp @@ -54,13 +54,14 @@ int PublisherExample::main() px4::Rate loop_rate(10); while (px4::ok()) { + loop_rate.sleep(); + _n.spinOnce(); + px4_rc_channels msg; msg.data().timestamp_last_valid = px4::get_time_micros(); PX4_INFO("%llu", msg.data().timestamp_last_valid); _rc_channels_pub->publish(msg); - _n.spinOnce(); - loop_rate.sleep(); } return 0; -- cgit v1.2.3 From 67a35eb0b509b2394f82773c460a6492471efd37 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 18:15:55 +0100 Subject: remove unneeded files --- src/platforms/nuttx/px4_nodehandle.cpp | 44 ---------------------------------- src/platforms/nuttx/px4_publisher.cpp | 41 ------------------------------- src/platforms/nuttx/px4_subscriber.cpp | 40 ------------------------------- src/platforms/ros/px4_subscriber.cpp | 41 ------------------------------- 4 files changed, 166 deletions(-) delete mode 100644 src/platforms/nuttx/px4_nodehandle.cpp delete mode 100644 src/platforms/nuttx/px4_publisher.cpp delete mode 100644 src/platforms/nuttx/px4_subscriber.cpp delete mode 100644 src/platforms/ros/px4_subscriber.cpp diff --git a/src/platforms/nuttx/px4_nodehandle.cpp b/src/platforms/nuttx/px4_nodehandle.cpp deleted file mode 100644 index ec557e8aa..000000000 --- a/src/platforms/nuttx/px4_nodehandle.cpp +++ /dev/null @@ -1,44 +0,0 @@ -/**************************************************************************** - * - * 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 px4_nodehandle.cpp - * - * PX4 Middleware Wrapper Nodehandle - */ -#include - -namespace px4 -{ - -} diff --git a/src/platforms/nuttx/px4_publisher.cpp b/src/platforms/nuttx/px4_publisher.cpp deleted file mode 100644 index 3bd70272f..000000000 --- a/src/platforms/nuttx/px4_publisher.cpp +++ /dev/null @@ -1,41 +0,0 @@ -/**************************************************************************** - * - * 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 px4_publisher.cpp - * - * PX4 Middleware Wrapper for Publisher - */ -#include - - diff --git a/src/platforms/nuttx/px4_subscriber.cpp b/src/platforms/nuttx/px4_subscriber.cpp deleted file mode 100644 index 426e646c9..000000000 --- a/src/platforms/nuttx/px4_subscriber.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/**************************************************************************** - * - * 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 px4_subscriber.cpp - * - * PX4 Middleware Wrapper Subscriber - */ -#include - diff --git a/src/platforms/ros/px4_subscriber.cpp b/src/platforms/ros/px4_subscriber.cpp deleted file mode 100644 index d040b860d..000000000 --- a/src/platforms/ros/px4_subscriber.cpp +++ /dev/null @@ -1,41 +0,0 @@ -/**************************************************************************** - * - * 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 px4_subscriber.cpp - * - * PX4 Middleware Wrapper Subscriber - */ - -#include - -- cgit v1.2.3 From 4c3a24ddeeee4187a527a1d750b406b96232f66b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 18:30:02 +0100 Subject: tiny cleanup --- src/examples/publisher/publisher_main.cpp | 2 -- src/examples/subscriber/subscriber_main.cpp | 1 - 2 files changed, 3 deletions(-) diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp index 85d24d4ef..11439acff 100644 --- a/src/examples/publisher/publisher_main.cpp +++ b/src/examples/publisher/publisher_main.cpp @@ -37,8 +37,6 @@ * * @author Thomas Gubler */ -#include -#include #include "publisher_example.h" bool thread_running = false; /**< Deamon status flag */ diff --git a/src/examples/subscriber/subscriber_main.cpp b/src/examples/subscriber/subscriber_main.cpp index 9752bc3cc..798c74163 100644 --- a/src/examples/subscriber/subscriber_main.cpp +++ b/src/examples/subscriber/subscriber_main.cpp @@ -40,7 +40,6 @@ #include "subscriber_example.h" bool thread_running = false; /**< Deamon status flag */ - int main(int argc, char **argv) { px4::init(argc, argv, "subscriber"); -- cgit v1.2.3 From 2d65e8cb3b792ae55c953df87fc429cdde2972b5 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 20 Jan 2015 16:50:59 +0100 Subject: initial commit for vagrant/docker based ros setup --- .gitignore | 1 + Tools/ros/docker/px4-ros/Dockerfile | 42 +++++++++++++++ Tools/ros/docker/px4-ros/px4-ros-sitl/Dockerfile | 22 ++++++++ .../ros/docker/px4-ros/px4-ros-sitl/get-sources.sh | 28 ++++++++++ Tools/ros/vagrant/docker-host-base/Vagrantfile | 59 ++++++++++++++++++++++ .../vagrant/docker-host-base/config/docker-default | 26 ++++++++++ .../ros/vagrant/docker-host-base/config/xsessionrc | 5 ++ Tools/ros/vagrant/docker-host/Vagrantfile | 25 +++++++++ Tools/ros/vagrant/px4-ros-sitl/Vagrantfile | 27 ++++++++++ 9 files changed, 235 insertions(+) create mode 100644 Tools/ros/docker/px4-ros/Dockerfile create mode 100644 Tools/ros/docker/px4-ros/px4-ros-sitl/Dockerfile create mode 100644 Tools/ros/docker/px4-ros/px4-ros-sitl/get-sources.sh create mode 100644 Tools/ros/vagrant/docker-host-base/Vagrantfile create mode 100644 Tools/ros/vagrant/docker-host-base/config/docker-default create mode 100644 Tools/ros/vagrant/docker-host-base/config/xsessionrc create mode 100644 Tools/ros/vagrant/docker-host/Vagrantfile create mode 100644 Tools/ros/vagrant/px4-ros-sitl/Vagrantfile diff --git a/.gitignore b/.gitignore index 1c4f49fde..d4e2a4637 100644 --- a/.gitignore +++ b/.gitignore @@ -43,3 +43,4 @@ src/modules/uORB/topics/* Firmware.zip unittests/build *.generated.h +.vagrant diff --git a/Tools/ros/docker/px4-ros/Dockerfile b/Tools/ros/docker/px4-ros/Dockerfile new file mode 100644 index 000000000..532a9af8b --- /dev/null +++ b/Tools/ros/docker/px4-ros/Dockerfile @@ -0,0 +1,42 @@ +# +# Base ROS Indigo +# + +FROM ubuntu:14.04.1 +MAINTAINER Andreas Antener "andreas@antener.name" + +# Install basics +RUN apt-get update \ + && apt-get -y install wget git mercurial + +# Main ROS Setup +# Following http://wiki.ros.org/indigo/Installation/Ubuntu +# Also adding dependencies for gazebo http://gazebosim.org/tutorials?tut=drcsim_install + +## add ROS repositories and keys +## install main ROS pacakges +RUN echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list \ + && wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | apt-key add - \ + && apt-get update \ + && apt-get -y install ros-indigo-desktop-full + +RUN rosdep init \ + && rosdep update + +## setup environment variables +RUN echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc + +## get rosinstall +RUN apt-get -y install python-rosinstall + +## additional dependencies +RUN apt-get -y install ros-indigo-octomap-msgs + + +## install drcsim +#RUN echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" > /etc/apt/sources.list.d/drc-latest.list \ +# && wget http://packages.osrfoundation.org/drc.key -O - | apt-key add - \ +# && apt-get update \ +# && apt-get -y install drcsim + +CMD /bin/bash diff --git a/Tools/ros/docker/px4-ros/px4-ros-sitl/Dockerfile b/Tools/ros/docker/px4-ros/px4-ros-sitl/Dockerfile new file mode 100644 index 000000000..4c47295a8 --- /dev/null +++ b/Tools/ros/docker/px4-ros/px4-ros-sitl/Dockerfile @@ -0,0 +1,22 @@ +# +# PX4 ROS SITL +# + +FROM px4ros/ros-base:no-drcsim +MAINTAINER Andreas Antener "andreas@antener.name" + +# TODO +#USER px4 + +RUN . /opt/ros/indigo/setup.sh \ + && mkdir -p /catkin_ws/src \ + && cd /catkin_ws/src \ + && catkin_init_workspace \ + && cd /catkin_ws \ + && catkin_make \ + && echo "source /catkin_ws/devel/setup.bash" >> ~/.bashrc + +COPY get-sources.sh /catkin_ws/src/ +RUN chmod +x /catkin_ws/src/get-sources.sh + +CMD /bin/bash diff --git a/Tools/ros/docker/px4-ros/px4-ros-sitl/get-sources.sh b/Tools/ros/docker/px4-ros/px4-ros-sitl/get-sources.sh new file mode 100644 index 000000000..237ae7500 --- /dev/null +++ b/Tools/ros/docker/px4-ros/px4-ros-sitl/get-sources.sh @@ -0,0 +1,28 @@ +#!/bin/sh +# +# Fetch source repositories +# + +# PX4 Firmware +git clone https://github.com/PX4/Firmware.git \ + && cd Firmware \ + && git checkout ros \ + && cd .. + +# euroc simulator +git clone https://github.com/PX4/euroc_simulator.git \ + && cd euroc_simulator \ + && git checkout px4_nodes \ + && cd .. + +# mav comm +git clone https://github.com/PX4/mav_comm.git + +# glog catkin +git clone https://github.com/ethz-asl/glog_catkin.git + +# catkin simple +git clone https://github.com/catkin/catkin_simple.git + +echo "Execute catkin_make to compile all the sources." + diff --git a/Tools/ros/vagrant/docker-host-base/Vagrantfile b/Tools/ros/vagrant/docker-host-base/Vagrantfile new file mode 100644 index 000000000..5897735d0 --- /dev/null +++ b/Tools/ros/vagrant/docker-host-base/Vagrantfile @@ -0,0 +1,59 @@ +# -*- mode: ruby -*- +# vi: set ft=ruby : + +# +# Vagrantfile to create docker-host-base +# +# After build, do "vagrant package --base docker-host-base" to package, +# and import as box: "vagrant box add --name docker-host-base package.box" +# +# To add local docker images into the docker host, configure your local +# docker client to control the docker daemon on the later running docker-host VM. +# This box configures docker to listen on any IP on port 2375. +# You can then also load an existing image, e.g.: +# "docker load -i px4ros_ros-sitl_no-drcsim_a4209708a04a.tar" +# +Vagrant.configure(2) do |config| + config.vm.box = "ubuntu/trusty64" + + config.vm.define "docker-host-base" + + config.vm.provider "virtualbox" do |vb| + vb.name = "docker-host-base" + vb.gui = true + vb.memory = "1024" + end + + config.vm.provision "file", source: "config/docker-default", destination: "/home/vagrant/docker-default" + config.vm.provision "file", source: "config/xsessionrc", destination: "/home/vagrant/.xsessionrc" + + config.vm.provision "shell", inline: <<-SHELL + # Update and install apps + sudo apt-get update + sudo apt-get upgrade -y + sudo apt-get install -y --no-install-recommends ubuntu-desktop + sudo apt-get install -y gnome-terminal unity-lens-applications + + # Reset the ssh key (because vagrant regenerates it during provisioning) + sudo wget --no-check-certificate https://raw.github.com/mitchellh/vagrant/master/keys/vagrant.pub -O /home/vagrant/.ssh/authorized_keys + sudo chmod 0700 /home/vagrant/.ssh + sudo chmod 0600 /home/vagrant/.ssh/authorized_keys + sudo chown -R vagrant /home/vagrant/.ssh + + # Copy docker config + sudo mv /home/vagrant/docker-default /etc/default/docker + + # Enable autologin so docker can start GUI apps + sudo echo "autologin-user=vagrant" >> /usr/share/lightdm/lightdm.conf.d/50-unity-greeter.conf + sudo echo "autologin-user-timeout=0" >> /usr/share/lightdm/lightdm.conf.d/50-unity-greeter.conf + + # X session RC + chmod +x /home/vagrant/.xsessionrc + SHELL + + config.vm.provision "docker" + + # Shutdown after provisioning. "vagrant halt" doesn't recognize the original ssh key anymore + # and would just kill the VM. This might lead to FS inconsistencies (e.g. in the docker DB). + config.vm.provision "shell", inline: "sudo shutdown -h now" +end diff --git a/Tools/ros/vagrant/docker-host-base/config/docker-default b/Tools/ros/vagrant/docker-host-base/config/docker-default new file mode 100644 index 000000000..e8a86ce6a --- /dev/null +++ b/Tools/ros/vagrant/docker-host-base/config/docker-default @@ -0,0 +1,26 @@ +# +# Default config for docker /etc/default/docker +# Copied from a provisioned vagrant box +# +# Modifications: +# - listen to TCP port +# - removing deprecated "-r=true" option which apparently doesn't work anymore +# > use restart policies for specific containers if necessary +# + +# Docker Upstart and SysVinit configuration file + +# Customize location of Docker binary (especially for development testing). +#DOCKER="/usr/local/bin/docker" + +# Use DOCKER_OPTS to modify the daemon startup options. +#DOCKER_OPTS="--dns 8.8.8.8 --dns 8.8.4.4" + +# If you need Docker to use an HTTP proxy, it can also be specified here. +#export http_proxy="http://127.0.0.1:3128/" + +# This is also a handy place to tweak where Docker's temporary files go. +#export TMPDIR="/mnt/bigdrive/docker-tmp" + +# Expose TCP port in addition to socket +DOCKER_OPTS="${DOCKER_OPTS} -H unix:///var/run/docker.sock -H 0.0.0.0:2375" diff --git a/Tools/ros/vagrant/docker-host-base/config/xsessionrc b/Tools/ros/vagrant/docker-host-base/config/xsessionrc new file mode 100644 index 000000000..d8ab6d53d --- /dev/null +++ b/Tools/ros/vagrant/docker-host-base/config/xsessionrc @@ -0,0 +1,5 @@ +#!/bin/sh +# +# Disable X access control so we can easily start GUI apps +# +xhost + diff --git a/Tools/ros/vagrant/docker-host/Vagrantfile b/Tools/ros/vagrant/docker-host/Vagrantfile new file mode 100644 index 000000000..2e5fcb774 --- /dev/null +++ b/Tools/ros/vagrant/docker-host/Vagrantfile @@ -0,0 +1,25 @@ +# -*- mode: ruby -*- +# vi: set ft=ruby : + +Vagrant.configure(2) do |config| + config.vm.box = "docker-host-base" + + config.vm.define "docker-host" + + config.vm.provider "virtualbox" do |vb| + vb.name = "docker-host" + vb.gui = true + vb.memory = "4096" + vb.cpus = 2 + vb.customize ["modifyvm", :id, "--graphicscontroller", "vboxvga"] + vb.customize ["modifyvm", :id, "--accelerate3d", "on"] + vb.customize ["modifyvm", :id, "--ioapic", "on"] + vb.customize ["modifyvm", :id, "--vram", "128"] + vb.customize ["modifyvm", :id, "--hwvirtex", "on"] + end + + config.vm.network "private_network", ip: "192.168.59.104" + + # TBD: would it be better to provision docker here instead of in the base box? + #config.vm.provision "docker" +end diff --git a/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile b/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile new file mode 100644 index 000000000..5357ce94d --- /dev/null +++ b/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile @@ -0,0 +1,27 @@ +# -*- mode: ruby -*- +# vi: set ft=ruby : + +Vagrant.configure(2) do |config| + # Configure docker host + config.vm.provider "docker" do |d| + d.vagrant_machine = "docker-host" + d.vagrant_vagrantfile = "../docker-host/Vagrantfile" + end + + # Configure docker apps to run + config.vm.define "gazebo" do |app| + app.vm.provider "docker" do |d| + d.name = "gazebo" + d.image = "px4ros/ros-sitl:no-drcsim" + + # share docker host x11 socket + d.volumes = ["/tmp/.X11-unix:/tmp/.X11-unix:ro"] + # TODO: get display number from host system + d.env = { + "DISPLAY" => ":0" + } + + d.cmd = ["xterm"] + end + end +end -- cgit v1.2.3 From 05589e40dfb39e83681ac909545d20bdb96ca773 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 21 Jan 2015 00:01:55 +0100 Subject: updated docker instrumentalization and info --- Tools/ros/vagrant/px4-ros-sitl/Vagrantfile | 33 ++++++++++++++++++++++++++++-- 1 file changed, 31 insertions(+), 2 deletions(-) diff --git a/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile b/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile index 5357ce94d..5edf8f0ac 100644 --- a/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile +++ b/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile @@ -1,6 +1,14 @@ # -*- mode: ruby -*- # vi: set ft=ruby : +# +# Boot docker SITL environment +# +# Build (first time up): +# Use the "--no-parallel" option so the containers will be built in order. +# e.g.: "vagrant up --no-parallel" +# + Vagrant.configure(2) do |config| # Configure docker host config.vm.provider "docker" do |d| @@ -9,10 +17,31 @@ Vagrant.configure(2) do |config| end # Configure docker apps to run + config.vm.define "ros" do |app| + app.vm.provider "docker" do |d| + d.name = "ros" + #d.image = "px4ros/ros-base:no-drcsim" + d.build_dir = "../../docker/px4-ros" + d.build_args = ["-t=px4ros/ros-base:no-drcsim"] + + # share docker host x11 socket + d.volumes = ["/tmp/.X11-unix:/tmp/.X11-unix:ro"] + # TODO: get display number from host system + d.env = { + "DISPLAY" => ":0" + } + + d.cmd = ["echo", "Base image done"] + d.remains_running = false + end + end + config.vm.define "gazebo" do |app| app.vm.provider "docker" do |d| d.name = "gazebo" - d.image = "px4ros/ros-sitl:no-drcsim" + #d.image = "px4ros/ros-sitl" + d.build_dir = "../../docker/px4-ros/px4-ros-sitl" + d.build_args = ["-t=px4ros/ros-sitl:no-drcsim"] # share docker host x11 socket d.volumes = ["/tmp/.X11-unix:/tmp/.X11-unix:ro"] @@ -21,7 +50,7 @@ Vagrant.configure(2) do |config| "DISPLAY" => ":0" } - d.cmd = ["xterm"] + d.cmd = ["gazebo"] end end end -- cgit v1.2.3 From 0bfeedef89785646bbb05fd5583f18f42c905770 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 21 Jan 2015 14:54:55 +0100 Subject: updated build and notes --- Tools/ros/docker/px4-ros/Dockerfile | 16 +++++++++-- Tools/ros/vagrant/docker-host-base/Vagrantfile | 1 + Tools/ros/vagrant/docker-host/Vagrantfile | 3 ++ Tools/ros/vagrant/px4-ros-sitl/Vagrantfile | 38 ++++++++++++-------------- 4 files changed, 35 insertions(+), 23 deletions(-) diff --git a/Tools/ros/docker/px4-ros/Dockerfile b/Tools/ros/docker/px4-ros/Dockerfile index 532a9af8b..668d50799 100644 --- a/Tools/ros/docker/px4-ros/Dockerfile +++ b/Tools/ros/docker/px4-ros/Dockerfile @@ -6,8 +6,11 @@ FROM ubuntu:14.04.1 MAINTAINER Andreas Antener "andreas@antener.name" # Install basics +## Use the "noninteractive" debconf frontend +ENV DEBIAN_FRONTEND noninteractive + RUN apt-get update \ - && apt-get -y install wget git mercurial + && apt-get -y install wget git mercurial openssh-server # Main ROS Setup # Following http://wiki.ros.org/indigo/Installation/Ubuntu @@ -39,4 +42,13 @@ RUN apt-get -y install ros-indigo-octomap-msgs # && apt-get update \ # && apt-get -y install drcsim -CMD /bin/bash +# some QT-Apps/Gazebo don't not show controls without this +ENV QT_X11_NO_MITSHM 1 + +#RUN apt-get install -y openssh-server +# Install x11-utils to get xdpyinfo, for X11 display debugging +# mesa-utils provides glxinfo, handy for understanding the 3D support. +#RUN apt-get -y install x11-utils +#RUN apt-get -y install mesa-utils + +#CMD ["/bin/sbin/sshd", "-D"] diff --git a/Tools/ros/vagrant/docker-host-base/Vagrantfile b/Tools/ros/vagrant/docker-host-base/Vagrantfile index 5897735d0..516be528c 100644 --- a/Tools/ros/vagrant/docker-host-base/Vagrantfile +++ b/Tools/ros/vagrant/docker-host-base/Vagrantfile @@ -29,6 +29,7 @@ Vagrant.configure(2) do |config| config.vm.provision "shell", inline: <<-SHELL # Update and install apps + export DEBIAN_FRONTEND=noninteractive sudo apt-get update sudo apt-get upgrade -y sudo apt-get install -y --no-install-recommends ubuntu-desktop diff --git a/Tools/ros/vagrant/docker-host/Vagrantfile b/Tools/ros/vagrant/docker-host/Vagrantfile index 2e5fcb774..42e63567a 100644 --- a/Tools/ros/vagrant/docker-host/Vagrantfile +++ b/Tools/ros/vagrant/docker-host/Vagrantfile @@ -1,6 +1,9 @@ # -*- mode: ruby -*- # vi: set ft=ruby : +# +# Actual docker host VM to run. +# Vagrant.configure(2) do |config| config.vm.box = "docker-host-base" diff --git a/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile b/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile index 5edf8f0ac..d670e6250 100644 --- a/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile +++ b/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile @@ -4,10 +4,19 @@ # # Boot docker SITL environment # -# Build (first time up): +# "vagrant up" will build the images. Should start "xterm" in the end. +# +# Notes +# (will change, need proper docs) +# +# Build with multiple dependent docker containers: # Use the "--no-parallel" option so the containers will be built in order. # e.g.: "vagrant up --no-parallel" # +# Running apps directly: +# "vagrant docker-run ros -- " +# Attention: will loose all data when stopped, vagrant runs this with "--rm" +# Vagrant.configure(2) do |config| # Configure docker host @@ -25,32 +34,19 @@ Vagrant.configure(2) do |config| d.build_args = ["-t=px4ros/ros-base:no-drcsim"] # share docker host x11 socket - d.volumes = ["/tmp/.X11-unix:/tmp/.X11-unix:ro"] + d.volumes = [ + "/tmp/.X11-unix:/tmp/.X11-unix:ro", + "/dev/dri:/dev/dri" + ] # TODO: get display number from host system d.env = { "DISPLAY" => ":0" } - d.cmd = ["echo", "Base image done"] - d.remains_running = false + d.remains_running = true + d.cmd = ["xterm"] + #d.has_ssh = true end end - config.vm.define "gazebo" do |app| - app.vm.provider "docker" do |d| - d.name = "gazebo" - #d.image = "px4ros/ros-sitl" - d.build_dir = "../../docker/px4-ros/px4-ros-sitl" - d.build_args = ["-t=px4ros/ros-sitl:no-drcsim"] - - # share docker host x11 socket - d.volumes = ["/tmp/.X11-unix:/tmp/.X11-unix:ro"] - # TODO: get display number from host system - d.env = { - "DISPLAY" => ":0" - } - - d.cmd = ["gazebo"] - end - end end -- cgit v1.2.3 From 353c230db5927b3e060bbfde9a0aa34e27abbe35 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 22 Jan 2015 17:31:47 +0100 Subject: restructured things a little bit --- Tools/ros/docker/px4-ros/Dockerfile | 22 ++++++--- Tools/ros/docker/px4-ros/px4-ros-sitl/Dockerfile | 22 --------- .../ros/docker/px4-ros/px4-ros-sitl/get-sources.sh | 28 ----------- .../ros/docker/px4-ros/scripts/setup-workspace.sh | 43 +++++++++++++++++ Tools/ros/vagrant/px4-ros-sitl/Vagrantfile | 52 -------------------- Tools/ros/vagrant/px4-ros/Vagrantfile | 55 ++++++++++++++++++++++ 6 files changed, 113 insertions(+), 109 deletions(-) delete mode 100644 Tools/ros/docker/px4-ros/px4-ros-sitl/Dockerfile delete mode 100644 Tools/ros/docker/px4-ros/px4-ros-sitl/get-sources.sh create mode 100644 Tools/ros/docker/px4-ros/scripts/setup-workspace.sh delete mode 100644 Tools/ros/vagrant/px4-ros-sitl/Vagrantfile create mode 100644 Tools/ros/vagrant/px4-ros/Vagrantfile diff --git a/Tools/ros/docker/px4-ros/Dockerfile b/Tools/ros/docker/px4-ros/Dockerfile index 668d50799..74291bd14 100644 --- a/Tools/ros/docker/px4-ros/Dockerfile +++ b/Tools/ros/docker/px4-ros/Dockerfile @@ -1,16 +1,21 @@ # # Base ROS Indigo # +# TODO +# - use https://github.com/phusion/baseimage-docker as base +# - add user, best synced with host +# - configure ssh to work with vagrant out of the box +# FROM ubuntu:14.04.1 -MAINTAINER Andreas Antener "andreas@antener.name" +MAINTAINER Andreas Antener "andreas@uaventure.com" # Install basics ## Use the "noninteractive" debconf frontend ENV DEBIAN_FRONTEND noninteractive RUN apt-get update \ - && apt-get -y install wget git mercurial openssh-server + && apt-get -y install wget git mercurial # Main ROS Setup # Following http://wiki.ros.org/indigo/Installation/Ubuntu @@ -42,13 +47,16 @@ RUN apt-get -y install ros-indigo-octomap-msgs # && apt-get update \ # && apt-get -y install drcsim -# some QT-Apps/Gazebo don't not show controls without this -ENV QT_X11_NO_MITSHM 1 - -#RUN apt-get install -y openssh-server # Install x11-utils to get xdpyinfo, for X11 display debugging # mesa-utils provides glxinfo, handy for understanding the 3D support. #RUN apt-get -y install x11-utils #RUN apt-get -y install mesa-utils -#CMD ["/bin/sbin/sshd", "-D"] +# Some QT-Apps/Gazebo don't not show controls without this +ENV QT_X11_NO_MITSHM 1 + +COPY scripts/setup-workspace.sh ~/ +RUN chmod +x ~/setup-workspace.sh + + +CMD ["/usr/bin/xterm"] diff --git a/Tools/ros/docker/px4-ros/px4-ros-sitl/Dockerfile b/Tools/ros/docker/px4-ros/px4-ros-sitl/Dockerfile deleted file mode 100644 index 4c47295a8..000000000 --- a/Tools/ros/docker/px4-ros/px4-ros-sitl/Dockerfile +++ /dev/null @@ -1,22 +0,0 @@ -# -# PX4 ROS SITL -# - -FROM px4ros/ros-base:no-drcsim -MAINTAINER Andreas Antener "andreas@antener.name" - -# TODO -#USER px4 - -RUN . /opt/ros/indigo/setup.sh \ - && mkdir -p /catkin_ws/src \ - && cd /catkin_ws/src \ - && catkin_init_workspace \ - && cd /catkin_ws \ - && catkin_make \ - && echo "source /catkin_ws/devel/setup.bash" >> ~/.bashrc - -COPY get-sources.sh /catkin_ws/src/ -RUN chmod +x /catkin_ws/src/get-sources.sh - -CMD /bin/bash diff --git a/Tools/ros/docker/px4-ros/px4-ros-sitl/get-sources.sh b/Tools/ros/docker/px4-ros/px4-ros-sitl/get-sources.sh deleted file mode 100644 index 237ae7500..000000000 --- a/Tools/ros/docker/px4-ros/px4-ros-sitl/get-sources.sh +++ /dev/null @@ -1,28 +0,0 @@ -#!/bin/sh -# -# Fetch source repositories -# - -# PX4 Firmware -git clone https://github.com/PX4/Firmware.git \ - && cd Firmware \ - && git checkout ros \ - && cd .. - -# euroc simulator -git clone https://github.com/PX4/euroc_simulator.git \ - && cd euroc_simulator \ - && git checkout px4_nodes \ - && cd .. - -# mav comm -git clone https://github.com/PX4/mav_comm.git - -# glog catkin -git clone https://github.com/ethz-asl/glog_catkin.git - -# catkin simple -git clone https://github.com/catkin/catkin_simple.git - -echo "Execute catkin_make to compile all the sources." - diff --git a/Tools/ros/docker/px4-ros/scripts/setup-workspace.sh b/Tools/ros/docker/px4-ros/scripts/setup-workspace.sh new file mode 100644 index 000000000..6b5ddb0a5 --- /dev/null +++ b/Tools/ros/docker/px4-ros/scripts/setup-workspace.sh @@ -0,0 +1,43 @@ +#!/bin/sh +# +# Create workspace at current location and fetch source repositories +# + +WDIR=`pwd` +WORKSPACE=$WDIR/catkin_ws + +# Setup workspace +mkdir -p $WORKSPACE/src +cd $WORKSPACE/src +catkin_init_workspace +cd $WORKSPACE +catkin_make +echo "source $WORKSPACE/devel/setup.bash" >> ~/.bashrc + +# PX4 Firmware +cd $WORKSPACE/src +git clone https://github.com/PX4/Firmware.git \ + && cd Firmware \ + && git checkout ros + +# euroc simulator +cd $WORKSPACE/src +git clone https://github.com/PX4/euroc_simulator.git \ + && cd euroc_simulator \ + && git checkout px4_nodes + +# mav comm +cd $WORKSPACE/src +git clone https://github.com/PX4/mav_comm.git + +# glog catkin +cd $WORKSPACE/src +git clone https://github.com/ethz-asl/glog_catkin.git + +# catkin simple +cd $WORKSPACE/src +git clone https://github.com/catkin/catkin_simple.git + +cd $WORKSPACE +echo "Execute catkin_make to compile all the sources." + diff --git a/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile b/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile deleted file mode 100644 index d670e6250..000000000 --- a/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile +++ /dev/null @@ -1,52 +0,0 @@ -# -*- mode: ruby -*- -# vi: set ft=ruby : - -# -# Boot docker SITL environment -# -# "vagrant up" will build the images. Should start "xterm" in the end. -# -# Notes -# (will change, need proper docs) -# -# Build with multiple dependent docker containers: -# Use the "--no-parallel" option so the containers will be built in order. -# e.g.: "vagrant up --no-parallel" -# -# Running apps directly: -# "vagrant docker-run ros -- " -# Attention: will loose all data when stopped, vagrant runs this with "--rm" -# - -Vagrant.configure(2) do |config| - # Configure docker host - config.vm.provider "docker" do |d| - d.vagrant_machine = "docker-host" - d.vagrant_vagrantfile = "../docker-host/Vagrantfile" - end - - # Configure docker apps to run - config.vm.define "ros" do |app| - app.vm.provider "docker" do |d| - d.name = "ros" - #d.image = "px4ros/ros-base:no-drcsim" - d.build_dir = "../../docker/px4-ros" - d.build_args = ["-t=px4ros/ros-base:no-drcsim"] - - # share docker host x11 socket - d.volumes = [ - "/tmp/.X11-unix:/tmp/.X11-unix:ro", - "/dev/dri:/dev/dri" - ] - # TODO: get display number from host system - d.env = { - "DISPLAY" => ":0" - } - - d.remains_running = true - d.cmd = ["xterm"] - #d.has_ssh = true - end - end - -end diff --git a/Tools/ros/vagrant/px4-ros/Vagrantfile b/Tools/ros/vagrant/px4-ros/Vagrantfile new file mode 100644 index 000000000..b5169e061 --- /dev/null +++ b/Tools/ros/vagrant/px4-ros/Vagrantfile @@ -0,0 +1,55 @@ +# -*- mode: ruby -*- +# vi: set ft=ruby : + +# +# Boot docker SITL environment +# +# "vagrant up" will build the images. Should start "xterm" in the end. +# +# Notes: +# (will change, need proper docs) +# +# Build with multiple dependent docker containers: +# Use the "--no-parallel" option so the containers will be built in order. +# e.g.: "vagrant up --no-parallel" +# +# Running apps directly: +# "vagrant docker-run ros -- " +# Attention: will loose all data when stopped, vagrant runs this with "--rm" +# +# TODO +# - maybe map a local working directory to compile stuff without loosing it in side the docker container +# + +Vagrant.configure(2) do |config| + # Configure docker host + config.vm.provider "docker" do |d| + d.vagrant_machine = "docker-host" + d.vagrant_vagrantfile = "../docker-host/Vagrantfile" + end + + # Configure docker apps to run + config.vm.define "ros" do |app| + app.vm.provider "docker" do |d| + d.name = "ros" + #d.image = "px4ros/ros-base:no-drcsim" + d.build_dir = "../../docker/px4-ros" + d.build_args = ["-t=px4ros/ros-base:no-drcsim"] + + # share docker host x11 socket + d.volumes = [ + "/tmp/.X11-unix:/tmp/.X11-unix:ro", + "/dev/dri:/dev/dri" + ] + # TODO: get display number from host system + d.env = { + "DISPLAY" => ":0" + } + + d.remains_running = true + d.cmd = ["xterm"] + #d.has_ssh = true + end + end + +end -- cgit v1.2.3 From da8c9af37b2aa183499fb8e2bb205633bce92ded Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 22 Jan 2015 18:30:15 +0100 Subject: fixed some stuff --- Tools/ros/docker/px4-ros/Dockerfile | 4 ++-- Tools/ros/docker/px4-ros/scripts/setup-workspace.sh | 2 +- Tools/ros/vagrant/px4-ros/Vagrantfile | 1 + 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/Tools/ros/docker/px4-ros/Dockerfile b/Tools/ros/docker/px4-ros/Dockerfile index 74291bd14..4b553fa66 100644 --- a/Tools/ros/docker/px4-ros/Dockerfile +++ b/Tools/ros/docker/px4-ros/Dockerfile @@ -55,8 +55,8 @@ RUN apt-get -y install ros-indigo-octomap-msgs # Some QT-Apps/Gazebo don't not show controls without this ENV QT_X11_NO_MITSHM 1 -COPY scripts/setup-workspace.sh ~/ -RUN chmod +x ~/setup-workspace.sh +COPY scripts/setup-workspace.sh /root/scripts +RUN chmod +x /root/setup-workspace.sh CMD ["/usr/bin/xterm"] diff --git a/Tools/ros/docker/px4-ros/scripts/setup-workspace.sh b/Tools/ros/docker/px4-ros/scripts/setup-workspace.sh index 6b5ddb0a5..231166e27 100644 --- a/Tools/ros/docker/px4-ros/scripts/setup-workspace.sh +++ b/Tools/ros/docker/px4-ros/scripts/setup-workspace.sh @@ -39,5 +39,5 @@ cd $WORKSPACE/src git clone https://github.com/catkin/catkin_simple.git cd $WORKSPACE -echo "Execute catkin_make to compile all the sources." +catkin_make diff --git a/Tools/ros/vagrant/px4-ros/Vagrantfile b/Tools/ros/vagrant/px4-ros/Vagrantfile index b5169e061..e54370ada 100644 --- a/Tools/ros/vagrant/px4-ros/Vagrantfile +++ b/Tools/ros/vagrant/px4-ros/Vagrantfile @@ -18,6 +18,7 @@ # Attention: will loose all data when stopped, vagrant runs this with "--rm" # # TODO +# - do not run the docker container with "--rm" (vagrant default). is that even possible? # - maybe map a local working directory to compile stuff without loosing it in side the docker container # -- cgit v1.2.3 From 4fda482a7385e132df1064d6380b762c6b11710e Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 22 Jan 2015 18:53:36 +0100 Subject: updated copy instruction --- Tools/ros/docker/px4-ros/Dockerfile | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Tools/ros/docker/px4-ros/Dockerfile b/Tools/ros/docker/px4-ros/Dockerfile index 4b553fa66..d896c76e7 100644 --- a/Tools/ros/docker/px4-ros/Dockerfile +++ b/Tools/ros/docker/px4-ros/Dockerfile @@ -55,8 +55,9 @@ RUN apt-get -y install ros-indigo-octomap-msgs # Some QT-Apps/Gazebo don't not show controls without this ENV QT_X11_NO_MITSHM 1 +# FIXME: this doesn't work yet when building from vagrant COPY scripts/setup-workspace.sh /root/scripts -RUN chmod +x /root/setup-workspace.sh +#RUN chmod +x /root/scripts/setup-workspace.sh CMD ["/usr/bin/xterm"] -- cgit v1.2.3 From 6db56b8df08e6dfe4670e1e10a3fb391470f32b4 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 22 Jan 2015 11:24:46 -0800 Subject: added privileged options to support hardware acceleration --- Tools/ros/vagrant/px4-ros/Vagrantfile | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/Tools/ros/vagrant/px4-ros/Vagrantfile b/Tools/ros/vagrant/px4-ros/Vagrantfile index e54370ada..ffe3a00ea 100644 --- a/Tools/ros/vagrant/px4-ros/Vagrantfile +++ b/Tools/ros/vagrant/px4-ros/Vagrantfile @@ -37,11 +37,13 @@ Vagrant.configure(2) do |config| d.build_dir = "../../docker/px4-ros" d.build_args = ["-t=px4ros/ros-base:no-drcsim"] - # share docker host x11 socket + # Share docker host x11 socket + # Run privileged to support 3d acceleration d.volumes = [ - "/tmp/.X11-unix:/tmp/.X11-unix:ro", - "/dev/dri:/dev/dri" + "/tmp/.X11-unix:/tmp/.X11-unix:ro" ] + d.create_args = ["--privileged"] + # TODO: get display number from host system d.env = { "DISPLAY" => ":0" -- cgit v1.2.3 From 6ab1f2168059bb7689548635ab4e746c8f320295 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 22 Jan 2015 21:53:11 +0100 Subject: updated setup, added maintainer --- Tools/ros/docker/px4-ros-base/Dockerfile | 62 +++++++++++++++++++++ .../docker/px4-ros-base/scripts/setup-workspace.sh | 43 +++++++++++++++ Tools/ros/docker/px4-ros/Dockerfile | 63 ---------------------- .../ros/docker/px4-ros/scripts/setup-workspace.sh | 43 --------------- Tools/ros/vagrant/docker-host-base/Vagrantfile | 2 + Tools/ros/vagrant/docker-host/Vagrantfile | 2 + Tools/ros/vagrant/px4-ros/Vagrantfile | 8 +-- 7 files changed, 114 insertions(+), 109 deletions(-) create mode 100644 Tools/ros/docker/px4-ros-base/Dockerfile create mode 100644 Tools/ros/docker/px4-ros-base/scripts/setup-workspace.sh delete mode 100644 Tools/ros/docker/px4-ros/Dockerfile delete mode 100644 Tools/ros/docker/px4-ros/scripts/setup-workspace.sh diff --git a/Tools/ros/docker/px4-ros-base/Dockerfile b/Tools/ros/docker/px4-ros-base/Dockerfile new file mode 100644 index 000000000..a8c377288 --- /dev/null +++ b/Tools/ros/docker/px4-ros-base/Dockerfile @@ -0,0 +1,62 @@ +# +# PX4 ROS base container +# +# TODO +# - use https://github.com/phusion/baseimage-docker as base +# - add user, best synced with host +# - configure ssh to work with vagrant out of the box +# + +FROM ubuntu:14.04.1 +MAINTAINER Andreas Antener + +# Install basics +## Use the "noninteractive" debconf frontend +ENV DEBIAN_FRONTEND noninteractive + +RUN apt-get update \ + && apt-get -y install wget git mercurial + +# Main ROS Setup +# Following http://wiki.ros.org/indigo/Installation/Ubuntu +# Also adding dependencies for gazebo http://gazebosim.org/tutorials?tut=drcsim_install + +## add ROS repositories and keys +## install main ROS pacakges +RUN echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list \ + && wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | apt-key add - \ + && apt-get update \ + && apt-get -y install ros-indigo-desktop-full + +RUN rosdep init \ + && rosdep update + +## setup environment variables +RUN echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc + +## get rosinstall +RUN apt-get -y install python-rosinstall + +## additional dependencies +RUN apt-get -y install ros-indigo-octomap-msgs + + +## install drcsim +#RUN echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" > /etc/apt/sources.list.d/drc-latest.list \ +# && wget http://packages.osrfoundation.org/drc.key -O - | apt-key add - \ +# && apt-get update \ +# && apt-get -y install drcsim + +# Install x11-utils to get xdpyinfo, for X11 display debugging +# mesa-utils provides glxinfo, handy for understanding the 3D support +RUN apt-get -y install x11-utils mesa-utils + +# Some QT-Apps/Gazebo don't not show controls without this +ENV QT_X11_NO_MITSHM 1 + +# FIXME: this doesn't work when building from vagrant +COPY scripts/setup-workspace.sh /root/scripts +#RUN chmod +x /root/scripts/setup-workspace.sh + + +CMD ["/usr/bin/xterm"] diff --git a/Tools/ros/docker/px4-ros-base/scripts/setup-workspace.sh b/Tools/ros/docker/px4-ros-base/scripts/setup-workspace.sh new file mode 100644 index 000000000..231166e27 --- /dev/null +++ b/Tools/ros/docker/px4-ros-base/scripts/setup-workspace.sh @@ -0,0 +1,43 @@ +#!/bin/sh +# +# Create workspace at current location and fetch source repositories +# + +WDIR=`pwd` +WORKSPACE=$WDIR/catkin_ws + +# Setup workspace +mkdir -p $WORKSPACE/src +cd $WORKSPACE/src +catkin_init_workspace +cd $WORKSPACE +catkin_make +echo "source $WORKSPACE/devel/setup.bash" >> ~/.bashrc + +# PX4 Firmware +cd $WORKSPACE/src +git clone https://github.com/PX4/Firmware.git \ + && cd Firmware \ + && git checkout ros + +# euroc simulator +cd $WORKSPACE/src +git clone https://github.com/PX4/euroc_simulator.git \ + && cd euroc_simulator \ + && git checkout px4_nodes + +# mav comm +cd $WORKSPACE/src +git clone https://github.com/PX4/mav_comm.git + +# glog catkin +cd $WORKSPACE/src +git clone https://github.com/ethz-asl/glog_catkin.git + +# catkin simple +cd $WORKSPACE/src +git clone https://github.com/catkin/catkin_simple.git + +cd $WORKSPACE +catkin_make + diff --git a/Tools/ros/docker/px4-ros/Dockerfile b/Tools/ros/docker/px4-ros/Dockerfile deleted file mode 100644 index d896c76e7..000000000 --- a/Tools/ros/docker/px4-ros/Dockerfile +++ /dev/null @@ -1,63 +0,0 @@ -# -# Base ROS Indigo -# -# TODO -# - use https://github.com/phusion/baseimage-docker as base -# - add user, best synced with host -# - configure ssh to work with vagrant out of the box -# - -FROM ubuntu:14.04.1 -MAINTAINER Andreas Antener "andreas@uaventure.com" - -# Install basics -## Use the "noninteractive" debconf frontend -ENV DEBIAN_FRONTEND noninteractive - -RUN apt-get update \ - && apt-get -y install wget git mercurial - -# Main ROS Setup -# Following http://wiki.ros.org/indigo/Installation/Ubuntu -# Also adding dependencies for gazebo http://gazebosim.org/tutorials?tut=drcsim_install - -## add ROS repositories and keys -## install main ROS pacakges -RUN echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list \ - && wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | apt-key add - \ - && apt-get update \ - && apt-get -y install ros-indigo-desktop-full - -RUN rosdep init \ - && rosdep update - -## setup environment variables -RUN echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc - -## get rosinstall -RUN apt-get -y install python-rosinstall - -## additional dependencies -RUN apt-get -y install ros-indigo-octomap-msgs - - -## install drcsim -#RUN echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" > /etc/apt/sources.list.d/drc-latest.list \ -# && wget http://packages.osrfoundation.org/drc.key -O - | apt-key add - \ -# && apt-get update \ -# && apt-get -y install drcsim - -# Install x11-utils to get xdpyinfo, for X11 display debugging -# mesa-utils provides glxinfo, handy for understanding the 3D support. -#RUN apt-get -y install x11-utils -#RUN apt-get -y install mesa-utils - -# Some QT-Apps/Gazebo don't not show controls without this -ENV QT_X11_NO_MITSHM 1 - -# FIXME: this doesn't work yet when building from vagrant -COPY scripts/setup-workspace.sh /root/scripts -#RUN chmod +x /root/scripts/setup-workspace.sh - - -CMD ["/usr/bin/xterm"] diff --git a/Tools/ros/docker/px4-ros/scripts/setup-workspace.sh b/Tools/ros/docker/px4-ros/scripts/setup-workspace.sh deleted file mode 100644 index 231166e27..000000000 --- a/Tools/ros/docker/px4-ros/scripts/setup-workspace.sh +++ /dev/null @@ -1,43 +0,0 @@ -#!/bin/sh -# -# Create workspace at current location and fetch source repositories -# - -WDIR=`pwd` -WORKSPACE=$WDIR/catkin_ws - -# Setup workspace -mkdir -p $WORKSPACE/src -cd $WORKSPACE/src -catkin_init_workspace -cd $WORKSPACE -catkin_make -echo "source $WORKSPACE/devel/setup.bash" >> ~/.bashrc - -# PX4 Firmware -cd $WORKSPACE/src -git clone https://github.com/PX4/Firmware.git \ - && cd Firmware \ - && git checkout ros - -# euroc simulator -cd $WORKSPACE/src -git clone https://github.com/PX4/euroc_simulator.git \ - && cd euroc_simulator \ - && git checkout px4_nodes - -# mav comm -cd $WORKSPACE/src -git clone https://github.com/PX4/mav_comm.git - -# glog catkin -cd $WORKSPACE/src -git clone https://github.com/ethz-asl/glog_catkin.git - -# catkin simple -cd $WORKSPACE/src -git clone https://github.com/catkin/catkin_simple.git - -cd $WORKSPACE -catkin_make - diff --git a/Tools/ros/vagrant/docker-host-base/Vagrantfile b/Tools/ros/vagrant/docker-host-base/Vagrantfile index 516be528c..cb66d4a68 100644 --- a/Tools/ros/vagrant/docker-host-base/Vagrantfile +++ b/Tools/ros/vagrant/docker-host-base/Vagrantfile @@ -4,6 +4,8 @@ # # Vagrantfile to create docker-host-base # +# Maintainer: Andreas Antener +# # After build, do "vagrant package --base docker-host-base" to package, # and import as box: "vagrant box add --name docker-host-base package.box" # diff --git a/Tools/ros/vagrant/docker-host/Vagrantfile b/Tools/ros/vagrant/docker-host/Vagrantfile index 42e63567a..9fd4695dd 100644 --- a/Tools/ros/vagrant/docker-host/Vagrantfile +++ b/Tools/ros/vagrant/docker-host/Vagrantfile @@ -4,6 +4,8 @@ # # Actual docker host VM to run. # +# Maintainer: Andreas Antener +# Vagrant.configure(2) do |config| config.vm.box = "docker-host-base" diff --git a/Tools/ros/vagrant/px4-ros/Vagrantfile b/Tools/ros/vagrant/px4-ros/Vagrantfile index ffe3a00ea..7009fd78a 100644 --- a/Tools/ros/vagrant/px4-ros/Vagrantfile +++ b/Tools/ros/vagrant/px4-ros/Vagrantfile @@ -4,7 +4,9 @@ # # Boot docker SITL environment # -# "vagrant up" will build the images. Should start "xterm" in the end. +# Maintainer: Andreas Antener +# +# "vagrant up" will build the images. Should eventually start "xterm" from within the docker container. # # Notes: # (will change, need proper docs) @@ -33,8 +35,8 @@ Vagrant.configure(2) do |config| config.vm.define "ros" do |app| app.vm.provider "docker" do |d| d.name = "ros" - #d.image = "px4ros/ros-base:no-drcsim" - d.build_dir = "../../docker/px4-ros" + #d.image = "px4ros/ros-base" + d.build_dir = "../../docker/px4-ros-base" d.build_args = ["-t=px4ros/ros-base:no-drcsim"] # Share docker host x11 socket -- cgit v1.2.3 From aae96a1b1631677dd07398e1134f77d4c5e38f08 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 23 Jan 2015 10:51:13 +0100 Subject: updated docs, fixed script copy, renamed stuff --- Tools/ros/docker/px4-ros-base/Dockerfile | 62 ---------------------- .../docker/px4-ros-base/scripts/setup-workspace.sh | 43 --------------- Tools/ros/docker/px4-ros-full/Dockerfile | 62 ++++++++++++++++++++++ .../docker/px4-ros-full/scripts/setup-workspace.sh | 43 +++++++++++++++ Tools/ros/vagrant/docker-host-base/Vagrantfile | 6 --- Tools/ros/vagrant/docker-host/Vagrantfile | 6 +++ Tools/ros/vagrant/px4-ros/Vagrantfile | 10 ++-- 7 files changed, 116 insertions(+), 116 deletions(-) delete mode 100644 Tools/ros/docker/px4-ros-base/Dockerfile delete mode 100644 Tools/ros/docker/px4-ros-base/scripts/setup-workspace.sh create mode 100644 Tools/ros/docker/px4-ros-full/Dockerfile create mode 100644 Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh diff --git a/Tools/ros/docker/px4-ros-base/Dockerfile b/Tools/ros/docker/px4-ros-base/Dockerfile deleted file mode 100644 index a8c377288..000000000 --- a/Tools/ros/docker/px4-ros-base/Dockerfile +++ /dev/null @@ -1,62 +0,0 @@ -# -# PX4 ROS base container -# -# TODO -# - use https://github.com/phusion/baseimage-docker as base -# - add user, best synced with host -# - configure ssh to work with vagrant out of the box -# - -FROM ubuntu:14.04.1 -MAINTAINER Andreas Antener - -# Install basics -## Use the "noninteractive" debconf frontend -ENV DEBIAN_FRONTEND noninteractive - -RUN apt-get update \ - && apt-get -y install wget git mercurial - -# Main ROS Setup -# Following http://wiki.ros.org/indigo/Installation/Ubuntu -# Also adding dependencies for gazebo http://gazebosim.org/tutorials?tut=drcsim_install - -## add ROS repositories and keys -## install main ROS pacakges -RUN echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list \ - && wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | apt-key add - \ - && apt-get update \ - && apt-get -y install ros-indigo-desktop-full - -RUN rosdep init \ - && rosdep update - -## setup environment variables -RUN echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc - -## get rosinstall -RUN apt-get -y install python-rosinstall - -## additional dependencies -RUN apt-get -y install ros-indigo-octomap-msgs - - -## install drcsim -#RUN echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" > /etc/apt/sources.list.d/drc-latest.list \ -# && wget http://packages.osrfoundation.org/drc.key -O - | apt-key add - \ -# && apt-get update \ -# && apt-get -y install drcsim - -# Install x11-utils to get xdpyinfo, for X11 display debugging -# mesa-utils provides glxinfo, handy for understanding the 3D support -RUN apt-get -y install x11-utils mesa-utils - -# Some QT-Apps/Gazebo don't not show controls without this -ENV QT_X11_NO_MITSHM 1 - -# FIXME: this doesn't work when building from vagrant -COPY scripts/setup-workspace.sh /root/scripts -#RUN chmod +x /root/scripts/setup-workspace.sh - - -CMD ["/usr/bin/xterm"] diff --git a/Tools/ros/docker/px4-ros-base/scripts/setup-workspace.sh b/Tools/ros/docker/px4-ros-base/scripts/setup-workspace.sh deleted file mode 100644 index 231166e27..000000000 --- a/Tools/ros/docker/px4-ros-base/scripts/setup-workspace.sh +++ /dev/null @@ -1,43 +0,0 @@ -#!/bin/sh -# -# Create workspace at current location and fetch source repositories -# - -WDIR=`pwd` -WORKSPACE=$WDIR/catkin_ws - -# Setup workspace -mkdir -p $WORKSPACE/src -cd $WORKSPACE/src -catkin_init_workspace -cd $WORKSPACE -catkin_make -echo "source $WORKSPACE/devel/setup.bash" >> ~/.bashrc - -# PX4 Firmware -cd $WORKSPACE/src -git clone https://github.com/PX4/Firmware.git \ - && cd Firmware \ - && git checkout ros - -# euroc simulator -cd $WORKSPACE/src -git clone https://github.com/PX4/euroc_simulator.git \ - && cd euroc_simulator \ - && git checkout px4_nodes - -# mav comm -cd $WORKSPACE/src -git clone https://github.com/PX4/mav_comm.git - -# glog catkin -cd $WORKSPACE/src -git clone https://github.com/ethz-asl/glog_catkin.git - -# catkin simple -cd $WORKSPACE/src -git clone https://github.com/catkin/catkin_simple.git - -cd $WORKSPACE -catkin_make - diff --git a/Tools/ros/docker/px4-ros-full/Dockerfile b/Tools/ros/docker/px4-ros-full/Dockerfile new file mode 100644 index 000000000..be98cba58 --- /dev/null +++ b/Tools/ros/docker/px4-ros-full/Dockerfile @@ -0,0 +1,62 @@ +# +# PX4 full ROS container +# +# TODO +# - use https://github.com/phusion/baseimage-docker as base +# - add user, best synced with host +# - configure ssh to work with vagrant out of the box +# + +FROM ubuntu:14.04.1 +MAINTAINER Andreas Antener + +# Install basics +## Use the "noninteractive" debconf frontend +ENV DEBIAN_FRONTEND noninteractive + +RUN apt-get update \ + && apt-get -y install wget git mercurial + +# Main ROS Setup +# Following http://wiki.ros.org/indigo/Installation/Ubuntu +# Also adding dependencies for gazebo http://gazebosim.org/tutorials?tut=drcsim_install + +## add ROS repositories and keys +## install main ROS pacakges +RUN echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list \ + && wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | apt-key add - \ + && apt-get update \ + && apt-get -y install ros-indigo-desktop-full + +RUN rosdep init \ + && rosdep update + +## setup environment variables +RUN echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc + +## get rosinstall +RUN apt-get -y install python-rosinstall + +## additional dependencies +RUN apt-get -y install ros-indigo-octomap-msgs + + +## install drcsim +#RUN echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" > /etc/apt/sources.list.d/drc-latest.list \ +# && wget http://packages.osrfoundation.org/drc.key -O - | apt-key add - \ +# && apt-get update \ +# && apt-get -y install drcsim + +# Install x11-utils to get xdpyinfo, for X11 display debugging +# mesa-utils provides glxinfo, handy for understanding the 3D support +RUN apt-get -y install x11-utils mesa-utils + +# Some QT-Apps/Gazebo don't not show controls without this +ENV QT_X11_NO_MITSHM 1 + +# FIXME: this doesn't work when building from vagrant +COPY scripts/setup-workspace.sh /root/scripts/ +RUN chmod +x -R /root/scripts/* +RUN chown -R root:root /root/scripts/* + +CMD ["/usr/bin/xterm"] diff --git a/Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh b/Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh new file mode 100644 index 000000000..231166e27 --- /dev/null +++ b/Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh @@ -0,0 +1,43 @@ +#!/bin/sh +# +# Create workspace at current location and fetch source repositories +# + +WDIR=`pwd` +WORKSPACE=$WDIR/catkin_ws + +# Setup workspace +mkdir -p $WORKSPACE/src +cd $WORKSPACE/src +catkin_init_workspace +cd $WORKSPACE +catkin_make +echo "source $WORKSPACE/devel/setup.bash" >> ~/.bashrc + +# PX4 Firmware +cd $WORKSPACE/src +git clone https://github.com/PX4/Firmware.git \ + && cd Firmware \ + && git checkout ros + +# euroc simulator +cd $WORKSPACE/src +git clone https://github.com/PX4/euroc_simulator.git \ + && cd euroc_simulator \ + && git checkout px4_nodes + +# mav comm +cd $WORKSPACE/src +git clone https://github.com/PX4/mav_comm.git + +# glog catkin +cd $WORKSPACE/src +git clone https://github.com/ethz-asl/glog_catkin.git + +# catkin simple +cd $WORKSPACE/src +git clone https://github.com/catkin/catkin_simple.git + +cd $WORKSPACE +catkin_make + diff --git a/Tools/ros/vagrant/docker-host-base/Vagrantfile b/Tools/ros/vagrant/docker-host-base/Vagrantfile index cb66d4a68..196a8fb12 100644 --- a/Tools/ros/vagrant/docker-host-base/Vagrantfile +++ b/Tools/ros/vagrant/docker-host-base/Vagrantfile @@ -9,12 +9,6 @@ # After build, do "vagrant package --base docker-host-base" to package, # and import as box: "vagrant box add --name docker-host-base package.box" # -# To add local docker images into the docker host, configure your local -# docker client to control the docker daemon on the later running docker-host VM. -# This box configures docker to listen on any IP on port 2375. -# You can then also load an existing image, e.g.: -# "docker load -i px4ros_ros-sitl_no-drcsim_a4209708a04a.tar" -# Vagrant.configure(2) do |config| config.vm.box = "ubuntu/trusty64" diff --git a/Tools/ros/vagrant/docker-host/Vagrantfile b/Tools/ros/vagrant/docker-host/Vagrantfile index 9fd4695dd..2b85bf8e9 100644 --- a/Tools/ros/vagrant/docker-host/Vagrantfile +++ b/Tools/ros/vagrant/docker-host/Vagrantfile @@ -6,6 +6,12 @@ # # Maintainer: Andreas Antener # +# To add local docker images into the docker host, configure your local +# docker client to control the docker daemon on the running "docker-host" VM. +# The box ("docker-host-base") configures docker to listen on any IP on port 2375. +# You can then load an existing image, e.g.: +# "docker load -i container-image.tar" +# Vagrant.configure(2) do |config| config.vm.box = "docker-host-base" diff --git a/Tools/ros/vagrant/px4-ros/Vagrantfile b/Tools/ros/vagrant/px4-ros/Vagrantfile index 7009fd78a..a122fb4a3 100644 --- a/Tools/ros/vagrant/px4-ros/Vagrantfile +++ b/Tools/ros/vagrant/px4-ros/Vagrantfile @@ -12,12 +12,12 @@ # (will change, need proper docs) # # Build with multiple dependent docker containers: -# Use the "--no-parallel" option so the containers will be built in order. +# Use the "--no-parallel" option so the containers will be built/started in order. # e.g.: "vagrant up --no-parallel" # # Running apps directly: # "vagrant docker-run ros -- " -# Attention: will loose all data when stopped, vagrant runs this with "--rm" +# Attention: will loose all data when stopped, vagrant runs docker always with "--rm" # # TODO # - do not run the docker container with "--rm" (vagrant default). is that even possible? @@ -35,9 +35,9 @@ Vagrant.configure(2) do |config| config.vm.define "ros" do |app| app.vm.provider "docker" do |d| d.name = "ros" - #d.image = "px4ros/ros-base" - d.build_dir = "../../docker/px4-ros-base" - d.build_args = ["-t=px4ros/ros-base:no-drcsim"] + d.image = "uaventure/px4-ros-full" + #d.build_dir = "../../docker/px4-ros-full" + #d.build_args = ["-t=uaventure/px4-ros-full"] # Share docker host x11 socket # Run privileged to support 3d acceleration -- cgit v1.2.3 From c4b309c06e0063559ac8c2fce736f3296875ea5a Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 23 Jan 2015 10:56:25 +0100 Subject: added readme for docker container --- Tools/ros/docker/px4-ros-full/Dockerfile | 5 ----- Tools/ros/docker/px4-ros-full/README.md | 10 ++++++++++ 2 files changed, 10 insertions(+), 5 deletions(-) create mode 100644 Tools/ros/docker/px4-ros-full/README.md diff --git a/Tools/ros/docker/px4-ros-full/Dockerfile b/Tools/ros/docker/px4-ros-full/Dockerfile index be98cba58..c60f66539 100644 --- a/Tools/ros/docker/px4-ros-full/Dockerfile +++ b/Tools/ros/docker/px4-ros-full/Dockerfile @@ -1,11 +1,6 @@ # # PX4 full ROS container # -# TODO -# - use https://github.com/phusion/baseimage-docker as base -# - add user, best synced with host -# - configure ssh to work with vagrant out of the box -# FROM ubuntu:14.04.1 MAINTAINER Andreas Antener diff --git a/Tools/ros/docker/px4-ros-full/README.md b/Tools/ros/docker/px4-ros-full/README.md new file mode 100644 index 000000000..7d75754d9 --- /dev/null +++ b/Tools/ros/docker/px4-ros-full/README.md @@ -0,0 +1,10 @@ +# PX4 ROS # + +Full desktop ROS container. + +**TODO:** + +- use https://github.com/phusion/baseimage-docker as base +- add user, best synced with host +- configure ssh to work with vagrant out of the box + -- cgit v1.2.3 From 700ca3c7d0b49b9c9f2403a630c3335c518ebddc Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 23 Jan 2015 11:53:54 +0100 Subject: naming updates --- Tools/ros/vagrant/docker-host-base/Vagrantfile | 2 +- Tools/ros/vagrant/docker-host/Vagrantfile | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/ros/vagrant/docker-host-base/Vagrantfile b/Tools/ros/vagrant/docker-host-base/Vagrantfile index 196a8fb12..bc9329bf8 100644 --- a/Tools/ros/vagrant/docker-host-base/Vagrantfile +++ b/Tools/ros/vagrant/docker-host-base/Vagrantfile @@ -7,7 +7,7 @@ # Maintainer: Andreas Antener # # After build, do "vagrant package --base docker-host-base" to package, -# and import as box: "vagrant box add --name docker-host-base package.box" +# and import as box: "vagrant box add --name uaventure/docker-host-base package.box" # Vagrant.configure(2) do |config| config.vm.box = "ubuntu/trusty64" diff --git a/Tools/ros/vagrant/docker-host/Vagrantfile b/Tools/ros/vagrant/docker-host/Vagrantfile index 2b85bf8e9..4ae94258d 100644 --- a/Tools/ros/vagrant/docker-host/Vagrantfile +++ b/Tools/ros/vagrant/docker-host/Vagrantfile @@ -13,7 +13,7 @@ # "docker load -i container-image.tar" # Vagrant.configure(2) do |config| - config.vm.box = "docker-host-base" + config.vm.box = "uaventure/docker-host-base" config.vm.define "docker-host" -- cgit v1.2.3 From 8194cb107169d7c1a642e7bbcbade92879da6c9b Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 23 Jan 2015 23:25:27 +0100 Subject: added ros-joy package and drcsim --- Tools/ros/docker/px4-ros-full/Dockerfile | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/Tools/ros/docker/px4-ros-full/Dockerfile b/Tools/ros/docker/px4-ros-full/Dockerfile index c60f66539..9c0fbc688 100644 --- a/Tools/ros/docker/px4-ros-full/Dockerfile +++ b/Tools/ros/docker/px4-ros-full/Dockerfile @@ -33,14 +33,13 @@ RUN echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc RUN apt-get -y install python-rosinstall ## additional dependencies -RUN apt-get -y install ros-indigo-octomap-msgs - +RUN apt-get -y install ros-indigo-octomap-msgs ros-indigo-joy ## install drcsim -#RUN echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" > /etc/apt/sources.list.d/drc-latest.list \ -# && wget http://packages.osrfoundation.org/drc.key -O - | apt-key add - \ -# && apt-get update \ -# && apt-get -y install drcsim +RUN echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" > /etc/apt/sources.list.d/drc-latest.list \ + && wget http://packages.osrfoundation.org/drc.key -O - | apt-key add - \ + && apt-get update \ + && apt-get -y install drcsim # Install x11-utils to get xdpyinfo, for X11 display debugging # mesa-utils provides glxinfo, handy for understanding the 3D support -- cgit v1.2.3 From 36a70debf4c51c7ad02651f2eb6a24da71bad2fc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 13:35:57 +0100 Subject: add act control 0 to 3 as msg --- msg/actuator_controls_1.msg | 5 +++++ msg/actuator_controls_2.msg | 5 +++++ msg/actuator_controls_3.msg | 5 +++++ 3 files changed, 15 insertions(+) create mode 100644 msg/actuator_controls_1.msg create mode 100644 msg/actuator_controls_2.msg create mode 100644 msg/actuator_controls_3.msg diff --git a/msg/actuator_controls_1.msg b/msg/actuator_controls_1.msg new file mode 100644 index 000000000..414eb06dd --- /dev/null +++ b/msg/actuator_controls_1.msg @@ -0,0 +1,5 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[8] control diff --git a/msg/actuator_controls_2.msg b/msg/actuator_controls_2.msg new file mode 100644 index 000000000..414eb06dd --- /dev/null +++ b/msg/actuator_controls_2.msg @@ -0,0 +1,5 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[8] control diff --git a/msg/actuator_controls_3.msg b/msg/actuator_controls_3.msg new file mode 100644 index 000000000..414eb06dd --- /dev/null +++ b/msg/actuator_controls_3.msg @@ -0,0 +1,5 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[8] control -- cgit v1.2.3 From f9b4a96bf254f12d7ef97a09fafbbff64a9d6e54 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 13:36:10 +0100 Subject: add msg/actuator_controls_virtual_fw.msg --- msg/actuator_controls_virtual_fw.msg | 5 +++++ 1 file changed, 5 insertions(+) create mode 100644 msg/actuator_controls_virtual_fw.msg diff --git a/msg/actuator_controls_virtual_fw.msg b/msg/actuator_controls_virtual_fw.msg new file mode 100644 index 000000000..414eb06dd --- /dev/null +++ b/msg/actuator_controls_virtual_fw.msg @@ -0,0 +1,5 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[8] control -- cgit v1.2.3 From 265139f836b1fefdacb1e9498f0d25ecf2ecb47a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 13:36:36 +0100 Subject: add msg/fw_virtual_rates_setpoint.msg --- msg/fw_virtual_rates_setpoint.msg | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100644 msg/fw_virtual_rates_setpoint.msg diff --git a/msg/fw_virtual_rates_setpoint.msg b/msg/fw_virtual_rates_setpoint.msg new file mode 100644 index 000000000..834113c79 --- /dev/null +++ b/msg/fw_virtual_rates_setpoint.msg @@ -0,0 +1,6 @@ +uint64 timestamp # in microseconds since system start + +float32 roll # body angular rates in NED frame +float32 pitch # body angular rates in NED frame +float32 yaw # body angular rates in NED frame +float32 thrust # thrust normalized to 0..1 -- cgit v1.2.3 From ee6395c5029737baeb328bd10ba82c8f05bf0648 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 13:38:47 +0100 Subject: enable mc att multiplatform in makefile --- makefiles/config_px4fmu-v2_multiplatform.mk | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk index bfa26f83c..76edade4b 100644 --- a/makefiles/config_px4fmu-v2_multiplatform.mk +++ b/makefiles/config_px4fmu-v2_multiplatform.mk @@ -86,7 +86,8 @@ MODULES += modules/position_estimator_inav #MODULES += modules/fw_pos_control_l1 #MODULES += modules/fw_att_control # MODULES += modules/mc_att_control -# MODULES += modules/mc_att_control_multiplatform +MODULES += modules/mc_att_control_multiplatform +MODULES += examples/subscriber MODULES += examples/publisher MODULES += modules/mc_pos_control MODULES += modules/vtol_att_control -- cgit v1.2.3 From 14aa5f9441759672a3c0d6ee9b6ff5256ae0a77e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 13:39:34 +0100 Subject: add topic header includes --- src/drivers/hil/hil.cpp | 22 ++++++++++++---------- src/drivers/mkblctrl/mkblctrl.cpp | 1 + src/drivers/px4fmu/fmu.cpp | 6 +++++- src/drivers/px4io/px4io.cpp | 8 ++++++-- src/modules/bottle_drop/bottle_drop.cpp | 4 ++++ src/modules/commander/commander.cpp | 4 ++++ src/modules/controllib/uorb/blocks.hpp | 4 ++++ .../position_estimator_inav_main.c | 4 ++++ src/modules/sdlog2/sdlog2.c | 6 +++++- .../vtol_att_control/vtol_att_control_main.cpp | 4 ++++ src/systemcmds/esc_calib/esc_calib.c | 4 ++++ 11 files changed, 53 insertions(+), 14 deletions(-) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 9b5c8133b..6ffa8252e 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -75,6 +75,8 @@ #include #include +#include +#include #include #include @@ -250,7 +252,7 @@ HIL::task_main_trampoline(int argc, char *argv[]) int HIL::set_mode(Mode mode) { - /* + /* * Configure for PWM output. * * Note that regardless of the configured mode, the task is always @@ -269,19 +271,19 @@ HIL::set_mode(Mode mode) /* multi-port as 4 PWM outs */ _update_rate = 50; /* default output rate */ break; - + case MODE_8PWM: debug("MODE_8PWM"); /* multi-port as 8 PWM outs */ _update_rate = 50; /* default output rate */ break; - + case MODE_12PWM: debug("MODE_12PWM"); /* multi-port as 12 PWM outs */ _update_rate = 50; /* default output rate */ break; - + case MODE_16PWM: debug("MODE_16PWM"); /* multi-port as 16 PWM outs */ @@ -513,12 +515,12 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; case PWM_SERVO_SET_UPDATE_RATE: - // HIL always outputs at the alternate (usually faster) rate + // HIL always outputs at the alternate (usually faster) rate g_hil->set_pwm_rate(arg); break; case PWM_SERVO_SET_SELECT_UPDATE_RATE: - // HIL always outputs at the alternate (usually faster) rate + // HIL always outputs at the alternate (usually faster) rate break; case PWM_SERVO_SET(2): @@ -659,7 +661,7 @@ int hil_new_mode(PortMode new_mode) { // uint32_t gpio_bits; - + // /* reset to all-inputs */ // g_hil->ioctl(0, GPIO_RESET, 0); @@ -691,17 +693,17 @@ hil_new_mode(PortMode new_mode) /* select 2-pin PWM mode */ servo_mode = HIL::MODE_2PWM; break; - + case PORT2_8PWM: /* select 8-pin PWM mode */ servo_mode = HIL::MODE_8PWM; break; - + case PORT2_12PWM: /* select 12-pin PWM mode */ servo_mode = HIL::MODE_12PWM; break; - + case PORT2_16PWM: /* select 16-pin PWM mode */ servo_mode = HIL::MODE_16PWM; diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 1055487cb..a4505fe6f 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -74,6 +74,7 @@ #include #include +#include #include #include #include diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 112c01513..2552e7e6a 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -70,6 +70,10 @@ #include #include +#include +#include +#include +#include #include #include @@ -1144,7 +1148,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) * and PWM under control of the flight config * parameters. Note that this does not allow for * changing a set of pins to be used for serial on - * FMUv1 + * FMUv1 */ switch (arg) { case 0: diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 1c9a5adc9..0451bbd1b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -75,6 +75,10 @@ #include #include +#include +#include +#include +#include #include #include #include @@ -1199,7 +1203,7 @@ PX4IO::io_set_arming_state() if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; - + } else { clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; } @@ -2590,7 +2594,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) case PWM_SERVO_SET_RC_CONFIG: { /* enable setting of RC configuration without relying - on param_get() + on param_get() */ struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg; if (config->channel >= RC_INPUT_MAX_CHANNELS) { diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index d8116ea11..b267209fe 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -57,6 +57,10 @@ #include #include #include +#include +#include +#include +#include #include #include #include diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 247a2c5b8..a1c4e205d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -74,6 +74,10 @@ #include #include #include +#include +#include +#include +#include #include #include #include diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index a8a70507e..491d4681d 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -47,6 +47,10 @@ #include #include #include +#include +#include +#include +#include #include #include diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 68ebd0f4f..6de283396 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -53,6 +53,10 @@ #include #include #include +#include +#include +#include +#include #include #include #include diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index a3407e42c..ac08b0d23 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -70,6 +70,10 @@ #include #include #include +#include +#include +#include +#include #include #include #include @@ -1117,7 +1121,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); - + /* we need to rate-limit wind, as we do not need the full update rate */ orb_set_interval(subs.wind_sub, 90); subs.encoders_sub = orb_subscribe(ORB_ID(encoders)); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 8e68730b8..0a333eade 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -58,7 +58,11 @@ #include #include #include +#include +#include #include +#include +#include #include #include #include diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index 20967b541..32682f890 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -64,6 +64,10 @@ #include "drivers/drv_pwm_output.h" #include +#include +#include +#include +#include static void usage(const char *reason); __EXPORT int esc_calib_main(int argc, char *argv[]); -- cgit v1.2.3 From 35efcaa92c575c60ad31f5a7ad9f8e5756cdf2bc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 15:01:03 +0100 Subject: update topic header includes for multiplatform headers --- src/platforms/px4_includes.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 90ec78996..ab27e6b4d 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -76,6 +76,10 @@ #include #include #include +#include +#include +#include +#include #include #include #include -- cgit v1.2.3 From 1f0daea66586c32720083e5b2d703ce5fb9477f4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 15:01:37 +0100 Subject: remove uorb hacks --- src/modules/uORB/uORB.h | 19 ------------------- 1 file changed, 19 deletions(-) diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index f19fbba7c..672f8d8d1 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -290,25 +290,6 @@ __END_DECLS /* Diverse uORB header defines */ //XXX: move to better location #define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) -ORB_DECLARE(actuator_controls_0); -typedef struct actuator_controls_s actuator_controls_0_s; -ORB_DECLARE(actuator_controls_1); -typedef struct actuator_controls_s actuator_controls_1_s; -ORB_DECLARE(actuator_controls_2); -typedef struct actuator_controls_s actuator_controls_2_s; -ORB_DECLARE(actuator_controls_3); -typedef struct actuator_controls_s actuator_controls_3_s; -ORB_DECLARE(actuator_controls_virtual_mc); -typedef struct actuator_controls_s actuator_controls_virtual_mc_s; -ORB_DECLARE(actuator_controls_virtual_fw); -typedef struct actuator_controls_s actuator_controls_virtual_fw_s; -ORB_DECLARE(mc_virtual_rates_setpoint); -typedef struct vehicle_rates_setpoint_s mc_virtual_rates_setpoint_s; -ORB_DECLARE(fw_virtual_rates_setpoint); -typedef struct vehicle_rates_setpoint_s fw_virtual_rates_setpoint_s; -ORB_DECLARE(mc_virtual_attitude_setpoint); -typedef struct vehicle_attitude_setpoint_s mc_virtual_attitude_setpoint_s; -ORB_DECLARE(fw_virtual_attitude_setpoint); typedef struct vehicle_attitude_setpoint_s fw_virtual_attitude_setpoint_s; typedef uint8_t arming_state_t; typedef uint8_t main_state_t; -- cgit v1.2.3 From 8da83cfc80d2209e41cb47dd1dd2831d18a34012 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 15:02:06 +0100 Subject: objects common: use separate files --- src/modules/uORB/objects_common.cpp | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 78fdf4de7..204a114e1 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -121,8 +121,10 @@ ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s); #include "topics/vehicle_rates_setpoint.h" ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s); -ORB_DEFINE(mc_virtual_rates_setpoint, struct vehicle_rates_setpoint_s); -ORB_DEFINE(fw_virtual_rates_setpoint, struct vehicle_rates_setpoint_s); +#include "topics/mc_virtual_rates_setpoint.h" +ORB_DEFINE(mc_virtual_rates_setpoint, struct mc_virtual_rates_setpoint_s); +#include "topics/fw_virtual_rates_setpoint.h" +ORB_DEFINE(fw_virtual_rates_setpoint, struct fw_virtual_rates_setpoint_s); #include "topics/rc_channels.h" ORB_DEFINE(rc_channels, struct rc_channels_s); @@ -192,13 +194,19 @@ ORB_DEFINE(subsystem_info, struct subsystem_info_s); /* actuator controls, as requested by controller */ #include "topics/actuator_controls.h" -ORB_DEFINE(actuator_controls_0, struct actuator_controls_s); -ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); -ORB_DEFINE(actuator_controls_2, struct actuator_controls_s); -ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); +#include "topics/actuator_controls_0.h" +ORB_DEFINE(actuator_controls_0, struct actuator_controls_0_s); +#include "topics/actuator_controls_1.h" +ORB_DEFINE(actuator_controls_1, struct actuator_controls_1_s); +#include "topics/actuator_controls_2.h" +ORB_DEFINE(actuator_controls_2, struct actuator_controls_2_s); +#include "topics/actuator_controls_3.h" +ORB_DEFINE(actuator_controls_3, struct actuator_controls_3_s); //Virtual control groups, used for VTOL operation -ORB_DEFINE(actuator_controls_virtual_mc, struct actuator_controls_s); -ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_s); +#include "topics/actuator_controls_virtual_mc.h" +ORB_DEFINE(actuator_controls_virtual_mc, struct actuator_controls_virtual_mc_s); +#include "topics/actuator_controls_virtual_fw.h" +ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_virtual_fw_s); #include "topics/actuator_armed.h" ORB_DEFINE(actuator_armed, struct actuator_armed_s); -- 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 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 cd35ab26610650d93120d3a9eabd196a733c2e44 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 15:32:40 +0100 Subject: add another set of uorb headers --- src/drivers/roboclaw/RoboClaw.cpp | 15 ++++++++------- src/examples/fixedwing_control/main.c | 20 ++++++++++++-------- src/examples/hwtest/hwtest.c | 4 ++++ 3 files changed, 24 insertions(+), 15 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index eb9453b50..5b945e452 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -54,6 +54,7 @@ #include #include +#include #include #include @@ -114,7 +115,7 @@ int RoboClaw::readEncoder(e_motor motor) uint8_t rbuf[50]; usleep(5000); int nread = read(_uart, rbuf, 50); - if (nread < 6) { + if (nread < 6) { printf("failed to read\n"); return -1; } @@ -131,7 +132,7 @@ int RoboClaw::readEncoder(e_motor motor) countBytes[0] = rbuf[3]; uint8_t status = rbuf[4]; uint8_t checksum = rbuf[5]; - uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) & + uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) & checksum_mask; // check if checksum is valid if (checksum != checksum_computed) { @@ -156,11 +157,11 @@ int RoboClaw::readEncoder(e_motor motor) static int64_t overflowAmount = 0x100000000LL; if (motor == MOTOR_1) { _motor1Overflow += overFlow; - _motor1Position = float(int64_t(count) + + _motor1Position = float(int64_t(count) + _motor1Overflow*overflowAmount)/_pulsesPerRev; } else if (motor == MOTOR_2) { _motor2Overflow += overFlow; - _motor2Position = float(int64_t(count) + + _motor2Position = float(int64_t(count) + _motor2Overflow*overflowAmount)/_pulsesPerRev; } return 0; @@ -242,7 +243,7 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value) return -1; } -int RoboClaw::resetEncoders() +int RoboClaw::resetEncoders() { uint16_t sum = 0; return _sendCommand(CMD_RESET_ENCODERS, @@ -278,7 +279,7 @@ uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n) return sum; } -int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, +int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, size_t n_data, uint16_t & prev_sum) { tcflush(_uart, TCIOFLUSH); // flush buffers @@ -299,7 +300,7 @@ int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, return write(_uart, buf, n_data + 3); } -int roboclawTest(const char *deviceName, uint8_t address, +int roboclawTest(const char *deviceName, uint8_t address, uint16_t pulsesPerRev) { printf("roboclaw test: starting\n"); diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index fcbb54c8e..d8b777b91 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -61,6 +61,10 @@ #include #include #include +#include +#include +#include +#include #include #include #include @@ -108,7 +112,7 @@ static void usage(const char *reason); * @param att The current attitude. The controller should make the attitude match the setpoint * @param rates_sp The angular rate setpoint. This is the output of the controller. */ -void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, +void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators); /** @@ -133,18 +137,18 @@ static int deamon_task; /**< Handle of deamon task / thread */ static struct params p; static struct param_handles ph; -void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, +void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators) { - /* + /* * The PX4 architecture provides a mixer outside of the controller. * The mixer is fed with a default vector of actuator controls, representing * moments applied to the vehicle frame. This vector * is structured as: * * Control Group 0 (attitude): - * + * * 0 - roll (-1..+1) * 1 - pitch (-1..+1) * 2 - yaw (-1..+1) @@ -226,7 +230,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) * * Wikipedia description: * http://en.wikipedia.org/wiki/Publish–subscribe_pattern - * + * */ @@ -234,7 +238,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* * Declare and safely initialize all structs to zero. - * + * * These structs contain the system state and things * like attitude, position, the current waypoint, etc. */ @@ -300,7 +304,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) if (ret < 0) { /* * Poll error, this will not really happen in practice, - * but its good design practice to make output an error message. + * but its good design practice to make output an error message. */ warnx("poll error"); @@ -340,7 +344,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) } if (manual_sp_updated) - /* get the RC (or otherwise user based) input */ + /* get the RC (or otherwise user based) input */ orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); /* check if the throttle was ever more than 50% - go later only to failsafe if yes */ diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c index 95ff346bb..8fd8870da 100644 --- a/src/examples/hwtest/hwtest.c +++ b/src/examples/hwtest/hwtest.c @@ -47,6 +47,10 @@ #include #include #include +#include +#include +#include +#include #include __EXPORT int ex_hwtest_main(int argc, char *argv[]); -- cgit v1.2.3 From dafe6654ebd2730c0ac5c3c60bb5fe8946ab67a0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 15:35:25 +0100 Subject: add missing functional header --- src/platforms/px4_nodehandle.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 80be9ec52..83a3e422d 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -53,6 +53,7 @@ /* includes when building for NuttX */ #include #endif +#include namespace px4 { -- cgit v1.2.3 From 23229317ad5e08f758a4d48148d36fabe2a2cfbd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 15:35:42 +0100 Subject: remove unnecessary include --- src/platforms/px4_includes.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index ab27e6b4d..1bd4509ca 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -70,7 +70,6 @@ */ #include #include -#include #ifdef __cplusplus #include #include -- cgit v1.2.3 From 9b4a21049550f12f11ec0e6dd2d2a55e1b2ef1b3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 15:55:32 +0100 Subject: missing headers for fmu2 target --- src/modules/fw_att_control/fw_att_control_main.cpp | 4 ++++ src/modules/mc_att_control/mc_att_control_main.cpp | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index e0b61e2e2..88918333d 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -59,7 +59,11 @@ #include #include #include +#include +#include #include +#include +#include #include #include #include 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 228f142d9..562033db1 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -63,7 +63,11 @@ #include #include #include +#include +#include #include +#include +#include #include #include #include -- cgit v1.2.3 From f7dc81ded1bdd7eb7d88532e5513b168c6d4f118 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 16:13:39 +0100 Subject: missing headers for fmu1 target --- src/drivers/ardrone_interface/ardrone_interface.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 7f1b21a95..7d4b7d880 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -55,6 +55,10 @@ #include #include #include +#include +#include +#include +#include #include #include @@ -98,7 +102,7 @@ usage(const char *reason) * The deamon app only briefly exists to start * 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(). */ @@ -319,7 +323,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* get a local copy of the actuator controls */ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); - + /* for now only spin if armed and immediately shut down * if in failsafe */ -- cgit v1.2.3 From 5cb208c32f5baf52fffb97918f5b06b2498aec87 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 18:22:13 +0100 Subject: reenable mcatt ctl and other nodes for ros --- CMakeLists.txt | 84 +++++++++++++++++++++++++++++----------------------------- 1 file changed, 42 insertions(+), 42 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 25822d719..f76dbbf41 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -169,52 +169,52 @@ target_link_libraries(subscriber px4 ) -# ## MC Attitude Control -# add_executable(mc_att_control - # src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp - # src/modules/mc_att_control_multiplatform/mc_att_control.cpp - # src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp) -# add_dependencies(mc_att_control ${PROJECT_NAME}_generate_messages_cpp_cpp) -# target_link_libraries(mc_att_control - # ${catkin_LIBRARIES} - # px4 -# ) +## MC Attitude Control +add_executable(mc_att_control + src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp + src/modules/mc_att_control_multiplatform/mc_att_control.cpp + src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp) +add_dependencies(mc_att_control ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(mc_att_control + ${catkin_LIBRARIES} + px4 +) -# ## Attitude Estimator dummy -# add_executable(attitude_estimator - # src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp) -# add_dependencies(attitude_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp) -# target_link_libraries(attitude_estimator - # ${catkin_LIBRARIES} - # px4 -# ) +## Attitude Estimator dummy +add_executable(attitude_estimator + src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp) +add_dependencies(attitude_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(attitude_estimator + ${catkin_LIBRARIES} + px4 +) -# ## Manual input -# add_executable(manual_input - # src/platforms/ros/nodes/manual_input/manual_input.cpp) -# add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) -# target_link_libraries(manual_input - # ${catkin_LIBRARIES} - # px4 -# ) +## Manual input +add_executable(manual_input + src/platforms/ros/nodes/manual_input/manual_input.cpp) +add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(manual_input + ${catkin_LIBRARIES} + px4 +) -# ## Multicopter Mixer dummy -# add_executable(mc_mixer - # src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp) -# add_dependencies(mc_mixer ${PROJECT_NAME}_generate_messages_cpp_cpp) -# target_link_libraries(mc_mixer - # ${catkin_LIBRARIES} - # px4 -# ) +## Multicopter Mixer dummy +add_executable(mc_mixer + src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp) +add_dependencies(mc_mixer ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(mc_mixer + ${catkin_LIBRARIES} + px4 +) -# ## Commander -# add_executable(commander - # src/platforms/ros/nodes/commander/commander.cpp) -# add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) -# target_link_libraries(commander - # ${catkin_LIBRARIES} - # px4 -# ) +## Commander +add_executable(commander + src/platforms/ros/nodes/commander/commander.cpp) +add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(commander + ${catkin_LIBRARIES} + px4 +) ############# ## Install ## -- 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(-) 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 From 8dbb1fc8d443b1f8e2e454d4b82051fa63a48398 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 18:24:10 +0100 Subject: remove unneeded include --- src/platforms/px4_defines.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 325832f0f..c8e2cf290 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -141,9 +141,6 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) * Building for NuttX */ #include -#ifdef __cplusplus -#include -#endif /* Main entry point */ #define PX4_MAIN_FUNCTION(_prefix) int _prefix##_task_main(int argc, char *argv[]) -- cgit v1.2.3 From abeae7b6f6d70042c6b0bed493ade52727648c2d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 26 Jan 2015 14:50:23 +0100 Subject: extend subscriber/publisher example --- src/examples/publisher/publisher_example.cpp | 25 ++++++++++++++++++++----- src/examples/publisher/publisher_example.h | 2 ++ src/examples/subscriber/subscriber_example.cpp | 26 +++++++++++++++++++++++--- src/examples/subscriber/subscriber_example.h | 3 ++- 4 files changed, 47 insertions(+), 9 deletions(-) diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp index 9952a16e4..e7ab9eb5a 100644 --- a/src/examples/publisher/publisher_example.cpp +++ b/src/examples/publisher/publisher_example.cpp @@ -45,7 +45,9 @@ using namespace px4; PublisherExample::PublisherExample() : _n(), - _rc_channels_pub(_n.advertise()) + _rc_channels_pub(_n.advertise()), + _v_att_pub(_n.advertise()), + _parameter_update_pub(_n.advertise()) { } @@ -57,10 +59,23 @@ int PublisherExample::main() loop_rate.sleep(); _n.spinOnce(); - px4_rc_channels msg; - msg.data().timestamp_last_valid = px4::get_time_micros(); - PX4_INFO("%llu", msg.data().timestamp_last_valid); - _rc_channels_pub->publish(msg); + /* Publish example message */ + px4_rc_channels rc_channels_msg; + rc_channels_msg.data().timestamp_last_valid = px4::get_time_micros(); + PX4_INFO("rc: %llu", rc_channels_msg.data().timestamp_last_valid); + _rc_channels_pub->publish(rc_channels_msg); + + /* Publish example message */ + px4_vehicle_attitude v_att_msg; + v_att_msg.data().timestamp = px4::get_time_micros(); + PX4_INFO("att: %llu", v_att_msg.data().timestamp); + _v_att_pub->publish(v_att_msg); + + /* Publish example message */ + px4_parameter_update parameter_update_msg; + parameter_update_msg.data().timestamp = px4::get_time_micros(); + PX4_INFO("param update: %llu", parameter_update_msg.data().timestamp); + _parameter_update_pub->publish(parameter_update_msg); } diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h index 4ff59a226..8165b0ffc 100644 --- a/src/examples/publisher/publisher_example.h +++ b/src/examples/publisher/publisher_example.h @@ -50,4 +50,6 @@ public: protected: px4::NodeHandle _n; px4::Publisher * _rc_channels_pub; + px4::Publisher * _v_att_pub; + px4::Publisher * _parameter_update_pub; }; diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 781dde486..e1292f788 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -67,9 +67,15 @@ SubscriberExample::SubscriberExample() : /* No callback */ _sub_rc_chan = _n.subscribe(500); - /* Class Method */ + /* Class method */ _n.subscribe(&SubscriberExample::rc_channels_callback, this, 1000); + /* Another class method */ + _n.subscribe(&SubscriberExample::vehicle_attitude_callback, this, 1000); + + /* Yet antoher class method */ + _n.subscribe(&SubscriberExample::parameter_update_callback, this, 1000); + PX4_INFO("subscribed"); } @@ -78,8 +84,22 @@ SubscriberExample::SubscriberExample() : * Also the current value of the _sub_rc_chan subscription is printed */ void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) { - PX4_INFO("Callback (method): [%llu]", + PX4_INFO("rc_channels_callback (method): [%llu]", msg.data().timestamp_last_valid); - PX4_INFO("Callback (method): value of _sub_rc_chan: [%llu]", + PX4_INFO("rc_channels_callback (method): value of _sub_rc_chan: [%llu]", _sub_rc_chan->data().timestamp_last_valid); } + +void SubscriberExample::vehicle_attitude_callback(const px4_vehicle_attitude &msg) { + PX4_INFO("vehicle_attitude_callback (method): [%llu]", + msg.data().timestamp); +} + +void SubscriberExample::parameter_update_callback(const px4_parameter_update &msg) { + PX4_INFO("parameter_update_callback (method): [%llu]", + msg.data().timestamp); + PX4_PARAM_GET(_p_sub_interv, &_interval); + PX4_INFO("Param SUB_INTERV = %d", _interval); + PX4_PARAM_GET(_p_test_float, &_test_float); + PX4_INFO("Param SUB_TESTF = %.3f", (double)_test_float); +} diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index 8da3df438..9b6d890e3 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -59,6 +59,7 @@ protected: px4::Subscriber * _sub_rc_chan; void rc_channels_callback(const px4_rc_channels &msg); - + void vehicle_attitude_callback(const px4_vehicle_attitude &msg); + void parameter_update_callback(const px4_parameter_update &msg); }; -- cgit v1.2.3 From de44c549eb8d195a0356afdd875b23a277fd8c7a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 26 Jan 2015 15:53:49 +0100 Subject: mc attctl multiplatform: remove memsets --- src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) 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 0de832df9..fcfee28dc 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 @@ -55,13 +55,12 @@ using namespace std; #endif MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : + _v_att_sp_mod(), + _v_rates_sp_mod(), + _actuators(), _publish_att_sp(false) { - memset(&_v_att_sp_mod, 0, sizeof(_v_att_sp_mod)); - memset(&_v_rates_sp_mod, 0, sizeof(_v_rates_sp_mod)); - memset(&_actuators, 0, sizeof(_actuators)); - _params.att_p.zero(); _params.rate_p.zero(); _params.rate_i.zero(); -- cgit v1.2.3 From f23e603d02ba416ae250770cdaad6a859d6bae69 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 26 Jan 2015 15:54:19 +0100 Subject: mc attctl multiplatform: increase stack size --- src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index e982ae354..c2b847075 100644 --- 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 @@ -71,7 +71,7 @@ int mc_att_control_m_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("mc_att_control_m", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2000, + 3000, main, (argv) ? (char* const*)&argv[2] : (char* const*)NULL); -- cgit v1.2.3 From 856b10cc1a5a36a7b7d2978477185155792366c2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 16:29:25 +0100 Subject: Revert "temporarily re-enable stack checking, disable some modules to make firmware fit" This reverts commit 27b2701340648e2fde1992b175abfa591e0eee01. --- makefiles/config_px4fmu-v2_default.mk | 10 +++++----- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 5d6beffd7..2ce738844 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -37,11 +37,11 @@ MODULES += drivers/gps # MODULES += drivers/hott/hott_sensors # MODULES += drivers/blinkm MODULES += drivers/airspeed -# MODULES += drivers/ets_airspeed +MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed -# MODULES += drivers/frsky_telemetry +MODULES += drivers/frsky_telemetry MODULES += modules/sensors -# MODULES += drivers/mkblctrl +MODULES += drivers/mkblctrl MODULES += drivers/px4flow # @@ -70,7 +70,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led -# MODULES += modules/uavcan +MODULES += modules/uavcan MODULES += modules/land_detector # @@ -121,7 +121,7 @@ MODULES += platforms/nuttx # # OBC challenge # -# MODULES += modules/bottle_drop +MODULES += modules/bottle_drop # # Demo apps diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 9030a1f02..dedebdfa0 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -117,7 +117,7 @@ CONFIG_ARCH_HAVE_MPU=y # # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=y +CONFIG_ARMV7M_STACKCHECK=n CONFIG_SERIAL_TERMIOS=y CONFIG_SDIO_DMA=y CONFIG_SDIO_DMAPRIO=0x00010000 -- cgit v1.2.3 From c67cb25f9a4cb4ec507029865dd70837417ef894 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 07:54:58 +0100 Subject: port more uorb headers to msg --- msg/position_setpoint.msg | 35 +++++++ msg/position_setpoint_triplet.msg | 8 ++ msg/vehicle_global_velocity_setpoint.msg | 4 + msg/vehicle_local_position.msg | 36 +++++++ msg/vehicle_local_position_setpoint.msg | 7 ++ .../uORB/topics/position_setpoint_triplet.h | 116 --------------------- .../uORB/topics/vehicle_global_velocity_setpoint.h | 63 ----------- src/modules/uORB/topics/vehicle_local_position.h | 96 ----------------- .../uORB/topics/vehicle_local_position_setpoint.h | 67 ------------ 9 files changed, 90 insertions(+), 342 deletions(-) create mode 100644 msg/position_setpoint.msg create mode 100644 msg/position_setpoint_triplet.msg create mode 100644 msg/vehicle_global_velocity_setpoint.msg create mode 100644 msg/vehicle_local_position.msg create mode 100644 msg/vehicle_local_position_setpoint.msg delete mode 100644 src/modules/uORB/topics/position_setpoint_triplet.h delete mode 100644 src/modules/uORB/topics/vehicle_global_velocity_setpoint.h delete mode 100644 src/modules/uORB/topics/vehicle_local_position.h delete mode 100644 src/modules/uORB/topics/vehicle_local_position_setpoint.h diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg new file mode 100644 index 000000000..ae6756aa8 --- /dev/null +++ b/msg/position_setpoint.msg @@ -0,0 +1,35 @@ +# this file is only used in the position_setpoint triple as a dependency + +uint8 SETPOINT_TYPE_POSITION=0 # position setpoint +uint8 SETPOINT_TYPE_VELOCITY=1 # velocity setpoint +uint8 SETPOINT_TYPE_LOITER=2 # loiter setpoint +uint8 SETPOINT_TYPE_TAKEOFF=3 # takeoff setpoint +uint8 SETPOINT_TYPE_LAND=4 # land setpoint, altitude must be ignored, descend until landing +uint8 SETPOINT_TYPE_IDLE=5 # do nothing, switch off motors or keep at idle speed (MC) +uint8 SETPOINT_TYPE_OFFBOARD=6 # setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard + +bool valid # true if setpoint is valid +uint8 type # setpoint type to adjust behavior of position controller +float32 x # local position setpoint in m in NED +float32 y # local position setpoint in m in NED +float32 z # local position setpoint in m in NED +bool position_valid # true if local position setpoint valid +float32 vx # local velocity setpoint in m/s in NED +float32 vy # local velocity setpoint in m/s in NED +float32 vz # local velocity setpoint in m/s in NED +bool velocity_valid # true if local velocity setpoint valid +float64 lat # latitude, in deg +float64 lon # longitude, in deg +float32 alt # altitude AMSL, in m +float32 yaw # yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw +bool yaw_valid # true if yaw setpoint valid +float32 yawspeed # yawspeed (only for multirotors, in rad/s) +bool yawspeed_valid # true if yawspeed setpoint valid +float32 loiter_radius # loiter radius (only for fixed wing), in m +int8 loiter_direction # loiter direction: 1 = CW, -1 = CCW +float32 pitch_min # minimal pitch angle for fixed wing takeoff waypoints +float32 a_x # acceleration x setpoint +float32 a_y # acceleration y setpoint +float32 a_z # acceleration z setpoint +bool acceleration_valid # true if acceleration setpoint is valid/should be used +bool acceleration_is_force # interprete acceleration as force diff --git a/msg/position_setpoint_triplet.msg b/msg/position_setpoint_triplet.msg new file mode 100644 index 000000000..8717f65d0 --- /dev/null +++ b/msg/position_setpoint_triplet.msg @@ -0,0 +1,8 @@ +# Global position setpoint triplet in WGS84 coordinates. +# This are the three next waypoints (or just the next two or one). + +px4/position_setpoint previous +px4/position_setpoint current +px4/position_setpoint next + +uint8 nav_state # report the navigation state diff --git a/msg/vehicle_global_velocity_setpoint.msg b/msg/vehicle_global_velocity_setpoint.msg new file mode 100644 index 000000000..ca7dcc826 --- /dev/null +++ b/msg/vehicle_global_velocity_setpoint.msg @@ -0,0 +1,4 @@ +# Velocity setpoint in NED frame +float32 vx # in m/s NED +float32 vy # in m/s NED +float32 vz # in m/s NED diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg new file mode 100644 index 000000000..4da027ae7 --- /dev/null +++ b/msg/vehicle_local_position.msg @@ -0,0 +1,36 @@ +# Fused local position in NED. + +uint64 timestamp # Time of this estimate, in microseconds since system start +bool xy_valid # true if x and y are valid +bool z_valid # true if z is valid +bool v_xy_valid # true if vy and vy are valid +bool v_z_valid # true if vz is valid + +# Position in local NED frame +float32 x # X position in meters in NED earth-fixed frame +float32 y # X position in meters in NED earth-fixed frame +float32 z # Z position in meters in NED earth-fixed frame (negative altitude) + +# Velocity in NED frame +float32 vx # Ground X Speed (Latitude), m/s in NED +float32 vy # Ground Y Speed (Longitude), m/s in NED +float32 vz # Ground Z Speed (Altitude), m/s in NED + +# Heading +float32 yaw + +# Reference position in GPS / WGS84 frame +bool xy_global # true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) +bool z_global # true if z is valid and has valid global reference (ref_alt) +uint64 ref_timestamp # Time when reference position was set +float64 ref_lat # Reference point latitude in degrees +float64 ref_lon # Reference point longitude in degrees +float32 ref_alt # Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level + +# Distance to surface +float32 dist_bottom # Distance to bottom surface (ground) +float32 dist_bottom_rate # Distance to bottom surface (ground) change rate +uint64 surface_bottom_timestamp # Time when new bottom surface found +bool dist_bottom_valid # true if distance to bottom surface is valid +float32 eph +float32 epv diff --git a/msg/vehicle_local_position_setpoint.msg b/msg/vehicle_local_position_setpoint.msg new file mode 100644 index 000000000..a2ff8a4ae --- /dev/null +++ b/msg/vehicle_local_position_setpoint.msg @@ -0,0 +1,7 @@ +# Local position in NED frame + +uint64 timestamp # timestamp of the setpoint +float32 x # in meters NED +float32 y # in meters NED +float32 z # in meters NED +float32 yaw # in radians NED -PI..+PI diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h deleted file mode 100644 index cb2262534..000000000 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ /dev/null @@ -1,116 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 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 mission_item_triplet.h - * Definition of the global WGS84 position setpoint uORB topic. - * - * @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier - */ - -#ifndef TOPIC_MISSION_ITEM_TRIPLET_H_ -#define TOPIC_MISSION_ITEM_TRIPLET_H_ - -#include -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -enum SETPOINT_TYPE -{ - SETPOINT_TYPE_POSITION = 0, /**< position setpoint */ - SETPOINT_TYPE_VELOCITY, /**< velocity setpoint */ - SETPOINT_TYPE_LOITER, /**< loiter setpoint */ - SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */ - SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, descend until landing */ - SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */ - SETPOINT_TYPE_OFFBOARD, /**< setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard */ -}; - -struct position_setpoint_s -{ - bool valid; /**< true if setpoint is valid */ - enum SETPOINT_TYPE type; /**< setpoint type to adjust behavior of position controller */ - float x; /**< local position setpoint in m in NED */ - float y; /**< local position setpoint in m in NED */ - float z; /**< local position setpoint in m in NED */ - bool position_valid; /**< true if local position setpoint valid */ - float vx; /**< local velocity setpoint in m/s in NED */ - float vy; /**< local velocity setpoint in m/s in NED */ - float vz; /**< local velocity setpoint in m/s in NED */ - bool velocity_valid; /**< true if local velocity setpoint valid */ - double lat; /**< latitude, in deg */ - double lon; /**< longitude, in deg */ - float alt; /**< altitude AMSL, in m */ - float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */ - bool yaw_valid; /**< true if yaw setpoint valid */ - float yawspeed; /**< yawspeed (only for multirotors, in rad/s) */ - bool yawspeed_valid; /**< true if yawspeed setpoint valid */ - float loiter_radius; /**< loiter radius (only for fixed wing), in m */ - int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */ - float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ - float a_x; //**< acceleration x setpoint */ - float a_y; //**< acceleration y setpoint */ - float a_z; //**< acceleration z setpoint */ - bool acceleration_valid; //*< true if acceleration setpoint is valid/should be used */ - bool acceleration_is_force; //*< interprete acceleration as force */ -}; - -/** - * Global position setpoint triplet in WGS84 coordinates. - * - * This are the three next waypoints (or just the next two or one). - */ -struct position_setpoint_triplet_s -{ - struct position_setpoint_s previous; - struct position_setpoint_s current; - struct position_setpoint_s next; - - unsigned nav_state; /**< report the navigation state */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(position_setpoint_triplet); - -#endif diff --git a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h deleted file mode 100644 index 5dac877d0..000000000 --- a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h +++ /dev/null @@ -1,63 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: @author Anton Babushkin - * - * 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 vehicle_global_velocity_setpoint.h - * Definition of the global velocity setpoint uORB topic. - */ - -#ifndef TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_ -#define TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_ - -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -struct vehicle_global_velocity_setpoint_s { - float vx; /**< in m/s NED */ - float vy; /**< in m/s NED */ - float vz; /**< in m/s NED */ -}; /**< Velocity setpoint in NED frame */ - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_global_velocity_setpoint); - -#endif diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h deleted file mode 100644 index 8b46c5a3f..000000000 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ /dev/null @@ -1,96 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 vehicle_local_position.h - * Definition of the local fused NED position uORB topic. - * - * @author Lorenz Meier - * @author Anton Babushkin - */ - -#ifndef TOPIC_VEHICLE_LOCAL_POSITION_H_ -#define TOPIC_VEHICLE_LOCAL_POSITION_H_ - -#include -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * Fused local position in NED. - */ -struct vehicle_local_position_s { - uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ - bool xy_valid; /**< true if x and y are valid */ - bool z_valid; /**< true if z is valid */ - bool v_xy_valid; /**< true if vy and vy are valid */ - bool v_z_valid; /**< true if vz is valid */ - /* Position in local NED frame */ - float x; /**< X position in meters in NED earth-fixed frame */ - float y; /**< X position in meters in NED earth-fixed frame */ - float z; /**< Z position in meters in NED earth-fixed frame (negative altitude) */ - /* Velocity in NED frame */ - float vx; /**< Ground X Speed (Latitude), m/s in NED */ - float vy; /**< Ground Y Speed (Longitude), m/s in NED */ - float vz; /**< Ground Z Speed (Altitude), m/s in NED */ - /* Heading */ - float yaw; - /* Reference position in GPS / WGS84 frame */ - bool xy_global; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ - bool z_global; /**< true if z is valid and has valid global reference (ref_alt) */ - uint64_t ref_timestamp; /**< Time when reference position was set */ - double ref_lat; /**< Reference point latitude in degrees */ - double ref_lon; /**< Reference point longitude in degrees */ - float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ - /* Distance to surface */ - float dist_bottom; /**< Distance to bottom surface (ground) */ - float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */ - uint64_t surface_bottom_timestamp; /**< Time when new bottom surface found */ - bool dist_bottom_valid; /**< true if distance to bottom surface is valid */ - float eph; - float epv; -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_local_position); - -#endif diff --git a/src/modules/uORB/topics/vehicle_local_position_setpoint.h b/src/modules/uORB/topics/vehicle_local_position_setpoint.h deleted file mode 100644 index 6766bb58a..000000000 --- a/src/modules/uORB/topics/vehicle_local_position_setpoint.h +++ /dev/null @@ -1,67 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier - * - * 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 vehicle_local_position_setpoint.h - * Definition of the local NED position setpoint uORB topic. - */ - -#ifndef TOPIC_VEHICLE_LOCAL_POSITION_SETPOINT_H_ -#define TOPIC_VEHICLE_LOCAL_POSITION_SETPOINT_H_ - -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -struct vehicle_local_position_setpoint_s { - uint64_t timestamp; /**< timestamp of the setpoint */ - float x; /**< in meters NED */ - float y; /**< in meters NED */ - float z; /**< in meters NED */ - float yaw; /**< in radians NED -PI..+PI */ -}; /**< Local position in NED frame */ - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_local_position_setpoint); - -#endif -- cgit v1.2.3 From 01835a51a8a3a0b0f7e7362cdc25475bd029a9a8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 07:55:21 +0100 Subject: uorb constants are now defined inside class --- msg/templates/msg.h.template | 159 -------------------------------------- msg/templates/uorb/msg.h.template | 23 +++++- 2 files changed, 21 insertions(+), 161 deletions(-) delete mode 100644 msg/templates/msg.h.template diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template deleted file mode 100644 index cc128c1ea..000000000 --- a/msg/templates/msg.h.template +++ /dev/null @@ -1,159 +0,0 @@ -@############################################### -@# -@# PX4 ROS compatible message source code -@# generation for C++ -@# -@# EmPy template for generating .h files -@# Based on the original template for ROS -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - file_name_in (String) Source file -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@# - md5sum (String) MD5Sum of the .msg specification -@############################################### -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - - /* Auto-generated by genmsg_cpp from file @file_name_in */ - -@{ -import genmsg.msgs -import gencpp - -cpp_namespace = '::%s::'%(spec.package) # TODO handle nested namespace -cpp_class = '%s_'%spec.short_name -cpp_full_name = '%s%s'%(cpp_namespace,cpp_class) -cpp_full_name_with_alloc = '%s'%(cpp_full_name) -cpp_msg_definition = gencpp.escape_message_definition(msg_definition) -}@ - -#pragma once - -@############################## -@# Generic Includes -@############################## -#include -#include "../uORB.h" - -@############################## -@# Includes for dependencies -@############################## -@{ -for field in spec.parsed_fields(): - if (not field.is_builtin): - if (not field.is_header): - (package, name) = genmsg.names.package_resource_name(field.base_type) - package = package or spec.package # convert '' to package - print('#include '%(name)) - -}@ -@# Constants -@[for constant in spec.constants]@ -#define @(constant.name) @(int(constant.val)) -@[end for] - -/** - * @@addtogroup topics - * @@{ - */ - -@############################## -@# Main struct of message -@############################## -@{ - -type_map = {'int8': 'int8_t', - 'int16': 'int16_t', - 'int32': 'int32_t', - 'int64': 'int64_t', - 'uint8': 'uint8_t', - 'uint16': 'uint16_t', - 'uint32': 'uint32_t', - 'uint64': 'uint64_t', - 'float32': 'float', - 'bool': 'bool', - 'fence_vertex': 'fence_vertex'} - -# Function to print a standard ros type -def print_field_def(field): - type = field.type - # detect embedded types - sl_pos = type.find('/') - type_appendix = '' - type_prefix = '' - if (sl_pos >= 0): - type = type[sl_pos + 1:] - type_prefix = 'struct ' - type_appendix = '_s' - - # detect arrays - a_pos = type.find('[') - array_size = '' - if (a_pos >= 0): - # field is array - array_size = type[a_pos:] - type = type[:a_pos] - - if type in type_map: - # need to add _t: int8 --> int8_t - type_px4 = type_map[type] - else: - raise Exception("Type {0} not supported, add to to template file!".format(type)) - - print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size)) - -} -#ifdef __cplusplus -@#class @(spec.short_name)_s { -struct __EXPORT @(spec.short_name)_s { -@#public: -#else -struct @(spec.short_name)_s { -#endif -@{ -# loop over all fields and print the type and name -for field in spec.parsed_fields(): - if (not field.is_header): - print_field_def(field) -}@ -}; - -/** - * @@} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(@(spec.short_name)); diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template index 622641617..f16095c97 100644 --- a/msg/templates/uorb/msg.h.template +++ b/msg/templates/uorb/msg.h.template @@ -77,10 +77,13 @@ for field in spec.parsed_fields(): print('#include '%(name)) }@ -@# Constants + +@# Constants c style +#ifndef __cplusplus @[for constant in spec.constants]@ #define @(constant.name) @(int(constant.val)) @[end for] +#endif /** * @@addtogroup topics @@ -101,8 +104,10 @@ type_map = {'int8': 'int8_t', 'uint32': 'uint32_t', 'uint64': 'uint64_t', 'float32': 'float', + 'float64': 'double', 'bool': 'bool', - 'fence_vertex': 'fence_vertex'} + 'fence_vertex': 'fence_vertex', + 'position_setpoint': 'position_setpoint'} # Function to print a standard ros type def print_field_def(field): @@ -146,6 +151,20 @@ for field in spec.parsed_fields(): if (not field.is_header): print_field_def(field) }@ +#ifdef __cplusplus +@# Constants again c++-ified +@{ +for constant in spec.constants: + type = constant.type + if type in type_map: + # need to add _t: int8 --> int8_t + type_px4 = type_map[type] + else: + raise Exception("Type {0} not supported, add to to template file!".format(type)) + + print('\tstatic const %s %s = %s;'%(type_px4, constant.name, int(constant.val))) +} +#endif }; /** -- cgit v1.2.3 From 2d124852c1881d5b993b3c2ec9f7a79e1e03da1b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 07:58:42 +0100 Subject: propagate uorb contants change through all modules/drivers --- src/drivers/blinkm/blinkm.cpp | 12 +- src/drivers/px4fmu/fmu.cpp | 16 +- src/modules/commander/commander.cpp | 300 ++++++++++----------- src/modules/commander/commander_helper.cpp | 12 +- src/modules/commander/state_machine_helper.cpp | 190 ++++++------- .../ekf_att_pos_estimator_main.cpp | 4 +- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 18 +- src/modules/mavlink/mavlink_main.cpp | 2 +- src/modules/mavlink/mavlink_messages.cpp | 56 ++-- src/modules/mavlink/mavlink_receiver.cpp | 6 +- src/modules/mc_pos_control/mc_pos_control_main.cpp | 8 +- src/modules/navigator/loiter.cpp | 2 +- src/modules/navigator/mission.cpp | 4 +- src/modules/navigator/mission_block.cpp | 10 +- src/modules/navigator/navigator_main.cpp | 30 +-- src/modules/sensors/sensors.cpp | 92 +++---- 16 files changed, 381 insertions(+), 381 deletions(-) diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 6b14f5945..d0253a8d1 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -571,7 +571,7 @@ BlinkM::led() printf(" cells found:%d\n", num_of_cells); } else { - if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { + if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { /* LED Pattern for battery critical alerting */ led_color_1 = LED_RED; led_color_2 = LED_RED; @@ -595,7 +595,7 @@ BlinkM::led() led_color_8 = LED_BLUE; led_blink = LED_BLINK; - } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { + } else if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) { /* LED Pattern for battery low warning */ led_color_1 = LED_YELLOW; led_color_2 = LED_YELLOW; @@ -647,14 +647,14 @@ BlinkM::led() if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { /* indicate main control state */ - if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL) + if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_POSCTL) led_color_4 = LED_GREEN; /* TODO: add other Auto modes */ - else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO_MISSION) + else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION) led_color_4 = LED_BLUE; - else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL) + else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_ALTCTL) led_color_4 = LED_YELLOW; - else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) + else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; else led_color_4 = LED_OFF; diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 2552e7e6a..db6b34f99 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -142,11 +142,11 @@ private: uint32_t _groups_required; uint32_t _groups_subscribed; - int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS]; - actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS]; - orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS]; + int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; orb_id_t _actuator_output_topic; - pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS]; + pollfd _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; unsigned _poll_fds_num; pwm_limit_t _pwm_limit; @@ -514,7 +514,7 @@ PX4FMU::subscribe() uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; _poll_fds_num = 0; - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { warnx("subscribe to actuator_controls_%d", i); _control_subs[i] = orb_subscribe(_control_topics[i]); @@ -591,7 +591,7 @@ PX4FMU::task_main() } debug("adjusted actuator update interval to %ums", update_rate_in_ms); - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { orb_set_interval(_control_subs[i], update_rate_in_ms); } @@ -618,7 +618,7 @@ PX4FMU::task_main() /* get controls for required topics */ unsigned poll_id = 0; - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[poll_id].revents & POLLIN) { orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); @@ -751,7 +751,7 @@ PX4FMU::task_main() } - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { ::close(_control_subs[i]); _control_subs[i] = -1; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a1c4e205d..8f491acae 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -368,31 +368,31 @@ void print_status() const char *armed_str; switch (state.arming_state) { - case ARMING_STATE_INIT: + case vehicle_status_s::ARMING_STATE_INIT: armed_str = "INIT"; break; - case ARMING_STATE_STANDBY: + case vehicle_status_s::ARMING_STATE_STANDBY: armed_str = "STANDBY"; break; - case ARMING_STATE_ARMED: + case vehicle_status_s::ARMING_STATE_ARMED: armed_str = "ARMED"; break; - case ARMING_STATE_ARMED_ERROR: + case vehicle_status_s::ARMING_STATE_ARMED_ERROR: armed_str = "ARMED_ERROR"; break; - case ARMING_STATE_STANDBY_ERROR: + case vehicle_status_s::ARMING_STATE_STANDBY_ERROR: armed_str = "STANDBY_ERROR"; break; - case ARMING_STATE_REBOOT: + case vehicle_status_s::ARMING_STATE_REBOOT: armed_str = "REBOOT"; break; - case ARMING_STATE_IN_AIR_RESTORE: + case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE: armed_str = "IN_AIR_RESTORE"; break; @@ -415,7 +415,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. - arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, + arming_res = arming_state_transition(&status, &safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local); if (arming_res == TRANSITION_CHANGED && mavlink_fd) { @@ -452,7 +452,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s transition_result_t main_ret = TRANSITION_NOT_CHANGED; /* set HIL state */ - hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; + hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? vehicle_status_s::HIL_STATE_ON : vehicle_status_s::HIL_STATE_OFF; transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd); // Transition the arming state @@ -462,43 +462,43 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s /* use autopilot-specific mode */ if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { /* MANUAL */ - main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { /* ALTCTL */ - main_ret = main_state_transition(status_local, MAIN_STATE_ALTCTL); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ALTCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { /* POSCTL */ - main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ - main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { /* ACRO */ - main_ret = main_state_transition(status_local, MAIN_STATE_ACRO); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ACRO); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { /* OFFBOARD */ - main_ret = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD); } } else { /* use base mode */ if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { /* AUTO */ - main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION); } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { /* POSCTL */ - main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ - main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL); } } } @@ -525,7 +525,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s // Flick to inair restore first if this comes from an onboard system if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) { - status_local->arming_state = ARMING_STATE_IN_AIR_RESTORE; + status_local->arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE; } transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command"); @@ -550,12 +550,12 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s unsigned int mav_goto = (cmd->param1 + 0.5f); if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status_local->nav_state = NAVIGATION_STATE_AUTO_LOITER; + status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; mavlink_log_critical(mavlink_fd, "Pause mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status_local->nav_state = NAVIGATION_STATE_AUTO_MISSION; + status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; mavlink_log_critical(mavlink_fd, "Continue mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -668,14 +668,14 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case VEHICLE_CMD_NAV_GUIDED_ENABLE: { transition_result_t res = TRANSITION_DENIED; - static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL; + static main_state_t main_state_pre_offboard =vehicle_status_s::MAIN_STATE_MANUAL; - if (status_local->main_state != MAIN_STATE_OFFBOARD) { + if (status_local->main_state !=vehicle_status_s::MAIN_STATE_OFFBOARD) { main_state_pre_offboard = status_local->main_state; } if (cmd->param1 > 0.5f) { - res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD); if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); @@ -798,41 +798,41 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_ef_time_thres = param_find("COM_EF_TIME"); param_t _param_autostart_id = param_find("SYS_AUTOSTART"); - const char *main_states_str[MAIN_STATE_MAX]; - main_states_str[MAIN_STATE_MANUAL] = "MANUAL"; - main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL"; - main_states_str[MAIN_STATE_POSCTL] = "POSCTL"; - main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; - main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; - main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; - main_states_str[MAIN_STATE_ACRO] = "ACRO"; - main_states_str[MAIN_STATE_OFFBOARD] = "OFFBOARD"; - - const char *arming_states_str[ARMING_STATE_MAX]; - arming_states_str[ARMING_STATE_INIT] = "INIT"; - arming_states_str[ARMING_STATE_STANDBY] = "STANDBY"; - arming_states_str[ARMING_STATE_ARMED] = "ARMED"; - arming_states_str[ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR"; - arming_states_str[ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; - arming_states_str[ARMING_STATE_REBOOT] = "REBOOT"; - arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; - - const char *nav_states_str[NAVIGATION_STATE_MAX]; - nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL"; - nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL"; - nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL"; - nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; - nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; - nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; - nav_states_str[NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER"; - nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; - nav_states_str[NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL"; - nav_states_str[NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL"; - nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; - nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; - nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; - nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION"; - nav_states_str[NAVIGATION_STATE_OFFBOARD] = "OFFBOARD"; + const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX]; + main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL"; + main_states_str[vehicle_status_s::MAIN_STATE_ALTCTL] = "ALTCTL"; + main_states_str[vehicle_status_s::MAIN_STATE_POSCTL] = "POSCTL"; + main_states_str[vehicle_status_s::MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; + main_states_str[vehicle_status_s::MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; + main_states_str[vehicle_status_s::MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; + main_states_str[vehicle_status_s::MAIN_STATE_ACRO] = "ACRO"; + main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD] = "OFFBOARD"; + + const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX]; + arming_states_str[vehicle_status_s::ARMING_STATE_INIT] = "INIT"; + arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY] = "STANDBY"; + arming_states_str[vehicle_status_s::ARMING_STATE_ARMED] = "ARMED"; + arming_states_str[vehicle_status_s::ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR"; + arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; + arming_states_str[vehicle_status_s::ARMING_STATE_REBOOT] = "REBOOT"; + arming_states_str[vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; + + const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX]; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_ACRO] = "ACRO"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_LAND] = "LAND"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_DESCEND] = "DESCEND"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_TERMINATION] = "TERMINATION"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_OFFBOARD] = "OFFBOARD"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -853,10 +853,10 @@ int commander_thread_main(int argc, char *argv[]) status.condition_landed = true; // initialize to safe value // We want to accept RC inputs as default status.rc_input_blocked = false; - status.main_state = MAIN_STATE_MANUAL; - status.nav_state = NAVIGATION_STATE_MANUAL; - status.arming_state = ARMING_STATE_INIT; - status.hil_state = HIL_STATE_OFF; + status.main_state =vehicle_status_s::MAIN_STATE_MANUAL; + status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; + status.arming_state = vehicle_status_s::ARMING_STATE_INIT; + status.hil_state = vehicle_status_s::HIL_STATE_OFF; status.failsafe = false; /* neither manual nor offboard control commands have been received */ @@ -869,7 +869,7 @@ int commander_thread_main(int argc, char *argv[]) status.data_link_lost = true; /* set battery warning flag */ - status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_NONE; status.condition_battery_voltage_valid = false; // XXX for now just set sensors as initialized @@ -1143,14 +1143,14 @@ int commander_thread_main(int argc, char *argv[]) } /* disable manual override for all systems that rely on electronic stabilization */ - if (status.system_type == VEHICLE_TYPE_COAXIAL || - status.system_type == VEHICLE_TYPE_HELICOPTER || - status.system_type == VEHICLE_TYPE_TRICOPTER || - status.system_type == VEHICLE_TYPE_QUADROTOR || - status.system_type == VEHICLE_TYPE_HEXAROTOR || - status.system_type == VEHICLE_TYPE_OCTOROTOR || - (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) || - (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) { + if (status.system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL || + status.system_type == vehicle_status_s::VEHICLE_TYPE_HELICOPTER || + status.system_type == vehicle_status_s::VEHICLE_TYPE_TRICOPTER || + status.system_type == vehicle_status_s::VEHICLE_TYPE_QUADROTOR || + status.system_type == vehicle_status_s::VEHICLE_TYPE_HEXAROTOR || + status.system_type == vehicle_status_s::VEHICLE_TYPE_OCTOROTOR || + (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) || + (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) { status.is_rotary_wing = true; @@ -1159,8 +1159,8 @@ int commander_thread_main(int argc, char *argv[]) } /* set vehicle_status.is_vtol flag */ - status.is_vtol = (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || - (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR); + status.is_vtol = (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) || + (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR); /* check and update system / component ID */ param_get(_param_system_id, &(status.system_id)); @@ -1310,9 +1310,9 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(safety), safety_sub, &safety); /* disarm if safety is now on and still armed */ - if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : - ARMING_STATE_STANDBY_ERROR); + if (status.hil_state == vehicle_status_s::HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { + arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY : + vehicle_status_s::ARMING_STATE_STANDBY_ERROR); if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { @@ -1344,7 +1344,7 @@ int commander_thread_main(int argc, char *argv[]) status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab; /* Make sure that this is only adjusted if vehicle realy is of type vtol*/ - if ((status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR)) { + if ((status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR)) { status.is_rotary_wing = vtol_status.vtol_in_rw_mode; } } @@ -1516,7 +1516,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) { low_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED"); - status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; + status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW; status_changed = true; } else if (!on_usb_power && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f @@ -1524,10 +1524,10 @@ int commander_thread_main(int argc, char *argv[]) /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); - status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; + status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, + arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { @@ -1535,7 +1535,7 @@ int commander_thread_main(int argc, char *argv[]) } } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, + arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { @@ -1549,9 +1549,9 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ /* If in INIT state, try to proceed to STANDBY state */ - if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { + if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { /* TODO: check for sensors */ - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, + arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { @@ -1662,14 +1662,14 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm * do it only for rotary wings */ if (status.is_rotary_wing && - (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) && + (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) && + (status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || status.main_state == vehicle_status_s::MAIN_STATE_ACRO || status.condition_landed) && sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : - ARMING_STATE_STANDBY_ERROR); + arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY : + vehicle_status_s::ARMING_STATE_STANDBY_ERROR); arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd); @@ -1688,7 +1688,7 @@ int commander_thread_main(int argc, char *argv[]) } /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ - if (status.arming_state == ARMING_STATE_STANDBY && + if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY && sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { @@ -1696,11 +1696,11 @@ int commander_thread_main(int argc, char *argv[]) * for being in manual mode only applies to manual arming actions. * the system can be armed in auto if armed via the GCS. */ - if (status.main_state != MAIN_STATE_MANUAL) { + if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL) { print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, + arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { @@ -1719,7 +1719,7 @@ int commander_thread_main(int argc, char *argv[]) } if (arming_ret == TRANSITION_CHANGED) { - if (status.arming_state == ARMING_STATE_ARMED) { + if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { mavlink_log_info(mavlink_fd, "ARMED by RC"); } else { @@ -1857,10 +1857,10 @@ int commander_thread_main(int argc, char *argv[]) /* At this point the data link and the gps system have been checked * If we are not in a manual (RC stick controlled mode) * and both failed we want to terminate the flight */ - if (status.main_state != MAIN_STATE_MANUAL && - status.main_state != MAIN_STATE_ACRO && - status.main_state != MAIN_STATE_ALTCTL && - status.main_state != MAIN_STATE_POSCTL && + if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL && + status.main_state !=vehicle_status_s::MAIN_STATE_ACRO && + status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL && + status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL && ((status.data_link_lost && status.gps_failure) || (status.data_link_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; @@ -1881,10 +1881,10 @@ int commander_thread_main(int argc, char *argv[]) /* At this point the rc signal and the gps system have been checked * If we are in manual (controlled with RC): * if both failed we want to terminate the flight */ - if ((status.main_state == MAIN_STATE_ACRO || - status.main_state == MAIN_STATE_MANUAL || - status.main_state == MAIN_STATE_ALTCTL || - status.main_state == MAIN_STATE_POSCTL) && + if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO || + status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL || + status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL || + status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) && ((status.rc_signal_lost && status.gps_failure) || (status.rc_signal_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; @@ -1976,11 +1976,11 @@ int commander_thread_main(int argc, char *argv[]) set_tune(TONE_ARMING_WARNING_TUNE); arm_tune_played = true; - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { + } else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ set_tune(TONE_BATTERY_WARNING_FAST_TUNE); - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe) { + } else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status.failsafe) { /* play tune on battery warning or failsafe */ set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); @@ -2072,15 +2072,15 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu bool set_normal_color = false; /* set mode */ - if (status_local->arming_state == ARMING_STATE_ARMED) { + if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) { rgbled_set_mode(RGBLED_MODE_ON); set_normal_color = true; - } else if (status_local->arming_state == ARMING_STATE_ARMED_ERROR) { + } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { rgbled_set_mode(RGBLED_MODE_BLINK_FAST); rgbled_set_color(RGBLED_COLOR_RED); - } else if (status_local->arming_state == ARMING_STATE_STANDBY) { + } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { rgbled_set_mode(RGBLED_MODE_BREATHE); set_normal_color = true; @@ -2091,9 +2091,9 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu if (set_normal_color) { /* set color */ - if (status_local->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) { + if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) { rgbled_set_color(RGBLED_COLOR_AMBER); - /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ + /* vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL handled as vehicle_status_s::ARMING_STATE_ARMED_ERROR / vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ } else { if (status_local->condition_local_position_valid) { @@ -2149,12 +2149,12 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s /* if offboard is set allready by a mavlink command, abort */ if (status.offboard_control_set_by_command) { - return main_state_transition(status_local, MAIN_STATE_OFFBOARD); + return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD); } /* offboard switch overrides main switch */ - if (sp_man->offboard_switch == SWITCH_POS_ON) { - res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD); if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); @@ -2166,24 +2166,24 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s /* offboard switched off or denied, check main mode switch */ switch (sp_man->mode_switch) { - case SWITCH_POS_NONE: + case manual_control_setpoint_s::SWITCH_POS_NONE: res = TRANSITION_NOT_CHANGED; break; - case SWITCH_POS_OFF: // MANUAL - if (sp_man->acro_switch == SWITCH_POS_ON) { - res = main_state_transition(status_local, MAIN_STATE_ACRO); + case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL + if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO); } else { - res = main_state_transition(status_local, MAIN_STATE_MANUAL); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL); } // TRANSITION_DENIED is not possible here break; - case SWITCH_POS_MIDDLE: // ASSIST - if (sp_man->posctl_switch == SWITCH_POS_ON) { - res = main_state_transition(status_local, MAIN_STATE_POSCTL); + case manual_control_setpoint_s::SWITCH_POS_MIDDLE: // ASSIST + if (sp_man->posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -2193,24 +2193,24 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } // fallback to ALTCTL - res = main_state_transition(status_local, MAIN_STATE_ALTCTL); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } - if (sp_man->posctl_switch != SWITCH_POS_ON) { + if (sp_man->posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) { print_reject_mode(status_local, "ALTCTL"); } // fallback to MANUAL - res = main_state_transition(status_local, MAIN_STATE_MANUAL); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; - case SWITCH_POS_ON: // AUTO - if (sp_man->return_switch == SWITCH_POS_ON) { - res = main_state_transition(status_local, MAIN_STATE_AUTO_RTL); + case manual_control_setpoint_s::SWITCH_POS_ON: // AUTO + if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -2219,14 +2219,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s print_reject_mode(status_local, "AUTO_RTL"); // fallback to LOITER if home position not set - res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - } else if (sp_man->loiter_switch == SWITCH_POS_ON) { - res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); + } else if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -2235,7 +2235,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s print_reject_mode(status_local, "AUTO_LOITER"); } else { - res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_MISSION); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -2244,7 +2244,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s print_reject_mode(status_local, "AUTO_MISSION"); // fallback to LOITER if home position not set - res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -2252,21 +2252,21 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } // fallback to POSCTL - res = main_state_transition(status_local, MAIN_STATE_POSCTL); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to ALTCTL - res = main_state_transition(status_local, MAIN_STATE_ALTCTL); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to MANUAL - res = main_state_transition(status_local, MAIN_STATE_MANUAL); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; @@ -2283,11 +2283,11 @@ set_control_mode() /* set vehicle_control_mode according to set_navigation_state */ control_mode.flag_armed = armed.armed; control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol); - control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; + control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON; control_mode.flag_control_offboard_enabled = false; switch (status.nav_state) { - case NAVIGATION_STATE_MANUAL: + case vehicle_status_s::NAVIGATION_STATE_MANUAL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab); @@ -2299,7 +2299,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_ALTCTL: + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -2311,7 +2311,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_POSCTL: + case vehicle_status_s::NAVIGATION_STATE_POSCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -2323,12 +2323,12 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_AUTO_MISSION: - case NAVIGATION_STATE_AUTO_LOITER: - case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_RCRECOVER: - case NAVIGATION_STATE_AUTO_RTGS: - case NAVIGATION_STATE_AUTO_LANDENGFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; @@ -2340,7 +2340,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -2352,7 +2352,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_ACRO: + case vehicle_status_s::NAVIGATION_STATE_ACRO: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -2365,7 +2365,7 @@ set_control_mode() break; - case NAVIGATION_STATE_LAND: + case vehicle_status_s::NAVIGATION_STATE_LAND: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; @@ -2378,7 +2378,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_DESCEND: + case vehicle_status_s::NAVIGATION_STATE_DESCEND: /* TODO: check if this makes sense */ control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; @@ -2391,7 +2391,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_TERMINATION: + case vehicle_status_s::NAVIGATION_STATE_TERMINATION: /* disable all controllers on termination */ control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = false; @@ -2404,7 +2404,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = true; break; - case NAVIGATION_STATE_OFFBOARD: + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_offboard_enabled = true; @@ -2603,7 +2603,7 @@ void *commander_low_prio_loop(void *arg) int calib_ret = ERROR; /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; @@ -2667,7 +2667,7 @@ void *commander_low_prio_loop(void *arg) tune_negative(true); } - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); break; } diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 2022e99fb..8a4451100 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -72,16 +72,16 @@ static const int ERROR = -1; bool is_multirotor(const struct vehicle_status_s *current_status) { - return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || - (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); + return ((current_status->system_type == vehicle_status_s::VEHICLE_TYPE_QUADROTOR) || + (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_OCTOROTOR) || + (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_TRICOPTER)); } bool is_rotary_wing(const struct vehicle_status_s *current_status) { - return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) - || (current_status->system_type == VEHICLE_TYPE_COAXIAL); + return is_multirotor(current_status) || (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_HELICOPTER) + || (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL); } static int buzzer = -1; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 465f9cdc5..40da9c77b 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -76,19 +76,19 @@ static const int ERROR = -1; // will be true for a valid transition or false for a invalid transition. In some cases even // though the transition is marked as true additional checks must be made. See arming_state_transition // code for those checks. -static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = { +static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = { // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE - { /* ARMING_STATE_INIT */ true, true, false, false, false, false, false }, - { /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, - { /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, - { /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, - { /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, - { /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, - { /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI + { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, false, false, false }, + { /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, + { /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, + { /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, + { /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, + { /* vehicle_status_s::ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, + { /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI }; // You can index into the array with an arming_state_t in order to get it's textual representation -static const char * const state_names[ARMING_STATE_MAX] = { +static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = { "ARMING_STATE_INIT", "ARMING_STATE_STANDBY", "ARMING_STATE_ARMED", @@ -107,8 +107,8 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none { // Double check that our static arrays are still valid - ASSERT(ARMING_STATE_INIT == 0); - ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1); + ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0); + ASSERT(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1); transition_result_t ret = TRANSITION_DENIED; arming_state_t current_arming_state = status->arming_state; @@ -126,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s int prearm_ret = OK; /* only perform the check if we have to */ - if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) { + if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { prearm_ret = prearm_check(status, mavlink_fd); } @@ -136,7 +136,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s irqstate_t flags = irqsave(); /* enforce lockdown in HIL */ - if (status->hil_state == HIL_STATE_ON) { + if (status->hil_state == vehicle_status_s::HIL_STATE_ON) { armed->lockdown = true; } else { @@ -148,12 +148,12 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s if (valid_transition) { // We have a good transition. Now perform any secondary validation. - if (new_arming_state == ARMING_STATE_ARMED) { + if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { // Do not perform pre-arm checks if coming from in air restore - // Allow if HIL_STATE_ON - if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && - status->hil_state == HIL_STATE_OFF) { + // Allow if vehicle_status_s::HIL_STATE_ON + if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE && + status->hil_state == vehicle_status_s::HIL_STATE_OFF) { // Fail transition if pre-arm check fails if (prearm_ret) { @@ -200,18 +200,18 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s } - } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { - new_arming_state = ARMING_STATE_STANDBY_ERROR; + } else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { + new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; } } // HIL can always go to standby - if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) { + if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { valid_transition = true; } /* Sensors need to be initialized for STANDBY state */ - if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { + if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational."); feedback_provided = true; valid_transition = false; @@ -219,8 +219,8 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // Finish up the state transition if (valid_transition) { - armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR; - armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY; + armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR; + armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY; ret = TRANSITION_CHANGED; status->arming_state = new_arming_state; } @@ -264,12 +264,12 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta /* transition may be denied even if the same state is requested because conditions may have changed */ switch (new_main_state) { - case MAIN_STATE_MANUAL: - case MAIN_STATE_ACRO: + case vehicle_status_s::MAIN_STATE_MANUAL: + case vehicle_status_s::MAIN_STATE_ACRO: ret = TRANSITION_CHANGED; break; - case MAIN_STATE_ALTCTL: + case vehicle_status_s::MAIN_STATE_ALTCTL: /* need at minimum altitude estimate */ /* TODO: add this for fixedwing as well */ if (!status->is_rotary_wing || @@ -279,7 +279,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } break; - case MAIN_STATE_POSCTL: + case vehicle_status_s::MAIN_STATE_POSCTL: /* need at minimum local position estimate */ if (status->condition_local_position_valid || status->condition_global_position_valid) { @@ -287,22 +287,22 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } break; - case MAIN_STATE_AUTO_LOITER: + case vehicle_status_s::MAIN_STATE_AUTO_LOITER: /* need global position estimate */ if (status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } break; - case MAIN_STATE_AUTO_MISSION: - case MAIN_STATE_AUTO_RTL: + case vehicle_status_s::MAIN_STATE_AUTO_MISSION: + case vehicle_status_s::MAIN_STATE_AUTO_RTL: /* need global position and home position */ if (status->condition_global_position_valid && status->condition_home_position_valid) { ret = TRANSITION_CHANGED; } break; - case MAIN_STATE_OFFBOARD: + case vehicle_status_s::MAIN_STATE_OFFBOARD: /* need offboard signal */ if (!status->offboard_control_signal_lost) { @@ -311,7 +311,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; - case MAIN_STATE_MAX: + case vehicle_status_s::MAIN_STATE_MAX: default: break; } @@ -338,16 +338,16 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, } else { switch (new_state) { - case HIL_STATE_OFF: + case vehicle_status_s::HIL_STATE_OFF: /* we're in HIL and unexpected things can happen if we disable HIL now */ mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)"); ret = TRANSITION_DENIED; break; - case HIL_STATE_ON: - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY - || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) { + case vehicle_status_s::HIL_STATE_ON: + if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT + || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY + || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { /* Disable publication of all attached sensors */ /* list directory */ @@ -448,55 +448,55 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en { navigation_state_t nav_state_old = status->nav_state; - bool armed = (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR); + bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR); status->failsafe = false; /* evaluate main state to decide in normal (non-failsafe) mode */ switch (status->main_state) { - case MAIN_STATE_ACRO: - case MAIN_STATE_MANUAL: - case MAIN_STATE_ALTCTL: - case MAIN_STATE_POSCTL: + case vehicle_status_s::MAIN_STATE_ACRO: + case vehicle_status_s::MAIN_STATE_MANUAL: + case vehicle_status_s::MAIN_STATE_ALTCTL: + case vehicle_status_s::MAIN_STATE_POSCTL: /* require RC for all manual modes */ if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } } else { switch (status->main_state) { - case MAIN_STATE_ACRO: - status->nav_state = NAVIGATION_STATE_ACRO; + case vehicle_status_s::MAIN_STATE_ACRO: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO; break; - case MAIN_STATE_MANUAL: - status->nav_state = NAVIGATION_STATE_MANUAL; + case vehicle_status_s::MAIN_STATE_MANUAL: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; break; - case MAIN_STATE_ALTCTL: - status->nav_state = NAVIGATION_STATE_ALTCTL; + case vehicle_status_s::MAIN_STATE_ALTCTL: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; break; - case MAIN_STATE_POSCTL: - status->nav_state = NAVIGATION_STATE_POSCTL; + case vehicle_status_s::MAIN_STATE_POSCTL: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; break; default: - status->nav_state = NAVIGATION_STATE_MANUAL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; break; } } break; - case MAIN_STATE_AUTO_MISSION: + case vehicle_status_s::MAIN_STATE_AUTO_MISSION: /* go into failsafe * - if commanded to do so @@ -505,19 +505,19 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en /* first look at the commands */ if (status->engine_failure_cmd) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (status->data_link_lost_cmd) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; } else if (status->gps_failure_cmd) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->rc_signal_lost_cmd) { - status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; /* finished handling commands which have priority, now handle failures */ } else if (status->gps_failure) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->engine_failure) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; /* datalink loss enabled: * check for datalink lost: this should always trigger RTGS */ @@ -525,13 +525,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } /* datalink loss disabled: @@ -542,37 +542,37 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } /* stay where you are if you should stay in failsafe, otherwise everything is perfect */ } else if (!stay_in_failsafe){ - status->nav_state = NAVIGATION_STATE_AUTO_MISSION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; } break; - case MAIN_STATE_AUTO_LOITER: + case vehicle_status_s::MAIN_STATE_AUTO_LOITER: /* go into failsafe on a engine failure */ if (status->engine_failure) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; /* also go into failsafe if just datalink is lost */ } else if (status->data_link_lost && data_link_loss_enabled) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } /* go into failsafe if RC is lost and datalink loss is not set up */ @@ -580,65 +580,65 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } /* don't bother if RC is lost if datalink is connected */ } else if (status->rc_signal_lost) { /* this mode is ok, we don't need RC for loitering */ - status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; } else { /* everything is perfect */ - status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; } break; - case MAIN_STATE_AUTO_RTL: + case vehicle_status_s::MAIN_STATE_AUTO_RTL: /* require global position and home, also go into failsafe on an engine failure */ if (status->engine_failure) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) { status->failsafe = true; if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } } else { - status->nav_state = NAVIGATION_STATE_AUTO_RTL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; } break; - case MAIN_STATE_OFFBOARD: + case vehicle_status_s::MAIN_STATE_OFFBOARD: /* require offboard control, otherwise stay where you are */ if (status->offboard_control_signal_lost && !status->rc_signal_lost) { status->failsafe = true; - status->nav_state = NAVIGATION_STATE_POSCTL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; } else if (status->offboard_control_signal_lost && status->rc_signal_lost) { status->failsafe = true; if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } } else { - status->nav_state = NAVIGATION_STATE_OFFBOARD; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD; } default: break; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index d51075b8c..e33691b0c 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -817,7 +817,7 @@ FixedwingEstimator::task_main() if (fds[1].revents & POLLIN) { /* check vehicle status for changes to publication state */ - bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON); + bool prev_hil = (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON); vehicle_status_poll(); bool accel_updated; @@ -826,7 +826,7 @@ FixedwingEstimator::task_main() perf_count(_perf_gyro); /* Reset baro reference if switching to HIL, reset sensor states */ - if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { + if (!prev_hil && (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON)) { /* system is in HIL now, wait for measurements to come in one last round */ usleep(60000); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 74249c9c5..0bdc285e7 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -816,7 +816,7 @@ void FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet) { - if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) { + if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) { /* rotate ground speed vector with current attitude */ math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); @@ -963,7 +963,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - if (pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION) { + if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { /* waypoint is a plain navigation waypoint */ _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); @@ -974,7 +974,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); - } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { + } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { /* waypoint is a loiter waypoint */ _l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius, @@ -987,7 +987,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); - } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); @@ -1140,7 +1140,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi ground_speed); } - } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { + } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { /* Perform launch detection */ if (launchDetector.launchDetectionEnabled() && @@ -1235,12 +1235,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } /* reset landing state */ - if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) { + if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LAND) { reset_landing_state(); } /* reset takeoff/launch state */ - if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) { + if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { reset_takeoff_state(); } @@ -1327,7 +1327,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Set thrust to 0 to minimize damage */ _att_sp.thrust = 0.0f; } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto - pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons */ @@ -1341,7 +1341,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* During a takeoff waypoint while waiting for launch the pitch sp is set * already (not by tecs) */ if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && - pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && launch_detection_state == LAUNCHDETECTION_RES_NONE)) { _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); } diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9e4ab00df..676b65adc 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1431,7 +1431,7 @@ Mavlink::task_main(int argc, char *argv[]) if (status_sub->update(&status_time, &status)) { /* switch HIL mode if required */ - set_hil_enabled(status.hil_state == HIL_STATE_ON); + set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON); } /* check for requested subscriptions */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 6765100c7..6642fb2ac 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -103,13 +103,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_custom_mode = 0; /* HIL */ - if (status->hil_state == HIL_STATE_ON) { + if (status->hil_state == vehicle_status_s::HIL_STATE_ON) { *mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; } /* arming state */ - if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + if (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED + || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } @@ -121,31 +121,31 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set switch (status->nav_state) { - case NAVIGATION_STATE_MANUAL: + case vehicle_status_s::NAVIGATION_STATE_MANUAL: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; break; - case NAVIGATION_STATE_ACRO: + case vehicle_status_s::NAVIGATION_STATE_ACRO: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; break; - case NAVIGATION_STATE_ALTCTL: + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; break; - case NAVIGATION_STATE_POSCTL: + case vehicle_status_s::NAVIGATION_STATE_POSCTL: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; break; - case NAVIGATION_STATE_AUTO_MISSION: + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -153,7 +153,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; break; - case NAVIGATION_STATE_AUTO_LOITER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -161,9 +161,9 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; break; - case NAVIGATION_STATE_AUTO_RTL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: /* fallthrough */ - case NAVIGATION_STATE_AUTO_RCRECOVER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -171,11 +171,11 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; break; - case NAVIGATION_STATE_LAND: - case NAVIGATION_STATE_AUTO_LANDENGFAIL: - case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + case vehicle_status_s::NAVIGATION_STATE_LAND: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: /* fallthrough */ - case NAVIGATION_STATE_DESCEND: + case vehicle_status_s::NAVIGATION_STATE_DESCEND: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -183,7 +183,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; break; - case NAVIGATION_STATE_AUTO_RTGS: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -191,19 +191,19 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS; break; - case NAVIGATION_STATE_TERMINATION: + case vehicle_status_s::NAVIGATION_STATE_TERMINATION: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; break; - case NAVIGATION_STATE_OFFBOARD: + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD; break; - case NAVIGATION_STATE_MAX: + case vehicle_status_s::NAVIGATION_STATE_MAX: /* this is an unused case, ignore */ break; @@ -212,21 +212,21 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_custom_mode = custom_mode.data; /* set system state */ - if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE - || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + if (status->arming_state == vehicle_status_s::ARMING_STATE_INIT + || status->arming_state == vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE + || status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { // TODO review *mavlink_state = MAV_STATE_UNINIT; - } else if (status->arming_state == ARMING_STATE_ARMED) { + } else if (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED) { *mavlink_state = MAV_STATE_ACTIVE; - } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { + } else if (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { *mavlink_state = MAV_STATE_CRITICAL; - } else if (status->arming_state == ARMING_STATE_STANDBY) { + } else if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { *mavlink_state = MAV_STATE_STANDBY; - } else if (status->arming_state == ARMING_STATE_REBOOT) { + } else if (status->arming_state == vehicle_status_s::ARMING_STATE_REBOOT) { *mavlink_state = MAV_STATE_POWEROFF; } else { @@ -1438,7 +1438,7 @@ protected: updated |= _pos_sp_triplet_sub->update(&_pos_sp_triplet_time, &pos_sp_triplet); updated |= _status_sub->update(&_status_time, &status); - if (updated && (status.arming_state == ARMING_STATE_ARMED)) { + if (updated && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state; uint8_t mavlink_base_mode; @@ -2205,7 +2205,7 @@ protected: msg.param2 = 0; msg.param3 = 0; /* set camera capture ON/OFF depending on arming state */ - msg.param4 = (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) ? 1 : 0; + msg.param4 = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) ? 1 : 0; msg.param5 = 0; msg.param6 = 0; msg.param7 = 0; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 2e28a6d2c..b1ba91cac 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -617,7 +617,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t pos_sp_triplet.previous.valid = false; pos_sp_triplet.next.valid = false; pos_sp_triplet.current.valid = true; - pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others + pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; //XXX support others /* set the local pos values if the setpoint type is 'local pos' and none * of the local pos fields is set to 'ignore' */ @@ -986,10 +986,10 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) } else if (tsync.tc1 > 0) { - int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ; + int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ; int64_t dt = _time_offset - offset_ns; - if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew + if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew _time_offset = offset_ns; warnx("[timesync] Hard setting offset."); } else { 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 60682fb8e..b9692ffbf 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -768,7 +768,7 @@ MulticopterPositionControl::control_auto(float dt) /* by default use current setpoint as is */ math::Vector<3> pos_sp_s = curr_sp_s; - if (_pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) { + if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) { /* follow "previous - current" line */ math::Vector<3> prev_sp; map_projection_project(&_ref_pos, @@ -998,7 +998,7 @@ MulticopterPositionControl::task_main() } - if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); @@ -1037,7 +1037,7 @@ MulticopterPositionControl::task_main() } /* use constant descend rate when landing, ignore altitude setpoint */ - if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { _vel_sp(2) = _params.land_speed; } @@ -1124,7 +1124,7 @@ MulticopterPositionControl::task_main() /* adjust limits for landing mode */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && - _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.tilt_max_land; diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index f827e70c9..a744d58cf 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -82,7 +82,7 @@ Loiter::on_activation() mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); _navigator->set_position_setpoint_triplet_updated(); } diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 9b0a092da..b87bebd0c 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -388,7 +388,7 @@ Mission::set_mission_items() pos_sp_triplet->next.valid = false; /* reuse setpoint for LOITER only if it's not IDLE */ - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); set_mission_finished(); @@ -462,7 +462,7 @@ Mission::set_mission_items() mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); /* require takeoff after landing or idle */ - if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) { + if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _need_takeoff = true; } diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 723caec7c..c936489d5 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -193,25 +193,25 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite switch (item->nav_cmd) { case NAV_CMD_IDLE: - sp->type = SETPOINT_TYPE_IDLE; + sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE; break; case NAV_CMD_TAKEOFF: - sp->type = SETPOINT_TYPE_TAKEOFF; + sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; break; case NAV_CMD_LAND: - sp->type = SETPOINT_TYPE_LAND; + sp->type = position_setpoint_s::SETPOINT_TYPE_LAND; break; case NAV_CMD_LOITER_TIME_LIMIT: case NAV_CMD_LOITER_TURN_COUNT: case NAV_CMD_LOITER_UNLIMITED: - sp->type = SETPOINT_TYPE_LOITER; + sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER; break; default: - sp->type = SETPOINT_TYPE_POSITION; + sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION; break; } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 3f7670ec4..e35b0bd6a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -217,7 +217,7 @@ Navigator::vehicle_status_update() { if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) { /* in case the commander is not be running */ - _vstatus.arming_state = ARMING_STATE_STANDBY; + _vstatus.arming_state = vehicle_status_s::ARMING_STATE_STANDBY; } } @@ -419,25 +419,25 @@ Navigator::task_main() /* Do stuff according to navigation state set by commander */ switch (_vstatus.nav_state) { - case NAVIGATION_STATE_MANUAL: - case NAVIGATION_STATE_ACRO: - case NAVIGATION_STATE_ALTCTL: - case NAVIGATION_STATE_POSCTL: - case NAVIGATION_STATE_LAND: - case NAVIGATION_STATE_TERMINATION: - case NAVIGATION_STATE_OFFBOARD: + case vehicle_status_s::NAVIGATION_STATE_MANUAL: + case vehicle_status_s::NAVIGATION_STATE_ACRO: + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: + case vehicle_status_s::NAVIGATION_STATE_POSCTL: + case vehicle_status_s::NAVIGATION_STATE_LAND: + case vehicle_status_s::NAVIGATION_STATE_TERMINATION: + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; - case NAVIGATION_STATE_AUTO_MISSION: + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_mission; break; - case NAVIGATION_STATE_AUTO_LOITER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_loiter; break; - case NAVIGATION_STATE_AUTO_RCRECOVER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: _pos_sp_triplet_published_invalid_once = false; if (_param_rcloss_obc.get() != 0) { _navigation_mode = &_rcLoss; @@ -445,11 +445,11 @@ Navigator::task_main() _navigation_mode = &_rtl; } break; - case NAVIGATION_STATE_AUTO_RTL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_rtl; break; - case NAVIGATION_STATE_AUTO_RTGS: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ _pos_sp_triplet_published_invalid_once = false; @@ -459,11 +459,11 @@ Navigator::task_main() _navigation_mode = &_rtl; } break; - case NAVIGATION_STATE_AUTO_LANDENGFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_engineFailure; break; - case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_gpsFailure; break; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6aa6b6bbd..82bb1eb8e 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -813,28 +813,28 @@ Sensors::parameters_update() _parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th); /* update RC function mappings */ - _rc.function[RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1; - _rc.function[RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1; - _rc.function[RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1; - _rc.function[RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1; - - _rc.function[RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1; - _rc.function[RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1; - _rc.function[RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1; - _rc.function[RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1; - _rc.function[RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1; - _rc.function[RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1; - - _rc.function[RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1; - - _rc.function[RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1; - _rc.function[RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1; - _rc.function[RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1; - _rc.function[RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1; - _rc.function[RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1; + + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1; + + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1; + + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1; for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - _rc.function[RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1; } /* gyro offsets */ @@ -1498,7 +1498,7 @@ Sensors::rc_parameter_map_poll(bool forced) orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map); /* update paramter handles to which the RC channels are mapped */ for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - if (_rc.function[RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { + if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) * or no request to map this channel to a param has been sent via mavlink */ @@ -1673,17 +1673,17 @@ Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mi float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; if (on_inv ? value < on_th : value > on_th) { - return SWITCH_POS_ON; + return manual_control_setpoint_s::SWITCH_POS_ON; } else if (mid_inv ? value < mid_th : value > mid_th) { - return SWITCH_POS_MIDDLE; + return manual_control_setpoint_s::SWITCH_POS_MIDDLE; } else { - return SWITCH_POS_OFF; + return manual_control_setpoint_s::SWITCH_POS_OFF; } } else { - return SWITCH_POS_NONE; + return manual_control_setpoint_s::SWITCH_POS_NONE; } } @@ -1694,14 +1694,14 @@ Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv) float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; if (on_inv ? value < on_th : value > on_th) { - return SWITCH_POS_ON; + return manual_control_setpoint_s::SWITCH_POS_ON; } else { - return SWITCH_POS_OFF; + return manual_control_setpoint_s::SWITCH_POS_OFF; } } else { - return SWITCH_POS_NONE; + return manual_control_setpoint_s::SWITCH_POS_NONE; } } @@ -1710,14 +1710,14 @@ Sensors::set_params_from_rc() { static float param_rc_values[RC_PARAM_MAP_NCHAN] = {}; for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - if (_rc.function[RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { + if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) * or no request to map this channel to a param has been sent via mavlink */ continue; } - float rc_val = get_rc_value((RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0); + float rc_val = get_rc_value( (rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0); /* Check if the value has changed, * maybe we need to introduce a more aggressive limit here */ if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) { @@ -1847,24 +1847,24 @@ Sensors::rc_poll() manual.timestamp = rc_input.timestamp_last_signal; /* limit controls */ - manual.y = get_rc_value(RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); - manual.x = get_rc_value(RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); - manual.r = get_rc_value(RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); - manual.z = get_rc_value(RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); - manual.flaps = get_rc_value(RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); - manual.aux1 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); - manual.aux2 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); - manual.aux3 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); - manual.aux4 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); - manual.aux5 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0); + manual.y = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); + manual.x = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); + manual.r = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); + manual.z = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); + manual.flaps = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); + manual.aux1 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); + manual.aux2 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); + manual.aux3 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); + manual.aux4 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); + manual.aux5 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0); /* mode switches */ - manual.mode_switch = get_rc_sw3pos_position(RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); - manual.posctl_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); - manual.return_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); - manual.loiter_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); - manual.acro_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); - manual.offboard_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv); + manual.mode_switch = get_rc_sw3pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); + manual.posctl_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); + manual.return_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); + manual.loiter_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); + manual.acro_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); + manual.offboard_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv); /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { -- cgit v1.2.3 From 719edf93e4c3994d3d61bff4435d5ac0760d912b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 08:01:22 +0100 Subject: ported more geo functions to cpp --- src/platforms/ros/geo.cpp | 93 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 93 insertions(+) diff --git a/src/platforms/ros/geo.cpp b/src/platforms/ros/geo.cpp index 6fad681c9..04094de8b 100644 --- a/src/platforms/ros/geo.cpp +++ b/src/platforms/ros/geo.cpp @@ -171,3 +171,96 @@ __EXPORT float _wrap_360(float bearing) return bearing; } + +__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref) +{ + return ref->init_done; +} + +__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref) +{ + return ref->timestamp; +} + +__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +{ + + ref->lat_rad = lat_0 * M_DEG_TO_RAD; + ref->lon_rad = lon_0 * M_DEG_TO_RAD; + ref->sin_lat = sin(ref->lat_rad); + ref->cos_lat = cos(ref->lat_rad); + + ref->timestamp = timestamp; + ref->init_done = true; + + return 0; +} + +__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +{ + return map_projection_init_timestamped(ref, lat_0, lon_0, px4::get_time_micros()); +} + +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad) +{ + if (!map_projection_initialized(ref)) { + return -1; + } + + *ref_lat_rad = ref->lat_rad; + *ref_lon_rad = ref->lon_rad; + + return 0; +} + +__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y) +{ + if (!map_projection_initialized(ref)) { + return -1; + } + + double lat_rad = lat * M_DEG_TO_RAD; + double lon_rad = lon * M_DEG_TO_RAD; + + double sin_lat = sin(lat_rad); + double cos_lat = cos(lat_rad); + double cos_d_lon = cos(lon_rad - ref->lon_rad); + + double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon); + double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c)); + + *x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; + *y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH; + + return 0; +} + +__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) +{ + if (!map_projection_initialized(ref)) { + return -1; + } + + double x_rad = x / CONSTANTS_RADIUS_OF_EARTH; + double y_rad = y / CONSTANTS_RADIUS_OF_EARTH; + double c = sqrtf(x_rad * x_rad + y_rad * y_rad); + double sin_c = sin(c); + double cos_c = cos(c); + + double lat_rad; + double lon_rad; + + if (fabs(c) > DBL_EPSILON) { + lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c); + lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c)); + + } else { + lat_rad = ref->lat_rad; + lon_rad = ref->lon_rad; + } + + *lat = lat_rad * 180.0 / M_PI; + *lon = lon_rad * 180.0 / M_PI; + + return 0; +} -- cgit v1.2.3 From 6ba1912309fef5b7aa3fc2cda73a124b6b01a01f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 08:02:08 +0100 Subject: add angle conversion defines for ros --- src/platforms/px4_defines.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index c8e2cf290..fa4e1398e 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -135,6 +135,8 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) #define M_IVLN10_F 0.43429448190325182765f /* 1 / log(10) */ #define M_LOG2_E_F _M_LN2_F #define M_INVLN2_F 1.4426950408889633870E0f /* 1 / log(2) */ +#define M_DEG_TO_RAD 0.01745329251994 +#define M_RAD_TO_DEG 57.2957795130823 #else /* -- cgit v1.2.3 From 19155372810e2f70d273b7d06069748d1e01071b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 08:04:00 +0100 Subject: initial port of multiplatform version of mc_pos_control --- CMakeLists.txt | 15 + ROMFS/px4fmu_common/init.d/rc.mc_apps | 8 +- makefiles/config_px4fmu-v2_multiplatform.mk | 3 +- .../mc_pos_control.cpp | 947 +++++++++++++++++++++ .../mc_pos_control_multiplatform/mc_pos_control.h | 219 +++++ .../mc_pos_control_main.cpp | 63 ++ .../mc_pos_control_params.c | 212 +++++ .../mc_pos_control_params.h | 61 ++ .../mc_pos_control_start_nuttx.cpp | 99 +++ src/modules/mc_pos_control_multiplatform/module.mk | 43 + src/platforms/px4_includes.h | 8 + 11 files changed, 1676 insertions(+), 2 deletions(-) create mode 100644 src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp create mode 100644 src/modules/mc_pos_control_multiplatform/mc_pos_control.h create mode 100644 src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp create mode 100644 src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c create mode 100644 src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h create mode 100644 src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp create mode 100644 src/modules/mc_pos_control_multiplatform/module.mk diff --git a/CMakeLists.txt b/CMakeLists.txt index f76dbbf41..6a368d78f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,6 +69,11 @@ add_message_files( actuator_armed.msg parameter_update.msg vehicle_status.msg + vehicle_local_position.msg + position_setpoint.msg + position_setpoint_triplet.msg + vehicle_local_position_setpoint.msg + vehicle_global_velocity_setpoint.msg ) ## Generate services in the 'srv' folder @@ -180,6 +185,16 @@ target_link_libraries(mc_att_control px4 ) +## MC Position Control +add_executable(mc_pos_control + src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp + src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp) +add_dependencies(mc_pos_control ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(mc_pos_control + ${catkin_LIBRARIES} + px4 +) + ## Attitude Estimator dummy add_executable(attitude_estimator src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 7b29fb3a7..2ecc104df 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -14,7 +14,13 @@ else # try the multiplatform version mc_att_control_m start fi -mc_pos_control start + +if mc_pos_control start +then +else + # try the multiplatform version + mc_pos_control_m start +fi # # Start Land Detector diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk index 76edade4b..29eb68096 100644 --- a/makefiles/config_px4fmu-v2_multiplatform.mk +++ b/makefiles/config_px4fmu-v2_multiplatform.mk @@ -89,7 +89,8 @@ MODULES += modules/position_estimator_inav MODULES += modules/mc_att_control_multiplatform MODULES += examples/subscriber MODULES += examples/publisher -MODULES += modules/mc_pos_control +# MODULES += modules/mc_pos_control +MODULES += modules/mc_pos_control_multiplatform MODULES += modules/vtol_att_control # diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp new file mode 100644 index 000000000..2685a99c3 --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -0,0 +1,947 @@ +/**************************************************************************** + * + * 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_pos_control.cpp + * Multicopter position controller. + * + * @author Anton Babushkin + * @author Thomas Gubler + */ + +#include "mc_pos_control.h" +#include "mc_pos_control_params.h" + +#define TILT_COS_MAX 0.7f +#define SIGMA 0.000001f +#define MIN_DIST 0.01f + +MulticopterPositionControl::MulticopterPositionControl() : + + _task_should_exit(false), + _control_task(-1), + _mavlink_fd(-1), + +/* publications */ + _att_sp_pub(nullptr), + _local_pos_sp_pub(nullptr), + _global_vel_sp_pub(nullptr), + + _ref_alt(0.0f), + _ref_timestamp(0), + + _reset_pos_sp(true), + _reset_alt_sp(true), + _mode_auto(false), + _thrust_int(), + _R() +{ + memset(&_ref_pos, 0, sizeof(_ref_pos)); + + /* + * Do subscriptions + */ + _att = _n.subscribe(&MulticopterPositionControl::handle_vehicle_attitude, this, 0); + _v_att_sp = _n.subscribe(0); + _control_mode = _n.subscribe(0); + _parameter_update = _n.subscribe( + &MulticopterPositionControl::handle_parameter_update, this, 1000); + _manual_control_sp = _n.subscribe(0); + _armed = _n.subscribe(0); + _local_pos_sp = _n.subscribe(0); + _global_vel_sp = _n.subscribe(0); + + + + _params.pos_p.zero(); + _params.vel_p.zero(); + _params.vel_i.zero(); + _params.vel_d.zero(); + _params.vel_max.zero(); + _params.vel_ff.zero(); + _params.sp_offs_max.zero(); + + _pos.zero(); + _pos_sp.zero(); + _vel.zero(); + _vel_sp.zero(); + _vel_prev.zero(); + _vel_ff.zero(); + _sp_move_rate.zero(); + + _params_handles.thr_min = PX4_PARAM_INIT(MPC_THR_MIN); + _params_handles.thr_max = PX4_PARAM_INIT(MPC_THR_MAX); + _params_handles.z_p = PX4_PARAM_INIT(MPC_Z_P); + _params_handles.z_vel_p = PX4_PARAM_INIT(MPC_Z_VEL_P); + _params_handles.z_vel_i = PX4_PARAM_INIT(MPC_Z_VEL_I); + _params_handles.z_vel_d = PX4_PARAM_INIT(MPC_Z_VEL_D); + _params_handles.z_vel_max = PX4_PARAM_INIT(MPC_Z_VEL_MAX); + _params_handles.z_ff = PX4_PARAM_INIT(MPC_Z_FF); + _params_handles.xy_p = PX4_PARAM_INIT(MPC_XY_P); + _params_handles.xy_vel_p = PX4_PARAM_INIT(MPC_XY_VEL_P); + _params_handles.xy_vel_i = PX4_PARAM_INIT(MPC_XY_VEL_I); + _params_handles.xy_vel_d = PX4_PARAM_INIT(MPC_XY_VEL_D); + _params_handles.xy_vel_max = PX4_PARAM_INIT(MPC_XY_VEL_MAX); + _params_handles.xy_ff = PX4_PARAM_INIT(MPC_XY_FF); + _params_handles.tilt_max_air = PX4_PARAM_INIT(MPC_TILTMAX_AIR); + _params_handles.land_speed = PX4_PARAM_INIT(MPC_LAND_SPEED); + _params_handles.tilt_max_land = PX4_PARAM_INIT(MPC_TILTMAX_LND); + + /* fetch initial parameter values */ + parameters_update(); + + _R.identity(); +} + +MulticopterPositionControl::~MulticopterPositionControl() +{ +} + +int +MulticopterPositionControl::parameters_update() +{ + PX4_PARAM_GET(_params_handles.thr_min, &_params.thr_min); + PX4_PARAM_GET(_params_handles.thr_max, &_params.thr_max); + PX4_PARAM_GET(_params_handles.tilt_max_air, &_params.tilt_max_air); + _params.tilt_max_air = math::radians(_params.tilt_max_air); + PX4_PARAM_GET(_params_handles.land_speed, &_params.land_speed); + PX4_PARAM_GET(_params_handles.tilt_max_land, &_params.tilt_max_land); + _params.tilt_max_land = math::radians(_params.tilt_max_land); + + float v; + PX4_PARAM_GET(_params_handles.xy_p, &v); + _params.pos_p(0) = v; + _params.pos_p(1) = v; + PX4_PARAM_GET(_params_handles.z_p, &v); + _params.pos_p(2) = v; + PX4_PARAM_GET(_params_handles.xy_vel_p, &v); + _params.vel_p(0) = v; + _params.vel_p(1) = v; + PX4_PARAM_GET(_params_handles.z_vel_p, &v); + _params.vel_p(2) = v; + PX4_PARAM_GET(_params_handles.xy_vel_i, &v); + _params.vel_i(0) = v; + _params.vel_i(1) = v; + PX4_PARAM_GET(_params_handles.z_vel_i, &v); + _params.vel_i(2) = v; + PX4_PARAM_GET(_params_handles.xy_vel_d, &v); + _params.vel_d(0) = v; + _params.vel_d(1) = v; + PX4_PARAM_GET(_params_handles.z_vel_d, &v); + _params.vel_d(2) = v; + PX4_PARAM_GET(_params_handles.xy_vel_max, &v); + _params.vel_max(0) = v; + _params.vel_max(1) = v; + PX4_PARAM_GET(_params_handles.z_vel_max, &v); + _params.vel_max(2) = v; + PX4_PARAM_GET(_params_handles.xy_ff, &v); + v = math::constrain(v, 0.0f, 1.0f); + _params.vel_ff(0) = v; + _params.vel_ff(1) = v; + PX4_PARAM_GET(_params_handles.z_ff, &v); + v = math::constrain(v, 0.0f, 1.0f); + _params.vel_ff(2) = v; + + _params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f; + + return OK; +} + + +float +MulticopterPositionControl::scale_control(float ctl, float end, float dz) +{ + if (ctl > dz) { + return (ctl - dz) / (end - dz); + + } else if (ctl < -dz) { + return (ctl + dz) / (end - dz); + + } else { + return 0.0f; + } +} + +void +MulticopterPositionControl::update_ref() +{ + if (_local_pos->data().ref_timestamp != _ref_timestamp) { + double lat_sp, lon_sp; + float alt_sp = 0.0f; + + if (_ref_timestamp != 0) { + /* calculate current position setpoint in global frame */ + map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp); + alt_sp = _ref_alt - _pos_sp(2); + } + + /* update local projection reference */ + map_projection_init(&_ref_pos, _local_pos->data().ref_lat, _local_pos->data().ref_lon); + _ref_alt = _local_pos->data().ref_alt; + + if (_ref_timestamp != 0) { + /* reproject position setpoint to new reference */ + map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]); + _pos_sp(2) = -(alt_sp - _ref_alt); + } + + _ref_timestamp = _local_pos->data().ref_timestamp; + } +} + +void +MulticopterPositionControl::reset_pos_sp() +{ + if (_reset_pos_sp) { + _reset_pos_sp = false; + /* shift position setpoint to make attitude setpoint continuous */ + _pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp_msg.data().R_body, 0, 2) * _att_sp_msg.data().thrust / _params.vel_p(0) + - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); + _pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp_msg.data().R_body, 1, 2) * _att_sp_msg.data().thrust / _params.vel_p(1) + - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); + + //XXX: port this once a mavlink like interface is available + // mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); + PX4_INFO("[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); + } +} + +void +MulticopterPositionControl::reset_alt_sp() +{ + if (_reset_alt_sp) { + _reset_alt_sp = false; + _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2); + + //XXX: port this once a mavlink like interface is available + // mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2)); + PX4_INFO("[mpc] reset alt sp: %d", -(int)_pos_sp(2)); + } +} + +void +MulticopterPositionControl::limit_pos_sp_offset() +{ + math::Vector<3> pos_sp_offs; + pos_sp_offs.zero(); + + if (_control_mode->data().flag_control_position_enabled) { + pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); + pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + } + + if (_control_mode->data().flag_control_altitude_enabled) { + pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + } + + float pos_sp_offs_norm = pos_sp_offs.length(); + + if (pos_sp_offs_norm > 1.0f) { + pos_sp_offs /= pos_sp_offs_norm; + _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + } +} + +void +MulticopterPositionControl::control_manual(float dt) +{ + _sp_move_rate.zero(); + + if (_control_mode->data().flag_control_altitude_enabled) { + /* move altitude setpoint with throttle stick */ + _sp_move_rate(2) = -scale_control(_manual_control_sp->data().z - 0.5f, 0.5f, alt_ctl_dz); + } + + if (_control_mode->data().flag_control_position_enabled) { + /* move position setpoint with roll/pitch stick */ + _sp_move_rate(0) = _manual_control_sp->data().x; + _sp_move_rate(1) = _manual_control_sp->data().y; + } + + /* limit setpoint move rate */ + float sp_move_norm = _sp_move_rate.length(); + + if (sp_move_norm > 1.0f) { + _sp_move_rate /= sp_move_norm; + } + + /* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */ + math::Matrix<3, 3> R_yaw_sp; + R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp_msg.data().yaw_body); + _sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max); + + if (_control_mode->data().flag_control_altitude_enabled) { + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); + } + + if (_control_mode->data().flag_control_position_enabled) { + /* reset position setpoint to current position if needed */ + reset_pos_sp(); + } + + /* feed forward setpoint move rate with weight vel_ff */ + _vel_ff = _sp_move_rate.emult(_params.vel_ff); + + /* move position setpoint */ + _pos_sp += _sp_move_rate * dt; + + /* check if position setpoint is too far from actual position */ + math::Vector<3> pos_sp_offs; + pos_sp_offs.zero(); + + if (_control_mode->data().flag_control_position_enabled) { + pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); + pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + } + + if (_control_mode->data().flag_control_altitude_enabled) { + pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + } + + float pos_sp_offs_norm = pos_sp_offs.length(); + + if (pos_sp_offs_norm > 1.0f) { + pos_sp_offs /= pos_sp_offs_norm; + _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + } +} + +void +MulticopterPositionControl::control_offboard(float dt) +{ + if (_pos_sp_triplet->data().current.valid) { + if (_control_mode->data().flag_control_position_enabled && _pos_sp_triplet->data().current.position_valid) { + /* control position */ + _pos_sp(0) = _pos_sp_triplet->data().current.x; + _pos_sp(1) = _pos_sp_triplet->data().current.y; + } else if (_control_mode->data().flag_control_velocity_enabled && _pos_sp_triplet->data().current.velocity_valid) { + /* control velocity */ + /* reset position setpoint to current position if needed */ + reset_pos_sp(); + + /* set position setpoint move rate */ + _sp_move_rate(0) = _pos_sp_triplet->data().current.vx; + _sp_move_rate(1) = _pos_sp_triplet->data().current.vy; + } + + if (_pos_sp_triplet->data().current.yaw_valid) { + _att_sp_msg.data().yaw_body = _pos_sp_triplet->data().current.yaw; + } else if (_pos_sp_triplet->data().current.yawspeed_valid) { + _att_sp_msg.data().yaw_body = _att_sp_msg.data().yaw_body + _pos_sp_triplet->data().current.yawspeed * dt; + } + + if (_control_mode->data().flag_control_altitude_enabled && _pos_sp_triplet->data().current.position_valid) { + /* Control altitude */ + _pos_sp(2) = _pos_sp_triplet->data().current.z; + } else if (_control_mode->data().flag_control_climb_rate_enabled && _pos_sp_triplet->data().current.velocity_valid) { + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); + + /* set altitude setpoint move rate */ + _sp_move_rate(2) = _pos_sp_triplet->data().current.vz; + } + + /* feed forward setpoint move rate with weight vel_ff */ + _vel_ff = _sp_move_rate.emult(_params.vel_ff); + + /* move position setpoint */ + _pos_sp += _sp_move_rate * dt; + + } else { + reset_pos_sp(); + reset_alt_sp(); + } +} + +bool +MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res) +{ + /* project center of sphere on line */ + /* normalized AB */ + math::Vector<3> ab_norm = line_b - line_a; + ab_norm.normalize(); + math::Vector<3> d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm); + float cd_len = (sphere_c - d).length(); + + /* we have triangle CDX with known CD and CX = R, find DX */ + if (sphere_r > cd_len) { + /* have two roots, select one in A->B direction from D */ + float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len); + res = d + ab_norm * dx_len; + return true; + + } else { + /* have no roots, return D */ + res = d; + return false; + } +} + +void +MulticopterPositionControl::control_auto(float dt) +{ + if (!_mode_auto) { + _mode_auto = true; + /* reset position setpoint on AUTO mode activation */ + reset_pos_sp(); + reset_alt_sp(); + } + + if (_pos_sp_triplet->data().current.valid) { + /* in case of interrupted mission don't go to waypoint but stay at current position */ + _reset_pos_sp = true; + _reset_alt_sp = true; + + /* project setpoint to local frame */ + math::Vector<3> curr_sp; + map_projection_project(&_ref_pos, + _pos_sp_triplet->data().current.lat, _pos_sp_triplet->data().current.lon, + &curr_sp.data[0], &curr_sp.data[1]); + curr_sp(2) = -(_pos_sp_triplet->data().current.alt - _ref_alt); + + /* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */ + math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here + + /* convert current setpoint to scaled space */ + math::Vector<3> curr_sp_s = curr_sp.emult(scale); + + /* by default use current setpoint as is */ + math::Vector<3> pos_sp_s = curr_sp_s; + + if (_pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_POSITION && _pos_sp_triplet->data().previous.valid) { + /* follow "previous - current" line */ + math::Vector<3> prev_sp; + map_projection_project(&_ref_pos, + _pos_sp_triplet->data().previous.lat, _pos_sp_triplet->data().previous.lon, + &prev_sp.data[0], &prev_sp.data[1]); + prev_sp(2) = -(_pos_sp_triplet->data().previous.alt - _ref_alt); + + if ((curr_sp - prev_sp).length() > MIN_DIST) { + + /* find X - cross point of L1 sphere and trajectory */ + math::Vector<3> pos_s = _pos.emult(scale); + math::Vector<3> prev_sp_s = prev_sp.emult(scale); + math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s; + math::Vector<3> curr_pos_s = pos_s - curr_sp_s; + float curr_pos_s_len = curr_pos_s.length(); + if (curr_pos_s_len < 1.0f) { + /* copter is closer to waypoint than L1 radius */ + /* check next waypoint and use it to avoid slowing down when passing via waypoint */ + if (_pos_sp_triplet->data().next.valid) { + math::Vector<3> next_sp; + map_projection_project(&_ref_pos, + _pos_sp_triplet->data().next.lat, _pos_sp_triplet->data().next.lon, + &next_sp.data[0], &next_sp.data[1]); + next_sp(2) = -(_pos_sp_triplet->data().next.alt - _ref_alt); + + if ((next_sp - curr_sp).length() > MIN_DIST) { + math::Vector<3> next_sp_s = next_sp.emult(scale); + + /* calculate angle prev - curr - next */ + math::Vector<3> curr_next_s = next_sp_s - curr_sp_s; + math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized(); + + /* cos(a) * curr_next, a = angle between current and next trajectory segments */ + float cos_a_curr_next = prev_curr_s_norm * curr_next_s; + + /* cos(b), b = angle pos - curr_sp - prev_sp */ + float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len; + + if (cos_a_curr_next > 0.0f && cos_b > 0.0f) { + float curr_next_s_len = curr_next_s.length(); + /* if curr - next distance is larger than L1 radius, limit it */ + if (curr_next_s_len > 1.0f) { + cos_a_curr_next /= curr_next_s_len; + } + + /* feed forward position setpoint offset */ + math::Vector<3> pos_ff = prev_curr_s_norm * + cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) * + (1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f)); + pos_sp_s += pos_ff; + } + } + } + + } else { + bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s); + if (near) { + /* L1 sphere crosses trajectory */ + + } else { + /* copter is too far from trajectory */ + /* if copter is behind prev waypoint, go directly to prev waypoint */ + if ((pos_sp_s - prev_sp_s) * prev_curr_s < 0.0f) { + pos_sp_s = prev_sp_s; + } + + /* if copter is in front of curr waypoint, go directly to curr waypoint */ + if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) { + pos_sp_s = curr_sp_s; + } + + pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized(); + } + } + } + } + + /* move setpoint not faster than max allowed speed */ + math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale); + + /* difference between current and desired position setpoints, 1 = max speed */ + math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p); + float d_pos_m_len = d_pos_m.length(); + if (d_pos_m_len > dt) { + pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p); + } + + /* scale result back to normal space */ + _pos_sp = pos_sp_s.edivide(scale); + + /* update yaw setpoint if needed */ + if (isfinite(_pos_sp_triplet->data().current.yaw)) { + _att_sp_msg.data().yaw_body = _pos_sp_triplet->data().current.yaw; + } + + } else { + /* no waypoint, do nothing, setpoint was already reset */ + } +} + +void MulticopterPositionControl::handle_parameter_update(const px4_parameter_update &msg) +{ + parameters_update(); +} + +void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg) +{ + static bool reset_int_z = true; + static bool reset_int_z_manual = false; + static bool reset_int_xy = true; + static bool was_armed = false; + static uint64_t t_prev = 0; + + uint64_t t = get_time_micros(); + float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f; + t_prev = t; + + if (_control_mode->data().flag_armed && !was_armed) { + /* reset setpoints and integrals on arming */ + _reset_pos_sp = true; + _reset_alt_sp = true; + reset_int_z = true; + reset_int_xy = true; + } + + was_armed = _control_mode->data().flag_armed; + + update_ref(); + + if (_control_mode->data().flag_control_altitude_enabled || + _control_mode->data().flag_control_position_enabled || + _control_mode->data().flag_control_climb_rate_enabled || + _control_mode->data().flag_control_velocity_enabled) { + + _pos(0) = _local_pos->data().x; + _pos(1) = _local_pos->data().y; + _pos(2) = _local_pos->data().z; + + _vel(0) = _local_pos->data().vx; + _vel(1) = _local_pos->data().vy; + _vel(2) = _local_pos->data().vz; + + _vel_ff.zero(); + _sp_move_rate.zero(); + + /* select control source */ + if (_control_mode->data().flag_control_manual_enabled) { + /* manual control */ + control_manual(dt); + _mode_auto = false; + + } else if (_control_mode->data().flag_control_offboard_enabled) { + /* offboard control */ + control_offboard(dt); + _mode_auto = false; + + } else { + /* AUTO */ + control_auto(dt); + } + + /* fill local position setpoint */ + _local_pos_sp_msg.data().timestamp = get_time_micros(); + _local_pos_sp_msg.data().x = _pos_sp(0); + _local_pos_sp_msg.data().y = _pos_sp(1); + _local_pos_sp_msg.data().z = _pos_sp(2); + _local_pos_sp_msg.data().yaw = _att_sp_msg.data().yaw_body; + + /* publish local position setpoint */ + if (_local_pos_sp_pub != nullptr) { + _local_pos_sp_pub->publish(_local_pos_sp_msg); + + } else { + _local_pos_sp_pub = _n.advertise(); + } + + + if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_IDLE) { + /* idle state, don't run controller and set zero thrust */ + _R.identity(); + memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body)); + _att_sp_msg.data().R_valid = true; + + _att_sp_msg.data().roll_body = 0.0f; + _att_sp_msg.data().pitch_body = 0.0f; + _att_sp_msg.data().yaw_body = _att->data().yaw; + _att_sp_msg.data().thrust = 0.0f; + + _att_sp_msg.data().timestamp = get_time_micros(); + + /* publish attitude setpoint */ + if (_att_sp_pub != nullptr) { + _att_sp_pub->publish(_att_sp_msg); + + } else { + _att_sp_pub = _n.advertise(); + } + + } else { + /* run position & altitude controllers, calculate velocity setpoint */ + math::Vector<3> pos_err = _pos_sp - _pos; + + _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; + + if (!_control_mode->data().flag_control_altitude_enabled) { + _reset_alt_sp = true; + _vel_sp(2) = 0.0f; + } + + if (!_control_mode->data().flag_control_position_enabled) { + _reset_pos_sp = true; + _vel_sp(0) = 0.0f; + _vel_sp(1) = 0.0f; + } + + /* use constant descend rate when landing, ignore altitude setpoint */ + if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_LAND) { + _vel_sp(2) = _params.land_speed; + } + + _global_vel_sp_msg.data().vx = _vel_sp(0); + _global_vel_sp_msg.data().vy = _vel_sp(1); + _global_vel_sp_msg.data().vz = _vel_sp(2); + + /* publish velocity setpoint */ + if (_global_vel_sp_pub != nullptr) { + _global_vel_sp_pub->publish(_global_vel_sp_msg); + + } else { + _global_vel_sp_pub = _n.advertise(); + } + + if (_control_mode->data().flag_control_climb_rate_enabled || _control_mode->data().flag_control_velocity_enabled) { + /* reset integrals if needed */ + if (_control_mode->data().flag_control_climb_rate_enabled) { + if (reset_int_z) { + reset_int_z = false; + float i = _params.thr_min; + + if (reset_int_z_manual) { + i = _manual_control_sp->data().z; + + if (i < _params.thr_min) { + i = _params.thr_min; + + } else if (i > _params.thr_max) { + i = _params.thr_max; + } + } + + _thrust_int(2) = -i; + } + + } else { + reset_int_z = true; + } + + if (_control_mode->data().flag_control_velocity_enabled) { + if (reset_int_xy) { + reset_int_xy = false; + _thrust_int(0) = 0.0f; + _thrust_int(1) = 0.0f; + } + + } else { + reset_int_xy = true; + } + + /* velocity error */ + math::Vector<3> vel_err = _vel_sp - _vel; + + /* derivative of velocity error, not includes setpoint acceleration */ + math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; + _vel_prev = _vel; + + /* thrust vector in NED frame */ + math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + _thrust_int; + + if (!_control_mode->data().flag_control_velocity_enabled) { + thrust_sp(0) = 0.0f; + thrust_sp(1) = 0.0f; + } + + if (!_control_mode->data().flag_control_climb_rate_enabled) { + thrust_sp(2) = 0.0f; + } + + /* limit thrust vector and check for saturation */ + bool saturation_xy = false; + bool saturation_z = false; + + /* limit min lift */ + float thr_min = _params.thr_min; + + if (!_control_mode->data().flag_control_velocity_enabled && thr_min < 0.0f) { + /* don't allow downside thrust direction in manual attitude mode */ + thr_min = 0.0f; + } + + float tilt_max = _params.tilt_max_air; + + /* adjust limits for landing mode */ + if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && + _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_LAND) { + /* limit max tilt and min lift when landing */ + tilt_max = _params.tilt_max_land; + + if (thr_min < 0.0f) { + thr_min = 0.0f; + } + } + + /* limit min lift */ + if (-thrust_sp(2) < thr_min) { + thrust_sp(2) = -thr_min; + saturation_z = true; + } + + if (_control_mode->data().flag_control_velocity_enabled) { + /* limit max tilt */ + if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) { + /* absolute horizontal thrust */ + float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); + + if (thrust_sp_xy_len > 0.01f) { + /* max horizontal thrust for given vertical thrust*/ + float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max); + + if (thrust_sp_xy_len > thrust_xy_max) { + float k = thrust_xy_max / thrust_sp_xy_len; + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + } + } + + } else { + /* thrust compensation for altitude only control mode */ + float att_comp; + + if (PX4_R(_att->data().R, 2, 2) > TILT_COS_MAX) { + att_comp = 1.0f / PX4_R(_att->data().R, 2, 2); + + } else if (PX4_R(_att->data().R, 2, 2) > 0.0f) { + att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att->data().R, 2, 2) + 1.0f; + saturation_z = true; + + } else { + att_comp = 1.0f; + saturation_z = true; + } + + thrust_sp(2) *= att_comp; + } + + /* limit max thrust */ + float thrust_abs = thrust_sp.length(); + + if (thrust_abs > _params.thr_max) { + if (thrust_sp(2) < 0.0f) { + if (-thrust_sp(2) > _params.thr_max) { + /* thrust Z component is too large, limit it */ + thrust_sp(0) = 0.0f; + thrust_sp(1) = 0.0f; + thrust_sp(2) = -_params.thr_max; + saturation_xy = true; + saturation_z = true; + + } else { + /* preserve thrust Z component and lower XY, keeping altitude is more important than position */ + float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2)); + float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); + float k = thrust_xy_max / thrust_xy_abs; + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + + } else { + /* Z component is negative, going down, simply limit thrust vector */ + float k = _params.thr_max / thrust_abs; + thrust_sp *= k; + saturation_xy = true; + saturation_z = true; + } + + thrust_abs = _params.thr_max; + } + + /* update integrals */ + if (_control_mode->data().flag_control_velocity_enabled && !saturation_xy) { + _thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt; + _thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt; + } + + if (_control_mode->data().flag_control_climb_rate_enabled && !saturation_z) { + _thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; + + /* protection against flipping on ground when landing */ + if (_thrust_int(2) > 0.0f) { + _thrust_int(2) = 0.0f; + } + } + + /* calculate attitude setpoint from thrust vector */ + if (_control_mode->data().flag_control_velocity_enabled) { + /* desired body_z axis = -normalize(thrust_vector) */ + math::Vector<3> body_x; + math::Vector<3> body_y; + math::Vector<3> body_z; + + if (thrust_abs > SIGMA) { + body_z = -thrust_sp / thrust_abs; + + } else { + /* no thrust, set Z axis to safe value */ + body_z.zero(); + body_z(2) = 1.0f; + } + + /* vector of desired yaw direction in XY plane, rotated by PI/2 */ + math::Vector<3> y_C(-sinf(_att_sp_msg.data().yaw_body), cosf(_att_sp_msg.data().yaw_body), 0.0f); + + if (fabsf(body_z(2)) > SIGMA) { + /* desired body_x axis, orthogonal to body_z */ + body_x = y_C % body_z; + + /* keep nose to front while inverted upside down */ + if (body_z(2) < 0.0f) { + body_x = -body_x; + } + + body_x.normalize(); + + } else { + /* desired thrust is in XY plane, set X downside to construct correct matrix, + * but yaw component will not be used actually */ + body_x.zero(); + body_x(2) = 1.0f; + } + + /* desired body_y axis */ + body_y = body_z % body_x; + + /* fill rotation matrix */ + for (int i = 0; i < 3; i++) { + _R(i, 0) = body_x(i); + _R(i, 1) = body_y(i); + _R(i, 2) = body_z(i); + } + + /* copy rotation matrix to attitude setpoint topic */ + memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body)); + _att_sp_msg.data().R_valid = true; + + /* calculate euler angles, for logging only, must not be used for control */ + math::Vector<3> euler = _R.to_euler(); + _att_sp_msg.data().roll_body = euler(0); + _att_sp_msg.data().pitch_body = euler(1); + /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ + + } else if (!_control_mode->data().flag_control_manual_enabled) { + /* autonomous altitude control without position control (failsafe landing), + * force level attitude, don't change yaw */ + _R.from_euler(0.0f, 0.0f, _att_sp_msg.data().yaw_body); + + /* copy rotation matrix to attitude setpoint topic */ + memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body)); + _att_sp_msg.data().R_valid = true; + + _att_sp_msg.data().roll_body = 0.0f; + _att_sp_msg.data().pitch_body = 0.0f; + } + + _att_sp_msg.data().thrust = thrust_abs; + + _att_sp_msg.data().timestamp = get_time_micros(); + + /* publish attitude setpoint */ + if (_att_sp_pub != nullptr) { + _att_sp_pub->publish(_att_sp_msg); + + } else { + _att_sp_pub = _n.advertise(); + } + + } else { + reset_int_z = true; + } + } + + } else { + /* position controller disabled, reset setpoints */ + _reset_alt_sp = true; + _reset_pos_sp = true; + _mode_auto = false; + reset_int_z = true; + reset_int_xy = true; + } + + /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ + reset_int_z_manual = _control_mode->data().flag_armed && _control_mode->data().flag_control_manual_enabled && !_control_mode->data().flag_control_climb_rate_enabled; +} diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h new file mode 100644 index 000000000..05bd1387b --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h @@ -0,0 +1,219 @@ +/**************************************************************************** + * + * Copyright (c) 2013 - 2015 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_pos_control.h + * Multicopter position controller. + * + * @author Anton Babushkin + * @author Thomas Gubler + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +// #include +// #include +// #include +// #include +#include +#include +// #include + +using namespace px4; + +class MulticopterPositionControl +{ +public: + /** + * Constructor + */ + MulticopterPositionControl(); + + /** + * Destructor, also kills task. + */ + ~MulticopterPositionControl(); + + /* Callbacks for topics */ + void handle_vehicle_attitude(const px4_vehicle_attitude &msg); + void handle_parameter_update(const px4_parameter_update &msg); + + void spin() { _n.spin(); } + +protected: + const float alt_ctl_dz = 0.2f; + + bool _task_should_exit; /**< if true, task should exit */ + int _control_task; /**< task handle for task */ + int _mavlink_fd; /**< mavlink fd */ + + Publisher *_att_sp_pub; /**< attitude setpoint publication */ + Publisher *_local_pos_sp_pub; /**< vehicle local position setpoint publication */ + Publisher *_global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ + + + Subscriber *_att; /**< vehicle attitude */ + Subscriber *_v_att_sp; /**< vehicle attitude setpoint */ + Subscriber *_control_mode; /**< vehicle control mode */ + Subscriber *_parameter_update; /**< parameter update */ + Subscriber *_manual_control_sp; /**< manual control setpoint */ + Subscriber *_armed; /**< actuator arming status */ + Subscriber *_local_pos; /**< local position */ + Subscriber *_pos_sp_triplet; /**< local position */ + Subscriber *_local_pos_sp; /**< local position */ + Subscriber *_global_vel_sp; /**< local position */ + + px4_vehicle_attitude_setpoint _att_sp_msg; + px4_vehicle_local_position_setpoint _local_pos_sp_msg; + px4_vehicle_global_velocity_setpoint _global_vel_sp_msg; + + px4::NodeHandle _n; + + + struct { + px4_param_t thr_min; + px4_param_t thr_max; + px4_param_t z_p; + px4_param_t z_vel_p; + px4_param_t z_vel_i; + px4_param_t z_vel_d; + px4_param_t z_vel_max; + px4_param_t z_ff; + px4_param_t xy_p; + px4_param_t xy_vel_p; + px4_param_t xy_vel_i; + px4_param_t xy_vel_d; + px4_param_t xy_vel_max; + px4_param_t xy_ff; + px4_param_t tilt_max_air; + px4_param_t land_speed; + px4_param_t tilt_max_land; + } _params_handles; /**< handles for interesting parameters */ + + struct { + float thr_min; + float thr_max; + float tilt_max_air; + float land_speed; + float tilt_max_land; + + math::Vector<3> pos_p; + math::Vector<3> vel_p; + math::Vector<3> vel_i; + math::Vector<3> vel_d; + math::Vector<3> vel_ff; + math::Vector<3> vel_max; + math::Vector<3> sp_offs_max; + } _params; + + struct map_projection_reference_s _ref_pos; + float _ref_alt; + uint64_t _ref_timestamp; + + bool _reset_pos_sp; + bool _reset_alt_sp; + bool _mode_auto; + + math::Vector<3> _pos; + math::Vector<3> _pos_sp; + math::Vector<3> _vel; + math::Vector<3> _vel_sp; + math::Vector<3> _vel_prev; /**< velocity on previous step */ + math::Vector<3> _vel_ff; + math::Vector<3> _sp_move_rate; + + math::Vector<3> _thrust_int; + math::Matrix<3, 3> _R; + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Update control outputs + */ + void control_update(); + + static float scale_control(float ctl, float end, float dz); + + /** + * Update reference for local position projection + */ + void update_ref(); + /** + * Reset position setpoint to current position + */ + void reset_pos_sp(); + + /** + * Reset altitude setpoint to current altitude + */ + void reset_alt_sp(); + + /** + * Check if position setpoint is too far from current position and adjust it if needed. + */ + void limit_pos_sp_offset(); + + /** + * Set position setpoint using manual control + */ + void control_manual(float dt); + + /** + * Set position setpoint using offboard control + */ + void control_offboard(float dt); + + bool cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res); + + /** + * Set position setpoint for AUTO + */ + void control_auto(float dt); + + /** + * Select between barometric and global (AMSL) altitudes + */ + void select_alt(bool global); +}; diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp new file mode 100644 index 000000000..0b5775736 --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (c) 2013 - 2015 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_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 splitted to thrust direction + * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself). + * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. + * + * @author Anton Babushkin + * @author Thomas Gubler + */ + +#include "mc_pos_control.h" + +bool thread_running = false; /**< Deamon status flag */ + +int main(int argc, char **argv) +{ + px4::init(argc, argv, "mc_pos_control_m"); + + PX4_INFO("starting"); + MulticopterPositionControl posctl; + thread_running = true; + posctl.spin(); + + PX4_INFO("exiting."); + thread_running = false; + return 0; +} diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c new file mode 100644 index 000000000..c741a7f0a --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c @@ -0,0 +1,212 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Anton Babushkin + * + * 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) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_pos_control_params.c + * Multicopter position controller parameters. + * + * @author Anton Babushkin + */ + +#include +#include "mc_pos_control_params.h" +#include + +/** + * Minimum thrust + * + * Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_THR_MIN); + +/** + * Maximum thrust + * + * Limit max allowed thrust. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_THR_MAX); + +/** + * Proportional gain for vertical position error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_P); + +/** + * Proportional gain for vertical velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_P); + +/** + * Integral gain for vertical velocity error + * + * Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff. + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_I); + +/** + * Differential gain for vertical velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_D); + +/** + * Maximum vertical velocity + * + * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL). + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX); + +/** + * Vertical velocity feed forward + * + * Feed forward weight for altitude control in stabilized modes (ALTCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_FF); + +/** + * Proportional gain for horizontal position error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_P); + +/** + * Proportional gain for horizontal velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_P); + +/** + * Integral gain for horizontal velocity error + * + * Non-zero value allows to resist wind. + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_I); + +/** + * Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_D); + +/** + * Maximum horizontal velocity + * + * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL). + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX); + +/** + * Horizontal velocity feed forward + * + * Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_FF); + +/** + * Maximum tilt angle in air + * + * Limits maximum tilt in AUTO and POSCTRL modes during flight. + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR); + +/** + * Maximum tilt during landing + * + * Limits maximum tilt angle on landing. + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND); + +/** + * Landing descend rate + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_LAND_SPEED); + diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h new file mode 100644 index 000000000..fec3bcb7c --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Anton Babushkin + * + * 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_pos_control_params.h + * Multicopter position controller parameters. + * + * @author Anton Babushkin + */ + +#pragma once + +#define PARAM_MPC_THR_MIN_DEFAULT 0.1f +#define PARAM_MPC_THR_MAX_DEFAULT 1.0f +#define PARAM_MPC_Z_P_DEFAULT 1.0f +#define PARAM_MPC_Z_VEL_P_DEFAULT 0.1f +#define PARAM_MPC_Z_VEL_I_DEFAULT 0.02f +#define PARAM_MPC_Z_VEL_D_DEFAULT 0.0f +#define PARAM_MPC_Z_VEL_MAX_DEFAULT 5.0f +#define PARAM_MPC_Z_FF_DEFAULT 0.5f +#define PARAM_MPC_XY_P_DEFAULT 1.0f +#define PARAM_MPC_XY_VEL_P_DEFAULT 0.1f +#define PARAM_MPC_XY_VEL_I_DEFAULT 0.02f +#define PARAM_MPC_XY_VEL_D_DEFAULT 0.01f +#define PARAM_MPC_XY_VEL_MAX_DEFAULT 5.0f +#define PARAM_MPC_XY_FF_DEFAULT 0.5f +#define PARAM_MPC_TILTMAX_AIR_DEFAULT 45.0f +#define PARAM_MPC_TILTMAX_LND_DEFAULT 15.0f +#define PARAM_MPC_LAND_SPEED_DEFAULT 1.0f + diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp new file mode 100644 index 000000000..87996d93b --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (C) 2013 - 2015 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_pos_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_pos_control_m_main(int argc, char *argv[]); +int mc_pos_control_m_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "usage: mc_pos_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_pos_control_m", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 3000, + 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; +} diff --git a/src/modules/mc_pos_control_multiplatform/module.mk b/src/modules/mc_pos_control_multiplatform/module.mk new file mode 100644 index 000000000..2852ebbec --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2013 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. +# +############################################################################ + +# +# Build multicopter position controller +# + +MODULE_COMMAND = mc_pos_control_m + +SRCS = mc_pos_control_main.cpp \ + mc_pos_control_start_nuttx.cpp \ + mc_pos_control.cpp \ + mc_pos_control_params.c diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 1bd4509ca..f8561fa3b 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -62,6 +62,10 @@ #include #include #include +#include +#include +#include +#include #endif #else @@ -85,6 +89,10 @@ #include #include #include +#include +#include +#include +#include #endif #include #include -- cgit v1.2.3 From 8e7974e2e29a75daf5a55e723f6d6a4b416c252f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 08:25:42 +0100 Subject: fix uorb constants for test functions --- .../commander_tests/state_machine_helper_test.cpp | 346 ++++++++++----------- 1 file changed, 173 insertions(+), 173 deletions(-) diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 3cfa8b4c6..4ddb4e0fb 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -73,7 +73,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) bool armed; // actuator_armed_s.armed bool ready_to_arm; // actuator_armed_s.ready_to_arm } ArmingTransitionVolatileState_t; - + // This structure represents a test case for arming_state_transition. It contains the machine // state prior to transtion, the requested state to transition to and finally the expected // machine state after transition. @@ -88,7 +88,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) ArmingTransitionVolatileState_t expected_state; // Expected machine state after transition transition_result_t expected_transition_result; // Expected result from arming_state_transition } ArmingTransitionTest_t; - + // We use these defines so that our test cases are more readable #define ATT_ARMED true #define ATT_DISARMED false @@ -100,182 +100,182 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) #define ATT_SAFETY_NOT_AVAILABLE true #define ATT_SAFETY_OFF true #define ATT_SAFETY_ON false - + // These are test cases for arming_state_transition static const ArmingTransitionTest_t rgArmingTransitionTests[] = { // TRANSITION_NOT_CHANGED tests - + { "no transition: identical states", - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_INIT, - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED }, - + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_INIT, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED }, + // TRANSITION_CHANGED tests - + // Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s - + { "transition: init to standby", - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: init to standby error", - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY_ERROR, - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY_ERROR, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: init to reboot", - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_REBOOT, - { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_REBOOT, + { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: standby to init", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_INIT, - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_INIT, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: standby to standby error", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY_ERROR, - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY_ERROR, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: standby to reboot", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_REBOOT, - { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_REBOOT, + { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: armed to standby", - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: armed to armed error", - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED_ERROR, - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED_ERROR, + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: armed error to standby error", - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY_ERROR, - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY_ERROR, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: standby error to reboot", - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_REBOOT, - { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_REBOOT, + { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: in air restore to armed", - { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED, - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: in air restore to reboot", - { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_REBOOT, - { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_REBOOT, + { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + // hil on tests, standby error to standby not normally allowed - + { "transition: standby error to standby, hil on", - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + // Safety switch arming tests - + { "transition: standby to armed, no safety switch", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, - ARMING_STATE_ARMED, - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: standby to armed, safety switch off", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, - ARMING_STATE_ARMED, - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + // standby error { "transition: armed error to standby error requested standby", - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + // TRANSITION_DENIED tests - + // Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s - + { "no transition: init to armed", - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED, - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: standby to armed error", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED_ERROR, - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED_ERROR, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: armed to init", - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_INIT, - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_INIT, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: armed to reboot", - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_REBOOT, - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_REBOOT, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: armed error to armed", - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED, - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: armed error to reboot", - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_REBOOT, - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_REBOOT, + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: standby error to armed", - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED, - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: standby error to standby", - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: reboot to armed", - { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED, - { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: in air restore to standby", - { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + // Sensor tests - + { "no transition: init to standby - sensors not initialized", - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + // Safety switch arming tests - + { "no transition: init to standby, safety switch on", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED, - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, }; - + struct vehicle_status_s status; struct safety_s safety; struct actuator_armed_s armed; - + size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]); for (size_t i=0; icurrent_state.arming_state; status.condition_system_sensors_initialized = test->condition_system_sensors_initialized; @@ -286,10 +286,10 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) safety.safety_off = test->safety_off; armed.armed = test->current_state.armed; armed.ready_to_arm = test->current_state.ready_to_arm; - + // Attempt transition transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */); - + // Validate result of transition ut_assert(test->assertMsg, test->expected_transition_result == result); ut_assert(test->assertMsg, status.arming_state == test->expected_state.arming_state); @@ -310,7 +310,7 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) main_state_t to_state; // State to transition to transition_result_t expected_transition_result; // Expected result from main_state_transition call } MainTransitionTest_t; - + // Bits for condition_bits #define MTT_ALL_NOT_VALID 0 #define MTT_ROTARY_WING 1 << 0 @@ -318,108 +318,108 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) #define MTT_LOC_POS_VALID 1 << 2 #define MTT_HOME_POS_VALID 1 << 3 #define MTT_GLOBAL_POS_VALID 1 << 4 - + static const MainTransitionTest_t rgMainTransitionTests[] = { - + // TRANSITION_NOT_CHANGED tests - + { "no transition: identical states", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED }, + // TRANSITION_CHANGED tests - + { "transition: MANUAL to ACRO", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_ACRO, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ACRO, TRANSITION_CHANGED }, { "transition: ACRO to MANUAL", MTT_ALL_NOT_VALID, - MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_ACRO, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to AUTO_MISSION - global position valid, home position valid", MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED }, { "transition: AUTO_MISSION to MANUAL - global position valid, home position valid", MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, - MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_AUTO_MISSION, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to AUTO_LOITER - global position valid", MTT_GLOBAL_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED }, { "transition: AUTO_LOITER to MANUAL - global position valid", MTT_GLOBAL_POS_VALID, - MAIN_STATE_AUTO_LOITER, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_AUTO_LOITER, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to AUTO_RTL - global position valid, home position valid", MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED }, { "transition: AUTO_RTL to MANUAL - global position valid, home position valid", MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, - MAIN_STATE_AUTO_RTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_AUTO_RTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to ALTCTL - not rotary", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, { "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid", MTT_ROTARY_WING | MTT_LOC_ALT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + { "transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid", MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + { "transition: ALTCTL to MANUAL - local altitude valid", MTT_LOC_ALT_VALID, - MAIN_STATE_ALTCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_ALTCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to POSCTL - local position not valid, global position valid", MTT_GLOBAL_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED }, { "transition: MANUAL to POSCTL - local position valid, global position not valid", MTT_LOC_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED }, { "transition: POSCTL to MANUAL - local position valid, global position valid", MTT_LOC_POS_VALID, - MAIN_STATE_POSCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_POSCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, // TRANSITION_DENIED tests { "no transition: MANUAL to AUTO_MISSION - global position not valid", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED }, + { "no transition: MANUAL to AUTO_LOITER - global position not valid", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED }, + { "no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + { "no transition: MANUAL to AUTO_RTL - global position not valid, home position valid", MTT_HOME_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, { "no transition: MANUAL to AUTO_RTL - global position valid, home position not valid", MTT_GLOBAL_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + { "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid", MTT_ROTARY_WING, - MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_DENIED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED }, + { "no transition: MANUAL to POSCTL - local position not valid, global position not valid", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_DENIED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_DENIED }, }; - + size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]); for (size_t i=0; icondition_bits & MTT_LOC_POS_VALID; current_state.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID; current_state.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID; - + // Attempt transition transition_result_t result = main_state_transition(¤t_state, test->to_state); - + // Validate result of transition ut_assert(test->assertMsg, test->expected_transition_result == result); if (test->expected_transition_result == result) { @@ -495,8 +495,8 @@ bool StateMachineHelperTest::run_tests(void) ut_run_test(armingStateTransitionTest); ut_run_test(mainStateTransitionTest); ut_run_test(isSafeTest); - + return (_tests_failed == 0); } -ut_declare_test(stateMachineHelperTest, StateMachineHelperTest) \ No newline at end of file +ut_declare_test(stateMachineHelperTest, StateMachineHelperTest) -- cgit v1.2.3 From 66007d56ef35ebc1f11ac83f2347bfc22b9664f9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 09:08:58 +0100 Subject: fix uorb constants in uavcan module --- src/modules/uavcan/uavcan_main.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 4dc03b61b..b93a95f96 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -381,7 +381,7 @@ int UavcanNode::run() } else { // get controls for required topics bool controls_updated = false; - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[_poll_ids[i]].revents & POLLIN) { controls_updated = true; @@ -393,14 +393,14 @@ int UavcanNode::run() /* see if we have any direct actuator updates */ - if (_actuator_direct_sub != -1 && + if (_actuator_direct_sub != -1 && (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) && orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK && !_test_in_progress) { if (_actuator_direct.nvalues > NUM_ACTUATOR_OUTPUTS) { _actuator_direct.nvalues = NUM_ACTUATOR_OUTPUTS; } - memcpy(&_outputs.output[0], &_actuator_direct.values[0], + memcpy(&_outputs.output[0], &_actuator_direct.values[0], _actuator_direct.nvalues*sizeof(float)); _outputs.noutputs = _actuator_direct.nvalues; new_output = true; @@ -476,7 +476,7 @@ int UavcanNode::run() if (updated) { orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - // Update the armed status and check that we're not locked down and motor + // Update the armed status and check that we're not locked down and motor // test is not running bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress; @@ -502,7 +502,7 @@ UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t co int UavcanNode::teardown() { - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { ::close(_control_subs[i]); _control_subs[i] = -1; @@ -526,7 +526,7 @@ UavcanNode::subscribe() uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; // the first fd used by CAN - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { warnx("subscribe to actuator_controls_%d", i); _control_subs[i] = orb_subscribe(_control_topics[i]); -- cgit v1.2.3 From a6ca4c2796a825ac1878c1d570afc0afc4fed74d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 11:17:23 +0100 Subject: initialize all subscribers --- src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index 2685a99c3..aa23e0a60 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -78,6 +78,8 @@ MulticopterPositionControl::MulticopterPositionControl() : &MulticopterPositionControl::handle_parameter_update, this, 1000); _manual_control_sp = _n.subscribe(0); _armed = _n.subscribe(0); + _local_pos = _n.subscribe(0); + _pos_sp_triplet = _n.subscribe(0); _local_pos_sp = _n.subscribe(0); _global_vel_sp = _n.subscribe(0); -- cgit v1.2.3 From 9245f28fb84e87505c6024121720bbd0776e64a0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 17:46:22 +0100 Subject: mc attctl multiplatform: fix memcpy --- src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) 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 fcfee28dc..5577c5f03 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 @@ -97,7 +97,8 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) 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->data(), sizeof(_v_att_sp_mod)); + //XXX get rid of memcpy + memcpy(&(_v_att_sp_mod.data()), &_v_att_sp->data(), sizeof(_v_att_sp_mod.data())); } if (!_v_control_mode->data().flag_control_climb_rate_enabled) { @@ -156,7 +157,8 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) } else { /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ - memcpy(&_v_att_sp_mod, &_v_att_sp->data(), sizeof(_v_att_sp_mod)); + //XXX get rid of memcpy + memcpy(&(_v_att_sp_mod.data()), &_v_att_sp->data(), sizeof(_v_att_sp_mod.data())); /* reset yaw setpoint after non-manual control mode */ _reset_yaw_sp = true; -- cgit v1.2.3 From 8a6b94adbfa58245d13354216168de86e888c782 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 10:28:56 +0100 Subject: attitude estimator dummy node: add timestamp --- src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp index bcee0b479..f744aa11c 100644 --- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp @@ -43,6 +43,7 @@ #include #include #include +#include AttitudeEstimator::AttitudeEstimator() : _n(), @@ -92,6 +93,7 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP // msg_v_att.pitchspeed = -(float)msg->twist[index].angular.y; // msg_v_att.yawspeed = -(float)msg->twist[index].angular.z; + msg_v_att.timestamp = px4::get_time_micros(); _vehicle_attitude_pub.publish(msg_v_att); } -- cgit v1.2.3 From 6f4f5d637de51272a2bc9fd3a8db0a2755d763c6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 10:29:15 +0100 Subject: first version of position estimator dummy node --- CMakeLists.txt | 9 ++ .../position_estimator/position_estimator.cpp | 107 +++++++++++++++++++++ .../nodes/position_estimator/position_estimator.h | 62 ++++++++++++ 3 files changed, 178 insertions(+) create mode 100644 src/platforms/ros/nodes/position_estimator/position_estimator.cpp create mode 100644 src/platforms/ros/nodes/position_estimator/position_estimator.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 6a368d78f..0c81607b0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -204,6 +204,15 @@ target_link_libraries(attitude_estimator px4 ) +## Position Estimator dummy +add_executable(position_estimator + src/platforms/ros/nodes/position_estimator/position_estimator.cpp) +add_dependencies(position_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(position_estimator + ${catkin_LIBRARIES} + px4 +) + ## Manual input add_executable(manual_input src/platforms/ros/nodes/manual_input/manual_input.cpp) diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp new file mode 100644 index 000000000..231e9763a --- /dev/null +++ b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * 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 position_estimator.cpp + * + * @author Thomas Gubler + * @author Roman Bapst +*/ + +#include "position_estimator.h" + +#include +#include +#include +#include +#include +#include +#include + +PositionEstimator::PositionEstimator() : + _n(), + _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &PositionEstimator::model_states_callback, this)), + _vehicle_position_pub(_n.advertise("vehicle_local_position", 1)), + _startup_time(px4::get_time_micros()) +{ +} + +void PositionEstimator::model_states_callback(const gazebo_msgs::ModelStatesConstPtr &msg) +{ + //XXX: use a proper sensor topic + + px4::vehicle_local_position msg_v_l_pos; + + /* Fill px4 position topic with contents from modelstates topic */ + int index = 0; + //XXX: maybe a more clever approach would be to do this not on every loop, need to check if and when + //gazebo rearranges indexes. + for(std::vector::const_iterator it = msg->name.begin(); it != msg->name.end(); ++it) { + if (*it == "iris" || *it == "ardrone") { + index = it - msg->name.begin(); + break; + } + } + msg_v_l_pos.xy_valid = true; + msg_v_l_pos.z_valid = true; + msg_v_l_pos.v_xy_valid = true; + msg_v_l_pos.v_z_valid = true; + + msg_v_l_pos.x = msg->pose[index].position.x; + msg_v_l_pos.y = -msg->pose[index].position.y; + msg_v_l_pos.z = -msg->pose[index].position.z; + msg_v_l_pos.vx = msg->twist[index].linear.x; + msg_v_l_pos.vy = -msg->twist[index].linear.y; + msg_v_l_pos.vz = -msg->twist[index].linear.z; + + msg_v_l_pos.xy_global = true; + msg_v_l_pos.z_global = true; + msg_v_l_pos.ref_timestamp = _startup_time; + msg_v_l_pos.ref_lat = 47.378301; + msg_v_l_pos.ref_lon = 8.538777; + msg_v_l_pos.ref_alt = 1200.0f; + + + msg_v_l_pos.timestamp = px4::get_time_micros(); + _vehicle_position_pub.publish(msg_v_l_pos); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "position_estimator"); + PositionEstimator m; + + ros::spin(); + + return 0; +} diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.h b/src/platforms/ros/nodes/position_estimator/position_estimator.h new file mode 100644 index 000000000..40c4baa23 --- /dev/null +++ b/src/platforms/ros/nodes/position_estimator/position_estimator.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * 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 position_estimator.h + * Dummy position estimator that forwards attitude from gazebo to px4 topic + * + * @author Thomas Gubler +*/ + +#include "ros/ros.h" +#include +#include + +class PositionEstimator +{ +public: + PositionEstimator(); + + ~PositionEstimator() {} + +protected: + void model_states_callback(const gazebo_msgs::ModelStatesConstPtr &msg); + + ros::NodeHandle _n; + ros::Subscriber _sub_modelstates; + ros::Publisher _vehicle_position_pub; + + uint64_t _startup_time; + + +}; -- cgit v1.2.3 From 8b62e003f0edbd8fe82286eaead83f0b3ef88a8c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 10:30:07 +0100 Subject: add dummy position estimator to launch file --- launch/multicopter.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/launch/multicopter.launch b/launch/multicopter.launch index 96ff3ad99..5d1a35126 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -6,6 +6,7 @@ + -- cgit v1.2.3 From 9a5ea31784bacb57b9a86134fb612119609fc08c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 10:33:12 +0100 Subject: add pos controller to launch file --- launch/multicopter.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/launch/multicopter.launch b/launch/multicopter.launch index 5d1a35126..0f8cc5132 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -8,6 +8,7 @@ + -- cgit v1.2.3 From 5b5a4b73864bb3331830d232fc3bb1bf7372d844 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 13:36:53 +0100 Subject: function rename --- src/platforms/ros/nodes/position_estimator/position_estimator.cpp | 4 ++-- src/platforms/ros/nodes/position_estimator/position_estimator.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp index 231e9763a..fae304bf4 100644 --- a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp +++ b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp @@ -50,13 +50,13 @@ PositionEstimator::PositionEstimator() : _n(), - _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &PositionEstimator::model_states_callback, this)), + _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &PositionEstimator::ModelStatesCallback, this)), _vehicle_position_pub(_n.advertise("vehicle_local_position", 1)), _startup_time(px4::get_time_micros()) { } -void PositionEstimator::model_states_callback(const gazebo_msgs::ModelStatesConstPtr &msg) +void PositionEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg) { //XXX: use a proper sensor topic diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.h b/src/platforms/ros/nodes/position_estimator/position_estimator.h index 40c4baa23..4d396f39a 100644 --- a/src/platforms/ros/nodes/position_estimator/position_estimator.h +++ b/src/platforms/ros/nodes/position_estimator/position_estimator.h @@ -50,7 +50,7 @@ public: ~PositionEstimator() {} protected: - void model_states_callback(const gazebo_msgs::ModelStatesConstPtr &msg); + void ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg); ros::NodeHandle _n; ros::Subscriber _sub_modelstates; -- cgit v1.2.3 From 3c79b2a586286615ef00e1584c7c2f74887e38cd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 13:37:37 +0100 Subject: manual input dummy node: extend to support switching between modes also fixing thrust input --- .../ros/nodes/manual_input/manual_input.cpp | 84 ++++++++++++++++------ .../ros/nodes/manual_input/manual_input.h | 23 +++++- 2 files changed, 84 insertions(+), 23 deletions(-) diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index 688df50e0..7e1ae7a17 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.cpp +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -40,13 +40,14 @@ #include "manual_input.h" -#include #include ManualInput::ManualInput() : _n(), _joy_sub(_n.subscribe("joy", 1, &ManualInput::JoyCallback, this)), - _man_ctrl_sp_pub(_n.advertise("manual_control_setpoint", 1)) + _man_ctrl_sp_pub(_n.advertise("manual_control_setpoint", 1)), + _buttons_state(), + _msg_mc_sp() { double dz_default = 0.2; /* Load parameters, default values work for Microsoft XBox Controller */ @@ -63,40 +64,46 @@ ManualInput::ManualInput() : _n.param("map_z", _param_axes_map[2], 2); _n.param("scale_z", _param_axes_scale[2], -0.5); _n.param("off_z", _param_axes_offset[2], -1.0); - _n.param("dz_z", _param_axes_dz[2], dz_default); + _n.param("dz_z", _param_axes_dz[2], 0.0); _n.param("map_r", _param_axes_map[3], 0); _n.param("scale_r", _param_axes_scale[3], -1.0); _n.param("off_r", _param_axes_offset[3], 0.0); _n.param("dz_r", _param_axes_dz[3], dz_default); + _n.param("map_manual", _param_buttons_map[0], 0); + _n.param("map_altctl", _param_buttons_map[1], 1); + _n.param("map_posctl", _param_buttons_map[2], 2); + _n.param("map_auto_mission", _param_buttons_map[3], 3); + _n.param("map_auto_loiter", _param_buttons_map[4], 4); + _n.param("map_auto_rtl", _param_buttons_map[5], 4); + + /* Default to manual */ + _msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + _msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + _msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + _msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + _msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + } void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr &msg) { - px4::manual_control_setpoint msg_out; /* Fill px4 manual control setpoint topic with contents from ros joystick */ /* Map sticks to x, y, z, r */ - MapAxis(msg, _param_axes_map[0], _param_axes_scale[0], _param_axes_offset[0], _param_axes_dz[0], msg_out.x); - MapAxis(msg, _param_axes_map[1], _param_axes_scale[1], _param_axes_offset[1], _param_axes_dz[1], msg_out.y); - MapAxis(msg, _param_axes_map[2], _param_axes_scale[2], _param_axes_offset[2], _param_axes_dz[2], msg_out.z); - MapAxis(msg, _param_axes_map[3], _param_axes_scale[3], _param_axes_offset[3], _param_axes_dz[3], msg_out.r); - //ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_out.x, msg_out.y, msg_out.z, msg_out.r); + MapAxis(msg, _param_axes_map[0], _param_axes_scale[0], _param_axes_offset[0], _param_axes_dz[0], _msg_mc_sp.x); + MapAxis(msg, _param_axes_map[1], _param_axes_scale[1], _param_axes_offset[1], _param_axes_dz[1], _msg_mc_sp.y); + MapAxis(msg, _param_axes_map[2], _param_axes_scale[2], _param_axes_offset[2], _param_axes_dz[2], _msg_mc_sp.z); + MapAxis(msg, _param_axes_map[3], _param_axes_scale[3], _param_axes_offset[3], _param_axes_dz[3], _msg_mc_sp.r); + //ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_mc_sp.x, msg_out.y, msg_out.z, msg_out.r); /* Map buttons to switches */ - //XXX todo - /* for now just publish switches in position for manual flight */ - msg_out.mode_switch = 0; - msg_out.return_switch = 0; - msg_out.posctl_switch = 0; - msg_out.loiter_switch = 0; - msg_out.acro_switch = 0; - msg_out.offboard_switch = 0; - - msg_out.timestamp = px4::get_time_micros(); - - _man_ctrl_sp_pub.publish(msg_out); + MapButtons(msg, _msg_mc_sp); + + _msg_mc_sp.timestamp = px4::get_time_micros(); + + _man_ctrl_sp_pub.publish(_msg_mc_sp); } void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset, @@ -106,6 +113,41 @@ void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, do if (val + offset > deadzone || val + offset < -deadzone) { out = (float)((val + offset)) * scale; + } else { + out = 0.0f; + } +} + +void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp) { + msg_mc_sp.acro_switch = px4::manual_control_setpoint::SWITCH_POS_NONE; + msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_NONE; + + if (_buttons_state[MAIN_STATE_MANUAL] != msg->buttons[_param_buttons_map[MAIN_STATE_MANUAL]] == true) { + msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + return; + + } else if (_buttons_state[MAIN_STATE_ALTCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_ALTCTL]] == true) { + msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_MIDDLE; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + return; + + } else if (_buttons_state[MAIN_STATE_POSCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_POSCTL]] == true) { + msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_MIDDLE; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_ON; + msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + return; + + } else { + ROS_WARN("requested mode via joystick that is not implemented"); } } diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h index 93e0abe64..bf704f675 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.h +++ b/src/platforms/ros/nodes/manual_input/manual_input.h @@ -39,6 +39,7 @@ */ #include "ros/ros.h" +#include #include class ManualInput @@ -55,20 +56,38 @@ protected: void JoyCallback(const sensor_msgs::JoyConstPtr &msg); /** - * Helper function to map and scale joystick input + * Helper function to map and scale joystick axis */ void MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset, double deadzone, float &out); + /** + * Helper function to map joystick buttons + */ + void MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp); ros::NodeHandle _n; ros::Subscriber _joy_sub; ros::Publisher _man_ctrl_sp_pub; /* Parameters */ + enum MAIN_STATE { + MAIN_STATE_MANUAL = 0, + MAIN_STATE_ALTCTL, + MAIN_STATE_POSCTL, + MAIN_STATE_AUTO_MISSION, + MAIN_STATE_AUTO_LOITER, + MAIN_STATE_AUTO_RTL, + MAIN_STATE_MAX + }; + + int _param_buttons_map[MAIN_STATE_MAX]; /**< joystick button mapping, order according to MAIN_STATE */ + bool _buttons_state[MAIN_STATE_MAX]; /**< joystick button state of last iteration, + order according to MAIN_STATE */ + int _param_axes_map[4]; double _param_axes_scale[4]; double _param_axes_offset[4]; double _param_axes_dz[4]; - + px4::manual_control_setpoint _msg_mc_sp; }; -- cgit v1.2.3 From 5237364a5a6a5e765bbb30cb20eb399dcd14489a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 13:38:39 +0100 Subject: commander dummy node: extend to support switching between modes --- src/platforms/ros/nodes/commander/commander.cpp | 64 ++++++++++++++++++++----- src/platforms/ros/nodes/commander/commander.h | 9 ++++ 2 files changed, 61 insertions(+), 12 deletions(-) diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index b9fc296f9..fee32b55f 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -40,9 +40,6 @@ #include "commander.h" -#include -#include -#include #include Commander::Commander() : @@ -62,12 +59,9 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon px4::vehicle_control_mode msg_vehicle_control_mode; px4::vehicle_status msg_vehicle_status; - /* fill vehicle control mode */ - //XXX hardcoded + /* fill vehicle control mode based on (faked) stick positions*/ + EvalSwitches(msg, msg_vehicle_status, msg_vehicle_control_mode); msg_vehicle_control_mode.timestamp = px4::get_time_micros(); - msg_vehicle_control_mode.flag_control_manual_enabled = true; - msg_vehicle_control_mode.flag_control_rates_enabled = true; - msg_vehicle_control_mode.flag_control_attitude_enabled = true; /* fill actuator armed */ float arm_th = 0.95; @@ -77,21 +71,19 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon /* Check for disarm */ if (msg->r < -arm_th && msg->z < (1 - arm_th)) { _msg_actuator_armed.armed = false; + msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_STANDBY; } } else { /* Check for arm */ if (msg->r > arm_th && msg->z < (1 - arm_th)) { _msg_actuator_armed.armed = true; + msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED; } } /* fill vehicle status */ - //XXX hardcoded msg_vehicle_status.timestamp = px4::get_time_micros(); - msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_MANUAL; - msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_MANUAL; - msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED; msg_vehicle_status.hil_state = msg_vehicle_status.HIL_STATE_OFF; msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; msg_vehicle_status.is_rotary_wing = true; @@ -107,6 +99,54 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon } } +void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, + px4::vehicle_status &msg_vehicle_status, + px4::vehicle_control_mode &msg_vehicle_control_mode) { + // XXX this is a minimal implementation. If more advanced functionalities are + // needed consider a full port of the commander + + switch (msg->mode_switch) { + case px4::manual_control_setpoint::SWITCH_POS_NONE: + ROS_WARN("Joystick button mapping error, main mode not set"); + break; + + case px4::manual_control_setpoint::SWITCH_POS_OFF: // MANUAL + msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_MANUAL; + msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_MANUAL; + msg_vehicle_control_mode.flag_control_manual_enabled = true; + msg_vehicle_control_mode.flag_control_rates_enabled = true; + msg_vehicle_control_mode.flag_control_attitude_enabled = true; + msg_vehicle_control_mode.flag_control_altitude_enabled = false; + msg_vehicle_control_mode.flag_control_climb_rate_enabled = false; + msg_vehicle_control_mode.flag_control_position_enabled = false; + + break; + + case px4::manual_control_setpoint::SWITCH_POS_MIDDLE: // ASSIST + if (msg->posctl_switch == px4::manual_control_setpoint::SWITCH_POS_ON) { + msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_POSCTL; + msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_POSCTL; + msg_vehicle_control_mode.flag_control_manual_enabled = true; + msg_vehicle_control_mode.flag_control_rates_enabled = true; + msg_vehicle_control_mode.flag_control_attitude_enabled = true; + msg_vehicle_control_mode.flag_control_altitude_enabled = true; + msg_vehicle_control_mode.flag_control_climb_rate_enabled = true; + msg_vehicle_control_mode.flag_control_position_enabled = true; + } else { + msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_ALTCTL; + msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_ALTCTL; + msg_vehicle_control_mode.flag_control_manual_enabled = true; + msg_vehicle_control_mode.flag_control_rates_enabled = true; + msg_vehicle_control_mode.flag_control_attitude_enabled = true; + msg_vehicle_control_mode.flag_control_altitude_enabled = true; + msg_vehicle_control_mode.flag_control_climb_rate_enabled = true; + msg_vehicle_control_mode.flag_control_position_enabled = false; + } + break; + } + +} + int main(int argc, char **argv) { ros::init(argc, argv, "commander"); diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h index bd4092b3a..f251f7c1a 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -40,6 +40,8 @@ #include "ros/ros.h" #include +#include +#include #include #include @@ -56,6 +58,13 @@ protected: */ void ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg); + /** + * Set control mode flags based on stick positions (equiv to code in px4 commander) + */ + void EvalSwitches(const px4::manual_control_setpointConstPtr &msg, + px4::vehicle_status &msg_vehicle_status, + px4::vehicle_control_mode &msg_vehicle_control_mode); + ros::NodeHandle _n; ros::Subscriber _man_ctrl_sp_sub; ros::Publisher _vehicle_control_mode_pub; -- cgit v1.2.3 From 6dead1d576b91811ba4bcd988ed4f4151eacafcc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 14:09:25 +0100 Subject: commander dummy node: set control velocity enabled flag --- src/platforms/ros/nodes/commander/commander.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index fee32b55f..3829cbc32 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -119,6 +119,7 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, msg_vehicle_control_mode.flag_control_altitude_enabled = false; msg_vehicle_control_mode.flag_control_climb_rate_enabled = false; msg_vehicle_control_mode.flag_control_position_enabled = false; + msg_vehicle_control_mode.flag_control_velocity_enabled = false; break; @@ -132,6 +133,7 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, msg_vehicle_control_mode.flag_control_altitude_enabled = true; msg_vehicle_control_mode.flag_control_climb_rate_enabled = true; msg_vehicle_control_mode.flag_control_position_enabled = true; + msg_vehicle_control_mode.flag_control_velocity_enabled = true; } else { msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_ALTCTL; msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_ALTCTL; @@ -141,6 +143,7 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, msg_vehicle_control_mode.flag_control_altitude_enabled = true; msg_vehicle_control_mode.flag_control_climb_rate_enabled = true; msg_vehicle_control_mode.flag_control_position_enabled = false; + msg_vehicle_control_mode.flag_control_velocity_enabled = false; } break; } -- cgit v1.2.3 From a0ae5aeebb9eea7e90cf365d931c9a29790ebba1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 14:23:03 +0100 Subject: commander dummy node: small fix for vehicle_control_mode --- src/platforms/ros/nodes/commander/commander.cpp | 12 +++++++----- src/platforms/ros/nodes/commander/commander.h | 1 + 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index 3829cbc32..2673122c7 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -50,18 +50,18 @@ Commander::Commander() : _vehicle_status_pub(_n.advertise("vehicle_status", 10)), _parameter_update_pub(_n.advertise("parameter_update", 10)), _msg_parameter_update(), - _msg_actuator_armed() + _msg_actuator_armed(), + _msg_vehicle_control_mode() { } void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg) { - px4::vehicle_control_mode msg_vehicle_control_mode; px4::vehicle_status msg_vehicle_status; /* fill vehicle control mode based on (faked) stick positions*/ - EvalSwitches(msg, msg_vehicle_status, msg_vehicle_control_mode); - msg_vehicle_control_mode.timestamp = px4::get_time_micros(); + EvalSwitches(msg, msg_vehicle_status, _msg_vehicle_control_mode); + _msg_vehicle_control_mode.timestamp = px4::get_time_micros(); /* fill actuator armed */ float arm_th = 0.95; @@ -71,6 +71,7 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon /* Check for disarm */ if (msg->r < -arm_th && msg->z < (1 - arm_th)) { _msg_actuator_armed.armed = false; + _msg_vehicle_control_mode.flag_armed = false; msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_STANDBY; } @@ -78,6 +79,7 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon /* Check for arm */ if (msg->r > arm_th && msg->z < (1 - arm_th)) { _msg_actuator_armed.armed = true; + _msg_vehicle_control_mode.flag_armed = true; msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED; } } @@ -88,7 +90,7 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; msg_vehicle_status.is_rotary_wing = true; - _vehicle_control_mode_pub.publish(msg_vehicle_control_mode); + _vehicle_control_mode_pub.publish(_msg_vehicle_control_mode); _actuator_armed_pub.publish(_msg_actuator_armed); _vehicle_status_pub.publish(msg_vehicle_status); diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h index f251f7c1a..58b7257b7 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -74,5 +74,6 @@ protected: px4::parameter_update _msg_parameter_update; px4::actuator_armed _msg_actuator_armed; + px4::vehicle_control_mode _msg_vehicle_control_mode; }; -- cgit v1.2.3 From 211b58cd1a0b8903a4f1c7a6f9e4e51deff8e7e2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 14:40:28 +0100 Subject: fix typo in comment --- src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 5577c5f03..1f0d88bcd 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 @@ -138,7 +138,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) // _publish_att_sp = true; } - /* reset yaw setpint to current position if needed */ + /* reset yaw setpoint to current position if needed */ if (_reset_yaw_sp) { _reset_yaw_sp = false; _v_att_sp_mod.data().yaw_body = _v_att->data().yaw; -- cgit v1.2.3 From 869d4115faaf6120715289b0e81ead14a2737553 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 16:02:58 +0100 Subject: ros mixer: remove debug output --- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 54f5fa78b..0749c8e92 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -223,19 +223,15 @@ void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_contro _n.getParamCached("mixer", mixer_name); if (mixer_name == "x") { _rotors = _config_index[0]; - ROS_WARN("using x"); } else if (mixer_name == "+") { _rotors = _config_index[1]; } else if (mixer_name == "e") { _rotors = _config_index[2]; } else if (mixer_name == "w") { _rotors = _config_index[3]; - ROS_WARN("using w"); } else if (mixer_name == "i") { _rotors = _config_index[4]; - ROS_WARN("using i"); } - ROS_WARN("mixer_name %s", mixer_name.c_str()); // mix mix(); -- cgit v1.2.3 From 990573aef3f875bb122f524c5f563248c2e96343 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 17:11:01 +0100 Subject: fix logic error in manual input node --- src/platforms/ros/nodes/manual_input/manual_input.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index 7e1ae7a17..72f6e252f 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.cpp +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -145,10 +145,8 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; return; - - } else { - ROS_WARN("requested mode via joystick that is not implemented"); } + } int main(int argc, char **argv) -- cgit v1.2.3 From 76d2417cc53c493a90689582ebae9057a3987641 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 19:02:43 +0100 Subject: fix comment --- src/platforms/ros/nodes/position_estimator/position_estimator.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.h b/src/platforms/ros/nodes/position_estimator/position_estimator.h index 4d396f39a..da1fc2c5a 100644 --- a/src/platforms/ros/nodes/position_estimator/position_estimator.h +++ b/src/platforms/ros/nodes/position_estimator/position_estimator.h @@ -33,7 +33,7 @@ /** * @file position_estimator.h - * Dummy position estimator that forwards attitude from gazebo to px4 topic + * Dummy position estimator that forwards position from gazebo to px4 topic * * @author Thomas Gubler */ -- cgit v1.2.3 From 6a122ab643e8a541329803117fc481c04aed902b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 30 Jan 2015 12:24:33 +0100 Subject: ros att estimator dummy node: publish timestamp --- src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp index f744aa11c..1d36e3d99 100644 --- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp @@ -136,6 +136,7 @@ void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr &msg) msg_v_att.pitchspeed = -(float)msg->angular_velocity.y; msg_v_att.yawspeed = -(float)msg->angular_velocity.z; + msg_v_att.timestamp = px4::get_time_micros(); _vehicle_attitude_pub.publish(msg_v_att); } -- cgit v1.2.3 From 82b8a42929e5bdcd53e6f8f7b2ee6995fbdf0707 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Feb 2015 10:53:48 +0100 Subject: fix timestamp problem in dummy pos estimator --- src/platforms/ros/nodes/position_estimator/position_estimator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp index fae304bf4..ed3a4efa5 100644 --- a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp +++ b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp @@ -52,7 +52,7 @@ PositionEstimator::PositionEstimator() : _n(), _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &PositionEstimator::ModelStatesCallback, this)), _vehicle_position_pub(_n.advertise("vehicle_local_position", 1)), - _startup_time(px4::get_time_micros()) + _startup_time(1) { } -- cgit v1.2.3 From 7c958a2cca90a6262dc491fe9ef86d85bacdf3da Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Feb 2015 10:57:56 +0100 Subject: multiplatform pos ctrl: fix division by zero --- .../mc_pos_control_multiplatform/mc_pos_control.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index aa23e0a60..a46163e6f 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -227,14 +227,14 @@ MulticopterPositionControl::reset_pos_sp() if (_reset_pos_sp) { _reset_pos_sp = false; /* shift position setpoint to make attitude setpoint continuous */ - _pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp_msg.data().R_body, 0, 2) * _att_sp_msg.data().thrust / _params.vel_p(0) - - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); - _pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp_msg.data().R_body, 1, 2) * _att_sp_msg.data().thrust / _params.vel_p(1) - - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); + _pos_sp(0) = _pos(0); //+ (_vel(0) - PX4_R(_att_sp_msg.data().R_body, 0, 2) * _att_sp_msg.data().thrust / _params.vel_p(0) + // - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); + _pos_sp(1) = _pos(1); //+ (_vel(1) - PX4_R(_att_sp_msg.data().R_body, 1, 2) * _att_sp_msg.data().thrust / _params.vel_p(1) + // - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); //XXX: port this once a mavlink like interface is available // mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); - PX4_INFO("[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); + PX4_INFO("[mpc] reset pos sp: %2.3f, %2.3f", (double)_pos_sp(0), (double)_pos_sp(1)); } } @@ -247,7 +247,7 @@ MulticopterPositionControl::reset_alt_sp() //XXX: port this once a mavlink like interface is available // mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2)); - PX4_INFO("[mpc] reset alt sp: %d", -(int)_pos_sp(2)); + PX4_INFO("[mpc] reset alt sp: %2.3f", -(double)_pos_sp(2)); } } @@ -557,7 +557,7 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti static uint64_t t_prev = 0; uint64_t t = get_time_micros(); - float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f; + float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.005f; t_prev = t; if (_control_mode->data().flag_armed && !was_armed) { -- cgit v1.2.3 From 3b07890361d56ce80d881e3969ff097b5cd96af4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Feb 2015 11:57:31 +0100 Subject: update sitl default params, make posctrl very slow for now --- launch/ardrone.launch | 19 ++++++++----------- launch/iris.launch | 17 ++++++----------- 2 files changed, 14 insertions(+), 22 deletions(-) diff --git a/launch/ardrone.launch b/launch/ardrone.launch index 495388be5..faf3bf6da 100644 --- a/launch/ardrone.launch +++ b/launch/ardrone.launch @@ -3,17 +3,14 @@ - - - - - - - - - - - + + + + + + + + diff --git a/launch/iris.launch b/launch/iris.launch index 44a0ae034..7b5b13250 100644 --- a/launch/iris.launch +++ b/launch/iris.launch @@ -3,18 +3,13 @@ - - - - - - - - - - - + + + + + + -- cgit v1.2.3 From 68e35ef0fd05504a42ed802f2a744dc253a6f747 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Feb 2015 12:19:42 +0100 Subject: remove unintended/leftover changes in test makefile --- makefiles/config_px4fmu-v2_test.mk | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index af80cae8f..c360f722b 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -66,10 +66,10 @@ MODULES += examples/publisher MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/uORB +LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/conversion -LIBRARIES += lib/mathlib/CMSIS MODULES += platforms/nuttx # @@ -92,6 +92,7 @@ MODULES += drivers/pca8574 # # Tests # + MODULES += modules/unit_test MODULES += modules/mavlink/mavlink_tests MODULES += modules/commander/commander_tests -- cgit v1.2.3 From 0dc511a76b873ceba8cf0677dcd54cd0c566cb3b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Feb 2015 12:20:54 +0100 Subject: remove unintended/leftover changes in fmu2 default makefile --- makefiles/config_px4fmu-v2_default.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 2ce738844..aa2e01229 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -31,7 +31,7 @@ MODULES += drivers/mb12xx MODULES += drivers/ll40ls # MODULES += drivers/trone MODULES += drivers/gps -# MODULES += drivers/hil +MODULES += drivers/hil # MODULES += drivers/hott # MODULES += drivers/hott/hott_telemetry # MODULES += drivers/hott/hott_sensors @@ -77,7 +77,7 @@ MODULES += modules/land_detector # Estimation modules (EKF/ SO3 / other filters) # MODULES += modules/attitude_estimator_ekf -# MODULES += modules/ekf_att_pos_estimator +MODULES += modules/ekf_att_pos_estimator MODULES += modules/position_estimator_inav # -- cgit v1.2.3 From 2979d146422908e108cecfe37094c191521810ed Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Feb 2015 12:25:55 +0100 Subject: remove unneeded include --- src/include/containers/List.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/include/containers/List.hpp b/src/include/containers/List.hpp index 5c0ba59fd..13cbda938 100644 --- a/src/include/containers/List.hpp +++ b/src/include/containers/List.hpp @@ -38,7 +38,6 @@ */ #pragma once -#include template class __EXPORT ListNode -- cgit v1.2.3 From 33a32fca142b33e8058ad579c73dfd7cf6b8e030 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Feb 2015 12:55:15 +0100 Subject: Tools/ros: add license info --- Tools/ros/docker/px4-ros-full/Dockerfile | 1 + Tools/ros/docker/px4-ros-full/README.md | 2 ++ Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh | 2 ++ Tools/ros/px4_ros_installation_ubuntu.sh | 1 + Tools/ros/px4_workspace_create.sh | 1 + Tools/ros/px4_workspace_setup.sh | 2 ++ Tools/ros/vagrant/docker-host-base/Vagrantfile | 4 +++- Tools/ros/vagrant/docker-host-base/config/docker-default | 3 +++ Tools/ros/vagrant/docker-host-base/config/xsessionrc | 1 + Tools/ros/vagrant/docker-host/Vagrantfile | 2 ++ Tools/ros/vagrant/px4-ros/Vagrantfile | 1 + 11 files changed, 19 insertions(+), 1 deletion(-) diff --git a/Tools/ros/docker/px4-ros-full/Dockerfile b/Tools/ros/docker/px4-ros-full/Dockerfile index 9c0fbc688..1242b56b5 100644 --- a/Tools/ros/docker/px4-ros-full/Dockerfile +++ b/Tools/ros/docker/px4-ros-full/Dockerfile @@ -1,6 +1,7 @@ # # PX4 full ROS container # +# License: according to LICENSE.md in the root directory of the PX4 Firmware repository FROM ubuntu:14.04.1 MAINTAINER Andreas Antener diff --git a/Tools/ros/docker/px4-ros-full/README.md b/Tools/ros/docker/px4-ros-full/README.md index 7d75754d9..af5170c70 100644 --- a/Tools/ros/docker/px4-ros-full/README.md +++ b/Tools/ros/docker/px4-ros-full/README.md @@ -2,6 +2,8 @@ Full desktop ROS container. +License: according to LICENSE.md in the root directory of the PX4 Firmware repository + **TODO:** - use https://github.com/phusion/baseimage-docker as base diff --git a/Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh b/Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh index 231166e27..2de5f8bec 100644 --- a/Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh +++ b/Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh @@ -3,6 +3,8 @@ # Create workspace at current location and fetch source repositories # +# License: according to LICENSE.md in the root directory of the PX4 Firmware repository + WDIR=`pwd` WORKSPACE=$WDIR/catkin_ws diff --git a/Tools/ros/px4_ros_installation_ubuntu.sh b/Tools/ros/px4_ros_installation_ubuntu.sh index 3b8b29381..7efc400cd 100755 --- a/Tools/ros/px4_ros_installation_ubuntu.sh +++ b/Tools/ros/px4_ros_installation_ubuntu.sh @@ -1,4 +1,5 @@ #!/bin/sh +# License: according to LICENSE.md in the root directory of the PX4 Firmware repository # main ROS Setup # following http://wiki.ros.org/indigo/Installation/Ubuntu diff --git a/Tools/ros/px4_workspace_create.sh b/Tools/ros/px4_workspace_create.sh index 4055f7320..cf80bcf8d 100755 --- a/Tools/ros/px4_workspace_create.sh +++ b/Tools/ros/px4_workspace_create.sh @@ -1,5 +1,6 @@ #!/bin/sh # this script creates a catkin_ws in the current folder +# License: according to LICENSE.md in the root directory of the PX4 Firmware repository mkdir -p catkin_ws/src cd catkin_ws/src diff --git a/Tools/ros/px4_workspace_setup.sh b/Tools/ros/px4_workspace_setup.sh index 1675e54f3..53568b4fb 100755 --- a/Tools/ros/px4_workspace_setup.sh +++ b/Tools/ros/px4_workspace_setup.sh @@ -1,4 +1,6 @@ #!/bin/bash +# License: according to LICENSE.md in the root directory of the PX4 Firmware repository + # run this script from the root of your catkin_ws source devel/setup.bash cd src diff --git a/Tools/ros/vagrant/docker-host-base/Vagrantfile b/Tools/ros/vagrant/docker-host-base/Vagrantfile index bc9329bf8..06e4e897d 100644 --- a/Tools/ros/vagrant/docker-host-base/Vagrantfile +++ b/Tools/ros/vagrant/docker-host-base/Vagrantfile @@ -9,6 +9,8 @@ # After build, do "vagrant package --base docker-host-base" to package, # and import as box: "vagrant box add --name uaventure/docker-host-base package.box" # +# License: according to LICENSE.md in the root directory of the PX4 Firmware repository + Vagrant.configure(2) do |config| config.vm.box = "ubuntu/trusty64" @@ -19,7 +21,7 @@ Vagrant.configure(2) do |config| vb.gui = true vb.memory = "1024" end - + config.vm.provision "file", source: "config/docker-default", destination: "/home/vagrant/docker-default" config.vm.provision "file", source: "config/xsessionrc", destination: "/home/vagrant/.xsessionrc" diff --git a/Tools/ros/vagrant/docker-host-base/config/docker-default b/Tools/ros/vagrant/docker-host-base/config/docker-default index e8a86ce6a..19c466b6b 100644 --- a/Tools/ros/vagrant/docker-host-base/config/docker-default +++ b/Tools/ros/vagrant/docker-host-base/config/docker-default @@ -23,4 +23,7 @@ #export TMPDIR="/mnt/bigdrive/docker-tmp" # Expose TCP port in addition to socket + +# License: according to LICENSE.md in the root directory of the PX4 Firmware repository + DOCKER_OPTS="${DOCKER_OPTS} -H unix:///var/run/docker.sock -H 0.0.0.0:2375" diff --git a/Tools/ros/vagrant/docker-host-base/config/xsessionrc b/Tools/ros/vagrant/docker-host-base/config/xsessionrc index d8ab6d53d..1bd4f8d3f 100644 --- a/Tools/ros/vagrant/docker-host-base/config/xsessionrc +++ b/Tools/ros/vagrant/docker-host-base/config/xsessionrc @@ -2,4 +2,5 @@ # # Disable X access control so we can easily start GUI apps # +# License: according to LICENSE.md in the root directory of the PX4 Firmware repository xhost + diff --git a/Tools/ros/vagrant/docker-host/Vagrantfile b/Tools/ros/vagrant/docker-host/Vagrantfile index 4ae94258d..2a0a02271 100644 --- a/Tools/ros/vagrant/docker-host/Vagrantfile +++ b/Tools/ros/vagrant/docker-host/Vagrantfile @@ -12,6 +12,8 @@ # You can then load an existing image, e.g.: # "docker load -i container-image.tar" # +# License: according to LICENSE.md in the root directory of the PX4 Firmware repository + Vagrant.configure(2) do |config| config.vm.box = "uaventure/docker-host-base" diff --git a/Tools/ros/vagrant/px4-ros/Vagrantfile b/Tools/ros/vagrant/px4-ros/Vagrantfile index a122fb4a3..5b372a94d 100644 --- a/Tools/ros/vagrant/px4-ros/Vagrantfile +++ b/Tools/ros/vagrant/px4-ros/Vagrantfile @@ -23,6 +23,7 @@ # - do not run the docker container with "--rm" (vagrant default). is that even possible? # - maybe map a local working directory to compile stuff without loosing it in side the docker container # +# License: according to LICENSE.md in the root directory of the PX4 Firmware repository Vagrant.configure(2) do |config| # Configure docker host -- cgit v1.2.3 From 04a905041009e4763cdc9e08a853c16a7e0b7dca Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 1 Feb 2015 12:07:16 +1100 Subject: ms5611: allow for all 4 bus combinations in ms5611 driver this uses the same table driven approach as the hmc5883 driver, and allows for a ms5611 baro on any of the 4 bus combinations. A simple "ms5611 start" will start all baros that are found. --- src/drivers/ms5611/ms5611.cpp | 258 +++++++++++++++++++++----------------- src/drivers/ms5611/ms5611.h | 6 +- src/drivers/ms5611/ms5611_i2c.cpp | 18 +-- src/drivers/ms5611/ms5611_spi.cpp | 25 ++-- 4 files changed, 160 insertions(+), 147 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 75b1f65fd..f03d6404f 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -69,6 +69,14 @@ #include "ms5611.h" +enum MS5611_BUS { + MS5611_BUS_ALL = 0, + MS5611_BUS_I2C_INTERNAL, + MS5611_BUS_I2C_EXTERNAL, + MS5611_BUS_SPI_INTERNAL, + MS5611_BUS_SPI_EXTERNAL +}; + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -784,15 +792,38 @@ MS5611::print_info() namespace ms5611 { -/* initialize explicitely for clarity */ -MS5611 *g_dev_ext = nullptr; -MS5611 *g_dev_int = nullptr; +/* + list of supported bus configurations + */ +struct ms5611_bus_option { + enum MS5611_BUS busid; + const char *devpath; + MS5611_constructor interface_constructor; + uint8_t busnum; + MS5611 *dev; +} bus_options[] = { +#if defined(PX4_SPIDEV_EXT_BARO) && defined(PX4_SPI_BUS_EXT) + { MS5611_BUS_SPI_EXTERNAL, "/dev/ms5611_spi_ext", &MS5611_spi_interface, PX4_SPI_BUS_EXT, NULL }, +#endif +#ifdef PX4_SPIDEV_BARO + { MS5611_BUS_SPI_INTERNAL, "/dev/ms5611_spi_int", &MS5611_spi_interface, PX4_SPI_BUS_SENSORS, NULL }, +#endif +#ifdef PX4_I2C_BUS_ONBOARD + { MS5611_BUS_I2C_INTERNAL, "/dev/ms5611_int", &MS5611_i2c_interface, PX4_I2C_BUS_ONBOARD, NULL }, +#endif +#ifdef PX4_I2C_BUS_EXPANSION + { MS5611_BUS_I2C_EXTERNAL, "/dev/ms5611_ext", &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION, NULL }, +#endif +}; +#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) -void start(bool external_bus); -void test(bool external_bus); -void reset(bool external_bus); +bool start_bus(struct ms5611_bus_option &bus); +struct ms5611_bus_option &find_bus(enum MS5611_BUS busid); +void start(enum MS5611_BUS busid); +void test(enum MS5611_BUS busid); +void reset(enum MS5611_BUS busid); void info(); -void calibrate(unsigned altitude, bool external_bus); +void calibrate(unsigned altitude, enum MS5611_BUS busid); void usage(); /** @@ -845,89 +876,89 @@ crc4(uint16_t *n_prom) /** * Start the driver. */ -void -start(bool external_bus) +bool +start_bus(struct ms5611_bus_option &bus) { - int fd; - prom_u prom_buf; - - if (external_bus && (g_dev_ext != nullptr)) { - /* if already started, the still command succeeded */ - errx(0, "ext already started"); - } else if (!external_bus && (g_dev_int != nullptr)) { - /* if already started, the still command succeeded */ - errx(0, "int already started"); - } - - device::Device *interface = nullptr; - - /* create the driver, try SPI first, fall back to I2C if unsuccessful */ - if (MS5611_spi_interface != nullptr) - interface = MS5611_spi_interface(prom_buf, external_bus); - if (interface == nullptr && (MS5611_i2c_interface != nullptr)) - interface = MS5611_i2c_interface(prom_buf); - - if (interface == nullptr) - errx(1, "failed to allocate an interface"); + ::printf("starting bus %s\n", bus.devpath); + usleep(5000); + if (bus.dev != nullptr) + errx(1,"bus option already started"); + prom_u prom_buf; + device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); if (interface->init() != OK) { delete interface; - errx(1, "interface init failed"); + warnx("no device on bus %u", (unsigned)bus.busid); + return false; } - if (external_bus) { - g_dev_ext = new MS5611(interface, prom_buf, MS5611_BARO_DEVICE_PATH_EXT); - if (g_dev_ext == nullptr) { - delete interface; - errx(1, "failed to allocate driver"); - } - - if (g_dev_ext->init() != OK) - goto fail; - - fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY); - - } else { - - g_dev_int = new MS5611(interface, prom_buf, MS5611_BARO_DEVICE_PATH_INT); - if (g_dev_int == nullptr) { - delete interface; - errx(1, "failed to allocate driver"); - } - - if (g_dev_int->init() != OK) - goto fail; - - fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY); - + bus.dev = new MS5611(interface, prom_buf, bus.devpath); + if (bus.dev != nullptr && OK != bus.dev->init()) { + delete bus.dev; + bus.dev = NULL; + return false; } + + int fd = open(bus.devpath, O_RDONLY); /* set the poll rate to default, starts automatic data collection */ - - if (fd < 0) { - warnx("can't open baro device"); - goto fail; + if (fd == -1) { + errx(1, "can't open baro device"); } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - warnx("failed setting default poll rate"); - goto fail; + close(fd); + errx(1, "failed setting default poll rate"); } - exit(0); + close(fd); + return true; +} -fail: - if (g_dev_int != nullptr) { - delete g_dev_int; - g_dev_int = nullptr; - } +/** + * Start the driver. + * + * This function call only returns once the driver + * is either successfully up and running or failed to start. + */ +void +start(enum MS5611_BUS busid) +{ + uint8_t i; + bool started = false; - if (g_dev_ext != nullptr) { - delete g_dev_ext; - g_dev_ext = nullptr; + for (i=0; iprint_info(); - } - - if (g_dev_int) { - warnx("int:"); - g_dev_int->print_info(); + for (uint8_t i=0; iprint_info(); + } } - exit(0); } @@ -1053,19 +1071,16 @@ info() * Calculate actual MSL pressure given current altitude */ void -calibrate(unsigned altitude, bool external_bus) +calibrate(unsigned altitude, enum MS5611_BUS busid) { + struct ms5611_bus_option &bus = find_bus(busid); struct baro_report report; float pressure; float p1; int fd; - if (external_bus) { - fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY); - } else { - fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY); - } + fd = open(bus.devpath, O_RDONLY); if (fd < 0) err(1, "open failed (try 'ms5611 start' if the driver is not running)"); @@ -1120,6 +1135,7 @@ calibrate(unsigned altitude, bool external_bus) if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) err(1, "BAROIOCSMSLPRESSURE"); + close(fd); exit(0); } @@ -1128,7 +1144,10 @@ usage() { warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'"); warnx("options:"); - warnx(" -X (external bus)"); + warnx(" -X (external I2C bus)"); + warnx(" -I (intternal I2C bus)"); + warnx(" -S (external SPI bus)"); + warnx(" -s (internal SPI bus)"); } } // namespace @@ -1136,14 +1155,23 @@ usage() int ms5611_main(int argc, char *argv[]) { - bool external_bus = false; + enum MS5611_BUS busid = MS5611_BUS_ALL; int ch; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "X")) != EOF) { + while ((ch = getopt(argc, argv, "XISs")) != EOF) { switch (ch) { case 'X': - external_bus = true; + busid = MS5611_BUS_I2C_EXTERNAL; + break; + case 'I': + busid = MS5611_BUS_I2C_INTERNAL; + break; + case 'S': + busid = MS5611_BUS_SPI_EXTERNAL; + break; + case 's': + busid = MS5611_BUS_SPI_INTERNAL; break; default: ms5611::usage(); @@ -1153,29 +1181,23 @@ ms5611_main(int argc, char *argv[]) const char *verb = argv[optind]; - if (argc > optind+1) { - if (!strcmp(argv[optind+1], "-X")) { - external_bus = true; - } - } - /* * Start/load the driver. */ if (!strcmp(verb, "start")) - ms5611::start(external_bus); + ms5611::start(busid); /* * Test the driver/device. */ if (!strcmp(verb, "test")) - ms5611::test(external_bus); + ms5611::test(busid); /* * Reset the driver. */ if (!strcmp(verb, "reset")) - ms5611::reset(external_bus); + ms5611::reset(busid); /* * Print driver information. @@ -1192,7 +1214,7 @@ ms5611_main(int argc, char *argv[]) long altitude = strtol(argv[optind+1], nullptr, 10); - ms5611::calibrate(altitude, external_bus); + ms5611::calibrate(altitude, busid); } errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h index 3f1f6c473..c8211b8dd 100644 --- a/src/drivers/ms5611/ms5611.h +++ b/src/drivers/ms5611/ms5611.h @@ -80,6 +80,6 @@ extern bool crc4(uint16_t *n_prom); } /* namespace */ /* interface factories */ -extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus) weak_function; -extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf) weak_function; - +extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function; +extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function; +typedef device::Device* (*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum); diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp index 87d9b94a6..ca651409f 100644 --- a/src/drivers/ms5611/ms5611_i2c.cpp +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -56,14 +56,6 @@ #include "board_config.h" -#ifdef PX4_I2C_OBDEV_MS5611 - -#ifndef PX4_I2C_BUS_ONBOARD - #define MS5611_BUS 1 -#else - #define MS5611_BUS PX4_I2C_BUS_ONBOARD -#endif - #define MS5611_ADDRESS_1 0x76 /* address select pins pulled high (PX4FMU series v1.6+) */ #define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */ @@ -74,7 +66,7 @@ device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf); class MS5611_I2C : public device::I2C { public: - MS5611_I2C(int bus, ms5611::prom_u &prom_buf); + MS5611_I2C(uint8_t bus, ms5611::prom_u &prom_buf); virtual ~MS5611_I2C(); virtual int init(); @@ -113,12 +105,12 @@ private: }; device::Device * -MS5611_i2c_interface(ms5611::prom_u &prom_buf) +MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) { - return new MS5611_I2C(MS5611_BUS, prom_buf); + return new MS5611_I2C(busnum, prom_buf); } -MS5611_I2C::MS5611_I2C(int bus, ms5611::prom_u &prom) : +MS5611_I2C::MS5611_I2C(uint8_t bus, ms5611::prom_u &prom) : I2C("MS5611_I2C", nullptr, bus, 0, 400000), _prom(prom) { @@ -274,5 +266,3 @@ MS5611_I2C::_read_prom() /* calculate CRC and return success/failure accordingly */ return ms5611::crc4(&_prom.c[0]) ? OK : -EIO; } - -#endif /* PX4_I2C_OBDEV_MS5611 */ diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 5234ce8d6..554a1d659 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -60,14 +60,14 @@ #define DIR_WRITE (0<<7) #define ADDR_INCREMENT (1<<6) -#ifdef PX4_SPIDEV_BARO +#if defined(PX4_SPIDEV_BARO) || defined(PX4_SPIDEV_EXT_BARO) device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus); class MS5611_SPI : public device::SPI { public: - MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf); + MS5611_SPI(uint8_t bus, spi_dev_e device, ms5611::prom_u &prom_buf); virtual ~MS5611_SPI(); virtual int init(); @@ -115,20 +115,21 @@ private: }; device::Device * -MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus) +MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) { - if (external_bus) { - #ifdef PX4_SPI_BUS_EXT - return new MS5611_SPI(PX4_SPI_BUS_EXT, (spi_dev_e)PX4_SPIDEV_EXT_BARO, prom_buf); - #else +#ifdef PX4_SPI_BUS_EXT + if (busnum == PX4_SPI_BUS_EXT) { +#ifdef PX4_SPIDEV_EXT_BARO + return new MS5611_SPI(busnum, (spi_dev_e)PX4_SPIDEV_EXT_BARO, prom_buf); +#else return nullptr; - #endif - } else { - return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); +#endif } +#endif + return new MS5611_SPI(busnum, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); } -MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) : +MS5611_SPI::MS5611_SPI(uint8_t bus, spi_dev_e device, ms5611::prom_u &prom_buf) : SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */), _prom(prom_buf) { @@ -281,4 +282,4 @@ MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len) return transfer(send, recv, len); } -#endif /* PX4_SPIDEV_BARO */ +#endif /* PX4_SPIDEV_BARO || PX4_SPIDEV_EXT_BARO */ -- cgit v1.2.3 From 2f25469bbe129873e4456fe0408bdf476db515f7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 1 Feb 2015 12:08:06 +1100 Subject: px4fmu-v1: removed baro I2C address in board_config.h the ms5611 can be on two addresses. The driver handles this, not the board config --- src/drivers/boards/px4fmu-v1/board_config.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 188885be2..882ec6aa2 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -114,7 +114,6 @@ __BEGIN_DECLS * Note that these are unshifted addresses. */ #define PX4_I2C_OBDEV_HMC5883 0x1e -#define PX4_I2C_OBDEV_MS5611 0x76 #define PX4_I2C_OBDEV_EEPROM NOTDEFINED #define PX4_I2C_OBDEV_LED 0x55 -- cgit v1.2.3 From f51dd7556e1977dc58d3bb9893b8634c77e3fba6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Feb 2015 13:10:45 +0100 Subject: FMUv1: Re-enable missing sensors --- makefiles/config_px4fmu-v1_default.mk | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 852edb788..8d2733051 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -24,18 +24,18 @@ MODULES += drivers/l3gd20 MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 -#MODULES += drivers/ll40ls +MODULES += drivers/ll40ls MODULES += drivers/trone -#MODULES += drivers/mb12xx +MODULES += drivers/mb12xx MODULES += drivers/gps MODULES += drivers/hil -#MODULES += drivers/blinkm +MODULES += drivers/blinkm MODULES += drivers/rgbled MODULES += drivers/mkblctrl MODULES += drivers/airspeed -#MODULES += drivers/ets_airspeed +MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed -#MODULES += drivers/frsky_telemetry +MODULES += drivers/frsky_telemetry MODULES += modules/sensors # -- cgit v1.2.3 From a3f577e64297602beea587bfb3c2c6b003f53ab0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Feb 2015 13:11:05 +0100 Subject: FMUv2: Re-enable missing sensors --- makefiles/config_px4fmu-v2_default.mk | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 3abebd82f..3e2d94236 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -27,15 +27,15 @@ MODULES += drivers/l3gd20 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx -# MODULES += drivers/sf0x +MODULES += drivers/sf0x MODULES += drivers/ll40ls -# MODULES += drivers/trone +MODULES += drivers/trone MODULES += drivers/gps MODULES += drivers/hil -# MODULES += drivers/hott -# MODULES += drivers/hott/hott_telemetry -# MODULES += drivers/hott/hott_sensors -# MODULES += drivers/blinkm +MODULES += drivers/hott +MODULES += drivers/hott/hott_telemetry +MODULES += drivers/hott/hott_sensors +MODULES += drivers/blinkm MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed -- cgit v1.2.3 From 344e9694289a12a8e962a589c99000ed9dab5928 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Feb 2015 13:11:24 +0100 Subject: FMUv2 test: Enable sensors module --- makefiles/config_px4fmu-v2_test.mk | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 91fc2a751..7c63839ad 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -26,6 +26,9 @@ MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/pca8574 MODULES += drivers/roboclaw +MODULES += drivers/airspeed +MODULES += drivers/ets_airspeed +MODULES += drivers/meas_airspeed MODULES += systemcmds/perf MODULES += systemcmds/reboot MODULES += systemcmds/tests @@ -33,6 +36,7 @@ MODULES += systemcmds/nshterm MODULES += systemcmds/mtd MODULES += systemcmds/ver MODULES += systemcmds/top +MODULES += modules/sensors # # Testing modules -- cgit v1.2.3 From f7d875d58d1286f9a3c63482772f4addbaf74eb0 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Wed, 28 Jan 2015 17:10:25 -0800 Subject: px4io: add safety_arm and safety_disarm commands This will be used to make updating firmware on boot for vehicles with no safety switch possible without power cycling. The startup script needs to be able to force safety on to allow the reboot to work. --- src/drivers/px4io/px4io.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 653d0d5d7..3b42afb6c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -3267,7 +3267,23 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "safety_arm")) { + int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0); + if (ret != OK) { + printf("failed to arm px4io\n"); + exit(1); + } + exit(0); + } + if (!strcmp(argv[1], "safety_disarm")) { + int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_ON, 0); + if (ret != OK) { + printf("failed to disarm px4io\n"); + exit(1); + } + exit(0); + } if (!strcmp(argv[1], "recovery")) { @@ -3396,6 +3412,6 @@ px4io_main(int argc, char *argv[]) out: errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug ',\n" - "'recovery', 'limit ', 'current', 'bind', 'checkcrc',\n" + "'recovery', 'limit ', 'current', 'bind', 'checkcrc', 'safety_arm', 'safety_disarm',\n" "'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm'"); } -- cgit v1.2.3 From 99a4724ef166d4ce71d873cb58229130fdb2d219 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Feb 2015 13:14:30 +0100 Subject: IO driver: Fix naming of safety command line argument --- src/drivers/px4io/px4io.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 3b42afb6c..24ece4298 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -3267,19 +3267,19 @@ px4io_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "safety_arm")) { + if (!strcmp(argv[1], "safety_off")) { int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0); if (ret != OK) { - printf("failed to arm px4io\n"); + printf("failed to disable safety\n"); exit(1); } exit(0); } - if (!strcmp(argv[1], "safety_disarm")) { + if (!strcmp(argv[1], "safety_on")) { int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_ON, 0); if (ret != OK) { - printf("failed to disarm px4io\n"); + printf("failed to enable safety\n"); exit(1); } exit(0); @@ -3412,6 +3412,6 @@ px4io_main(int argc, char *argv[]) out: errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug ',\n" - "'recovery', 'limit ', 'current', 'bind', 'checkcrc', 'safety_arm', 'safety_disarm',\n" + "'recovery', 'limit ', 'current', 'bind', 'checkcrc', 'safety_on', 'safety_off',\n" "'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm'"); } -- cgit v1.2.3 From 5d56a1c6a9a259ce9ff669192e54c88631e0effe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Feb 2015 13:58:47 +0100 Subject: Test latency of publication. --- src/modules/uORB/uORB.cpp | 85 +++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 75 insertions(+), 10 deletions(-) diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 6f021459c..764e33179 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -56,6 +56,7 @@ #include #include #include +#include #include @@ -682,9 +683,11 @@ namespace { ORBDevMaster *g_dev; +bool pubsubtest_passed = false; struct orb_test { int val; + hrt_abstime time; }; ORB_DEFINE(orb_test, struct orb_test); @@ -718,6 +721,46 @@ test_note(const char *fmt, ...) return OK; } +int pubsublatency_main(void) +{ + /* poll on test topic and output latency */ + float latency_integral = 0.0f; + + /* wakeup source(s) */ + struct pollfd fds[1]; + + int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_multitest), 0); + + fds[0].fd = test_multi_sub; + fds[0].events = POLLIN; + + struct orb_test t; + + const unsigned maxruns = 10; + + for (unsigned i = 0; i < maxruns; i++) { + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); + orb_copy(ORB_ID(orb_multitest), test_multi_sub, &t); + + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + hrt_abstime elt = hrt_elapsed_time(&t.time); + latency_integral += elt; + } + + orb_unsubscribe(test_multi_sub); + + warnx("mean: %8.4f", static_cast(latency_integral / maxruns)); + + pubsubtest_passed = true; + + return OK; +} + int test() { @@ -779,8 +822,7 @@ test() int instance0; int pfd0 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX); - test_note("advertised"); - usleep(300000); + test_note("advertised"); int instance1; int pfd1 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN); @@ -795,8 +837,7 @@ test() if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t)) return test_fail("mult. pub0 fail"); - test_note("published"); - usleep(300000); + test_note("published"); t.val = 203; if (OK != orb_publish(ORB_ID(orb_multitest), pfd1, &t)) @@ -805,19 +846,19 @@ test() /* subscribe to both topics and ensure valid data is received */ int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0); - if (OK != orb_copy(ORB_ID(orb_multitest), sfd0, &t)) + if (OK != orb_copy(ORB_ID(orb_multitest), sfd0, &u)) return test_fail("sub #0 copy failed: %d", errno); - if (t.val != 103) - return test_fail("sub #0 val. mismatch: %d", t.val); + if (u.val != 103) + return test_fail("sub #0 val. mismatch: %d", u.val); int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1); - if (OK != orb_copy(ORB_ID(orb_multitest), sfd1, &t)) + if (OK != orb_copy(ORB_ID(orb_multitest), sfd1, &u)) return test_fail("sub #1 copy failed: %d", errno); - if (t.val != 203) - return test_fail("sub #1 val. mismatch: %d", t.val); + if (u.val != 203) + return test_fail("sub #1 val. mismatch: %d", u.val); /* test priorities */ int prio; @@ -833,6 +874,30 @@ test() if (prio != ORB_PRIO_MIN) return test_fail("prio: %d", prio); + /* test pub / sub latency */ + + int pubsub_task = task_spawn_cmd("uorb_latency", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + (main_t)&pubsublatency_main, + nullptr); + + /* give the test task some data */ + while (!pubsubtest_passed) { + t.val = 303; + t.time = hrt_absolute_time(); + if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t)) + return test_fail("mult. pub0 timing fail"); + + /* simulate >800 Hz system operation */ + usleep(1000); + } + + if (pubsub_task < 0) { + return test_fail("failed launching task"); + } + return test_note("PASS"); } -- cgit v1.2.3 From 143508529feb10d5e767794b1fdd236d6915901b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Feb 2015 17:16:23 +0100 Subject: move rosbag launch and comment out --- launch/ardrone.launch | 2 -- launch/multicopter.launch | 1 + 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/launch/ardrone.launch b/launch/ardrone.launch index faf3bf6da..d53333b11 100644 --- a/launch/ardrone.launch +++ b/launch/ardrone.launch @@ -11,6 +11,4 @@ - - diff --git a/launch/multicopter.launch b/launch/multicopter.launch index 0f8cc5132..95400bd82 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -9,6 +9,7 @@ + -- cgit v1.2.3 From b0f2796aeb1ea05702220bd6fe334b346966cec4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Feb 2015 18:23:35 +0100 Subject: Remove MTD test --- src/systemcmds/tests/test_mtd.c | 236 -------------------------------------- src/systemcmds/tests/tests.h | 3 +- src/systemcmds/tests/tests_main.c | 3 +- 3 files changed, 2 insertions(+), 240 deletions(-) delete mode 100644 src/systemcmds/tests/test_mtd.c diff --git a/src/systemcmds/tests/test_mtd.c b/src/systemcmds/tests/test_mtd.c deleted file mode 100644 index 43231ccad..000000000 --- a/src/systemcmds/tests/test_mtd.c +++ /dev/null @@ -1,236 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-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 test_mtd.c - * - * Param storage / file test. - * - * @author Lorenz Meier - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "tests.h" - -#define PARAM_FILE_NAME "/fs/mtd_params" - -static int check_user_abort(int fd); -static void print_fail(void); -static void print_success(void); - -int check_user_abort(int fd) { - /* check if user wants to abort */ - char c; - - struct pollfd fds; - int ret; - fds.fd = 0; /* stdin */ - fds.events = POLLIN; - ret = poll(&fds, 1, 0); - - if (ret > 0) { - - read(0, &c, 1); - - switch (c) { - case 0x03: // ctrl-c - case 0x1b: // esc - case 'c': - case 'q': - { - warnx("Test aborted."); - fsync(fd); - close(fd); - return OK; - /* not reached */ - } - } - } - - return 1; -} - -void print_fail() -{ - printf("<[T]: MTD: FAIL>\n"); -} - -void print_success() -{ - printf("<[T]: MTD: OK>\n"); -} - -int -test_mtd(int argc, char *argv[]) -{ - unsigned iterations= 0; - - /* check if microSD card is mounted */ - struct stat buffer; - if (stat(PARAM_FILE_NAME, &buffer)) { - warnx("file %s not found, aborting MTD test", PARAM_FILE_NAME); - print_fail(); - return 1; - } - - // XXX get real storage space here - unsigned file_size = 4096; - - /* perform tests for a range of chunk sizes */ - unsigned chunk_sizes[] = {256, 512, 4096}; - - for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { - - printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]); - - uint8_t write_buf[chunk_sizes[c]] __attribute__((aligned(64))); - - /* fill write buffer with known values */ - for (unsigned i = 0; i < sizeof(write_buf); i++) { - /* this will wrap, but we just need a known value with spacing */ - write_buf[i] = i+11; - } - - uint8_t read_buf[chunk_sizes[c]] __attribute__((aligned(64))); - hrt_abstime start, end; - - int fd = open(PARAM_FILE_NAME, O_RDONLY); - int rret = read(fd, read_buf, chunk_sizes[c]); - close(fd); - if (rret <= 0) { - err(1, "read error"); - } - - fd = open(PARAM_FILE_NAME, O_WRONLY); - - printf("printing 2 percent of the first chunk:\n"); - for (unsigned i = 0; i < sizeof(read_buf) / 50; i++) { - printf("%02X", read_buf[i]); - } - printf("\n"); - - iterations = file_size / chunk_sizes[c]; - - start = hrt_absolute_time(); - for (unsigned i = 0; i < iterations; i++) { - int wret = write(fd, write_buf, chunk_sizes[c]); - - if (wret != (int)chunk_sizes[c]) { - warn("WRITE ERROR!"); - print_fail(); - return 1; - } - - fsync(fd); - - if (!check_user_abort(fd)) - return OK; - - } - end = hrt_absolute_time(); - - warnx("write took %llu us", (end - start)); - - close(fd); - fd = open(PARAM_FILE_NAME, O_RDONLY); - - /* read back data for validation */ - for (unsigned i = 0; i < iterations; i++) { - int rret2 = read(fd, read_buf, chunk_sizes[c]); - - if (rret2 != (int)chunk_sizes[c]) { - warnx("READ ERROR!"); - print_fail(); - return 1; - } - - /* compare value */ - bool compare_ok = true; - - for (unsigned j = 0; j < chunk_sizes[c]; j++) { - if (read_buf[j] != write_buf[j]) { - warnx("COMPARISON ERROR: byte %d", j); - print_fail(); - compare_ok = false; - break; - } - } - - if (!compare_ok) { - warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); - print_fail(); - return 1; - } - - if (!check_user_abort(fd)) - return OK; - - } - - - close(fd); - - } - - /* fill the file with 0xFF to make it look new again */ - char ffbuf[64]; - memset(ffbuf, 0xFF, sizeof(ffbuf)); - int fd = open(PARAM_FILE_NAME, O_WRONLY); - for (unsigned i = 0; i < file_size / sizeof(ffbuf); i++) { - int ret = write(fd, ffbuf, sizeof(ffbuf)); - - if (ret != sizeof(ffbuf)) { - warnx("ERROR! Could not fill file with 0xFF"); - close(fd); - print_fail(); - return 1; - } - } - - (void)close(fd); - print_success(); - - return 0; -} diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index ad55e1410..69fb0e57b 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 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 @@ -112,7 +112,6 @@ extern int test_mixer(int argc, char *argv[]); extern int test_rc(int argc, char *argv[]); extern int test_conv(int argc, char *argv[]); extern int test_mount(int argc, char *argv[]); -extern int test_mtd(int argc, char *argv[]); extern int test_mathlib(int argc, char *argv[]); __END_DECLS diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 0f56704e6..2f586fa85 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 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 @@ -109,7 +109,6 @@ const struct { {"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST}, {"conv", test_conv, OPT_NOJIGTEST | OPT_NOALLTEST}, {"mount", test_mount, OPT_NOJIGTEST | OPT_NOALLTEST}, - {"mtd", test_mtd, 0}, #ifndef TESTS_MATHLIB_DISABLE {"mathlib", test_mathlib, 0}, #endif -- cgit v1.2.3 From 21d3e18e3ca424093fb60e04ead3678fa09dd11d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Feb 2015 22:11:20 +0100 Subject: Remove C file from build --- src/systemcmds/tests/module.mk | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 0dc333f0a..1d728e857 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -32,8 +32,7 @@ SRCS = test_adc.c \ test_ppm_loopback.c \ test_rc.c \ test_conv.cpp \ - test_mount.c \ - test_mtd.c + test_mount.c EXTRACXXFLAGS = -Wframe-larger-than=2500 -Wno-float-equal -Wno-double-promotion -- cgit v1.2.3 From 52bff670766cb338afbb131c0dc326f3b19a8817 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 2 Feb 2015 07:21:27 +1100 Subject: ms5611: removed debug code --- src/drivers/ms5611/ms5611.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index f03d6404f..31dbdbcd3 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -879,8 +879,6 @@ crc4(uint16_t *n_prom) bool start_bus(struct ms5611_bus_option &bus) { - ::printf("starting bus %s\n", bus.devpath); - usleep(5000); if (bus.dev != nullptr) errx(1,"bus option already started"); -- cgit v1.2.3 From e6a7dc7a3ffd4eb988e9039171f1460f656b28c8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Feb 2015 21:03:19 +0100 Subject: Fixed unit test usage of visibility macros --- src/modules/systemlib/err.h | 3 ++- unittests/CMakeLists.txt | 3 +++ unittests/sbus2_test.cpp | 2 +- 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/err.h b/src/modules/systemlib/err.h index ca13d6265..af90c2560 100644 --- a/src/modules/systemlib/err.h +++ b/src/modules/systemlib/err.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 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 @@ -66,6 +66,7 @@ #define _SYSTEMLIB_ERR_H #include +#include "visibility.h" __BEGIN_DECLS diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 666570a71..3e44f822d 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -23,8 +23,11 @@ include_directories(${CMAKE_SOURCE_DIR}) include_directories(${PX_SRC}) include_directories(${PX_SRC}/modules) include_directories(${PX_SRC}/lib) +include_directories(${PX_SRC}/drivers) add_definitions(-D__EXPORT=) +add_definitions(-DERROR=-1) +add_definitions(-DOK=0) # check add_custom_target(unittests COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure) diff --git a/unittests/sbus2_test.cpp b/unittests/sbus2_test.cpp index ee4f3d1d6..15a21c86b 100644 --- a/unittests/sbus2_test.cpp +++ b/unittests/sbus2_test.cpp @@ -11,7 +11,7 @@ #include "gtest/gtest.h" TEST(SBUS2Test, SBUS2) { - char *filepath = "testdata/sbus2_r7008SB.txt"; + const char *filepath = "testdata/sbus2_r7008SB.txt"; FILE *fp; fp = fopen(filepath,"rt"); -- cgit v1.2.3 From 19cd49615747128c4d7a8a4a3010bf4c51eb1961 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Feb 2015 18:24:13 +0100 Subject: Commander: Use indexed sensor names in calibration routines --- .../commander/accelerometer_calibration.cpp | 16 +++++++-------- src/modules/commander/gyro_calibration.cpp | 24 ++++++++++++++-------- src/modules/commander/mag_calibration.cpp | 16 +++++++-------- 3 files changed, 32 insertions(+), 24 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 13ab966ab..d9e7e21fc 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 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 @@ -221,17 +221,17 @@ int do_accel_calibration(int mavlink_fd) accel_scale.z_scale = accel_T_rotated(2, 2); /* set parameters */ - if (param_set(param_find("SENS_ACC_XOFF"), &(accel_scale.x_offset)) - || param_set(param_find("SENS_ACC_YOFF"), &(accel_scale.y_offset)) - || param_set(param_find("SENS_ACC_ZOFF"), &(accel_scale.z_offset)) - || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale.x_scale)) - || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale.y_scale)) - || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale.z_scale))) { + if (param_set(param_find("CAL_ACC0_XOFF"), &(accel_scale.x_offset)) + || param_set(param_find("CAL_ACC0_YOFF"), &(accel_scale.y_offset)) + || param_set(param_find("CAL_ACC0_ZOFF"), &(accel_scale.z_offset)) + || param_set(param_find("CAL_ACC0_XSCALE"), &(accel_scale.x_scale)) + || param_set(param_find("CAL_ACC0_YSCALE"), &(accel_scale.y_scale)) + || param_set(param_find("CAL_ACC0_ZSCALE"), &(accel_scale.z_scale))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); res = ERROR; } - if (param_set(param_find("SENS_ACC_ID"), &(device_id))) { + if (param_set(param_find("CAL_ACC0_ID"), &(device_id))) { res = ERROR; } } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 8410297ef..e941e327c 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 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 @@ -51,6 +51,7 @@ #include #include #include +#include /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -80,6 +81,13 @@ int do_gyro_calibration(int mavlink_fd) int res = OK; + /* store board ID */ + uint32_t mcu_id[3]; + mcu_unique_id(&mcu_id[0]); + + /* store last 32bit number - not unique, but unique in a given set */ + param_set(param_find("CAL_BOARD_ID"), &mcu_id[2]); + /* reset all offsets to zero and all scales to one */ int fd = open(GYRO_DEVICE_PATH, 0); @@ -149,9 +157,9 @@ int do_gyro_calibration(int mavlink_fd) if (res == OK) { /* set offset parameters to new values */ - if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset)) - || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset)) - || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) { + if (param_set(param_find("CAL_GYRO0_XOFF"), &(gyro_scale.x_offset)) + || param_set(param_find("CAL_GYRO0_YOFF"), &(gyro_scale.y_offset)) + || param_set(param_find("CAL_GYRO0_ZOFF"), &(gyro_scale.z_offset))) { mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params"); res = ERROR; } @@ -275,13 +283,13 @@ int do_gyro_calibration(int mavlink_fd) if (res == OK) { /* set scale parameters to new values */ - if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale)) - || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale)) - || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) { + if (param_set(param_find("CAL_GYRO0_XSCALE"), &(gyro_scale.x_scale)) + || param_set(param_find("CAL_GYRO0_YSCALE"), &(gyro_scale.y_scale)) + || param_set(param_find("CAL_GYRO0_ZSCALE"), &(gyro_scale.z_scale))) { mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params"); res = ERROR; } - if (param_set(param_find("SENS_GYRO_ID"), &(device_id))) { + if (param_set(param_find("CAL_GYRO0_ID"), &(device_id))) { res = ERROR; } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 2afb9a440..7147fb283 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 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 @@ -263,30 +263,30 @@ int do_mag_calibration(int mavlink_fd) if (res == OK) { /* set parameters */ - if (param_set(param_find("SENS_MAG_ID"), &(device_id))) { + if (param_set(param_find("CAL_MAG0_ID"), &(device_id))) { res = ERROR; } - if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { + if (param_set(param_find("CAL_MAG0_XOFF"), &(mscale.x_offset))) { res = ERROR; } - if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { + if (param_set(param_find("CAL_MAG0_YOFF"), &(mscale.y_offset))) { res = ERROR; } - if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { + if (param_set(param_find("CAL_MAG0_ZOFF"), &(mscale.z_offset))) { res = ERROR; } - if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { + if (param_set(param_find("CAL_MAG0_XSCALE"), &(mscale.x_scale))) { res = ERROR; } - if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { + if (param_set(param_find("CAL_MAG0_YSCALE"), &(mscale.y_scale))) { res = ERROR; } - if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { + if (param_set(param_find("CAL_MAG0_ZSCALE"), &(mscale.z_scale))) { res = ERROR; } -- cgit v1.2.3 From 23e7823b510397fea2ee93ecfc2211d334148eb6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Feb 2015 18:24:29 +0100 Subject: sensors app: Use indexed param names --- src/modules/sensors/sensor_params.c | 386 +++++++++++++++++++++++++++++++++--- src/modules/sensors/sensors.cpp | 36 ++-- 2 files changed, 379 insertions(+), 43 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 67b7feef7..5e04241fe 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 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 @@ -36,20 +36,192 @@ * * Parameters defined by the sensors task. * - * @author Lorenz Meier - * @author Julian Oes - * @author Thomas Gubler + * @author Lorenz Meier + * @author Julian Oes + * @author Thomas Gubler */ #include #include +/** + * ID of the board this parameter set was calibrated on. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_BOARD_ID, 0); + +/** + * ID of the Gyro that the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_GYRO0_ID, 0); + +/** + * Gyro X-axis offset + * + * @min -10.0 + * @max 10.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_GYRO0_XOFF, 0.0f); + +/** + * Gyro Y-axis offset + * + * @min -10.0 + * @max 10.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_GYRO0_YOFF, 0.0f); + +/** + * Gyro Z-axis offset + * + * @min -5.0 + * @max 5.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_GYRO0_ZOFF, 0.0f); + +/** + * Gyro X-axis scaling factor + * + * @min -1.5 + * @max 1.5 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_GYRO0_XSCALE, 1.0f); + +/** + * Gyro Y-axis scaling factor + * + * @min -1.5 + * @max 1.5 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_GYRO0_YSCALE, 1.0f); + +/** + * Gyro Z-axis scaling factor + * + * @min -1.5 + * @max 1.5 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_GYRO0_ZSCALE, 1.0f); + +/** + * ID of Magnetometer the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG0_ID, 0); + +/** + * Magnetometer X-axis offset + * + * @min -500.0 + * @max 500.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG0_XOFF, 0.0f); + +/** + * Magnetometer Y-axis offset + * + * @min -500.0 + * @max 500.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG0_YOFF, 0.0f); + +/** + * Magnetometer Z-axis offset + * + * @min -500.0 + * @max 500.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG0_ZOFF, 0.0f); + +/** + * Magnetometer X-axis scaling factor + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG0_XSCALE, 1.0f); + +/** + * Magnetometer Y-axis scaling factor + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG0_YSCALE, 1.0f); + +/** + * Magnetometer Z-axis scaling factor + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG0_ZSCALE, 1.0f); + +/** + * ID of the Accelerometer that the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_ACC0_ID, 0); + +/** + * Accelerometer X-axis offset + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_ACC0_XOFF, 0.0f); + +/** + * Accelerometer Y-axis offset + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_ACC0_YOFF, 0.0f); + +/** + * Accelerometer Z-axis offset + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_ACC0_ZOFF, 0.0f); + +/** + * Accelerometer X-axis scaling factor + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_ACC0_XSCALE, 1.0f); + +/** + * Accelerometer Y-axis scaling factor + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_ACC0_YSCALE, 1.0f); + +/** + * Accelerometer Z-axis scaling factor + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_ACC0_ZSCALE, 1.0f); + /** * ID of the Gyro that the calibration is for. * * @group Sensor Calibration */ -PARAM_DEFINE_INT32(SENS_GYRO_ID, 0); +PARAM_DEFINE_INT32(CAL_GYRO1_ID, 0); /** * Gyro X-axis offset @@ -58,7 +230,7 @@ PARAM_DEFINE_INT32(SENS_GYRO_ID, 0); * @max 10.0 * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); +PARAM_DEFINE_FLOAT(CAL_GYRO1_XOFF, 0.0f); /** * Gyro Y-axis offset @@ -67,7 +239,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); * @max 10.0 * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); +PARAM_DEFINE_FLOAT(CAL_GYRO1_YOFF, 0.0f); /** * Gyro Z-axis offset @@ -76,7 +248,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); * @max 5.0 * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); +PARAM_DEFINE_FLOAT(CAL_GYRO1_ZOFF, 0.0f); /** * Gyro X-axis scaling factor @@ -85,7 +257,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); * @max 1.5 * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); +PARAM_DEFINE_FLOAT(CAL_GYRO1_XSCALE, 1.0f); /** * Gyro Y-axis scaling factor @@ -94,7 +266,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); * @max 1.5 * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f); +PARAM_DEFINE_FLOAT(CAL_GYRO1_YSCALE, 1.0f); /** * Gyro Z-axis scaling factor @@ -103,14 +275,14 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f); * @max 1.5 * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f); +PARAM_DEFINE_FLOAT(CAL_GYRO1_ZSCALE, 1.0f); /** * ID of Magnetometer the calibration is for. * * @group Sensor Calibration */ -PARAM_DEFINE_INT32(SENS_MAG_ID, 0); +PARAM_DEFINE_INT32(CAL_MAG1_ID, 0); /** * Magnetometer X-axis offset @@ -119,7 +291,7 @@ PARAM_DEFINE_INT32(SENS_MAG_ID, 0); * @max 500.0 * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f); +PARAM_DEFINE_FLOAT(CAL_MAG1_XOFF, 0.0f); /** * Magnetometer Y-axis offset @@ -128,7 +300,7 @@ PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f); * @max 500.0 * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f); +PARAM_DEFINE_FLOAT(CAL_MAG1_YOFF, 0.0f); /** * Magnetometer Z-axis offset @@ -137,78 +309,242 @@ PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f); * @max 500.0 * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f); +PARAM_DEFINE_FLOAT(CAL_MAG1_ZOFF, 0.0f); /** * Magnetometer X-axis scaling factor * * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 1.0f); +PARAM_DEFINE_FLOAT(CAL_MAG1_XSCALE, 1.0f); /** * Magnetometer Y-axis scaling factor * * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 1.0f); +PARAM_DEFINE_FLOAT(CAL_MAG1_YSCALE, 1.0f); /** * Magnetometer Z-axis scaling factor * * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 1.0f); +PARAM_DEFINE_FLOAT(CAL_MAG1_ZSCALE, 1.0f); /** * ID of the Accelerometer that the calibration is for. * * @group Sensor Calibration */ -PARAM_DEFINE_INT32(SENS_ACC_ID, 0); +PARAM_DEFINE_INT32(CAL_ACC1_ID, 0); /** * Accelerometer X-axis offset * * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_ACC_XOFF, 0.0f); +PARAM_DEFINE_FLOAT(CAL_ACC1_XOFF, 0.0f); /** * Accelerometer Y-axis offset * * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_ACC_YOFF, 0.0f); +PARAM_DEFINE_FLOAT(CAL_ACC1_YOFF, 0.0f); /** * Accelerometer Z-axis offset * * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_ACC_ZOFF, 0.0f); +PARAM_DEFINE_FLOAT(CAL_ACC1_ZOFF, 0.0f); /** * Accelerometer X-axis scaling factor * * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); +PARAM_DEFINE_FLOAT(CAL_ACC1_XSCALE, 1.0f); /** * Accelerometer Y-axis scaling factor * * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); +PARAM_DEFINE_FLOAT(CAL_ACC1_YSCALE, 1.0f); /** * Accelerometer Z-axis scaling factor * * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); +PARAM_DEFINE_FLOAT(CAL_ACC1_ZSCALE, 1.0f); + +/** + * ID of the Gyro that the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_GYRO2_ID, 0); + +/** + * Gyro X-axis offset + * + * @min -10.0 + * @max 10.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_GYRO2_XOFF, 0.0f); + +/** + * Gyro Y-axis offset + * + * @min -10.0 + * @max 10.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_GYRO2_YOFF, 0.0f); + +/** + * Gyro Z-axis offset + * + * @min -5.0 + * @max 5.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_GYRO2_ZOFF, 0.0f); + +/** + * Gyro X-axis scaling factor + * + * @min -1.5 + * @max 1.5 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_GYRO2_XSCALE, 1.0f); + +/** + * Gyro Y-axis scaling factor + * + * @min -1.5 + * @max 1.5 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_GYRO2_YSCALE, 1.0f); + +/** + * Gyro Z-axis scaling factor + * + * @min -1.5 + * @max 1.5 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_GYRO2_ZSCALE, 1.0f); + +/** + * ID of Magnetometer the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG2_ID, 0); + +/** + * Magnetometer X-axis offset + * + * @min -500.0 + * @max 500.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG2_XOFF, 0.0f); + +/** + * Magnetometer Y-axis offset + * + * @min -500.0 + * @max 500.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG2_YOFF, 0.0f); +/** + * Magnetometer Z-axis offset + * + * @min -500.0 + * @max 500.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG2_ZOFF, 0.0f); + +/** + * Magnetometer X-axis scaling factor + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG2_XSCALE, 1.0f); + +/** + * Magnetometer Y-axis scaling factor + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG2_YSCALE, 1.0f); + +/** + * Magnetometer Z-axis scaling factor + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG2_ZSCALE, 1.0f); + +/** + * ID of the Accelerometer that the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_ACC2_ID, 0); + +/** + * Accelerometer X-axis offset + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_ACC2_XOFF, 0.0f); + +/** + * Accelerometer Y-axis offset + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_ACC2_YOFF, 0.0f); + +/** + * Accelerometer Z-axis offset + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_ACC2_ZOFF, 0.0f); + +/** + * Accelerometer X-axis scaling factor + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_ACC2_XSCALE, 1.0f); + +/** + * Accelerometer Y-axis scaling factor + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_ACC2_YSCALE, 1.0f); + +/** + * Accelerometer Z-axis scaling factor + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_ACC2_ZSCALE, 1.0f); /** * Differential pressure sensor offset diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 630c54335..77df1b0a6 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -619,29 +619,29 @@ Sensors::Sensors() : _parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH"); /* gyro offsets */ - _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); - _parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF"); - _parameter_handles.gyro_offset[2] = param_find("SENS_GYRO_ZOFF"); - _parameter_handles.gyro_scale[0] = param_find("SENS_GYRO_XSCALE"); - _parameter_handles.gyro_scale[1] = param_find("SENS_GYRO_YSCALE"); - _parameter_handles.gyro_scale[2] = param_find("SENS_GYRO_ZSCALE"); + _parameter_handles.gyro_offset[0] = param_find("CAL_GYRO0_XOFF"); + _parameter_handles.gyro_offset[1] = param_find("CAL_GYRO0_YOFF"); + _parameter_handles.gyro_offset[2] = param_find("CAL_GYRO0_ZOFF"); + _parameter_handles.gyro_scale[0] = param_find("CAL_GYRO0_XSCALE"); + _parameter_handles.gyro_scale[1] = param_find("CAL_GYRO0_YSCALE"); + _parameter_handles.gyro_scale[2] = param_find("CAL_GYRO0_ZSCALE"); /* accel offsets */ - _parameter_handles.accel_offset[0] = param_find("SENS_ACC_XOFF"); - _parameter_handles.accel_offset[1] = param_find("SENS_ACC_YOFF"); - _parameter_handles.accel_offset[2] = param_find("SENS_ACC_ZOFF"); - _parameter_handles.accel_scale[0] = param_find("SENS_ACC_XSCALE"); - _parameter_handles.accel_scale[1] = param_find("SENS_ACC_YSCALE"); - _parameter_handles.accel_scale[2] = param_find("SENS_ACC_ZSCALE"); + _parameter_handles.accel_offset[0] = param_find("CAL_ACC0_XOFF"); + _parameter_handles.accel_offset[1] = param_find("CAL_ACC0_YOFF"); + _parameter_handles.accel_offset[2] = param_find("CAL_ACC0_ZOFF"); + _parameter_handles.accel_scale[0] = param_find("CAL_ACC0_XSCALE"); + _parameter_handles.accel_scale[1] = param_find("CAL_ACC0_YSCALE"); + _parameter_handles.accel_scale[2] = param_find("CAL_ACC0_ZSCALE"); /* mag offsets */ - _parameter_handles.mag_offset[0] = param_find("SENS_MAG_XOFF"); - _parameter_handles.mag_offset[1] = param_find("SENS_MAG_YOFF"); - _parameter_handles.mag_offset[2] = param_find("SENS_MAG_ZOFF"); + _parameter_handles.mag_offset[0] = param_find("CAL_MAG0_XOFF"); + _parameter_handles.mag_offset[1] = param_find("CAL_MAG0_YOFF"); + _parameter_handles.mag_offset[2] = param_find("CAL_MAG0_ZOFF"); - _parameter_handles.mag_scale[0] = param_find("SENS_MAG_XSCALE"); - _parameter_handles.mag_scale[1] = param_find("SENS_MAG_YSCALE"); - _parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE"); + _parameter_handles.mag_scale[0] = param_find("CAL_MAG0_XSCALE"); + _parameter_handles.mag_scale[1] = param_find("CAL_MAG0_YSCALE"); + _parameter_handles.mag_scale[2] = param_find("CAL_MAG0_ZSCALE"); /* Differential pressure offset */ _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); -- cgit v1.2.3 From ed98dc1fdfad3f4ec501c5a9a8cfdaaf9b377614 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Feb 2015 18:25:06 +0100 Subject: Config app: Use indexed sensor names --- src/systemcmds/config/config.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index f54342f06..78db19801 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -195,7 +195,7 @@ do_gyro(int argc, char *argv[]) int id = ioctl(fd, DEVIOCGDEVICEID,0); int32_t calibration_id = 0; - param_get(param_find("SENS_GYRO_ID"), &(calibration_id)); + param_get(param_find("CAL_GYRO0_ID"), &(calibration_id)); warnx("gyro: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", id, calibration_id, srate, prate, range); @@ -272,7 +272,7 @@ do_mag(int argc, char *argv[]) int id = ioctl(fd, DEVIOCGDEVICEID,0); int32_t calibration_id = 0; - param_get(param_find("SENS_MAG_ID"), &(calibration_id)); + param_get(param_find("CAL_MAG0_ID"), &(calibration_id)); warnx("mag: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range); @@ -349,7 +349,7 @@ do_accel(int argc, char *argv[]) int id = ioctl(fd, DEVIOCGDEVICEID,0); int32_t calibration_id = 0; - param_get(param_find("SENS_ACC_ID"), &(calibration_id)); + param_get(param_find("CAL_ACC0_ID"), &(calibration_id)); warnx("accel: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", id, calibration_id, srate, prate, range); -- cgit v1.2.3 From 9d0c74e8ec67ffa4847ee88c0385b088ce73b0ed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Feb 2015 18:26:00 +0100 Subject: Preflight check: Use indexed sensor params --- src/systemcmds/preflight_check/preflight_check.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 3e1f76714..c5c959cf3 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -99,7 +99,7 @@ int preflight_check_main(int argc, char *argv[]) } devid = ioctl(fd, DEVIOCGDEVICEID,0); - param_get(param_find("SENS_MAG_ID"), &(calibration_devid)); + param_get(param_find("CAL_MAG0_ID"), &(calibration_devid)); if (devid != calibration_devid){ warnx("magnetometer calibration is for a different device - calibrate magnetometer first"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CAL ID"); @@ -122,7 +122,7 @@ int preflight_check_main(int argc, char *argv[]) fd = open(ACCEL_DEVICE_PATH, O_RDONLY); devid = ioctl(fd, DEVIOCGDEVICEID,0); - param_get(param_find("SENS_ACC_ID"), &(calibration_devid)); + param_get(param_find("CAL_ACC0_ID"), &(calibration_devid)); if (devid != calibration_devid){ warnx("accelerometer calibration is for a different device - calibrate accelerometer first"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACC CAL ID"); @@ -168,7 +168,7 @@ int preflight_check_main(int argc, char *argv[]) fd = open(GYRO_DEVICE_PATH, 0); devid = ioctl(fd, DEVIOCGDEVICEID,0); - param_get(param_find("SENS_GYRO_ID"), &(calibration_devid)); + param_get(param_find("CAL_GYRO0_ID"), &(calibration_devid)); if (devid != calibration_devid){ warnx("gyro calibration is for a different device - calibrate gyro first"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CAL ID"); -- cgit v1.2.3 From 5a12688ebe99e9a2f8674b971c0280cdb8073ca6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Feb 2015 23:58:30 +0100 Subject: Make mcu version header C++ safe --- src/modules/systemlib/mcu_version.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/systemlib/mcu_version.h b/src/modules/systemlib/mcu_version.h index c8a0bf5cd..e951e1e39 100644 --- a/src/modules/systemlib/mcu_version.h +++ b/src/modules/systemlib/mcu_version.h @@ -35,6 +35,8 @@ #include +__BEGIN_DECLS + /* magic numbers from reference manual */ enum MCU_REV { MCU_REV_STM32F4_REV_A = 0x1000, @@ -61,3 +63,5 @@ __EXPORT void mcu_unique_id(uint32_t *uid_96_bit); * @return The silicon revision / version number as integer */ __EXPORT int mcu_version(char* rev, char** revstr); + +__END_DECLS -- cgit v1.2.3 From b42b6c50d4aabe370282973bdf87373d7de478c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Feb 2015 14:21:31 +0100 Subject: Improve Navigator output --- src/modules/navigator/navigator_main.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 3f7670ec4..edc61dcc6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 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 @@ -247,9 +247,6 @@ Navigator::task_main_trampoline(int argc, char *argv[]) void Navigator::task_main() { - /* inform about start */ - warnx("Initializing.."); - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); _geofence.setMavlinkFd(_mavlink_fd); @@ -263,7 +260,7 @@ Navigator::task_main() _geofence.loadFromFile(GEOFENCE_FILENAME); } else { - mavlink_log_critical(_mavlink_fd, "#audio: No geofence file"); + mavlink_log_info(_mavlink_fd, "No geofence set"); if (_geofence.clearDm() > 0) warnx("Geofence cleared"); else -- cgit v1.2.3 From 0e1832452fdbba0ecc3de944bbc25c2eb9c2f313 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 19:13:03 +0100 Subject: drv_accel: introduce default samplerate --- src/drivers/drv_accel.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 1eca6155b..52e024c91 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -97,6 +97,8 @@ ORB_DECLARE(sensor_accel); /** set the accel internal sample rate to at least (arg) Hz */ #define ACCELIOCSSAMPLERATE _ACCELIOC(0) +#define ACCEL_SAMPLERATE_DEFAULT 1000003 /**< default sample rate */ + /** return the accel internal sample rate in Hz */ #define ACCELIOCGSAMPLERATE _ACCELIOC(1) -- cgit v1.2.3 From e2524b9aed144db6fc023e169e716435c9c93052 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 19:13:30 +0100 Subject: drv_gyro: introduce default samplerate --- src/drivers/drv_gyro.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 5e0334a18..1f2bc35c4 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -93,6 +93,8 @@ ORB_DECLARE(sensor_gyro); /** set the gyro internal sample rate to at least (arg) Hz */ #define GYROIOCSSAMPLERATE _GYROIOC(0) +#define GYRO_SAMPLERATE_DEFAULT 1000003 /**< default sample rate */ + /** return the gyro internal sample rate in Hz */ #define GYROIOCGSAMPLERATE _GYROIOC(1) -- cgit v1.2.3 From 90b29efebf846da195af53492ee2ee0c50b6d7fa Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 19:18:46 +0100 Subject: l3gd20: check for default sample rate --- src/drivers/l3gd20/l3gd20.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index f583bced4..1cfffb4fd 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -784,7 +784,7 @@ L3GD20::set_samplerate(unsigned frequency) { uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; - if (frequency == 0) + if (frequency == 0 || frequency == GYRO_SAMPLERATE_DEFAULT) frequency = _is_l3g4200d ? 800 : 760; /* -- cgit v1.2.3 From dedba4307de2b99ee3a9fd2d6c31c031bd4524ed Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 19:19:00 +0100 Subject: lsm303d: check for default sample rate --- src/drivers/lsm303d/lsm303d.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 2a0bf522b..6d017b4d2 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1307,7 +1307,7 @@ LSM303D::accel_set_samplerate(unsigned frequency) uint8_t setbits = 0; uint8_t clearbits = REG1_RATE_BITS_A; - if (frequency == 0) + if (frequency == 0 || frequency == ACCEL_SAMPLERATE_DEFAULT) frequency = 1600; if (frequency <= 100) { -- cgit v1.2.3 From a4961092af2d0e09a3dbe04e8c2f0a362476d21d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 19:19:23 +0100 Subject: mpu6000: check for default sample rate Also check if input variable is 0 and fix indentation --- src/drivers/mpu6000/mpu6000.cpp | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index e322e8b3a..b8c6cedb6 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -402,7 +402,7 @@ private: /* set sample rate (approximate) - 1kHz to 5Hz */ - void _set_sample_rate(uint16_t desired_sample_rate_hz); + void _set_sample_rate(unsigned desired_sample_rate_hz); /* check that key registers still have the right value @@ -794,13 +794,20 @@ MPU6000::probe() set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro */ void -MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz) +MPU6000::_set_sample_rate(unsigned desired_sample_rate_hz) { - uint8_t div = 1000 / desired_sample_rate_hz; - if(div>200) div=200; - if(div<1) div=1; - write_checked_reg(MPUREG_SMPLRT_DIV, div-1); - _sample_rate = 1000 / div; + if (desired_sample_rate_hz == 0 || + desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || + desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) + { + desired_sample_rate_hz = MPU6000_GYRO_DEFAULT_RATE; + } + + uint8_t div = 1000 / desired_sample_rate_hz; + if(div>200) div=200; + if(div<1) div=1; + write_checked_reg(MPUREG_SMPLRT_DIV, div-1); + _sample_rate = 1000 / div; } /* -- cgit v1.2.3 From f53aa5628e12cdc0cd47906c4b6846f8717e3bc0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 19:22:35 +0100 Subject: sensors: init accel and gyro with default rates --- src/modules/sensors/sensors.cpp | 56 ++++++++--------------------------------- 1 file changed, 10 insertions(+), 46 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 77df1b0a6..88dc23500 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -891,8 +891,8 @@ Sensors::parameters_update() return ERROR; } close(flowfd); - } - + } + get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); @@ -942,28 +942,11 @@ Sensors::accel_init() } else { - // XXX do the check more elegantly - -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - - /* set the accel internal sampling rate up to at leat 1000Hz */ - ioctl(fd, ACCELIOCSSAMPLERATE, 1000); - - /* set the driver to poll at 1000Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 1000); - -#elif CONFIG_ARCH_BOARD_PX4FMU_V2 || CONFIG_ARCH_BOARD_AEROCORE + /* set the accel internal sampling rate to default rate */ + ioctl(fd, ACCELIOCSSAMPLERATE, ACCEL_SAMPLERATE_DEFAULT); - /* set the accel internal sampling rate up to at leat 800Hz */ - ioctl(fd, ACCELIOCSSAMPLERATE, 800); - - /* set the driver to poll at 800Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 800); - -#else -#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1, CONFIG_ARCH_BOARD_PX4FMU_V2 or CONFIG_ARCH_BOARD_AEROCORE - -#endif + /* set the driver to poll at default rate */ + ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); close(fd); } @@ -984,31 +967,12 @@ Sensors::gyro_init() } else { - // XXX do the check more elegantly + /* set the gyro internal sampling rate to default rate */ + ioctl(fd, GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT); -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* set the driver to poll at default rate */ + ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - /* set the gyro internal sampling rate up to at least 1000Hz */ - if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK) { - ioctl(fd, GYROIOCSSAMPLERATE, 800); - } - - /* set the driver to poll at 1000Hz */ - if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK) { - ioctl(fd, SENSORIOCSPOLLRATE, 800); - } - -#else - - /* set the gyro internal sampling rate up to at least 760Hz */ - ioctl(fd, GYROIOCSSAMPLERATE, 760); - - /* set the driver to poll at 760Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 760); - -#endif - - close(fd); } return OK; -- cgit v1.2.3 From 2252a9696d78625290b484cc73327730ea0bd379 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Feb 2015 13:02:30 +0100 Subject: fix codestyle in sensors.cpp --- src/modules/sensors/sensors.cpp | 98 +++++++++++++++++++++++++++-------------- 1 file changed, 66 insertions(+), 32 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 88dc23500..1bd3bc58c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -189,7 +189,8 @@ private: /** * Get switch position for specified function. */ - switch_pos_t get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv); + switch_pos_t get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, + bool mid_inv); switch_pos_t get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv); /** @@ -512,7 +513,7 @@ Sensors::Sensors() : _hil_enabled(false), _publishing(true), -/* subscriptions */ + /* subscriptions */ _gyro_sub(-1), _accel_sub(-1), _mag_sub(-1), @@ -530,7 +531,7 @@ Sensors::Sensors() : _rc_parameter_map_sub(-1), _manual_control_sub(-1), -/* publications */ + /* publications */ _sensor_pub(-1), _manual_control_pub(-1), _actuator_group_3_pub(-1), @@ -539,7 +540,7 @@ Sensors::Sensors() : _airspeed_pub(-1), _diff_pres_pub(-1), -/* performance counters */ + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), _mag_is_external(false), @@ -786,9 +787,11 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { param_get(_parameter_handles.rc_map_param[i], &(_parameters.rc_map_param[i])); } + param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr)); param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th)); _parameters.rc_assist_inv = (_parameters.rc_assist_th < 0); @@ -883,15 +886,18 @@ Sensors::parameters_update() /* set px4flow rotation */ int flowfd; flowfd = open(PX4FLOW_DEVICE_PATH, 0); + if (flowfd >= 0) { - int flowret = ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation); - if (flowret) { - warnx("flow rotation could not be set"); + int flowret = ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation); + + if (flowret) { + warnx("flow rotation could not be set"); close(flowfd); return ERROR; - } + } + close(flowfd); - } + } get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); @@ -902,9 +908,9 @@ Sensors::parameters_update() /** fine tune board offset on parameter update **/ math::Matrix<3, 3> board_rotation_offset; - board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0], - M_DEG_TO_RAD_F * _parameters.board_offset[1], - M_DEG_TO_RAD_F * _parameters.board_offset[2]); + board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _parameters.board_offset[0], + M_DEG_TO_RAD_F * _parameters.board_offset[1], + M_DEG_TO_RAD_F * _parameters.board_offset[2]); _board_rotation = _board_rotation * board_rotation_offset; @@ -912,17 +918,20 @@ Sensors::parameters_update() param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh)); int barofd; barofd = open(BARO_DEVICE_PATH, 0); + if (barofd < 0) { warnx("ERROR: no barometer foundon %s", BARO_DEVICE_PATH); return ERROR; } else { int baroret = ioctl(barofd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); + if (baroret) { warnx("qnh could not be set"); close(barofd); return ERROR; } + close(barofd); } @@ -1312,14 +1321,17 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) raw.differential_pressure_timestamp = _diff_pres.timestamp; raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; - float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : + (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); _airspeed.timestamp = _diff_pres.timestamp; /* don't risk to feed negative airspeed into the system */ - _airspeed.indicated_airspeed_m_s = math::max(0.0f, calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa)); - _airspeed.true_airspeed_m_s = math::max(0.0f, calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, - raw.baro_pres_mbar * 1e2f, air_temperature_celsius)); + _airspeed.indicated_airspeed_m_s = math::max(0.0f, + calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa)); + _airspeed.true_airspeed_m_s = math::max(0.0f, + calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, + raw.baro_pres_mbar * 1e2f, air_temperature_celsius)); _airspeed.air_temperature_celsius = air_temperature_celsius; /* announce the airspeed if needed, just publish else */ @@ -1443,8 +1455,10 @@ Sensors::parameter_update_poll(bool forced) } #if 0 - printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.channels[0].scaling_factor * 10000), (int)(_rc.channels[0].mid), (int)_rc.function[0]); - printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.channels[1].scaling_factor * 10000), (int)(_rc.channels[1].mid), (int)_rc.function[1]); + printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], + (int)(_rc.channels[0].scaling_factor * 10000), (int)(_rc.channels[0].mid), (int)_rc.function[0]); + printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], + (int)(_rc.channels[1].scaling_factor * 10000), (int)(_rc.channels[1].mid), (int)_rc.function[1]); printf("MAN: %d %d\n", (int)(_rc.channels[0] * 100), (int)(_rc.channels[1] * 100)); fflush(stdout); usleep(5000); @@ -1460,6 +1474,7 @@ Sensors::rc_parameter_map_poll(bool forced) if (map_updated) { orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map); + /* update paramter handles to which the RC channels are mapped */ for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { if (_rc.function[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { @@ -1472,21 +1487,24 @@ Sensors::rc_parameter_map_poll(bool forced) /* Set the handle by index if the index is set, otherwise use the id */ if (_rc_parameter_map.param_index[i] >= 0) { _parameter_handles.rc_param[i] = param_for_index((unsigned)_rc_parameter_map.param_index[i]); + } else { _parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]); } } + warnx("rc to parameter map updated"); + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { warnx("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f", - i, - _rc_parameter_map.param_id[i], - (double)_rc_parameter_map.scale[i], - (double)_rc_parameter_map.value0[i], - (double)_rc_parameter_map.value_min[i], - (double)_rc_parameter_map.value_max[i] - ); + i, + _rc_parameter_map.param_id[i], + (double)_rc_parameter_map.scale[i], + (double)_rc_parameter_map.value0[i], + (double)_rc_parameter_map.value_min[i], + (double)_rc_parameter_map.value_max[i] + ); } } } @@ -1580,7 +1598,8 @@ Sensors::adc_poll(struct sensor_combined_s &raw) _diff_pres.timestamp = t; _diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw; - _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa_raw * 0.1f); + _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + + (diff_pres_pa_raw * 0.1f); _diff_pres.temperature = -1000.0f; /* announce the airspeed if needed, just publish else */ @@ -1673,6 +1692,7 @@ void Sensors::set_params_from_rc() { static float param_rc_values[RC_PARAM_MAP_NCHAN] = {}; + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { if (_rc.function[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) @@ -1682,13 +1702,14 @@ Sensors::set_params_from_rc() } float rc_val = get_rc_value((enum RC_CHANNELS_FUNCTION)(PARAM_1 + i), -1.0, 1.0); + /* Check if the value has changed, * maybe we need to introduce a more aggressive limit here */ if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) { param_rc_values[i] = rc_val; float param_val = math::constrain( - _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val, - _rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]); + _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val, + _rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]); param_set(_parameter_handles.rc_param[i], ¶m_val); } } @@ -1772,10 +1793,12 @@ Sensors::rc_poll() * DO NOT REMOVE OR ALTER STEP 1! */ if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) { - _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]); + _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)( + _parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]); } else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) { - _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]); + _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)( + _parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]); } else { /* in the configured dead zone, output zero */ @@ -1823,7 +1846,8 @@ Sensors::rc_poll() manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0); /* mode switches */ - manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); + manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, + _parameters.rc_assist_th, _parameters.rc_assist_inv); manual.posctl_switch = get_rc_sw2pos_position(POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); @@ -1863,6 +1887,7 @@ Sensors::rc_poll() /* Update parameters from RC Channels (tuning with RC) if activated */ static hrt_abstime last_rc_to_param_map_time = 0; + if (hrt_elapsed_time(&last_rc_to_param_map_time) > 1e6) { set_params_from_rc(); last_rc_to_param_map_time = hrt_absolute_time(); @@ -1884,22 +1909,31 @@ Sensors::task_main() /* start individual sensors */ int ret; ret = accel_init(); + if (ret) { goto exit_immediate; } + ret = gyro_init(); + if (ret) { goto exit_immediate; } + ret = mag_init(); + if (ret) { goto exit_immediate; } + ret = baro_init(); + if (ret) { goto exit_immediate; } + ret = adc_init(); + if (ret) { goto exit_immediate; } @@ -2039,7 +2073,7 @@ Sensors::start() nullptr); /* wait until the task is up and running or has failed */ - while(_sensors_task > 0 && _task_should_exit) { + while (_sensors_task > 0 && _task_should_exit) { usleep(100); } -- cgit v1.2.3 From a7126cc61d12bb87dd4b5b1709d0b350a22c8d10 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 3 Feb 2015 18:20:21 +0100 Subject: l3gd20: fix if style --- src/drivers/l3gd20/l3gd20.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 1cfffb4fd..547bb6868 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -784,8 +784,9 @@ L3GD20::set_samplerate(unsigned frequency) { uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; - if (frequency == 0 || frequency == GYRO_SAMPLERATE_DEFAULT) + if (frequency == 0 || frequency == GYRO_SAMPLERATE_DEFAULT) { frequency = _is_l3g4200d ? 800 : 760; + } /* * Use limits good for H or non-H models. Rates are slightly different -- cgit v1.2.3 From 8ac05dc8b044d0883da684d7131b23040af4dbe8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 3 Feb 2015 18:20:43 +0100 Subject: lsm303d: fix if style --- src/drivers/lsm303d/lsm303d.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 6d017b4d2..6edb9d787 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1307,8 +1307,9 @@ LSM303D::accel_set_samplerate(unsigned frequency) uint8_t setbits = 0; uint8_t clearbits = REG1_RATE_BITS_A; - if (frequency == 0 || frequency == ACCEL_SAMPLERATE_DEFAULT) + if (frequency == 0 || frequency == ACCEL_SAMPLERATE_DEFAULT) { frequency = 1600; + } if (frequency <= 100) { setbits |= REG1_RATE_100HZ_A; -- cgit v1.2.3 From 3e7faa6018dbff54860304a2e1a35d853160ef64 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 3 Feb 2015 18:21:04 +0100 Subject: mpu6000: fix if style --- src/drivers/mpu6000/mpu6000.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index b8c6cedb6..1c4dfb89e 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -798,8 +798,7 @@ MPU6000::_set_sample_rate(unsigned desired_sample_rate_hz) { if (desired_sample_rate_hz == 0 || desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || - desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) - { + desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) { desired_sample_rate_hz = MPU6000_GYRO_DEFAULT_RATE; } -- cgit v1.2.3 From 37ec1ec8defd67a76e476944123ed43006516dad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Feb 2015 20:27:51 +0100 Subject: Improve submodule checking --- Makefile | 7 +++--- Tools/check_submodules.sh | 59 +++++++++++++++++++++++++++++++++++++++-------- 2 files changed, 53 insertions(+), 13 deletions(-) diff --git a/Makefile b/Makefile index af93504ee..05e0d90a2 100644 --- a/Makefile +++ b/Makefile @@ -1,5 +1,5 @@ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2012-2015 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 @@ -161,7 +161,7 @@ $(foreach config,$(FMU_CONFIGS),$(eval $(call FMU_DEP,$(config)))) # NUTTX_ARCHIVES = $(foreach board,$(BOARDS),$(ARCHIVE_DIR)$(board).export) .PHONY: archives -archives: $(NUTTX_ARCHIVES) +archives: checksubmodules $(NUTTX_ARCHIVES) # We cannot build these parallel; note that we also force -j1 for the # sub-make invocations. @@ -211,8 +211,7 @@ menuconfig: @$(ECHO) "" endif -$(NUTTX_SRC): - $(Q) ($(PX4_BASE)/Tools/check_submodules.sh) +$(NUTTX_SRC): checksubmodules $(UAVCAN_DIR): $(Q) (./Tools/check_submodules.sh) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 3904a2775..5e6e57164 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -12,17 +12,16 @@ if [ -d NuttX/nuttx ]; if [ -z "$STATUSRETVAL" ]; then echo "Checked NuttX submodule, correct version found" else + echo "" + echo "" + echo "New commits required:" + echo "$(git submodule summary)" echo "" echo "" echo " NuttX sub repo not at correct version. Try 'git submodule update'" echo " or follow instructions on http://pixhawk.org/dev/git/submodules" echo "" echo " DO NOT FORGET TO RUN 'make distclean && make archives' AFTER EACH NUTTX UPDATE!" - echo "" - echo "" - echo "New commits required:" - echo "$(git submodule summary)" - echo "" exit 1 fi else @@ -37,15 +36,14 @@ if [ -d mavlink/include/mavlink/v1.0 ]; if [ -z "$STATUSRETVAL" ]; then echo "Checked mavlink submodule, correct version found" else - echo "" - echo "" - echo "mavlink sub repo not at correct version. Try 'git submodule update'" - echo "or follow instructions on http://pixhawk.org/dev/git/submodules" echo "" echo "" echo "New commits required:" echo "$(git submodule summary)" echo "" + echo "" + echo "mavlink sub repo not at correct version. Try 'git submodule update'" + echo "or follow instructions on http://pixhawk.org/dev/git/submodules" exit 1 fi else @@ -61,15 +59,58 @@ then then echo "Checked uavcan submodule, correct version found" else + echo "" + echo "" + echo "New commits required:" + echo "$(git submodule summary)" echo "" echo "" echo "uavcan sub repo not at correct version. Try 'git submodule update'" echo "or follow instructions on http://pixhawk.org/dev/git/submodules" + exit 1 + fi +else + git submodule init; + git submodule update; +fi + +if [ -d Tools/gencpp ] +then + STATUSRETVAL=$(git submodule summary | grep -A20 -i gencpp | grep "<") + if [ -z "$STATUSRETVAL" ] + then + echo "Checked gencpp submodule, correct version found" + else echo "" echo "" echo "New commits required:" echo "$(git submodule summary)" echo "" + echo "" + echo "gencpp sub repo not at correct version. Try 'git submodule update'" + echo "or follow instructions on http://pixhawk.org/dev/git/submodules" + exit 1 + fi +else + git submodule init; + git submodule update; +fi + +if [ -d Tools/genmsg ] +then + STATUSRETVAL=$(git submodule summary | grep -A20 -i genmsg | grep "<") + if [ -z "$STATUSRETVAL" ] + then + echo "Checked genmsg submodule, correct version found" + else + echo "" + echo "" + echo "New commits required:" + echo "$(git submodule summary)" + echo "" + echo "" + echo "genmsg sub repo not at correct version. Try 'git submodule update'" + echo "or follow instructions on http://pixhawk.org/dev/git/submodules" exit 1 fi else -- cgit v1.2.3 From 515266c1632e6b891f34d8047621c2e0e1d72a75 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Feb 2015 20:28:14 +0100 Subject: Update header generation script --- Tools/px_generate_uorb_topic_headers.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py index 4bcab4d54..3e24e66b2 100755 --- a/Tools/px_generate_uorb_topic_headers.py +++ b/Tools/px_generate_uorb_topic_headers.py @@ -1,7 +1,7 @@ #!/usr/bin/env python ############################################################################# # -# Copyright (C) 2013-2014 PX4 Development Team. All rights reserved. +# Copyright (C) 2013-2015 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 -- cgit v1.2.3 From 18cc20b04b735c19607636a99f0656602d3d6fee Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 2 Feb 2015 23:02:42 +0100 Subject: parametrize imu input for ros dummy att estimator --- launch/ardrone.launch | 1 + launch/gazebo_ardrone_empty_world.launch | 1 + src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp | 4 +++- 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/launch/ardrone.launch b/launch/ardrone.launch index d53333b11..3173e7cf1 100644 --- a/launch/ardrone.launch +++ b/launch/ardrone.launch @@ -9,6 +9,7 @@ + diff --git a/launch/gazebo_ardrone_empty_world.launch b/launch/gazebo_ardrone_empty_world.launch index 395e70b00..063d51096 100644 --- a/launch/gazebo_ardrone_empty_world.launch +++ b/launch/gazebo_ardrone_empty_world.launch @@ -1,6 +1,7 @@ + diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp index 1d36e3d99..a3e4bca30 100644 --- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp @@ -48,9 +48,11 @@ AttitudeEstimator::AttitudeEstimator() : _n(), // _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &AttitudeEstimator::ModelStatesCallback, this)), - _sub_imu(_n.subscribe("/px4_multicopter/imu", 1, &AttitudeEstimator::ImuCallback, this)), _vehicle_attitude_pub(_n.advertise("vehicle_attitude", 1)) { + std::string vehicle_model; + _n.param("vehicle_model", vehicle_model, std::string("iris")); + _sub_imu = _n.subscribe("/" + vehicle_model + "/imu", 1, &AttitudeEstimator::ImuCallback, this); } void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg) -- cgit v1.2.3 From 4320afabf1c3b85c067469c5264e879ad1f2a044 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 4 Feb 2015 07:29:55 +0100 Subject: iris launch file set vehicle model param --- launch/gazebo_ardrone_empty_world.launch | 1 - launch/iris.launch | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/gazebo_ardrone_empty_world.launch b/launch/gazebo_ardrone_empty_world.launch index 063d51096..395e70b00 100644 --- a/launch/gazebo_ardrone_empty_world.launch +++ b/launch/gazebo_ardrone_empty_world.launch @@ -1,7 +1,6 @@ - diff --git a/launch/iris.launch b/launch/iris.launch index 7b5b13250..be33cb85f 100644 --- a/launch/iris.launch +++ b/launch/iris.launch @@ -10,6 +10,7 @@ + -- cgit v1.2.3 From 087bf05ae3b6111e113ff580a3ad096c0c024a28 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Feb 2015 12:44:16 +0100 Subject: Made PX4IO update an unit test entry --- ROMFS/px4fmu_test/init.d/rcS | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index 4b9a9b68a..3d1c4a907 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -2,6 +2,8 @@ # # PX4FMU startup script for test hackery. # +set unit_test_failure 0 + uorb start if sercon @@ -41,6 +43,9 @@ fi if px4io start then echo "PX4IO OK" +else + set unit_test_failure 1 + set unit_test_failure_list "${unit_test_failure_list} px4io_start" fi if px4io checkcrc $io_file @@ -51,7 +56,6 @@ else tone_alarm MBABGP if px4io forceupdate 14662 $io_file then - usleep 500000 if px4io start then echo "PX4IO restart OK" @@ -59,12 +63,14 @@ else else echo "PX4IO restart failed" tone_alarm MNGGG - sleep 5 - reboot + set unit_test_failure 1 + set unit_test_failure_list "${unit_test_failure_list} px4io_flash" fi else echo "PX4IO update failed" tone_alarm MNGGG + set unit_test_failure 1 + set unit_test_failure_list "${unit_test_failure_list} px4io_flash" fi fi @@ -81,8 +87,6 @@ fi # Add new unit tests using the same pattern as below. # -set unit_test_failure 0 - if mavlink_tests then else -- cgit v1.2.3 From 7e6198b3dd517e1158431c8344c5912a6c28b363 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 5 Feb 2015 19:44:30 +0100 Subject: include default PYTHONPATH in call to uorb header generation script --- Makefile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index 05e0d90a2..ab8c614e1 100644 --- a/Makefile +++ b/Makefile @@ -238,11 +238,11 @@ GENCPP_PYTHONPATH = $(PX4_BASE)Tools/gencpp/src .PHONY: generateuorbtopicheaders generateuorbtopicheaders: @$(ECHO) "Generating uORB topic headers" - $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH) $(PYTHON) \ + $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH):$(PYTHONPATH) $(PYTHON) \ $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ -d $(MSG_DIR) -o $(TOPICS_DIR) -e $(UORB_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR)) @$(ECHO) "Generating multiplatform uORB topic wrapper headers" - $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH) $(PYTHON) \ + $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH):$(PYTHONPATH) $(PYTHON) \ $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ -d $(MSG_DIR) -o $(MULTIPLATFORM_HEADER_DIR) -e $(MULTIPLATFORM_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR) -p $(MULTIPLATFORM_PREFIX)) # clean up temporary files -- cgit v1.2.3