Merge branch 'master' of github.com:PX4/Firmware into paul_estimator_numeric

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

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

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 

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

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;

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>
 

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;

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; }

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 <uORB/Subscription.hpp>
-#include <controllib/uorb/UOrbPublication.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. */
+	uORB::Subscription<sensor_combined_s> _sensors;          /**< sensors sub. */
-	control::UOrbSubscription<vehicle_gps_position_s> _gps;         /**< gps sub. */
+	uORB::Subscription<vehicle_gps_position_s> _gps;         /**< gps sub. */
-	control::UOrbSubscription<parameter_update_s> _param_update;    /**< parameter update sub. */
+	uORB::Subscription<parameter_update_s> _param_update;    /**< parameter update sub. */
 	// publications
-	control::UOrbPublication<vehicle_global_position_s> _pos;       /**< position pub. */
+	uORB::Publication<vehicle_global_position_s> _pos;       /**< position pub. */
-	control::UOrbPublication<vehicle_local_position_s> _localPos;   /**< local position pub. */
+	uORB::Publication<vehicle_local_position_s> _localPos;   /**< local position pub. */
-	control::UOrbPublication<vehicle_attitude_s> _att;              /**< attitude 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 */

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) {

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<uORB::SubscriptionBase *> & getSubscriptions() { return _subscriptions; }
-	List<UOrbPublicationBase *> & getPublications() { return _publications; }
+	List<uORB::PublicationBase *> & getPublications() { return _publications; }
 	List<BlockParamBase *> & getParams() { return _params; }
 // attributes
 	const char *_name;
 	SuperBlock *_parent;
 	float _dt;
-	List<UOrbSubscriptionBase *> _subscriptions;
+	List<uORB::SubscriptionBase *> _subscriptions;
-	List<UOrbPublicationBase *> _publications;
+	List<uORB::PublicationBase *> _publications;
 	List<BlockParamBase *> _params;
 };
 

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

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) :
+	BlockParam(Block *block, const char *name,
-		BlockParamBase(block, name, parent_prefix),
+			bool parent_prefix=true);
-		_val() {
+	T get();
-		update();
+	void set(T val);
-	}
+	void update();
-	float get() { return _val; }
+	virtual ~BlockParam();
-	void set(float val) { _val = val; }
-	void update() {
-		if (_handle != PARAM_INVALID) param_get(_handle, &_val);
-	}
 protected:
-	float _val;
+	T _val;
 };
 
-class __EXPORT BlockParamInt : public BlockParamBase
+typedef BlockParam<float> BlockParamFloat;
-{
+typedef BlockParam<int> BlockParamInt;
-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;
-};
 
 } // namespace control

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

src/modules/controllib/uorb/blocks.hpp

@@ -62,8 +62,8 @@ extern "C" {
 }
 
 #include "../blocks.hpp"
