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

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

@@ -108,7 +108,6 @@ then
 	set HIL no
 	set VEHICLE_TYPE none
 	set MIXER none
-	set USE_IO yes
 	set OUTPUT_MODE none
 	set PWM_OUTPUTS none
 	set PWM_RATE none
@@ -131,6 +130,16 @@ then
 	else
 		set DO_AUTOCONFIG no
 	fi
+	
+	#
+	# Set USE_IO flag
+	#
+	if param compare SYS_USE_IO 1
+	then
+		set USE_IO yes
+	else
+		set USE_IO no
+	fi
 		
 	#
 	# Set parameters and env variables for selected AUTOSTART

src/drivers/ets_airspeed/ets_airspeed.cpp

@@ -132,7 +132,6 @@ ETSAirspeed::measure()
 
 	if (OK != ret) {
 		perf_count(_comms_errors);
-		log("i2c::transfer returned %d", ret);
 	}
 
 	return ret;
@@ -308,7 +307,7 @@ fail:
 		g_dev = nullptr;
 	}
 
-	errx(1, "driver start failed");
+	errx(1, "no ETS airspeed sensor connected");
 }
 
 /**

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/meas_airspeed/meas_airspeed.cpp

@@ -104,7 +104,7 @@
 class MEASAirspeed : public Airspeed
 {
 public:
-	MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char* path = PATH_MS4525);
+	MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char *path = PATH_MS4525);
 
 protected:
 
@@ -123,7 +123,7 @@ protected:
  */
 extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
 
-MEASAirspeed::MEASAirspeed(int bus, int address, const char* path) : Airspeed(bus, address,
+MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address,
 			CONVERSION_INTERVAL, path)
 {
 
@@ -161,23 +161,25 @@ MEASAirspeed::collect()
 	ret = transfer(nullptr, 0, &val[0], 4);
 
 	if (ret < 0) {
-                perf_count(_comms_errors);
+		perf_count(_comms_errors);
-                perf_end(_sample_perf);
+		perf_end(_sample_perf);
 		return ret;
 	}
 
-	uint8_t status = val[0] & 0xC0;
+	uint8_t status = (val[0] & 0xC0) >> 6;
 
-	if (status == 2) {
+	switch (status) {
-		log("err: stale data");
+	case 0:
-                perf_count(_comms_errors);
+		break;
-                perf_end(_sample_perf);
+
-		return ret;
+	case 1:
-	} else if (status == 3) {
+		/* fallthrough */
-		log("err: fault");
+	case 2:
-                perf_count(_comms_errors);
+		/* fallthrough */
-                perf_end(_sample_perf);
+	case 3:
-		return ret;                
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return -EAGAIN;
 	}
 
 	int16_t dp_raw = 0, dT_raw = 0;
@@ -193,19 +195,21 @@ MEASAirspeed::collect()
 	*/
 	const float P_min = -1.0f;
 	const float P_max = 1.0f;
-	float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset;
+	float diff_press_pa = fabsf((((float)dp_raw - 0.1f * 16383.0f) * (P_max - P_min) / (0.8f * 16383.0f) + P_min) * 6894.8f) - _diff_pres_offset;
-        if (diff_press_pa < 0.0f)
+
-            diff_press_pa = 0.0f;
+	if (diff_press_pa < 0.0f) {
+		diff_press_pa = 0.0f;
+	}
 
 	struct differential_pressure_s report;
 
 	/* track maximum differential pressure measured (so we can work out top speed). */
 	if (diff_press_pa > _max_differential_pressure_pa) {
-	    _max_differential_pressure_pa = diff_press_pa;
+		_max_differential_pressure_pa = diff_press_pa;
 	}
 
 	report.timestamp = hrt_absolute_time();
-        report.error_count = perf_event_count(_comms_errors);
+	report.error_count = perf_event_count(_comms_errors);
 	report.temperature = temperature;
 	report.differential_pressure_pa = diff_press_pa;
 	report.voltage = 0;
@@ -261,8 +265,9 @@ MEASAirspeed::cycle()
 	}
 
 	/* measurement phase */
-	if (OK != measure())
+	if (OK != measure()) {
 		log("measure error");
+	}
 
 	/* next phase is collection */
 	_collect_phase = true;
@@ -303,15 +308,17 @@ start(int i2c_bus)
 {
 	int fd;
 
-	if (g_dev != nullptr)
+	if (g_dev != nullptr) {
 		errx(1, "already started");
+	}
 
 	/* create the driver, try the MS4525DO first */
 	g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525);
 
 	/* check if the MS4525DO was instantiated */
-	if (g_dev == nullptr)
+	if (g_dev == nullptr) {
 		goto fail;
+	}
 
 	/* try the MS5525DSO next if init fails */
 	if (OK != g_dev->Airspeed::init()) {
@@ -319,22 +326,26 @@ start(int i2c_bus)
 		g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525);
 
 		/* check if the MS5525DSO was instantiated */
-		if (g_dev == nullptr)
+		if (g_dev == nullptr) {
 			goto fail;
+		}
 
 		/* both versions failed if the init for the MS5525DSO fails, give up */
-		if (OK != g_dev->Airspeed::init())
+		if (OK != g_dev->Airspeed::init()) {
 			goto fail;
+		}
 	}
 
 	/* set the poll rate to default, starts automatic data collection */
 	fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		goto fail;
