From 0611677ee29ff00c04e40eab27aafeee06d95aba Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 25 Feb 2017 13:51:38 -0500 Subject: [PATCH] segway move to examples --- cmake/configs/nuttx_px4fmu-v3_default.cmake | 5 ++ .../configs/nuttx_px4fmu-v4pro_default.cmake | 5 ++ cmake/configs/nuttx_px4fmu-v5_default.cmake | 5 ++ cmake/configs/posix_sitl_default.cmake | 5 ++ src/examples/segway/BlockSegwayController.cpp | 66 +++++++++++++++ .../segway/BlockSegwayController.hpp | 9 +- .../segway/CMakeLists.txt | 6 +- src/{modules => examples}/segway/params.c | 1 - .../segway/segway_main.cpp | 0 src/lib/controllib/uorb/blocks.cpp | 13 ++- src/lib/controllib/uorb/blocks.hpp | 37 +++++---- src/modules/segway/BlockSegwayController.cpp | 60 -------------- src/modules/uORB/Publication.cpp | 44 +++++----- src/modules/uORB/Publication.hpp | 8 +- src/modules/uORB/Subscription.cpp | 82 +++++++++---------- 15 files changed, 187 insertions(+), 159 deletions(-) create mode 100644 src/examples/segway/BlockSegwayController.cpp rename src/{modules => examples}/segway/BlockSegwayController.hpp (85%) rename src/{modules => examples}/segway/CMakeLists.txt (94%) rename src/{modules => examples}/segway/params.c (99%) rename src/{modules => examples}/segway/segway_main.cpp (100%) delete mode 100644 src/modules/segway/BlockSegwayController.cpp diff --git a/cmake/configs/nuttx_px4fmu-v3_default.cmake b/cmake/configs/nuttx_px4fmu-v3_default.cmake index e9d10fcf26..f99634db34 100644 --- a/cmake/configs/nuttx_px4fmu-v3_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v3_default.cmake @@ -170,6 +170,11 @@ set(config_module_list # examples/rover_steering_control + # + # Segway + # + examples/segway + # # Demo apps # diff --git a/cmake/configs/nuttx_px4fmu-v4pro_default.cmake b/cmake/configs/nuttx_px4fmu-v4pro_default.cmake index 417ee1b7c3..2505c60963 100644 --- a/cmake/configs/nuttx_px4fmu-v4pro_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4pro_default.cmake @@ -171,6 +171,11 @@ set(config_module_list # examples/rover_steering_control + # + # Segway + # + #examples/segway + # # Demo apps # diff --git a/cmake/configs/nuttx_px4fmu-v5_default.cmake b/cmake/configs/nuttx_px4fmu-v5_default.cmake index cb97e9be21..4ce926b174 100644 --- a/cmake/configs/nuttx_px4fmu-v5_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v5_default.cmake @@ -169,6 +169,11 @@ set(config_module_list # #examples/rover_steering_control + # + # Segway + # + #examples/segway + # # Demo apps # diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index 095ca1fab1..52d947df68 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -138,6 +138,11 @@ set(config_module_list # examples/rover_steering_control + # + # Segway + # + examples/segway + # # Demo apps # diff --git a/src/examples/segway/BlockSegwayController.cpp b/src/examples/segway/BlockSegwayController.cpp new file mode 100644 index 0000000000..54aca0c793 --- /dev/null +++ b/src/examples/segway/BlockSegwayController.cpp @@ -0,0 +1,66 @@ +#include "BlockSegwayController.hpp" + +using matrix::Eulerf; +using matrix::Quatf; + +void BlockSegwayController::update() +{ + // wait for a sensor update, check for exit condition every 100 ms + if (px4_poll(&_attPoll, 1, 100) < 0) { return; } // poll error + + uint64_t newTimeStamp = hrt_absolute_time(); + float dt = (newTimeStamp - _timeStamp) / 1.0e6f; + _timeStamp = newTimeStamp; + + // check for sane values of dt + // to prevent large control responses + if (dt > 1.0f || dt < 0) { return; } + + // set dt for all child blocks + setDt(dt); + + // check for new updates + if (_param_update.updated()) { updateParams(); } + + // get new information from subscriptions + updateSubscriptions(); + + actuator_controls_s &actuators = _actuators.get(); + + // default all output to zero unless handled by mode + for (unsigned i = 2; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) { + actuators.control[i] = 0.0f; + } + + const uint8_t &nav_state = _status.get().nav_state; + + // only update guidance in auto mode + if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) { + // update guidance + } + + Eulerf euler = Eulerf(Quatf(_att.get().q)); + + // compute speed command + float spdCmd = -th2v.update(euler.theta()) - q2v.update(_att.get().pitchspeed); + + // handle autopilot modes + if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION || + nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL || + nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL) { + + actuators.control[0] = spdCmd; + actuators.control[1] = spdCmd; + + } else if (nav_state == vehicle_status_s::NAVIGATION_STATE_STAB) { + actuators.control[0] = spdCmd; + actuators.control[1] = spdCmd; + + } else if (nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL) { + actuators.control[CH_LEFT] = _manual.get().z; + actuators.control[CH_RIGHT] = -_manual.get().x; + } + + // update all publications + updatePublications(); +} diff --git a/src/modules/segway/BlockSegwayController.hpp b/src/examples/segway/BlockSegwayController.hpp similarity index 85% rename from src/modules/segway/BlockSegwayController.hpp rename to src/examples/segway/BlockSegwayController.hpp index 450b4e3ff9..33da80f85b 100644 --- a/src/modules/segway/BlockSegwayController.hpp +++ b/src/examples/segway/BlockSegwayController.hpp @@ -1,9 +1,11 @@ #pragma once #include +#include #include -using namespace control; +using control::BlockPI; +using control::BlockP; class BlockSegwayController : public control::BlockUorbEnabledAutopilot { @@ -18,12 +20,15 @@ public: _attPoll.fd = _att.getHandle(); _attPoll.events = POLLIN; } + void update(); + private: enum {CH_LEFT, CH_RIGHT}; + BlockPI th2v; BlockP q2v; + px4_pollfd_struct_t _attPoll; uint64_t _timeStamp; }; - diff --git a/src/modules/segway/CMakeLists.txt b/src/examples/segway/CMakeLists.txt similarity index 94% rename from src/modules/segway/CMakeLists.txt rename to src/examples/segway/CMakeLists.txt index 6e9dd93bb2..29d6779e7c 100644 --- a/src/modules/segway/CMakeLists.txt +++ b/src/examples/segway/CMakeLists.txt @@ -31,8 +31,10 @@ # ############################################################################ px4_add_module( - MODULE modules__segway + MODULE examples__segway MAIN segway + STACK_MAIN 1300 + STACK_MAX 1300 SRCS segway_main.cpp BlockSegwayController.cpp @@ -40,4 +42,4 @@ px4_add_module( DEPENDS platforms__common ) -# vim: set noet ft=cmake fenc=utf-8 ff=unix : +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/segway/params.c b/src/examples/segway/params.c similarity index 99% rename from src/modules/segway/params.c rename to src/examples/segway/params.c index d729237174..626ef6e99d 100644 --- a/src/modules/segway/params.c +++ b/src/examples/segway/params.c @@ -5,4 +5,3 @@ PARAM_DEFINE_FLOAT(SEG_TH2V_P, 10.0f); // pitch to voltage PARAM_DEFINE_FLOAT(SEG_TH2V_I, 0.0f); // pitch integral to voltage PARAM_DEFINE_FLOAT(SEG_TH2V_I_MAX, 0.0f); // integral limiter PARAM_DEFINE_FLOAT(SEG_Q2V, 1.0f); // pitch rate to voltage - diff --git a/src/modules/segway/segway_main.cpp b/src/examples/segway/segway_main.cpp similarity index 100% rename from src/modules/segway/segway_main.cpp rename to src/examples/segway/segway_main.cpp diff --git a/src/lib/controllib/uorb/blocks.cpp b/src/lib/controllib/uorb/blocks.cpp index 3a99617e4d..a2611649b5 100644 --- a/src/lib/controllib/uorb/blocks.cpp +++ b/src/lib/controllib/uorb/blocks.cpp @@ -84,20 +84,19 @@ void BlockWaypointGuidance::update( BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) : SuperBlock(parent, name), // subscriptions + _manual(ORB_ID(manual_control_setpoint), 20, 0, &getSubscriptions()), + _param_update(ORB_ID(parameter_update), 1000, 0, &getSubscriptions()), // limit to 1 Hz + _missionCmd(ORB_ID(position_setpoint_triplet), 20, 0, &getSubscriptions()), _att(ORB_ID(vehicle_attitude), 20, 0, &getSubscriptions()), _attCmd(ORB_ID(vehicle_attitude_setpoint), 20, 0, &getSubscriptions()), - _ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, 0, &getSubscriptions()), _pos(ORB_ID(vehicle_global_position), 20, 0, &getSubscriptions()), - _missionCmd(ORB_ID(position_setpoint_triplet), 20, 0, &getSubscriptions()), - _manual(ORB_ID(manual_control_setpoint), 20, 0, &getSubscriptions()), + _ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, 0, &getSubscriptions()), _status(ORB_ID(vehicle_status), 20, 0, &getSubscriptions()), - _param_update(ORB_ID(parameter_update), 1000, 0, &getSubscriptions()), // limit to 1 Hz + // publications _actuators(ORB_ID(actuator_controls_0), -1, &getPublications()) { } -BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {}; - -} // namespace control +} diff --git a/src/lib/controllib/uorb/blocks.hpp b/src/lib/controllib/uorb/blocks.hpp index 42aae92284..ebf2edd4e1 100644 --- a/src/lib/controllib/uorb/blocks.hpp +++ b/src/lib/controllib/uorb/blocks.hpp @@ -39,27 +39,27 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include - +#include +#include #include #include #include -#include +#include #include -#include -#include "../blocks.hpp" #include +#include +#include +#include +#include +#include +#include +#include +#include + #include +#include namespace control { @@ -90,19 +90,20 @@ class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock { protected: // subscriptions + uORB::Subscription _manual; + uORB::Subscription _param_update; + uORB::Subscription _missionCmd; uORB::Subscription _att; uORB::Subscription _attCmd; - uORB::Subscription _ratesCmd; uORB::Subscription _pos; - uORB::Subscription _missionCmd; - uORB::Subscription _manual; + uORB::Subscription _ratesCmd; uORB::Subscription _status; - uORB::Subscription _param_update; + // publications uORB::Publication _actuators; public: BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name); - virtual ~BlockUorbEnabledAutopilot(); + virtual ~BlockUorbEnabledAutopilot() = default; }; } // namespace control diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp deleted file mode 100644 index 4750595d33..0000000000 --- a/src/modules/segway/BlockSegwayController.cpp +++ /dev/null @@ -1,60 +0,0 @@ -#include "BlockSegwayController.hpp" - -void BlockSegwayController::update() -{ - // wait for a sensor update, check for exit condition every 100 ms - if (px4_poll(&_attPoll, 1, 100) < 0) { return; } // poll error - - uint64_t newTimeStamp = hrt_absolute_time(); - float dt = (newTimeStamp - _timeStamp) / 1.0e6f; - _timeStamp = newTimeStamp; - - // check for sane values of dt - // to prevent large control responses - if (dt > 1.0f || dt < 0) { return; } - - // set dt for all child blocks - setDt(dt); - - // check for new updates - if (_param_update.updated()) { updateParams(); } - - // get new information from subscriptions - updateSubscriptions(); - - // default all output to zero unless handled by mode - for (unsigned i = 2; i < NUM_ACTUATOR_CONTROLS; i++) { - _actuators.control[i] = 0.0f; - } - - // only update guidance in auto mode - if (_status.main_state == MAIN_STATE_AUTO) { - // update guidance - } - - // compute speed command - float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed); - - // handle autopilot modes - if (_status.main_state == MAIN_STATE_AUTO || - _status.main_state == MAIN_STATE_ALTCTL || - _status.main_state == MAIN_STATE_POSCTL) { - _actuators.control[0] = spdCmd; - _actuators.control[1] = spdCmd; - - } else if (_status.main_state == MAIN_STATE_MANUAL) { - if (_status.navigation_state == NAVIGATION_STATE_DIRECT) { - _actuators.control[CH_LEFT] = _manual.throttle; - _actuators.control[CH_RIGHT] = _manual.pitch; - - } else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) { - _actuators.control[0] = spdCmd; - _actuators.control[1] = spdCmd; - } - } - - // update all publications - updatePublications(); - -} - diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index fdfb22300c..d64256cde5 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -37,20 +37,20 @@ */ #include "Publication.hpp" -#include "topics/vehicle_attitude.h" -#include "topics/vehicle_local_position.h" -#include "topics/vehicle_global_position.h" -#include "topics/debug_key_value.h" #include "topics/actuator_controls.h" -#include "topics/vehicle_global_velocity_setpoint.h" -#include "topics/vehicle_attitude_setpoint.h" -#include "topics/vehicle_rates_setpoint.h" -#include "topics/actuator_outputs.h" #include "topics/actuator_direct.h" -#include "topics/tecs_status.h" -#include "topics/rc_channels.h" -#include "topics/filtered_bottom_flow.h" +#include "topics/actuator_outputs.h" +#include "topics/debug_key_value.h" #include "topics/ekf2_innovations.h" +#include "topics/filtered_bottom_flow.h" +#include "topics/rc_channels.h" +#include "topics/tecs_status.h" +#include "topics/vehicle_attitude.h" +#include "topics/vehicle_attitude_setpoint.h" +#include "topics/vehicle_global_position.h" +#include "topics/vehicle_global_velocity_setpoint.h" +#include "topics/vehicle_local_position.h" +#include "topics/vehicle_rates_setpoint.h" #include @@ -108,19 +108,19 @@ PublicationNode::PublicationNode(const struct orb_metadata *meta, } // explicit template instantiation -template class __EXPORT Publication; -template class __EXPORT Publication; -template class __EXPORT Publication; -template class __EXPORT Publication; template class __EXPORT Publication; -template class __EXPORT Publication; -template class __EXPORT Publication; -template class __EXPORT Publication; -template class __EXPORT Publication; template class __EXPORT Publication; -template class __EXPORT Publication; -template class __EXPORT Publication; -template class __EXPORT Publication; +template class __EXPORT Publication; +template class __EXPORT Publication; template class __EXPORT Publication; +template class __EXPORT Publication; +template class __EXPORT Publication; +template class __EXPORT Publication; +template class __EXPORT Publication; +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/Publication.hpp b/src/modules/uORB/Publication.hpp index 37658721ca..c04393d85a 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -117,9 +117,7 @@ public: * @param list A list interface for adding to * list during construction */ - PublicationNode(const struct orb_metadata *meta, - int priority = -1, - List *list = nullptr); + PublicationNode(const struct orb_metadata *meta, int priority = -1, List *list = nullptr); /** * This function is the callback for list traversal @@ -145,9 +143,7 @@ public: * @param list A list interface for adding to * list during construction */ - Publication(const struct orb_metadata *meta, - int priority = -1, - List *list = nullptr) : + Publication(const struct orb_metadata *meta, int priority = -1, List *list = nullptr) : PublicationNode(meta, priority, list), _data() { diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 98a7feeb16..2407739e9f 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -37,33 +37,33 @@ */ #include "Subscription.hpp" -#include "topics/parameter_update.h" +#include "topics/actuator_armed.h" #include "topics/actuator_controls.h" -#include "topics/vehicle_gps_position.h" -#include "topics/satellite_info.h" -#include "topics/sensor_combined.h" +#include "topics/att_pos_mocap.h" +#include "topics/battery_status.h" +#include "topics/control_state.h" +#include "topics/distance_sensor.h" #include "topics/hil_sensor.h" -#include "topics/vehicle_attitude.h" -#include "topics/vehicle_global_position.h" -#include "topics/position_setpoint_triplet.h" -#include "topics/vehicle_status.h" +#include "topics/home_position.h" +#include "topics/log_message.h" #include "topics/manual_control_setpoint.h" #include "topics/mavlink_log.h" -#include "topics/log_message.h" -#include "topics/vehicle_local_position_setpoint.h" -#include "topics/vehicle_local_position.h" -#include "topics/vehicle_attitude_setpoint.h" -#include "topics/vehicle_rates_setpoint.h" -#include "topics/rc_channels.h" -#include "topics/battery_status.h" #include "topics/optical_flow.h" -#include "topics/distance_sensor.h" -#include "topics/home_position.h" +#include "topics/parameter_update.h" +#include "topics/position_setpoint_triplet.h" +#include "topics/rc_channels.h" +#include "topics/satellite_info.h" +#include "topics/sensor_combined.h" +#include "topics/vehicle_attitude.h" +#include "topics/vehicle_attitude_setpoint.h" #include "topics/vehicle_control_mode.h" -#include "topics/actuator_armed.h" -#include "topics/att_pos_mocap.h" -#include "topics/control_state.h" +#include "topics/vehicle_global_position.h" +#include "topics/vehicle_gps_position.h" #include "topics/vehicle_land_detected.h" +#include "topics/vehicle_local_position.h" +#include "topics/vehicle_local_position_setpoint.h" +#include "topics/vehicle_rates_setpoint.h" +#include "topics/vehicle_status.h" #include @@ -154,32 +154,32 @@ bool Subscription::check_updated() template const T &Subscription::get() { return _data; } -template class __EXPORT Subscription; +template class __EXPORT Subscription; template class __EXPORT Subscription; -template class __EXPORT Subscription; -template class __EXPORT Subscription; -template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; template class __EXPORT Subscription; -template class __EXPORT Subscription; -template class __EXPORT Subscription; -template class __EXPORT Subscription; -template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; -template class __EXPORT Subscription; -template class __EXPORT Subscription; -template class __EXPORT Subscription; -template class __EXPORT Subscription; -template class __EXPORT Subscription; -template class __EXPORT Subscription; -template class __EXPORT Subscription; -template class __EXPORT Subscription; -template class __EXPORT Subscription; -template class __EXPORT Subscription; template class __EXPORT Subscription; -template class __EXPORT Subscription; -template class __EXPORT Subscription; -template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +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