-#include "UOrbSubscription.hpp"
+#include <uORB/Subscription.hpp>
-#include "UOrbPublication.hpp"
+#include <uORB/Publication.hpp>
 
 namespace control
 {
@@ -94,16 +94,16 @@ class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
 {
 protected:
 	// subscriptions
-	UOrbSubscription<vehicle_attitude_s> _att;
+	uORB::Subscription<vehicle_attitude_s> _att;
-	UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
+	uORB::Subscription<vehicle_attitude_setpoint_s> _attCmd;
-	UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
+	uORB::Subscription<vehicle_rates_setpoint_s> _ratesCmd;
-	UOrbSubscription<vehicle_global_position_s> _pos;
+	uORB::Subscription<vehicle_global_position_s> _pos;
-	UOrbSubscription<position_setpoint_triplet_s> _missionCmd;
+	uORB::Subscription<position_setpoint_triplet_s> _missionCmd;
-	UOrbSubscription<manual_control_setpoint_s> _manual;
+	uORB::Subscription<manual_control_setpoint_s> _manual;
-	UOrbSubscription<vehicle_status_s> _status;
+	uORB::Subscription<vehicle_status_s> _status;
-	UOrbSubscription<parameter_update_s> _param_update;
+	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();

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

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		_att_sub;			/**< vehicle attitude subscription */
-	int		_accel_sub;				/**< accelerometer 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 		_params_sub;			/**< notification of parameter updates */
-	int 	_manual_sub;			/**< notification of manual control 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 vehicle_attitude_s			_att;			/**< vehicle attitude */
-	struct accel_report						_accel;			/**< body frame accelerations */
+	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 airspeed_s				_airspeed;		/**< airspeed */
-	struct vehicle_control_mode_s			_vcontrol_mode;	/**< vehicle control mode */
+	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 actuator_controls_s			_actuators_airframe;	/**< actuator control inputs */
-	struct vehicle_global_position_s		_global_pos;	/**< global position */
+	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),
+	_setpoint_valid(false)
-	_airspeed_valid(false)
 {
 	/* safely initialize structs */
-	_att = {0};
+	_att = {};
-	_accel = {0};
+	_accel = {};
-	_att_sp = {0};
+	_att_sp = {};
-	_manual = {0};
+	_manual = {};
-	_airspeed = {0};
+	_airspeed = {};
-	_vcontrol_mode = {0};
+	_vcontrol_mode = {};
-	_actuators = {0};
+	_actuators = {};
-	_actuators_airframe = {0};
+	_actuators_airframe = {};
-	_global_pos = {0};
+	_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 ||
+				if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) ||
-				    (_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) ||
+				    !isfinite(_airspeed.indicated_airspeed_m_s) ||
-				    !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

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 */
 /**

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 ) {
+		if (0 == strcmp(argv[i], "-r") && i < argc - 1) {
-			rate = strtod(argv[i+1], nullptr);
+			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 {

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);

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->timestamp / 1000,
-						     pos->lat * 1e7,
+							     pos->lat * 1e7,
-						     pos->lon * 1e7,
+							     pos->lon * 1e7,
-						     pos->alt * 1000.0f,
+							     pos->alt * 1000.0f,
-						     (pos->alt - home->alt) * 1000.0f,
+							     (pos->alt - home->alt) * 1000.0f,
-						     pos->vel_n * 100.0f,
+							     pos->vel_n * 100.0f,
-						     pos->vel_e * 100.0f,
+							     pos->vel_e * 100.0f,
-						     pos->vel_d * 100.0f,
+							     pos->vel_d * 100.0f,
-						     _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 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);
+		if (pos_sp_sub->update(t)) {
-
+			mavlink_msg_local_position_setpoint_send(_channel,
-		mavlink_msg_local_position_setpoint_send(_channel,
+					MAV_FRAME_LOCAL_NED,
-				MAV_FRAME_LOCAL_NED,
+					pos_sp->x,
-				pos_sp->x,
+					pos_sp->y,
-				pos_sp->y,
+					pos_sp->z,
-				pos_sp->z,
+					pos_sp->yaw);
-				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);
-//
-//}

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;
+	if (isfinite(dt)) {
-	x[1] += x[2] * 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;
+	if (isfinite(e) && isfinite(w) && isfinite(dt)) {
-	x[i] += ewdt;
+		float ewdt = e * w * dt;
+		x[i] += ewdt;
 
-	if (i == 0) {
+		if (i == 0) {
-		x[1] += w * ewdt;
+			x[1] += w * ewdt;
-		x[2] += w * w * ewdt / 3.0;
+			x[2] += w * w * ewdt / 3.0;
 
-	} else if (i == 1) {
+		} else if (i == 1) {
-		x[2] += w * ewdt;
+			x[2] += w * ewdt;
+		}
 	}
 }

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);

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>;
+
+}

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 <containers/List.hpp>
-#include "../block/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(
+	PublicationBase(
-		List<UOrbPublicationBase *> * list,
+		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(
+	Publication(List<PublicationBase *> * list,
-		List<UOrbPublicationBase *> * list,
+		const struct orb_metadata *meta);
-		const struct orb_metadata *meta) :
+	virtual ~Publication();
-		T(), // initialize data structure to zero
-		UOrbPublicationBase(list, meta) {
-	}
-	virtual ~UOrbPublication() {}
 	/*
 	 * 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

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

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 <containers/List.hpp>
-#include "../block/List.hpp"
 
 
-namespace control
+namespace uORB
 {
 
-class Block;
-
 /**
  * Base subscription warapper class, used in list traversal
  * of various subscriptions.
  */
-class __EXPORT UOrbSubscriptionBase :
+class __EXPORT SubscriptionBase :
-	public ListNode<control::UOrbSubscriptionBase *>
+	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(
+	SubscriptionBase(
-		List<UOrbSubscriptionBase *> * list,
+		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(
+	Subscription(
-		List<UOrbSubscriptionBase *> * list,
+		List<SubscriptionBase *> * list,
-		const struct orb_metadata *meta, unsigned interval) :
+		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);
-	}
-
 	/**
 	 * 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); }
+	void *getDataVoidPtr();
-	T getData() { return T(*this); }
+	T getData();
 };
 
-} // namespace control
+} // namespace uORB

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

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);

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
 
-namespace control
+#include <stdint.h>
-{
+#include "../uORB.h"
 
-bool __EXPORT UOrbSubscriptionBase::updated()
+/**
-{
+ * @addtogroup topics
-	bool isUpdated = false;
+ * @{
-	orb_check(_handle, &isUpdated);
+ */
-	return isUpdated;
-}
 
-} // 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
+};
+
+/**
+ * @}
+ */
+
+ORB_DECLARE(encoders);
+
+#endif