+	}
 
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
 		goto fail;
+	}
 
 	exit(0);
 
@@ -345,7 +356,7 @@ fail:
 		g_dev = nullptr;
 	}
 
-	errx(1, "driver start failed");
+	errx(1, "no MS4525 airspeed sensor connected");
 }
 
 /**
@@ -379,21 +390,24 @@ test()
 
 	int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		err(1, "%s open failed (try 'meas_airspeed start' if the driver is not running", AIRSPEED_DEVICE_PATH);
+	}
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
 
-	if (sz != sizeof(report))
+	if (sz != sizeof(report)) {
 		err(1, "immediate read failed");
+	}
 
 	warnx("single read");
 	warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
 
 	/* start the sensor polling at 2Hz */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
 		errx(1, "failed to set 2Hz poll rate");
+	}
 
 	/* read the sensor 5x and report each value */
 	for (unsigned i = 0; i < 5; i++) {
@@ -404,14 +418,16 @@ test()
 		fds.events = POLLIN;
 		ret = poll(&fds, 1, 2000);
 
-		if (ret != 1)
+		if (ret != 1) {
 			errx(1, "timed out waiting for sensor data");
+		}
 
 		/* now go get it */
 		sz = read(fd, &report, sizeof(report));
 
-		if (sz != sizeof(report))
+		if (sz != sizeof(report)) {
 			err(1, "periodic read failed");
+		}
 
 		warnx("periodic read %u", i);
 		warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
@@ -419,8 +435,9 @@ test()
 	}
 
 	/* reset the sensor polling to its default rate */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT))
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
 		errx(1, "failed to set default rate");
+	}
 
 	errx(0, "PASS");
 }
@@ -433,14 +450,17 @@ reset()
 {
 	int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		err(1, "failed ");
+	}
 
-	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
 		err(1, "driver reset failed");
+	}
 
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
 		err(1, "driver poll restart failed");
+	}
 
 	exit(0);
 }
@@ -451,8 +471,9 @@ reset()
 void
 info()
 {
-	if (g_dev == nullptr)
+	if (g_dev == nullptr) {
 		errx(1, "driver not running");
+	}
 
 	printf("state @ %p\n", g_dev);
 	g_dev->print_info();
@@ -491,32 +512,37 @@ meas_airspeed_main(int argc, char *argv[])
 	/*
 	 * Start/load the driver.
 	 */
-	if (!strcmp(argv[1], "start"))
+	if (!strcmp(argv[1], "start")) {
 		meas_airspeed::start(i2c_bus);
+	}
 
 	/*
 	 * Stop the driver
 	 */
-	if (!strcmp(argv[1], "stop"))
+	if (!strcmp(argv[1], "stop")) {
 		meas_airspeed::stop();
+	}
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test"))
+	if (!strcmp(argv[1], "test")) {
 		meas_airspeed::test();
+	}
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset"))
+	if (!strcmp(argv[1], "reset")) {
 		meas_airspeed::reset();
+	}
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
 		meas_airspeed::info();
+	}
 
 	meas_airspeed_usage();
 	exit(0);

src/drivers/px4flow/px4flow.cpp

@@ -74,7 +74,7 @@
 
 /* Configuration Constants */
 #define PX4FLOW_BUS 			PX4_I2C_BUS_EXPANSION
-#define I2C_FLOW_ADDRESS 		0x45 //* 7-bit address. 8-bit address is 0x8A
+#define I2C_FLOW_ADDRESS 		0x42 //* 7-bit address. 8-bit address is 0x84
 //range 0x42 - 0x49
 
 /* PX4FLOW Registers addresses */

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/sensors/sensors.cpp

@@ -1030,6 +1030,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/systemlib/system_params.c

@@ -60,3 +60,14 @@ PARAM_DEFINE_INT32(SYS_AUTOSTART, 0);
  * @group System
  */
 PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0);
+
+/**
+ * Set usage of IO board
+ *
+ * Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up.
+ *
+ * @min 0
+ * @max 1
+ * @group System
+ */
+PARAM_DEFINE_INT32(SYS_USE_IO, 1);

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