diff options
30 files changed, 603 insertions, 323 deletions
diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index 2af3618d9..7ff381d97 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -7,4 +7,29 @@ sh /etc/init.d/rc.fw_defaults +if [ $DO_AUTOCONFIG == yes ] +then + param set BAT_N_CELLS 2 + param set FW_AIRSPD_MAX 15 + param set FW_AIRSPD_MIN 7 + param set FW_AIRSPD_TRIM 11 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.74 + param set FW_L1_PERIOD 12 + param set FW_LND_ANG 15 + param set FW_LND_FLALT 5 + param set FW_LND_HHDIST 15 + param set FW_LND_HVIRT 13 + param set FW_LND_TLALT 5 + param set FW_THR_LND_MAX 0 + param set FW_P_ROLLFF 2 + param set FW_PR_FF 0.6 + param set FW_PR_IMAX 0.2 + param set FW_PR_P 0.06 + param set FW_RR_FF 0.6 + param set FW_RR_IMA 0.2 + param set FW_RR_P 0.09 + param set FW_THR_CRUISE 0.65 +fi + set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 3a50fcf56..4cd73e23f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -11,4 +11,6 @@ then param set NAV_RTL_ALT 100 param set NAV_RTL_LAND_T -1 param set NAV_ACCEPT_RAD 50 + param set RC_SCALE_ROLL 1 + param set RC_SCALE_PITCH 1 fi
\ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 558be4275..2aeea5cbe 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -6,6 +6,10 @@ echo "Starting MAVLink on this USB console" mavlink start -r 10000 -d /dev/ttyACM0 +# Enable a number of interesting streams we want via USB +mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 +mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10 +mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30 # Exit shell to make it available to MAVLink exit diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index d43e3aef9..5d1f58b85 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -52,7 +52,7 @@ #include <arch/board/board.h> #include <mavlink/mavlink_log.h> -#include <controllib/uorb/UOrbPublication.hpp> +#include <uORB/Publication.hpp> #include <uORB/topics/debug_key_value.h> #include <drivers/drv_hrt.h> @@ -587,7 +587,7 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitu float prev_revolution = md25.getRevolutions1(); // debug publication - control::UOrbPublication<debug_key_value_s> debug_msg(NULL, + uORB::Publication<debug_key_value_s> debug_msg(NULL, ORB_ID(debug_key_value)); // sine wave for motor 1 diff --git a/src/drivers/md25/md25.hpp b/src/drivers/md25/md25.hpp index 1661f67f9..962c6b881 100644 --- a/src/drivers/md25/md25.hpp +++ b/src/drivers/md25/md25.hpp @@ -46,7 +46,7 @@ #include <poll.h> #include <stdio.h> -#include <controllib/uorb/UOrbSubscription.hpp> +#include <uORB/Subscription.hpp> #include <uORB/topics/actuator_controls.h> #include <drivers/device/i2c.h> @@ -270,7 +270,7 @@ private: struct pollfd _controlPoll; /** actuator controls subscription */ - control::UOrbSubscription<actuator_controls_s> _actuators; + uORB::Subscription<actuator_controls_s> _actuators; // local copy of data from i2c device uint8_t _version; diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index d65a9be36..dd5e4d3e0 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -53,7 +53,7 @@ #include <arch/board/board.h> #include <mavlink/mavlink_log.h> -#include <controllib/uorb/UOrbPublication.hpp> +#include <uORB/Publication.hpp> #include <uORB/topics/debug_key_value.h> #include <drivers/drv_hrt.h> diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index e9f35cf95..58994d6fa 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -45,7 +45,7 @@ #include <poll.h> #include <stdio.h> -#include <controllib/uorb/UOrbSubscription.hpp> +#include <uORB/Subscription.hpp> #include <uORB/topics/actuator_controls.h> #include <drivers/device/i2c.h> @@ -169,7 +169,7 @@ private: struct pollfd _controlPoll; /** actuator controls subscription */ - control::UOrbSubscription<actuator_controls_s> _actuators; + uORB::Subscription<actuator_controls_s> _actuators; // private data float _motor1Position; diff --git a/src/modules/controllib/block/List.hpp b/src/include/containers/List.hpp index 96b0b94d1..13cbda938 100644 --- a/src/modules/controllib/block/List.hpp +++ b/src/include/containers/List.hpp @@ -32,9 +32,9 @@ ****************************************************************************/ /** - * @file Node.h + * @file List.hpp * - * A node of a linked list. + * A linked list. */ #pragma once @@ -43,7 +43,7 @@ template<class T> class __EXPORT ListNode { public: - ListNode() : _sibling(NULL) { + ListNode() : _sibling(nullptr) { } void setSibling(T sibling) { _sibling = sibling; } T getSibling() { return _sibling; } diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index 46ee4b6c8..caf93bc78 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -47,8 +47,8 @@ #include <mathlib/mathlib.h> #include <controllib/blocks.hpp> #include <controllib/block/BlockParam.hpp> -#include <controllib/uorb/UOrbSubscription.hpp> -#include <controllib/uorb/UOrbPublication.hpp> +#include <uORB/Subscription.hpp> +#include <uORB/Publication.hpp> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_global_position.h> @@ -138,13 +138,13 @@ protected: math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */ math::Quaternion q; /**< quaternion from body to nav frame */ // subscriptions - control::UOrbSubscription<sensor_combined_s> _sensors; /**< sensors sub. */ - control::UOrbSubscription<vehicle_gps_position_s> _gps; /**< gps sub. */ - control::UOrbSubscription<parameter_update_s> _param_update; /**< parameter update sub. */ + uORB::Subscription<sensor_combined_s> _sensors; /**< sensors sub. */ + uORB::Subscription<vehicle_gps_position_s> _gps; /**< gps sub. */ + uORB::Subscription<parameter_update_s> _param_update; /**< parameter update sub. */ // publications - control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */ - control::UOrbPublication<vehicle_local_position_s> _localPos; /**< local position pub. */ - control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */ + uORB::Publication<vehicle_global_position_s> _pos; /**< position pub. */ + uORB::Publication<vehicle_local_position_s> _localPos; /**< local position pub. */ + uORB::Publication<vehicle_attitude_s> _att; /**< attitude pub. */ // time stamps uint64_t _pubTimeStamp; /**< output data publication time stamp */ uint64_t _predictTimeStamp; /**< prediction time stamp */ diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp index b964d40a3..6768bfa7e 100644 --- a/src/modules/controllib/block/Block.cpp +++ b/src/modules/controllib/block/Block.cpp @@ -41,10 +41,11 @@ #include <string.h> #include <stdio.h> +#include <uORB/Subscription.hpp> +#include <uORB/Publication.hpp> + #include "Block.hpp" #include "BlockParam.hpp" -#include "../uorb/UOrbSubscription.hpp" -#include "../uorb/UOrbPublication.hpp" namespace control { @@ -100,7 +101,7 @@ void Block::updateParams() void Block::updateSubscriptions() { - UOrbSubscriptionBase *sub = getSubscriptions().getHead(); + uORB::SubscriptionBase *sub = getSubscriptions().getHead(); int count = 0; while (sub != NULL) { @@ -118,7 +119,7 @@ void Block::updateSubscriptions() void Block::updatePublications() { - UOrbPublicationBase *pub = getPublications().getHead(); + uORB::PublicationBase *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 258701f27..736698e21 100644 --- a/src/modules/controllib/block/Block.hpp +++ b/src/modules/controllib/block/Block.hpp @@ -42,7 +42,13 @@ #include <stdint.h> #include <inttypes.h> -#include "List.hpp" +#include <containers/List.hpp> + +// forward declaration +namespace uORB { + class SubscriptionBase; + class PublicationBase; +} namespace control { @@ -55,8 +61,6 @@ static const uint8_t blockNameLengthMax = 80; // forward declaration class BlockParamBase; -class UOrbSubscriptionBase; -class UOrbPublicationBase; class SuperBlock; /** @@ -79,15 +83,15 @@ public: protected: // accessors SuperBlock *getParent() { return _parent; } - List<UOrbSubscriptionBase *> & getSubscriptions() { return _subscriptions; } - List<UOrbPublicationBase *> & getPublications() { return _publications; } + List<uORB::SubscriptionBase *> & getSubscriptions() { return _subscriptions; } + List<uORB::PublicationBase *> & getPublications() { return _publications; } List<BlockParamBase *> & getParams() { return _params; } // attributes const char *_name; SuperBlock *_parent; float _dt; - List<UOrbSubscriptionBase *> _subscriptions; - List<UOrbPublicationBase *> _publications; + List<uORB::SubscriptionBase *> _subscriptions; + List<uORB::PublicationBase *> _publications; List<BlockParamBase *> _params; }; diff --git a/src/modules/controllib/block/BlockParam.cpp b/src/modules/controllib/block/BlockParam.cpp index fd12e365d..8f98da74f 100644 --- a/src/modules/controllib/block/BlockParam.cpp +++ b/src/modules/controllib/block/BlockParam.cpp @@ -76,4 +76,29 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref printf("error finding param: %s\n", fullname); }; +template <class T> +BlockParam<T>::BlockParam(Block *block, const char *name, + bool parent_prefix) : + BlockParamBase(block, name, parent_prefix), + _val() { + update(); +} + +template <class T> +T BlockParam<T>::get() { return _val; } + +template <class T> +void BlockParam<T>::set(T val) { _val = val; } + +template <class T> +void BlockParam<T>::update() { + if (_handle != PARAM_INVALID) param_get(_handle, &_val); +} + +template <class T> +BlockParam<T>::~BlockParam() {}; + +template class __EXPORT BlockParam<float>; +template class __EXPORT BlockParam<int>; + } // namespace control diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index 36bc8c24b..a64d0139e 100644 --- a/src/modules/controllib/block/BlockParam.hpp +++ b/src/modules/controllib/block/BlockParam.hpp @@ -42,7 +42,7 @@ #include <systemlib/param/param.h> #include "Block.hpp" -#include "List.hpp" +#include <containers/List.hpp> namespace control { @@ -70,38 +70,21 @@ protected: * Parameters that are tied to blocks for updating and nameing. */ -class __EXPORT BlockParamFloat : public BlockParamBase +template <class T> +class BlockParam : public BlockParamBase { public: - BlockParamFloat(Block *block, const char *name, bool parent_prefix=true) : - BlockParamBase(block, name, parent_prefix), - _val() { - update(); - } - float get() { return _val; } - void set(float val) { _val = val; } - void update() { - if (_handle != PARAM_INVALID) param_get(_handle, &_val); - } + BlockParam(Block *block, const char *name, + bool parent_prefix=true); + T get(); + void set(T val); + void update(); + virtual ~BlockParam(); protected: - float _val; + T _val; }; -class __EXPORT BlockParamInt : public BlockParamBase -{ -public: - BlockParamInt(Block *block, const char *name, bool parent_prefix=true) : - BlockParamBase(block, name, parent_prefix), - _val() { - update(); - } - int get() { return _val; } - void set(int val) { _val = val; } - void update() { - if (_handle != PARAM_INVALID) param_get(_handle, &_val); - } -protected: - int _val; -}; +typedef BlockParam<float> BlockParamFloat; +typedef BlockParam<int> BlockParamInt; } // namespace control diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk index d815a8feb..f0139a4b7 100644 --- a/src/modules/controllib/module.mk +++ b/src/modules/controllib/module.mk @@ -37,7 +37,5 @@ SRCS = test_params.c \ block/Block.cpp \ block/BlockParam.cpp \ - uorb/UOrbPublication.cpp \ - uorb/UOrbSubscription.cpp \ uorb/blocks.cpp \ blocks.cpp diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 7c80c4b2b..a8a70507e 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -62,8 +62,8 @@ extern "C" { } #include "../blocks.hpp" -#include "UOrbSubscription.hpp" -#include "UOrbPublication.hpp" +#include <uORB/Subscription.hpp> +#include <uORB/Publication.hpp> namespace control { @@ -94,16 +94,16 @@ class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock { protected: // subscriptions - UOrbSubscription<vehicle_attitude_s> _att; - UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd; - UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd; - UOrbSubscription<vehicle_global_position_s> _pos; - UOrbSubscription<position_setpoint_triplet_s> _missionCmd; - UOrbSubscription<manual_control_setpoint_s> _manual; - UOrbSubscription<vehicle_status_s> _status; - UOrbSubscription<parameter_update_s> _param_update; + uORB::Subscription<vehicle_attitude_s> _att; + uORB::Subscription<vehicle_attitude_setpoint_s> _attCmd; + uORB::Subscription<vehicle_rates_setpoint_s> _ratesCmd; + uORB::Subscription<vehicle_global_position_s> _pos; + uORB::Subscription<position_setpoint_triplet_s> _missionCmd; + uORB::Subscription<manual_control_setpoint_s> _manual; + uORB::Subscription<vehicle_status_s> _status; + uORB::Subscription<parameter_update_s> _param_update; // publications - UOrbPublication<actuator_controls_s> _actuators; + uORB::Publication<actuator_controls_s> _actuators; public: BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name); virtual ~BlockUorbEnabledAutopilot(); diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index f7c0b6148..cfae07275 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -175,14 +175,14 @@ void BlockMultiModeBacksideAutopilot::update() // the min/max velocity float v = _vLimit.update(sqrtf( _pos.vel_n * _pos.vel_n + - _pos.vy * _pos.vy + + _pos.vel_e * _pos.vel_e + _pos.vel_d * _pos.vel_d)); // limit velocity command between min/max velocity float vCmd = _vLimit.update(_vCmd.get()); // altitude hold - float dThrottle = _h2Thr.update(_missionCmd.current.altitude - _pos.alt); + float dThrottle = _h2Thr.update(_missionCmd.current.alt - _pos.alt); // heading hold float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw); @@ -237,7 +237,7 @@ void BlockMultiModeBacksideAutopilot::update() // the min/max velocity float v = _vLimit.update(sqrtf( _pos.vel_n * _pos.vel_n + - _pos.vy * _pos.vy + + _pos.vel_e * _pos.vel_e + _pos.vel_d * _pos.vel_d)); // pitch channel -> rate of climb 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 5ade835ff..f139c25f4 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -108,14 +108,14 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ - int _att_sub; /**< vehicle attitude subscription */ - int _accel_sub; /**< accelerometer subscription */ + 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 _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ int _global_pos_sub; /**< global position subscription */ orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ @@ -123,20 +123,19 @@ private: 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_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 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 actuator_controls_s _actuators_airframe; /**< actuator control inputs */ + struct vehicle_global_position_s _global_pos; /**< global position */ perf_counter_t _loop_perf; /**< loop performance counter */ bool _setpoint_valid; /**< flag if the position control setpoint is valid */ - bool _airspeed_valid; /**< flag if the airspeed measurement is valid */ struct { float tconst; @@ -245,7 +244,7 @@ private: /** * Check for airspeed updates. */ - bool vehicle_airspeed_poll(); + void vehicle_airspeed_poll(); /** * Check for accel updates. @@ -308,19 +307,18 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), /* states */ - _setpoint_valid(false), - _airspeed_valid(false) + _setpoint_valid(false) { /* safely initialize structs */ - _att = {0}; - _accel = {0}; - _att_sp = {0}; - _manual = {0}; - _airspeed = {0}; - _vcontrol_mode = {0}; - _actuators = {0}; - _actuators_airframe = {0}; - _global_pos = {0}; + _att = {}; + _accel = {}; + _att_sp = {}; + _manual = {}; + _airspeed = {}; + _vcontrol_mode = {}; + _actuators = {}; + _actuators_airframe = {}; + _global_pos = {}; _parameter_handles.tconst = param_find("FW_ATT_TC"); @@ -482,7 +480,7 @@ FixedwingAttitudeControl::vehicle_manual_poll() } } -bool +void FixedwingAttitudeControl::vehicle_airspeed_poll() { /* check if there is a new position */ @@ -492,10 +490,7 @@ FixedwingAttitudeControl::vehicle_airspeed_poll() if (airspeed_updated) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); // warnx("airspeed poll: ind: %.4f, true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s); - return true; } - - return false; } void @@ -569,7 +564,7 @@ FixedwingAttitudeControl::task_main() parameters_update(); /* get an initial update for all sensor and status data */ - (void)vehicle_airspeed_poll(); + vehicle_airspeed_poll(); vehicle_setpoint_poll(); vehicle_accel_poll(); vehicle_control_mode_poll(); @@ -626,7 +621,7 @@ FixedwingAttitudeControl::task_main() /* load local copies */ orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); - _airspeed_valid = vehicle_airspeed_poll(); + vehicle_airspeed_poll(); vehicle_setpoint_poll(); @@ -666,9 +661,9 @@ FixedwingAttitudeControl::task_main() float airspeed; /* if airspeed is smaller than min, the sensor is not giving good readings */ - if (!_airspeed_valid || - (_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || - !isfinite(_airspeed.indicated_airspeed_m_s)) { + if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || + !isfinite(_airspeed.indicated_airspeed_m_s) || + hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; } else { @@ -791,10 +786,6 @@ FixedwingAttitudeControl::task_main() warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp); } - // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", - // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], - // _actuators.control[2], _actuators.control[3]); - /* * Lazily publish the rate setpoint (for analysis, the actuators are published below) * only once available diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 542bf0d7e..ad435b251 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -45,31 +45,6 @@ #include <string.h> #include "mavlink_bridge_header.h" #include <systemlib/param/param.h> -// #include <drivers/drv_hrt.h> -// #include <time.h> -// #include <float.h> -// #include <unistd.h> -// #include <nuttx/sched.h> -// #include <sys/prctl.h> -// #include <termios.h> -// #include <errno.h> -// #include <stdlib.h> -// #include <poll.h> - -// -// #include <systemlib/systemlib.h> -// #include <systemlib/err.h> -// #include <mavlink/mavlink_log.h> -// #include <commander/px4_custom_mode.h> - -// #include "waypoints.h" -// #include "orb_topics.h" -// #include "mavlink_hil.h" -// #include "util.h" -// #include "waypoints.h" -// #include "mavlink_parameters.h" - -// #include <uORB/topics/mission_result.h> /* define MAVLink specific parameters */ /** diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9c2e0178a..18df577fe 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -151,8 +151,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length } /* no valid instance, bail */ - if (!instance) + if (!instance) { return; + } int uart = instance->get_uart_fd(); @@ -165,12 +166,12 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length int buf_free = 0; if (instance->get_flow_control_enabled() - && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { + && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { if (buf_free == 0) { if (last_write_times[(unsigned)channel] != 0 && - hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) { + hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) { warnx("DISABLING HARDWARE FLOW CONTROL"); instance->enable_flow_control(false); @@ -225,36 +226,42 @@ Mavlink::Mavlink() : case 0: _channel = MAVLINK_COMM_0; break; + case 1: _channel = MAVLINK_COMM_1; break; + case 2: _channel = MAVLINK_COMM_2; break; + case 3: _channel = MAVLINK_COMM_3; break; #ifdef MAVLINK_COMM_4 + case 4: _channel = MAVLINK_COMM_4; break; #endif #ifdef MAVLINK_COMM_5 - case 5: + + case 5: _channel = MAVLINK_COMM_5; break; #endif #ifdef MAVLINK_COMM_6 + case 6: _channel = MAVLINK_COMM_6; break; #endif + default: errx(1, "instance ID is out of range"); break; } - LL_APPEND(_mavlink_instances, this); } Mavlink::~Mavlink() @@ -280,6 +287,7 @@ Mavlink::~Mavlink() } } while (_task_running); } + LL_DELETE(_mavlink_instances, this); } @@ -589,7 +597,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * /* setup output flow control */ if (enable_flow_control(true)) { - warnx("ERR FLOW CTRL EN"); + warnx("hardware flow control not supported"); } } @@ -605,12 +613,16 @@ Mavlink::enable_flow_control(bool enabled) } struct termios uart_config; + int ret = tcgetattr(_uart_fd, &uart_config); + if (enabled) { uart_config.c_cflag |= CRTSCTS; + } else { uart_config.c_cflag &= ~CRTSCTS; } + ret = tcsetattr(_uart_fd, TCSANOW, &uart_config); if (!ret) { @@ -1653,6 +1665,9 @@ Mavlink::task_main(int argc, char *argv[]) case 'm': if (strcmp(optarg, "custom") == 0) { _mode = MAVLINK_MODE_CUSTOM; + + } else if (strcmp(optarg, "camera") == 0) { + _mode = MAVLINK_MODE_CAMERA; } break; @@ -1696,6 +1711,10 @@ Mavlink::task_main(int argc, char *argv[]) warnx("mode: CUSTOM"); break; + case MAVLINK_MODE_CAMERA: + warnx("mode: CAMERA"); + break; + default: warnx("ERROR: Unknown mode"); break; @@ -1703,7 +1722,7 @@ Mavlink::task_main(int argc, char *argv[]) _mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; - warnx("data rate: %d bps, port: %s, baud: %d", _datarate, _device_name, (int)_baudrate); + warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate); /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); @@ -1763,6 +1782,14 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult); configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); + configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult); + break; + + case MAVLINK_MODE_CAMERA: + configure_stream("SYS_STATUS", 1.0f); + configure_stream("ATTITUDE", 15.0f * rate_mult); + configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult); + configure_stream("CAMERA_CAPTURE", 1.0f); break; default: @@ -1773,11 +1800,14 @@ Mavlink::task_main(int argc, char *argv[]) _mavlink_param_queue_index = param_count(); MavlinkRateLimiter slow_rate_limiter(2000000.0f / rate_mult); - MavlinkRateLimiter fast_rate_limiter(20000.0f / rate_mult); + MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult); /* set main loop delay depending on data rate to minimize CPU overhead */ _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; + /* now the instance is fully initialized and we can bump the instance count */ + LL_APPEND(_mavlink_instances, this); + while (!_task_should_exit) { /* main loop */ usleep(_main_loop_delay); @@ -1924,10 +1954,18 @@ int Mavlink::start_helper(int argc, char *argv[]) int Mavlink::start(int argc, char *argv[]) { + // Wait for the instance count to go up one + // before returning to the shell + int ic = Mavlink::instance_count(); + // Instantiate thread char buf[24]; - sprintf(buf, "mavlink_if%d", Mavlink::instance_count()); + sprintf(buf, "mavlink_if%d", ic); + // This is where the control flow splits + // between the starting task and the spawned + // task - start_helper() only returns + // when the started task exits. task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, @@ -1935,6 +1973,26 @@ Mavlink::start(int argc, char *argv[]) (main_t)&Mavlink::start_helper, (const char **)argv); + // Ensure that this shell command + // does not return before the instance + // is fully initialized. As this is also + // the only path to create a new instance, + // this is effectively a lock on concurrent + // instance starting. XXX do a real lock. + + // Sleep 500 us between each attempt + const unsigned sleeptime = 500; + + // Wait 100 ms max for the startup. + const unsigned limit = 100 * 1000 / sleeptime; + + unsigned count = 0; + + while (ic == Mavlink::instance_count() && count < limit) { + ::usleep(sleeptime); + count++; + } + return OK; } @@ -1960,20 +2018,26 @@ Mavlink::stream(int argc, char *argv[]) bool err_flag = false; int i = 0; + while (i < argc) { - if (0 == strcmp(argv[i], "-r") && i < argc - 1 ) { - rate = strtod(argv[i+1], nullptr); + if (0 == strcmp(argv[i], "-r") && i < argc - 1) { + rate = strtod(argv[i + 1], nullptr); + if (rate < 0.0f) { err_flag = true; } + i++; - } else if (0 == strcmp(argv[i], "-d") && i < argc - 1 ) { - device_name = argv[i+1]; + + } else if (0 == strcmp(argv[i], "-d") && i < argc - 1) { + device_name = argv[i + 1]; i++; - } else if (0 == strcmp(argv[i], "-s") && i < argc - 1 ) { - stream_name = argv[i+1]; + + } else if (0 == strcmp(argv[i], "-s") && i < argc - 1) { + stream_name = argv[i + 1]; i++; + } else { err_flag = true; } @@ -1988,7 +2052,10 @@ Mavlink::stream(int argc, char *argv[]) inst->configure_stream_threadsafe(stream_name, rate); } else { - errx(1, "mavlink for device %s is not running", device_name); + + // If the link is not running we should complain, but not fall over + // because this is so easy to get wrong and not fatal. Warning is sufficient. + errx(0, "mavlink for device %s is not running", device_name); } } else { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index f743c2504..5a118a0ad 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -146,7 +146,8 @@ public: enum MAVLINK_MODE { MAVLINK_MODE_NORMAL = 0, - MAVLINK_MODE_CUSTOM + MAVLINK_MODE_CUSTOM, + MAVLINK_MODE_CAMERA }; void set_mode(enum MAVLINK_MODE); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7d388f88d..037999af7 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -262,7 +262,6 @@ protected: void send(const hrt_abstime t) { if (status_sub->update(t)) { - mavlink_msg_sys_status_send(_channel, status->onboard_control_sensors_present, status->onboard_control_sensors_enabled, @@ -318,7 +317,6 @@ protected: void send(const hrt_abstime t) { if (sensor_sub->update(t)) { - uint16_t fields_updated = 0; if (accel_timestamp != sensor->accelerometer_timestamp) { @@ -385,7 +383,6 @@ protected: void send(const hrt_abstime t) { if (att_sub->update(t)) { - mavlink_msg_attitude_send(_channel, att->timestamp / 1000, att->roll, att->pitch, att->yaw, @@ -422,7 +419,6 @@ protected: void send(const hrt_abstime t) { if (att_sub->update(t)) { - mavlink_msg_attitude_quaternion_send(_channel, att->timestamp / 1000, att->q[0], @@ -494,7 +490,6 @@ protected: updated |= airspeed_sub->update(t); if (updated) { - float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e); uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F; float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f; @@ -538,7 +533,6 @@ protected: void send(const hrt_abstime t) { if (gps_sub->update(t)) { - mavlink_msg_gps_raw_int_send(_channel, gps->timestamp_position, gps->fix_type, @@ -592,15 +586,15 @@ protected: if (updated) { mavlink_msg_global_position_int_send(_channel, - pos->timestamp / 1000, - pos->lat * 1e7, - pos->lon * 1e7, - pos->alt * 1000.0f, - (pos->alt - home->alt) * 1000.0f, - pos->vel_n * 100.0f, - pos->vel_e * 100.0f, - pos->vel_d * 100.0f, - _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); + pos->timestamp / 1000, + pos->lat * 1e7, + pos->lon * 1e7, + pos->alt * 1000.0f, + (pos->alt - home->alt) * 1000.0f, + pos->vel_n * 100.0f, + pos->vel_e * 100.0f, + pos->vel_d * 100.0f, + _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); } } }; @@ -633,7 +627,6 @@ protected: void send(const hrt_abstime t) { if (pos_sub->update(t)) { - mavlink_msg_local_position_ned_send(_channel, pos->timestamp / 1000, pos->x, @@ -730,7 +723,6 @@ protected: void send(const hrt_abstime t) { if (act_sub->update(t)) { - mavlink_msg_servo_output_raw_send(_channel, act->timestamp / 1000, _n, @@ -790,7 +782,6 @@ protected: updated |= act_sub->update(t); if (updated) { - /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state; uint8_t mavlink_base_mode; @@ -870,7 +861,6 @@ protected: void send(const hrt_abstime t) { if (pos_sp_triplet_sub->update(t)) { - mavlink_msg_global_position_setpoint_int_send(_channel, MAV_FRAME_GLOBAL, (int32_t)(pos_sp_triplet->current.lat * 1e7), @@ -908,14 +898,14 @@ protected: void send(const hrt_abstime t) { - pos_sp_sub->update(t); - - mavlink_msg_local_position_setpoint_send(_channel, - MAV_FRAME_LOCAL_NED, - pos_sp->x, - pos_sp->y, - pos_sp->z, - pos_sp->yaw); + if (pos_sp_sub->update(t)) { + mavlink_msg_local_position_setpoint_send(_channel, + MAV_FRAME_LOCAL_NED, + pos_sp->x, + pos_sp->y, + pos_sp->z, + pos_sp->yaw); + } } }; @@ -947,7 +937,6 @@ protected: void send(const hrt_abstime t) { if (att_sp_sub->update(t)) { - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, att_sp->timestamp / 1000, att_sp->roll_body, @@ -986,7 +975,6 @@ protected: void send(const hrt_abstime t) { if (att_rates_sp_sub->update(t)) { - mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, att_rates_sp->timestamp / 1000, att_rates_sp->roll, @@ -1025,7 +1013,6 @@ protected: void send(const hrt_abstime t) { if (rc_sub->update(t)) { - const unsigned port_width = 8; for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { @@ -1075,7 +1062,6 @@ protected: void send(const hrt_abstime t) { if (manual_sub->update(t)) { - mavlink_msg_manual_control_send(_channel, mavlink_system.sysid, manual->roll * 1000, @@ -1115,7 +1101,6 @@ protected: void send(const hrt_abstime t) { if (flow_sub->update(t)) { - mavlink_msg_optical_flow_send(_channel, flow->timestamp, flow->sensor_id, @@ -1127,6 +1112,134 @@ protected: } }; +class MavlinkStreamAttitudeControls : public MavlinkStream +{ +public: + const char *get_name() + { + return "ATTITUDE_CONTROLS"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamAttitudeControls(); + } + +private: + MavlinkOrbSubscription *att_ctrl_sub; + struct actuator_controls_s *att_ctrl; + +protected: + void subscribe(Mavlink *mavlink) + { + att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + att_ctrl = (struct actuator_controls_s *)att_ctrl_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (att_ctrl_sub->update(t)) { + /* send, add spaces so that string buffer is at least 10 chars long */ + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "rll ctrl ", + att_ctrl->control[0]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "ptch ctrl ", + att_ctrl->control[1]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "yaw ctrl ", + att_ctrl->control[2]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "thr ctrl ", + att_ctrl->control[3]); + } + } +}; + +class MavlinkStreamNamedValueFloat : public MavlinkStream +{ +public: + const char *get_name() + { + return "NAMED_VALUE_FLOAT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamNamedValueFloat(); + } + +private: + MavlinkOrbSubscription *debug_sub; + struct debug_key_value_s *debug; + +protected: + void subscribe(Mavlink *mavlink) + { + debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); + debug = (struct debug_key_value_s *)debug_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (debug_sub->update(t)) { + /* enforce null termination */ + debug->key[sizeof(debug->key) - 1] = '\0'; + + mavlink_msg_named_value_float_send(_channel, + debug->timestamp_ms, + debug->key, + debug->value); + } + } +}; + +class MavlinkStreamCameraCapture : public MavlinkStream +{ +public: + const char *get_name() + { + return "CAMERA_CAPTURE"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamCameraCapture(); + } + +private: + MavlinkOrbSubscription *status_sub; + struct vehicle_status_s *status; + +protected: + void subscribe(Mavlink *mavlink) + { + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); + status = (struct vehicle_status_s *)status_sub->get_data(); + + + } + + void send(const hrt_abstime t) + { + (void)status_sub->update(t); + + if (status->arming_state == ARMING_STATE_ARMED + || status->arming_state == ARMING_STATE_ARMED_ERROR) { + + /* send camera capture on */ + mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + + } else { + /* send camera capture off */ + mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); + } + } +}; MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), @@ -1151,72 +1264,8 @@ MavlinkStream *streams_list[] = { new MavlinkStreamRCChannelsRaw(), new MavlinkStreamManualControl(), new MavlinkStreamOpticalFlow(), + new MavlinkStreamAttitudeControls(), + new MavlinkStreamNamedValueFloat(), + new MavlinkStreamCameraCapture(), nullptr }; - - - - - - - -// -// -// -// -// -// -//void -//MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l) -//{ -// orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { -// /* send, add spaces so that string buffer is at least 10 chars long */ -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// "ctrl0 ", -// l->listener->actuators_0.control[0]); -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// "ctrl1 ", -// l->listener->actuators_0.control[1]); -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// "ctrl2 ", -// l->listener->actuators_0.control[2]); -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// "ctrl3 ", -// l->listener->actuators_0.control[3]); -// } -//} -// -//void -//MavlinkOrbListener::l_debug_key_value(const struct listener *l) -//{ -// struct debug_key_value_s debug; -// -// orb_copy(ORB_ID(debug_key_value), l->mavlink->get_subs()->debug_key_value, &debug); -// -// /* Enforce null termination */ -// debug.key[sizeof(debug.key) - 1] = '\0'; -// -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// debug.key, -// debug.value); -//} -// -//void -//MavlinkOrbListener::l_nav_cap(const struct listener *l) -//{ -// -// orb_copy(ORB_ID(navigation_capabilities), l->mavlink->get_subs()->navigation_capabilities_sub, &l->listener->nav_cap); -// -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// hrt_absolute_time() / 1000, -// "turn dist", -// l->listener->nav_cap.turn_distance); -// -//} diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index 7cd076948..2f1b3c014 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -5,24 +5,30 @@ * Author: Anton Babushkin <rk3dov@gmail.com> */ +#include <math.h> + #include "inertial_filter.h" void inertial_filter_predict(float dt, float x[3]) { - x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; - x[1] += x[2] * dt; + if (isfinite(dt)) { + x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; + x[1] += x[2] * dt; + } } void inertial_filter_correct(float e, float dt, float x[3], int i, float w) { - float ewdt = e * w * dt; - x[i] += ewdt; + if (isfinite(e) && isfinite(w) && isfinite(dt)) { + float ewdt = e * w * dt; + x[i] += ewdt; - if (i == 0) { - x[1] += w * ewdt; - x[2] += w * w * ewdt / 3.0; + if (i == 0) { + x[1] += w * ewdt; + x[2] += w * w * ewdt / 3.0; - } else if (i == 1) { - x[2] += w * ewdt; + } else if (i == 1) { + x[2] += w * ewdt; + } } } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index a779b3924..12ed55480 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1032,6 +1032,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) raw.differential_pressure_pa = _diff_pres.differential_pressure_pa; raw.differential_pressure_timestamp = _diff_pres.timestamp; + _airspeed.timestamp = hrt_absolute_time(); _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa); _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f, raw.baro_pres_mbar * 1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); diff --git a/src/modules/controllib/uorb/UOrbPublication.cpp b/src/modules/uORB/Publication.cpp index f69b39d90..5a5981617 100644 --- a/src/modules/controllib/uorb/UOrbPublication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -32,8 +32,49 @@ ****************************************************************************/ /** - * @file UOrbPublication.cpp + * @file Publication.cpp * */ -#include "UOrbPublication.hpp" +#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/encoders.h" + +namespace uORB { + +template<class T> +Publication<T>::Publication( + List<PublicationBase *> * list, + const struct orb_metadata *meta) : + T(), // initialize data structure to zero + PublicationBase(list, meta) { +} + +template<class T> +Publication<T>::~Publication() {} + +template<class T> +void * Publication<T>::getDataVoidPtr() { + return (void *)(T *)(this); +} + +template class __EXPORT Publication<vehicle_attitude_s>; +template class __EXPORT Publication<vehicle_local_position_s>; +template class __EXPORT Publication<vehicle_global_position_s>; +template class __EXPORT Publication<debug_key_value_s>; +template class __EXPORT Publication<actuator_controls_s>; +template class __EXPORT Publication<vehicle_global_velocity_setpoint_s>; +template class __EXPORT Publication<vehicle_attitude_setpoint_s>; +template class __EXPORT Publication<vehicle_rates_setpoint_s>; +template class __EXPORT Publication<actuator_outputs_s>; +template class __EXPORT Publication<encoders_s>; + +} diff --git a/src/modules/controllib/uorb/UOrbPublication.hpp b/src/modules/uORB/Publication.hpp index 6f1f3fc1c..8650b3df8 100644 --- a/src/modules/controllib/uorb/UOrbPublication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -32,32 +32,29 @@ ****************************************************************************/ /** - * @file UOrbPublication.h + * @file Publication.h * */ #pragma once #include <uORB/uORB.h> -#include "../block/Block.hpp" -#include "../block/List.hpp" +#include <containers/List.hpp> -namespace control +namespace uORB { -class Block; - /** * Base publication warapper class, used in list traversal * of various publications. */ -class __EXPORT UOrbPublicationBase : public ListNode<control::UOrbPublicationBase *> +class __EXPORT PublicationBase : public ListNode<uORB::PublicationBase *> { public: - UOrbPublicationBase( - List<UOrbPublicationBase *> * list, + PublicationBase( + List<PublicationBase *> * list, const struct orb_metadata *meta) : _meta(meta), _handle(-1) { @@ -71,7 +68,7 @@ public: } } virtual void *getDataVoidPtr() = 0; - virtual ~UOrbPublicationBase() { + virtual ~PublicationBase() { orb_unsubscribe(getHandle()); } const struct orb_metadata *getMeta() { return _meta; } @@ -83,12 +80,12 @@ protected: }; /** - * UOrb Publication wrapper class + * Publication wrapper class */ template<class T> -class UOrbPublication : +class Publication : public T, // this must be first! - public UOrbPublicationBase + public PublicationBase { public: /** @@ -98,13 +95,9 @@ public: * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. */ - UOrbPublication( - List<UOrbPublicationBase *> * list, - const struct orb_metadata *meta) : - T(), // initialize data structure to zero - UOrbPublicationBase(list, meta) { - } - virtual ~UOrbPublication() {} + Publication(List<PublicationBase *> * list, + const struct orb_metadata *meta); + virtual ~Publication(); /* * XXX * This function gets the T struct, assuming @@ -112,7 +105,7 @@ public: * should use dynamic cast, but doesn't * seem to be available */ - void *getDataVoidPtr() { return (void *)(T *)(this); } + void *getDataVoidPtr(); }; -} // namespace control +} // namespace uORB diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp new file mode 100644 index 000000000..c1d1a938f --- /dev/null +++ b/src/modules/uORB/Subscription.cpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * 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 Subscription.cpp + * + */ + +#include "Subscription.hpp" +#include "topics/parameter_update.h" +#include "topics/actuator_controls.h" +#include "topics/vehicle_gps_position.h" +#include "topics/sensor_combined.h" +#include "topics/vehicle_attitude.h" +#include "topics/vehicle_global_position.h" +#include "topics/encoders.h" +#include "topics/position_setpoint_triplet.h" +#include "topics/vehicle_status.h" +#include "topics/manual_control_setpoint.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" + +namespace uORB +{ + +bool __EXPORT SubscriptionBase::updated() +{ + bool isUpdated = false; + orb_check(_handle, &isUpdated); + return isUpdated; +} + +template<class T> +Subscription<T>::Subscription( + List<SubscriptionBase *> * list, + const struct orb_metadata *meta, unsigned interval) : + T(), // initialize data structure to zero + SubscriptionBase(list, meta) { + setHandle(orb_subscribe(getMeta())); + orb_set_interval(getHandle(), interval); +} + +template<class T> +Subscription<T>::~Subscription() {} + +template<class T> +void * Subscription<T>::getDataVoidPtr() { + return (void *)(T *)(this); +} + +template<class T> +T Subscription<T>::getData() { + return T(*this); +} + +template class __EXPORT Subscription<parameter_update_s>; +template class __EXPORT Subscription<actuator_controls_s>; +template class __EXPORT Subscription<vehicle_gps_position_s>; +template class __EXPORT Subscription<sensor_combined_s>; +template class __EXPORT Subscription<vehicle_attitude_s>; +template class __EXPORT Subscription<vehicle_global_position_s>; +template class __EXPORT Subscription<encoders_s>; +template class __EXPORT Subscription<position_setpoint_triplet_s>; +template class __EXPORT Subscription<vehicle_status_s>; +template class __EXPORT Subscription<manual_control_setpoint_s>; +template class __EXPORT Subscription<vehicle_local_position_setpoint_s>; +template class __EXPORT Subscription<vehicle_local_position_s>; +template class __EXPORT Subscription<vehicle_attitude_setpoint_s>; +template class __EXPORT Subscription<vehicle_rates_setpoint_s>; + +} // namespace uORB diff --git a/src/modules/controllib/uorb/UOrbSubscription.hpp b/src/modules/uORB/Subscription.hpp index d337d89a8..34e9a83e0 100644 --- a/src/modules/controllib/uorb/UOrbSubscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -32,28 +32,25 @@ ****************************************************************************/ /** - * @file UOrbSubscription.h + * @file Subscription.h * */ #pragma once #include <uORB/uORB.h> -#include "../block/Block.hpp" -#include "../block/List.hpp" +#include <containers/List.hpp> -namespace control +namespace uORB { -class Block; - /** * Base subscription warapper class, used in list traversal * of various subscriptions. */ -class __EXPORT UOrbSubscriptionBase : - public ListNode<control::UOrbSubscriptionBase *> +class __EXPORT SubscriptionBase : + public ListNode<SubscriptionBase *> { public: // methods @@ -64,8 +61,8 @@ public: * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. */ - UOrbSubscriptionBase( - List<UOrbSubscriptionBase *> * list, + SubscriptionBase( + List<SubscriptionBase *> * list, const struct orb_metadata *meta) : _meta(meta), _handle() { @@ -78,7 +75,7 @@ public: } } virtual void *getDataVoidPtr() = 0; - virtual ~UOrbSubscriptionBase() { + virtual ~SubscriptionBase() { orb_unsubscribe(_handle); } // accessors @@ -93,12 +90,12 @@ protected: }; /** - * UOrb Subscription wrapper class + * Subscription wrapper class */ template<class T> -class __EXPORT UOrbSubscription : +class __EXPORT Subscription : public T, // this must be first! - public UOrbSubscriptionBase + public SubscriptionBase { public: /** @@ -109,19 +106,13 @@ public: * for the topic. * @param interval The minimum interval in milliseconds between updates */ - UOrbSubscription( - List<UOrbSubscriptionBase *> * list, - const struct orb_metadata *meta, unsigned interval) : - T(), // initialize data structure to zero - UOrbSubscriptionBase(list, meta) { - setHandle(orb_subscribe(getMeta())); - orb_set_interval(getHandle(), interval); - } - + Subscription( + List<SubscriptionBase *> * list, + const struct orb_metadata *meta, unsigned interval); /** * Deconstructor */ - virtual ~UOrbSubscription() {} + virtual ~Subscription(); /* * XXX @@ -130,8 +121,8 @@ public: * should use dynamic cast, but doesn't * seem to be available */ - void *getDataVoidPtr() { return (void *)(T *)(this); } - T getData() { return T(*this); } + void *getDataVoidPtr(); + T getData(); }; -} // namespace control +} // namespace uORB diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index 5ec31ab01..0c29101fe 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -41,4 +41,6 @@ MODULE_COMMAND = uorb MODULE_STACKSIZE = 4096 SRCS = uORB.cpp \ - objects_common.cpp + objects_common.cpp \ + Publication.cpp \ + Subscription.cpp diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4c84c1f25..fb24de8d1 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -190,3 +190,6 @@ ORB_DEFINE(navigation_capabilities, struct navigation_capabilities_s); #include "topics/esc_status.h" ORB_DEFINE(esc_status, struct esc_status_s); + +#include "topics/encoders.h" +ORB_DEFINE(encoders, struct encoders_s); diff --git a/src/modules/controllib/uorb/UOrbSubscription.cpp b/src/modules/uORB/topics/encoders.h index 022cadd24..588c0ddb1 100644 --- a/src/modules/controllib/uorb/UOrbSubscription.cpp +++ b/src/modules/uORB/topics/encoders.h @@ -32,20 +32,35 @@ ****************************************************************************/ /** - * @file UOrbSubscription.cpp + * @file encoders.h + * + * Encoders topic. * */ -#include "UOrbSubscription.hpp" +#ifndef TOPIC_ENCODERS_H +#define TOPIC_ENCODERS_H + +#include <stdint.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ -namespace control -{ +#define NUM_ENCODERS 4 + +struct encoders_s { + uint64_t timestamp; + int64_t counts[NUM_ENCODERS]; // counts of encoder + float velocity[NUM_ENCODERS]; // counts of encoder/ second +}; + +/** + * @} + */ -bool __EXPORT UOrbSubscriptionBase::updated() -{ - bool isUpdated = false; - orb_check(_handle, &isUpdated); - return isUpdated; -} +ORB_DECLARE(encoders); -} // namespace control +#endif |