Merge remote-tracking branch 'upstream/master' into offboard2_externalsetpointmessages

ROMFS/px4fmu_common/init.d/10016_3dr_iris

@@ -30,3 +30,6 @@ fi
 set MIXER FMU_quad_w
 
 set PWM_OUTPUTS 1234
+
+set PWM_MIN 1200
+set PWM_MAX 1950

ROMFS/px4fmu_common/init.d/4010_dji_f330

@@ -23,5 +23,5 @@ then
 	param set MC_YAWRATE_D 0.0
 fi
 
-set PWM_MIN 1175
-set PWM_MAX 1900
+set PWM_MIN 1200
+set PWM_MAX 1950

ROMFS/px4fmu_common/init.d/4011_dji_f450

@@ -24,5 +24,5 @@ then
 	param set MC_YAWRATE_D 0.0
 fi
 
-set PWM_MIN 1175
-set PWM_MAX 1900
+set PWM_MIN 1230
+set PWM_MAX 1950

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -6,28 +6,51 @@
 ms5611 start
 adc start
 
-# Mag might be external
-if hmc5883 start
+if mpu6000 -X start
 then
-	echo "[init] Using HMC5883"
 fi
 
 if mpu6000 start
 then
-	echo "[init] Using MPU6000"
+fi
+
+if l3gd20 -X start
+then
 fi
 
 if l3gd20 start
 then
-	echo "[init] Using L3GD20(H)"
+fi
+
+# MAG selection
+if param compare SENS_EXT_MAG 2
+then
+	if hmc5883 -I start
+	then
+	fi
+else
+	# Use only external as primary
+	if param compare SENS_EXT_MAG 1
+	then
+		if hmc5883 -X start
+		then
+		fi
+	else
+	# auto-detect the primary, prefer external
+		if hmc5883 start
+		then
+		fi
+	fi
 fi
 
 if ver hwcmp PX4FMU_V2
 then
-	# IMPORTANT: EXTERNAL BUSES SHOULD BE SCANNED FIRST
+	if lsm303d -X start
+	then
+	fi
+
 	if lsm303d start
 	then
-		echo "[init] Using LSM303D"
 	fi
 fi
 
@@ -38,11 +61,9 @@ then
 else
 	if ets_airspeed start
 	then
-		echo "[init] Using ETS airspeed sensor (bus 3)"
 	else
 		if ets_airspeed start -b 1
 		then
-			echo "[init] Using ETS airspeed sensor (bus 1)"
 		fi
 	fi
 fi

makefiles/config_px4fmu-v2_test.mk

@@ -33,6 +33,11 @@ MODULES		+= systemcmds/nshterm
 MODULES		+= systemcmds/mtd
 MODULES		+= systemcmds/ver
 
+#
+# Testing modules
+#
+MODULES		+= examples/matlab_csv_serial
+
 #
 # Library modules
 #

src/drivers/airspeed/airspeed.h

@@ -107,6 +107,10 @@ private:
 	RingBuffer		*_reports;
 	perf_counter_t		_buffer_overflows;
 
+	/* this class has pointer data members and should not be copied */
+	Airspeed(const Airspeed&);
+	Airspeed& operator=(const Airspeed&);
+
 protected:
 	virtual int	probe();
 

src/drivers/device/device.h

@@ -240,6 +240,7 @@ private:
 	 * @param context	Pointer to the interrupted context.
 	 */
 	static void	dev_interrupt(int irq, void *context);
+
 };
 
 /**
@@ -469,6 +470,10 @@ private:
 	 * @return		OK, or -errno on error.
 	 */
 	int		remove_poll_waiter(struct pollfd *fds);
+
+	/* do not allow copying this class */
+	CDev(const CDev&);
+	CDev operator=(const CDev&);
 };
 
 /**
@@ -538,6 +543,10 @@ private:
 } // namespace device
 
 // class instance for primary driver of each class
-#define CLASS_DEVICE_PRIMARY 0
+enum CLASS_DEVICE {
+	CLASS_DEVICE_PRIMARY=0,
+	CLASS_DEVICE_SECONDARY=1,
+	CLASS_DEVICE_TERTIARY=2
+};
 
 #endif /* _DEVICE_DEVICE_H */

src/drivers/device/i2c.h

@@ -138,6 +138,9 @@ private:
 	uint16_t		_address;
 	uint32_t		_frequency;
 	struct i2c_dev_s	*_dev;
+
+	I2C(const device::I2C&);
+	I2C operator=(const device::I2C&);
 };
 
 } // namespace device

src/drivers/device/ringbuffer.h

@@ -162,6 +162,10 @@ private:
 	volatile unsigned	_tail;	/**< removal point in _item_size units */
 
 	unsigned		_next(unsigned index);
+
+	/* we don't want this class to be copied */
+	RingBuffer(const RingBuffer&);
+	RingBuffer operator=(const RingBuffer&);
 };
 
 RingBuffer::RingBuffer(unsigned num_items, size_t item_size) :

src/drivers/device/spi.h

@@ -129,10 +129,15 @@ private:
 	uint32_t		_frequency;
 	struct spi_dev_s	*_dev;
 
+	/* this class does not allow copying */
+	SPI(const SPI&);
+	SPI operator=(const SPI&);
+
 protected:
 	int			_bus;
 
 	int	_transfer(uint8_t *send, uint8_t *recv, unsigned len);
+
 };
 
 } // namespace device

src/drivers/drv_accel.h

@@ -81,7 +81,9 @@ struct accel_scale {
 /*
  * ObjDev tag for raw accelerometer data.
  */
-ORB_DECLARE(sensor_accel);
+ORB_DECLARE(sensor_accel0);
+ORB_DECLARE(sensor_accel1);
+ORB_DECLARE(sensor_accel2);
 
 /*
  * ioctl() definitions

src/drivers/drv_baro.h

@@ -63,7 +63,8 @@ struct baro_report {
 /*
  * ObjDev tag for raw barometer data.
  */
-ORB_DECLARE(sensor_baro);
+ORB_DECLARE(sensor_baro0);
+ORB_DECLARE(sensor_baro1);
 
 /*
  * ioctl() definitions

src/drivers/drv_gyro.h

@@ -81,7 +81,9 @@ struct gyro_scale {
 /*
  * ObjDev tag for raw gyro data.
  */
-ORB_DECLARE(sensor_gyro);
+ORB_DECLARE(sensor_gyro0);
+ORB_DECLARE(sensor_gyro1);
+ORB_DECLARE(sensor_gyro2);
 
 /*
  * ioctl() definitions

src/drivers/drv_mag.h

@@ -79,8 +79,9 @@ struct mag_scale {
 /*
  * ObjDev tag for raw magnetometer data.
  */
-ORB_DECLARE(sensor_mag);
-
+ORB_DECLARE(sensor_mag0);
+ORB_DECLARE(sensor_mag1);
+ORB_DECLARE(sensor_mag2);
 
 /*
  * mag device types, for _device_id

src/drivers/ets_airspeed/ets_airspeed.cpp

@@ -286,6 +286,9 @@ void	info();
 
 /**
  * Start the driver.
+ *
+ * This function only returns if the sensor is up and running
+ * or could not be detected successfully.
  */
 void
 start(int i2c_bus)

src/drivers/ets_airspeed/module.mk

@@ -36,6 +36,7 @@
 #
 
 MODULE_COMMAND		= ets_airspeed
-MODULE_STACKSIZE	= 2048
 
 SRCS			= ets_airspeed.cpp
+
+MODULE_STACKSIZE	= 1200

src/drivers/hmc5883/hmc5883.cpp

@@ -162,6 +162,7 @@ private:
 
 	orb_advert_t		_mag_topic;
 	orb_advert_t		_subsystem_pub;
+	orb_id_t		_mag_orb_id;
 
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_comms_errors;
@@ -337,6 +338,9 @@ private:
 	 */
 	 int 			check_offset();
 
+	/* this class has pointer data members, do not allow copying it */
+	HMC5883(const HMC5883&);
+	HMC5883 operator=(const HMC5883&);
 };
 
 /*
@@ -347,14 +351,17 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
 
 HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) :
 	I2C("HMC5883", path, bus, HMC5883L_ADDRESS, 400000),
+	_work{},
 	_measure_ticks(0),
 	_reports(nullptr),
+	_scale{},
 	_range_scale(0), /* default range scale from counts to gauss */
 	_range_ga(1.3f),
 	_collect_phase(false),
 	_class_instance(-1),
 	_mag_topic(-1),
 	_subsystem_pub(-1),
+	_mag_orb_id(nullptr),
 	_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
@@ -423,6 +430,20 @@ HMC5883::init()
 
 	_class_instance = register_class_devname(MAG_DEVICE_PATH);
 
+	switch (_class_instance) {
+		case CLASS_DEVICE_PRIMARY:
+			_mag_orb_id = ORB_ID(sensor_mag0);
+			break;
+
+		case CLASS_DEVICE_SECONDARY:
+			_mag_orb_id = ORB_ID(sensor_mag1);
+			break;
+
+		case CLASS_DEVICE_TERTIARY:
+			_mag_orb_id = ORB_ID(sensor_mag2);
+			break;
+	}
+
 	ret = OK;
 	/* sensor is ok, but not calibrated */
 	_sensor_ok = true;
@@ -867,7 +888,8 @@ HMC5883::collect()
 	struct {
 		int16_t		x, y, z;
 	} report;
-	int	ret = -EIO;
+
+	int	ret;
 	uint8_t	cmd;
 	uint8_t check_counter;
 
@@ -945,16 +967,16 @@ HMC5883::collect()
 	// apply user specified rotation
 	rotate_3f(_rotation, new_report.x, new_report.y, new_report.z);
 
-	if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
+	if (!(_pub_blocked)) {
 
 		if (_mag_topic != -1) {
 			/* publish it */
-			orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
+			orb_publish(_mag_orb_id, _mag_topic, &new_report);
 		} else {
-			_mag_topic = orb_advertise(ORB_ID(sensor_mag), &new_report);
+			_mag_topic = orb_advertise(_mag_orb_id, &new_report);
 
 			if (_mag_topic < 0)
-				debug("failed to create sensor_mag publication");
+				debug("ADVERT FAIL");
 		}
 	}
 
@@ -1157,7 +1179,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 out:
 
 	if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
-		warn("WARNING: failed to set new scale / offsets for mag");
+		warn("failed to set new scale / offsets for mag");
 	}
 
 	/* set back to normal mode */
@@ -1341,8 +1363,8 @@ namespace hmc5883
 #endif
 const int ERROR = -1;
 
-HMC5883	*g_dev_int;
-HMC5883	*g_dev_ext;
+HMC5883	*g_dev_int = nullptr;
+HMC5883	*g_dev_ext = nullptr;
 
 void	start(int bus, enum Rotation rotation);
 void	test(int bus);
@@ -1353,6 +1375,9 @@ void	usage();
 
 /**
  * Start the driver.
+ *
+ * This function call only returns once the driver
+ * is either successfully up and running or failed to start.
  */
 void
 start(int bus, enum Rotation rotation)
@@ -1378,6 +1403,11 @@ start(int bus, enum Rotation rotation)
 			errx(0, "already started internal");
 		g_dev_int = new HMC5883(PX4_I2C_BUS_ONBOARD, HMC5883L_DEVICE_PATH_INT, rotation);
 		if (g_dev_int != nullptr && OK != g_dev_int->init()) {
+
+			/* tear down the failing onboard instance */
+			delete g_dev_int;
+			g_dev_int = nullptr;
+
 			if (bus == PX4_I2C_BUS_ONBOARD) {
 				goto fail;
 			}
@@ -1443,7 +1473,7 @@ test(int bus)
 	int fd = open(path, O_RDONLY);
 
 	if (fd < 0)
-		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path);
+		err(1, "%s open failed (try 'hmc5883 start')", path);
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));

src/drivers/hmc5883/module.mk

@@ -37,7 +37,8 @@
 
 MODULE_COMMAND	= hmc5883
 
-# XXX seems excessive, check if 2048 is sufficient
-MODULE_STACKSIZE	= 4096
-
 SRCS		= hmc5883.cpp
+
+MODULE_STACKSIZE	= 1200
+
+EXTRACXXFLAGS	= -Weffc++

src/drivers/l3gd20/l3gd20.cpp

@@ -213,6 +213,7 @@ private:
 	float			_gyro_range_scale;
 	float			_gyro_range_rad_s;
 	orb_advert_t		_gyro_topic;
+	orb_id_t		_orb_id;
 	int			_class_instance;
 
 	unsigned		_current_rate;
@@ -330,15 +331,22 @@ private:
 	 * @return 0 on success, 1 on failure
 	 */
 	 int 			self_test();
+
+	/* this class does not allow copying */
+	L3GD20(const L3GD20&);
+	L3GD20 operator=(const L3GD20&);
 };
 
 L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) :
 	SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */),
+	_call{},
 	_call_interval(0),
 	_reports(nullptr),
+	_gyro_scale{},
 	_gyro_range_scale(0.0f),
 	_gyro_range_rad_s(0.0f),
 	_gyro_topic(-1),
+	_orb_id(nullptr),
 	_class_instance(-1),
 	_current_rate(0),
 	_orientation(SENSOR_BOARD_ROTATION_DEFAULT),
@@ -399,21 +407,32 @@ L3GD20::init()
 
 	_class_instance = register_class_devname(GYRO_DEVICE_PATH);
 
+	switch (_class_instance) {
+		case CLASS_DEVICE_PRIMARY:
+			_orb_id = ORB_ID(sensor_gyro0);
+			break;
+
+		case CLASS_DEVICE_SECONDARY:
+			_orb_id = ORB_ID(sensor_gyro1);
+			break;
+
+		case CLASS_DEVICE_TERTIARY:
+			_orb_id = ORB_ID(sensor_gyro2);
+			break;
+	}
+
 	reset();
 
 	measure();
 
-	if (_class_instance == CLASS_DEVICE_PRIMARY) {
+	/* advertise sensor topic, measure manually to initialize valid report */
+	struct gyro_report grp;
+	_reports->get(&grp);
 
-		/* advertise sensor topic, measure manually to initialize valid report */
-		struct gyro_report grp;
-		_reports->get(&grp);
-
-		_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp);
-
-		if (_gyro_topic < 0)
-			debug("failed to create sensor_gyro publication");
+	_gyro_topic = orb_advertise(_orb_id, &grp);
 
+	if (_gyro_topic < 0) {
+		debug("failed to create sensor_gyro publication");
 	}
 
 	ret = OK;
@@ -931,9 +950,9 @@ L3GD20::measure()
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	if (_gyro_topic > 0 && !(_pub_blocked)) {
+	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
+		orb_publish(_orb_id, _gyro_topic, &report);
 	}
 
 	_read++;
@@ -990,6 +1009,9 @@ void	info();
 
 /**
  * Start the driver.
+ *
+ * This function call only returns once the driver
+ * started or failed to detect the sensor.
  */
 void
 start(bool external_bus, enum Rotation rotation)

src/drivers/l3gd20/module.mk

@@ -4,3 +4,7 @@
 
 MODULE_COMMAND	 = l3gd20
 SRCS		 = l3gd20.cpp
+
+MODULE_STACKSIZE	= 1200
+
+EXTRACXXFLAGS	= -Weffc++

src/drivers/lsm303d/lsm303d.cpp

@@ -284,6 +284,7 @@ private:
 	unsigned		_mag_samplerate;
 
 	orb_advert_t		_accel_topic;
+	orb_id_t		_accel_orb_id;
 	int			_accel_class_instance;
 
 	unsigned		_accel_read;
@@ -461,6 +462,10 @@ private:
 	 * @return		OK if the value can be supported.
 	 */
 	int			mag_set_samplerate(unsigned frequency);
+
+	/* this class cannot be copied */
+	LSM303D(const LSM303D&);
+	LSM303D operator=(const LSM303D&);
 };
 
 /**
@@ -485,29 +490,39 @@ private:
 	LSM303D				*_parent;
 
 	orb_advert_t			_mag_topic;
+	orb_id_t			_mag_orb_id;
 	int				_mag_class_instance;
 
 	void				measure();
 
 	void				measure_trampoline(void *arg);
+
+	/* this class does not allow copying due to ptr data members */
+	LSM303D_mag(const LSM303D_mag&);
+	LSM303D_mag operator=(const LSM303D_mag&);
 };
 
 
 LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation) :
 	SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */),
 	_mag(new LSM303D_mag(this)),
+	_accel_call{},
+	_mag_call{},
 	_call_accel_interval(0),
 	_call_mag_interval(0),
 	_accel_reports(nullptr),
 	_mag_reports(nullptr),
+	_accel_scale{},
 	_accel_range_m_s2(0.0f),
 	_accel_range_scale(0.0f),
 	_accel_samplerate(0),
 	_accel_onchip_filter_bandwith(0),
+	_mag_scale{},
 	_mag_range_ga(0.0f),
 	_mag_range_scale(0.0f),
 	_mag_samplerate(0),
 	_accel_topic(-1),
+	_accel_orb_id(nullptr),
 	_accel_class_instance(-1),
 	_accel_read(0),
 	_mag_read(0),
@@ -524,6 +539,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
 	_reg7_expected(0),
 	_accel_log_fd(-1),
 	_accel_logging_enabled(false),
+	_last_extreme_us(0),
 	_last_log_us(0),
 	_last_log_sync_us(0),
 	_last_log_reg_us(0),
@@ -611,34 +627,56 @@ LSM303D::init()
 	/* fill report structures */
 	measure();
 
-	if (_mag->_mag_class_instance == CLASS_DEVICE_PRIMARY) {
+	/* advertise sensor topic, measure manually to initialize valid report */
+	struct mag_report mrp;
+	_mag_reports->get(&mrp);
 
-		/* advertise sensor topic, measure manually to initialize valid report */
-		struct mag_report mrp;
-		_mag_reports->get(&mrp);
+	/* measurement will have generated a report, publish */
+	switch (_mag->_mag_class_instance) {
+		case CLASS_DEVICE_PRIMARY:
+			_mag->_mag_orb_id = ORB_ID(sensor_mag0);
+			break;
 
-		/* measurement will have generated a report, publish */
-		_mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mrp);
+		case CLASS_DEVICE_SECONDARY:
+			_mag->_mag_orb_id = ORB_ID(sensor_mag1);
+			break;
 
-		if (_mag->_mag_topic < 0)
-			debug("failed to create sensor_mag publication");
+		case CLASS_DEVICE_TERTIARY:
+			_mag->_mag_orb_id = ORB_ID(sensor_mag2);
+			break;
+	}
 
+	_mag->_mag_topic = orb_advertise(_mag->_mag_orb_id, &mrp);
+
+	if (_mag->_mag_topic < 0) {
+		warnx("ADVERT ERR");
 	}
 
 	_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
 
-	if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
+	/* advertise sensor topic, measure manually to initialize valid report */
+	struct accel_report arp;
+	_accel_reports->get(&arp);
 
-		/* advertise sensor topic, measure manually to initialize valid report */
-		struct accel_report arp;
-		_accel_reports->get(&arp);
+	/* measurement will have generated a report, publish */
+	switch (_accel_class_instance) {
+		case CLASS_DEVICE_PRIMARY:
+			_accel_orb_id = ORB_ID(sensor_accel0);
+			break;
 
-		/* measurement will have generated a report, publish */
-		_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
+		case CLASS_DEVICE_SECONDARY:
+			_accel_orb_id = ORB_ID(sensor_accel1);
+			break;
 
-		if (_accel_topic < 0)
-			debug("failed to create sensor_accel publication");
+		case CLASS_DEVICE_TERTIARY:
+			_accel_orb_id = ORB_ID(sensor_accel2);
+			break;
+	}
 
+	_accel_topic = orb_advertise(_accel_orb_id, &arp);
+
+	if (_accel_topic < 0) {
+		warnx("ADVERT ERR");
 	}
 
 out:
@@ -1557,9 +1595,9 @@ LSM303D::measure()
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
-	if (_accel_topic > 0 && !(_pub_blocked)) {
+	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
+		orb_publish(_accel_orb_id, _accel_topic, &accel_report);
 	}
 
 	_accel_read++;
@@ -1634,9 +1672,9 @@ LSM303D::mag_measure()
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
-	if (_mag->_mag_topic > 0 && !(_pub_blocked)) {
+	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
+		orb_publish(_mag->_mag_orb_id, _mag->_mag_topic, &mag_report);
 	}
 
 	_mag_read++;
@@ -1730,6 +1768,7 @@ LSM303D_mag::LSM303D_mag(LSM303D *parent) :
 	CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG),
 	_parent(parent),
 	_mag_topic(-1),
+	_mag_orb_id(nullptr),
 	_mag_class_instance(-1)
 {
 }
@@ -1803,6 +1842,9 @@ void	usage();
 
 /**
  * Start the driver.
+ *
+ * This function call only returns once the driver is
+ * up and running or failed to detect the sensor.
  */
 void
 start(bool external_bus, enum Rotation rotation)

src/drivers/lsm303d/module.mk

@@ -5,4 +5,6 @@
 MODULE_COMMAND	 = lsm303d
 SRCS		 = lsm303d.cpp
 
+MODULE_STACKSIZE	= 1200
 
+EXTRACXXFLAGS	= -Weffc++

src/drivers/meas_airspeed/meas_airspeed.cpp

@@ -140,9 +140,9 @@ extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
 MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address,
 	CONVERSION_INTERVAL, path),
 	_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ),
-	_t_system_power(-1)
+	_t_system_power(-1),
+	system_power{}
 {
-	memset(&system_power, 0, sizeof(system_power));
 }
 
 int
@@ -420,6 +420,9 @@ void	info();
 
 /**
  * Start the driver.
+ *
+ * This function call only returns once the driver is up and running
+ * or failed to detect the sensor.
  */
 void
 start(int i2c_bus)

src/drivers/meas_airspeed/module.mk

@@ -36,6 +36,9 @@
 #
 
 MODULE_COMMAND		= meas_airspeed
-MODULE_STACKSIZE	= 2048
 
 SRCS			= meas_airspeed.cpp
+
+MODULE_STACKSIZE	= 1200
+
+EXTRACXXFLAGS	= -Weffc++

src/drivers/mpu6000/module.mk

@@ -37,7 +37,8 @@
 
 MODULE_COMMAND	= mpu6000
 
-# XXX seems excessive, check if 2048 is not sufficient
-MODULE_STACKSIZE	 = 4096
-
 SRCS		= mpu6000.cpp
+
+MODULE_STACKSIZE	= 1200
+
+EXTRACXXFLAGS	= -Weffc++

src/drivers/mpu6000/mpu6000.cpp

@@ -215,6 +215,7 @@ private:
 	float			_accel_range_scale;
 	float			_accel_range_m_s2;
 	orb_advert_t		_accel_topic;
+	orb_id_t		_accel_orb_id;
 	int			_accel_class_instance;
 
 	RingBuffer		*_gyro_reports;
@@ -343,6 +344,9 @@ private:
 	*/
 	void _set_sample_rate(uint16_t desired_sample_rate_hz);
 
+	/* do not allow to copy this class due to pointer data members */
+	MPU6000(const MPU6000&);
+	MPU6000 operator=(const MPU6000&);
 };
 
 /**
@@ -367,8 +371,12 @@ protected:
 private:
 	MPU6000			*_parent;
 	orb_advert_t		_gyro_topic;
+	orb_id_t		_gyro_orb_id;
 	int			_gyro_class_instance;
 
+	/* do not allow to copy this class due to pointer data members */
+	MPU6000_gyro(const MPU6000_gyro&);
+	MPU6000_gyro operator=(const MPU6000_gyro&);
 };
 
 /** driver 'main' command */
@@ -378,13 +386,17 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
 	SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
 	_gyro(new MPU6000_gyro(this, path_gyro)),
 	_product(0),
+	_call{},
 	_call_interval(0),
 	_accel_reports(nullptr),
+	_accel_scale{},
 	_accel_range_scale(0.0f),
 	_accel_range_m_s2(0.0f),
 	_accel_topic(-1),
+	_accel_orb_id(nullptr),
 	_accel_class_instance(-1),
 	_gyro_reports(nullptr),
+	_gyro_scale{},
 	_gyro_range_scale(0.0f),
 	_gyro_range_rad_s(0.0f),
 	_sample_rate(1000),
@@ -498,31 +510,56 @@ MPU6000::init()
 
 	measure();
 
-	if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
+	/* advertise sensor topic, measure manually to initialize valid report */
+	struct accel_report arp;
+	_accel_reports->get(&arp);
 
-		/* advertise sensor topic, measure manually to initialize valid report */
-		struct accel_report arp;
-		_accel_reports->get(&arp);
+	/* measurement will have generated a report, publish */
+	switch (_accel_class_instance) {
+		case CLASS_DEVICE_PRIMARY:
+			_accel_orb_id = ORB_ID(sensor_accel0);
+			break;
+		
+		case CLASS_DEVICE_SECONDARY:
+			_accel_orb_id = ORB_ID(sensor_accel1);
+			break;
 
-		/* measurement will have generated a report, publish */
-		_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
-
-		if (_accel_topic < 0)
-			debug("failed to create sensor_accel publication");
+		case CLASS_DEVICE_TERTIARY:
+			_accel_orb_id = ORB_ID(sensor_accel2);
+			break;
 
 	}
 
-	if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY) {
+	_accel_topic = orb_advertise(_accel_orb_id, &arp);
 
-		/* advertise sensor topic, measure manually to initialize valid report */
-		struct gyro_report grp;
-		_gyro_reports->get(&grp);
+	if (_accel_topic < 0) {
+		warnx("ADVERT FAIL");
+	}
 
-		_gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp);
 
-		if (_gyro->_gyro_topic < 0)
-			debug("failed to create sensor_gyro publication");
+	/* advertise sensor topic, measure manually to initialize valid report */
+	struct gyro_report grp;
+	_gyro_reports->get(&grp);
 
+	switch (_gyro->_gyro_class_instance) {
+		case CLASS_DEVICE_PRIMARY:
+			_gyro->_gyro_orb_id = ORB_ID(sensor_gyro0);
+			break;
+
+		case CLASS_DEVICE_SECONDARY:
+			_gyro->_gyro_orb_id = ORB_ID(sensor_gyro1);
+			break;
+
+		case CLASS_DEVICE_TERTIARY:
+			_gyro->_gyro_orb_id = ORB_ID(sensor_gyro2);
+			break;
+
+	}
+
+	_gyro->_gyro_topic = orb_advertise(_gyro->_gyro_orb_id, &grp);
+
+	if (_gyro->_gyro_topic < 0) {
+		warnx("ADVERT FAIL");
 	}
 
 out:
@@ -1345,14 +1382,14 @@ MPU6000::measure()
 	poll_notify(POLLIN);
 	_gyro->parent_poll_notify();
 
-	if (_accel_topic > 0 && !(_pub_blocked)) {
+	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
+		orb_publish(_accel_orb_id, _accel_topic, &arb);
 	}
 
-	if (_gyro->_gyro_topic > 0 && !(_pub_blocked)) {
+	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
+		orb_publish(_gyro->_gyro_orb_id, _gyro->_gyro_topic, &grb);
 	}
 
 	/* stop measuring */
@@ -1373,6 +1410,7 @@ MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
 	CDev("MPU6000_gyro", path),
 	_parent(parent),
 	_gyro_topic(-1),
+	_gyro_orb_id(nullptr),
 	_gyro_class_instance(-1)
 {
 }
@@ -1437,6 +1475,9 @@ void	usage();
 
 /**
  * Start the driver.
+ *
+ * This function only returns if the driver is up and running
+ * or failed to detect the sensor.
  */
 void
 start(bool external_bus, enum Rotation rotation)
@@ -1507,7 +1548,7 @@ test(bool external_bus)
 	int fd = open(path_accel, O_RDONLY);
 
 	if (fd < 0)
-		err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
+		err(1, "%s open failed (try 'mpu6000 start')",
 		    path_accel);
 
 	/* get the driver */

src/drivers/ms5611/ms5611.cpp

@@ -300,12 +300,17 @@ MS5611::init()
 
 		ret = OK;
 
-		if (_class_instance == CLASS_DEVICE_PRIMARY) {
+		switch (_class_instance) {
+			case CLASS_DEVICE_PRIMARY:
+				_baro_topic = orb_advertise(ORB_ID(sensor_baro0), &brp);
+				break;
+			case CLASS_DEVICE_SECONDARY:
+				_baro_topic = orb_advertise(ORB_ID(sensor_baro1), &brp);
+				break;
+		}
 
-			_baro_topic = orb_advertise(ORB_ID(sensor_baro), &brp);
-
-			if (_baro_topic < 0)
-				debug("failed to create sensor_baro publication");
+		if (_baro_topic < 0) {
+			warnx("failed to create sensor_baro publication");
 		}
 
 	} while (0);
@@ -722,9 +727,17 @@ MS5611::collect()
 		report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
 
 		/* publish it */
-		if (_baro_topic > 0 && !(_pub_blocked)) {
+		if (!(_pub_blocked)) {
 			/* publish it */
-			orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
+			switch (_class_instance) {
+				case CLASS_DEVICE_PRIMARY:
+					orb_publish(ORB_ID(sensor_baro0), _baro_topic, &report);
+					break;
+
+				case CLASS_DEVICE_SECONDARY:
+					orb_publish(ORB_ID(sensor_baro1), _baro_topic, &report);
+					break;
+			}
 		}
 
 		if (_reports->force(&report)) {

src/drivers/pca8574/module.mk

@@ -4,3 +4,5 @@
 
 MODULE_COMMAND	 = pca8574
 SRCS		 = pca8574.cpp
+
+MAXOPTIMIZATION	 = -Os

src/drivers/px4fmu/fmu.cpp

@@ -179,6 +179,9 @@ private:
 	uint32_t	gpio_read(void);
 	int		gpio_ioctl(file *filp, int cmd, unsigned long arg);
 
+	/* do not allow to copy due to ptr data members */
+	PX4FMU(const PX4FMU&);
+	PX4FMU operator=(const PX4FMU&);
 };
 
 const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
@@ -242,6 +245,7 @@ PX4FMU::PX4FMU() :
 	_task(-1),
 	_armed_sub(-1),
 	_outputs_pub(-1),
+	_armed{},
 	_num_outputs(0),
 	_primary_pwm_device(false),
 	_task_should_exit(false),
@@ -252,6 +256,7 @@ PX4FMU::PX4FMU() :
 	_groups_subscribed(0),
 	_control_subs{-1},
 	_poll_fds_num(0),
+	_pwm_limit{},
 	_failsafe_pwm{0},
 	_disarmed_pwm{0},
 	_num_failsafe_set(0),

src/drivers/px4fmu/module.mk

@@ -6,3 +6,5 @@ MODULE_COMMAND	 = fmu
 SRCS		 = fmu.cpp
 
 MODULE_STACKSIZE = 1200
+
+EXTRACXXFLAGS	= -Weffc++

src/drivers/px4io/module.mk

@@ -46,3 +46,5 @@ SRCS			= px4io.cpp \
 INCLUDE_DIRS    += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
 
 MODULE_STACKSIZE = 1200
+
+EXTRACXXFLAGS	= -Weffc++

src/drivers/px4io/px4io.cpp

@@ -453,6 +453,9 @@ private:
 	 */
 	void			io_handle_vservo(uint16_t vservo, uint16_t vrssi);
 
+	/* do not allow to copy this class due to ptr data members */
+	PX4IO(const PX4IO&);
+	PX4IO operator=(const PX4IO&);
 };
 
 namespace
@@ -496,6 +499,8 @@ PX4IO::PX4IO(device::Device *interface) :
 	_to_battery(0),
 	_to_servorail(0),
 	_to_safety(0),
+	_outputs{},
+	_servorail_status{},
 	_primary_pwm_device(false),
 	_lockdown_override(false),
 	_battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor

src/drivers/px4io/px4io_uploader.cpp

@@ -204,12 +204,8 @@ PX4IO_Uploader::upload(const char *filenames[])
 
 		if (bl_rev <= 2) {
 			ret = verify_rev2(fw_size);
-		} else if(bl_rev == 3) {
-			ret = verify_rev3(fw_size);
 		} else {
-			/* verify rev 4 and higher still uses the same approach and
-			 * every version *needs* to be verified.
-			 */
+			/* verify rev 3 and higher. Every version *needs* to be verified. */
 			ret = verify_rev3(fw_size);
 		}
 
@@ -276,14 +272,14 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
 int
 PX4IO_Uploader::recv(uint8_t *p, unsigned count)
 {
+	int ret;
 	while (count--) {
-		int ret = recv(*p++, 5000);
+		ret = recv(*p++, 5000);
 
 		if (ret != OK)
-			return ret;
+			break;
 	}
-
-	return OK;
+	return ret;
 }
 
 void
@@ -314,21 +310,19 @@ PX4IO_Uploader::send(uint8_t c)
 #endif
 	if (write(_io_fd, &c, 1) != 1)
 		return -errno;
-
 	return OK;
 }
 
 int
 PX4IO_Uploader::send(uint8_t *p, unsigned count)
 {
+	int ret;
 	while (count--) {
-		int ret = send(*p++);
-
+		ret = send(*p++);
 		if (ret != OK)
-			return ret;
+			break;
 	}
-
-	return OK;
+	return ret;
 }
 
 int
@@ -419,12 +413,15 @@ PX4IO_Uploader::program(size_t fw_size)
 	int ret;
 	size_t sent = 0;
 
-	file_buf = (uint8_t *)malloc(PROG_MULTI_MAX);
+	file_buf = new uint8_t[PROG_MULTI_MAX];
 	if (!file_buf) {
 		log("Can't allocate program buffer");
 		return -ENOMEM;
 	}
 
+	ASSERT((fw_size & 3) == 0);
+	ASSERT((PROG_MULTI_MAX & 3) == 0);
+
 	log("programming %u bytes...", (unsigned)fw_size);
 
 	ret = lseek(_fw_fd, 0, SEEK_SET);
@@ -443,34 +440,26 @@ PX4IO_Uploader::program(size_t fw_size)
 			    (unsigned)sent,
 			    (int)count,
 			    (int)errno);
-		}
-
-		if (count == 0) {
-			free(file_buf);
-			return OK;
+			ret = -errno;
+			break;
 		}
 
 		sent += count;
 
-		if (count < 0)
-			return -errno;
-
-		ASSERT((count % 4) == 0);
-
 		send(PROTO_PROG_MULTI);
 		send(count);
-		send(&file_buf[0], count);
+		send(file_buf, count);
 		send(PROTO_EOC);
 
 		ret = get_sync(1000);
 
 		if (ret != OK) {
-			free(file_buf);
-			return ret;
+			break;
 		}
 	}
-	free(file_buf);
-	return OK;
+
+	delete [] file_buf;
+	return ret;
 }
 
 int

src/drivers/px4io/uploader.h

@@ -75,7 +75,6 @@ private:
 		INFO_FLASH_SIZE		= 4,		/**< max firmware size in bytes */
 
 		PROG_MULTI_MAX		= 60,		/**< protocol max is 255, must be multiple of 4 */
-		READ_MULTI_MAX		= 60,		/**< protocol max is 255, something overflows with >= 64 */
 
 	};
 

src/drivers/rgbled/module.mk

@@ -4,3 +4,5 @@
 
 MODULE_COMMAND	 = rgbled
 SRCS		 = rgbled.cpp
+
+MAXOPTIMIZATION	 = -Os

src/examples/matlab_csv_serial/matlab_csv_serial.c

@@ -0,0 +1,247 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.		 
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file matlab_csv_serial_main.c
+ *
+ * Matlab CSV / ASCII format interface at 921600 baud, 8 data bits,
+ * 1 stop bit, no parity
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <float.h>
+#include <nuttx/sched.h>
+#include <sys/prctl.h>
+#include <drivers/drv_hrt.h>
+#include <termios.h>
+#include <errno.h>
+#include <limits.h>
+#include <math.h>
+#include <uORB/uORB.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+#include <poll.h>
+
+__EXPORT int matlab_csv_serial_main(int argc, char *argv[]);
+static bool thread_should_exit = false;		/**< Daemon exit flag */
+static bool thread_running = false;		/**< Daemon status flag */
+static int daemon_task;				/**< Handle of daemon task / thread */
+
+int matlab_csv_serial_thread_main(int argc, char *argv[]);
+static void usage(const char *reason);
+
+static void usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+	fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
+	exit(1);
+}
+
+/**
+ * The daemon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_spawn_cmd().
+ */
+int matlab_csv_serial_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start"))
+	{
+		if (thread_running)
+		{
+			warnx("already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		daemon_task = task_spawn_cmd("matlab_csv_serial",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 5,
+					 2000,
+					 matlab_csv_serial_thread_main,
+					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop"))
+	{
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status"))
+	{
+		if (thread_running) {
+			warnx("running");
+		} else {
+			warnx("stopped");
+		}
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+int matlab_csv_serial_thread_main(int argc, char *argv[])
+{
+
+	if (argc < 2) {
+		errx(1, "need a serial port name as argument");
+	}
+
+	const char* uart_name = argv[1];
+
+	warnx("opening port %s", uart_name);
+
+	int serial_fd = open(uart_name, O_RDWR | O_NOCTTY);
+
+	unsigned speed = 921600;
+
+	if (serial_fd < 0) {
+		err(1, "failed to open port: %s", uart_name);
+	}
+
+	/* Try to set baud rate */
+	struct termios uart_config;
+	int termios_state;
+
+	/* Back up the original uart configuration to restore it after exit */
+	if ((termios_state = tcgetattr(serial_fd, &uart_config)) < 0) {
+		warnx("ERR GET CONF %s: %d\n", uart_name, termios_state);
+		close(serial_fd);
+		return -1;
+	}
+
+	/* Clear ONLCR flag (which appends a CR for every LF) */
+	uart_config.c_oflag &= ~ONLCR;
+
+	/* USB serial is indicated by /dev/ttyACM0*/
+	if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) {
+
+		/* Set baud rate */
+		if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
+			warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state);
+			close(serial_fd);
+			return -1;
+		}
+
+	}
+
+	if ((termios_state = tcsetattr(serial_fd, TCSANOW, &uart_config)) < 0) {
+		warnx("ERR SET CONF %s\n", uart_name);
+		close(serial_fd);
+		return -1;
+	}
+
+	/* subscribe to vehicle status, attitude, sensors and flow*/
+	struct accel_report accel0;
+	struct accel_report accel1;
+	struct gyro_report gyro0;
+	struct gyro_report gyro1;
+
+	/* subscribe to parameter changes */
+	int accel0_sub = orb_subscribe(ORB_ID(sensor_accel0));
+	int accel1_sub = orb_subscribe(ORB_ID(sensor_accel1));
+	int gyro0_sub = orb_subscribe(ORB_ID(sensor_gyro0));
+	int gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1));
+
+	thread_running = true;
+
+	while (!thread_should_exit)
+	{
+
+		/*This runs at the rate of the sensors */
+		struct pollfd fds[] = {
+				{ .fd = accel0_sub, .events = POLLIN }
+		};
+
+		/* wait for a sensor update, check for exit condition every 500 ms */
+		int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 500);
+
+		if (ret < 0)
+		{
+			/* poll error, ignore */
+
+		}
+		else if (ret == 0)
+		{
+			/* no return value, ignore */
+			warnx("no sensor data");
+		}
+		else
+		{
+
+			/* accel0 update available? */
+			if (fds[0].revents & POLLIN)
+			{
+				orb_copy(ORB_ID(sensor_accel0), accel0_sub, &accel0);
+				orb_copy(ORB_ID(sensor_accel1), accel1_sub, &accel1);
+				orb_copy(ORB_ID(sensor_gyro0), gyro0_sub, &gyro0);
+				orb_copy(ORB_ID(sensor_gyro1), gyro1_sub, &gyro1);
+
+				// write out on accel 0, but collect for all other sensors as they have updates
+				dprintf(serial_fd, "%llu,%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x_raw, (int)accel0.y_raw, (int)accel0.z_raw,
+					(int)accel1.x_raw, (int)accel1.y_raw, (int)accel1.z_raw);
+			}
+
+		}
+	}
+
+	warnx("exiting");
+	thread_running = false;
+
+	fflush(stdout);
+	return 0;
+}
+
+

src/examples/matlab_csv_serial/module.mk

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Build position estimator
+#
+
+MODULE_COMMAND		= matlab_csv_serial
+
+SRCS		= matlab_csv_serial.c

src/lib/mathlib/math/Matrix.hpp

@@ -69,27 +69,34 @@ public:
 
 	/**
 	 * trivial ctor
-	 * note that this ctor will not initialize elements
+	 * Initializes the elements to zero.
 	 */
-	MatrixBase() {
-		arm_mat = {M, N, &data[0][0]};
+	MatrixBase() : 
+		data{},
+		arm_mat{M, N, &data[0][0]}
+	{
 	}
 
+	virtual ~MatrixBase() {};
+
 	/**
 	 * copyt ctor
 	 */
-	MatrixBase(const MatrixBase<M, N> &m) {
-		arm_mat = {M, N, &data[0][0]};
+	MatrixBase(const MatrixBase<M, N> &m) :
+		arm_mat{M, N, &data[0][0]}
+	{
 		memcpy(data, m.data, sizeof(data));
 	}
 
-	MatrixBase(const float *d) {
-		arm_mat = {M, N, &data[0][0]};
+	MatrixBase(const float *d) :
+		arm_mat{M, N, &data[0][0]}
+	{
 		memcpy(data, d, sizeof(data));
 	}
 
-	MatrixBase(const float d[M][N]) {
-		arm_mat = {M, N, &data[0][0]};
+	MatrixBase(const float d[M][N]) : 
+		arm_mat{M, N, &data[0][0]}
+	{
 		memcpy(data, d, sizeof(data));
 	}
 

src/lib/mathlib/math/Vector.hpp

@@ -69,25 +69,32 @@ public:
 
 	/**
 	 * trivial ctor
-	 * note that this ctor will not initialize elements
+	 * initializes elements to zero
 	 */
-	VectorBase() {
-		arm_col = {N, 1, &data[0]};
+	VectorBase() :
+		data{},
+		arm_col{N, 1, &data[0]}
+	{
+
 	}
 
+	virtual ~VectorBase() {};
+
 	/**
 	 * copy ctor
 	 */
-	VectorBase(const VectorBase<N> &v) {
-		arm_col = {N, 1, &data[0]};
+	VectorBase(const VectorBase<N> &v) :
+		arm_col{N, 1, &data[0]}
+	{
 		memcpy(data, v.data, sizeof(data));
 	}
 
 	/**
 	 * setting ctor
 	 */
-	VectorBase(const float d[N]) {
-		arm_col = {N, 1, &data[0]};
+	VectorBase(const float d[N]) :
+		arm_col{N, 1, &data[0]}
+	{
 		memcpy(data, d, sizeof(data));
 	}
 

src/lib/mathlib/math/filter/LowPassFilter2p.hpp

@@ -46,10 +46,18 @@ class __EXPORT LowPassFilter2p
 {
 public:
     // constructor
-    LowPassFilter2p(float sample_freq, float cutoff_freq) {
+    LowPassFilter2p(float sample_freq, float cutoff_freq) : 
+        _cutoff_freq(cutoff_freq),
+        _a1(0.0f),
+        _a2(0.0f),
+        _b0(0.0f),
+        _b1(0.0f),
+        _b2(0.0f),
+        _delay_element_1(0.0f),
+        _delay_element_2(0.0f)
+    {
         // set initial parameters
         set_cutoff_frequency(sample_freq, cutoff_freq);
-        _delay_element_1 = _delay_element_2 = 0;
     }
 
     /**

src/modules/commander/commander.cpp

@@ -1463,7 +1463,7 @@ int commander_thread_main(int argc, char *argv[])
 		/* data links check */
 		bool have_link = false;
 		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
-			if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
+			if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
 				/* handle the case where data link was regained */
 				if (telemetry_lost[i]) {
 					mavlink_log_critical(mavlink_fd, "data link %i regained", i);

src/modules/commander/gyro_calibration.cpp

@@ -92,7 +92,7 @@ int do_gyro_calibration(int mavlink_fd)
 		unsigned poll_errcount = 0;
 
 		/* subscribe to gyro sensor topic */
-		int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro));
+		int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro0));
 		struct gyro_report gyro_report;
 
 		while (calibration_counter < calibration_count) {
@@ -104,7 +104,7 @@ int do_gyro_calibration(int mavlink_fd)
 			int poll_ret = poll(fds, 1, 1000);
 
 			if (poll_ret > 0) {
-				orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);
+				orb_copy(ORB_ID(sensor_gyro0), sub_sensor_gyro, &gyro_report);
 				gyro_scale.x_offset += gyro_report.x;
 				gyro_scale.y_offset += gyro_report.y;
 				gyro_scale.z_offset += gyro_report.z;

src/modules/commander/mag_calibration.cpp

@@ -145,7 +145,7 @@ int do_mag_calibration(int mavlink_fd)
 	}
 
 	if (res == OK) {
-		int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
+		int sub_mag = orb_subscribe(ORB_ID(sensor_mag0));
 		struct mag_report mag;
 
 		/* limit update rate to get equally spaced measurements over time (in ms) */
@@ -170,7 +170,7 @@ int do_mag_calibration(int mavlink_fd)
 			int poll_ret = poll(fds, 1, 1000);
 
 			if (poll_ret > 0) {
-				orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
+				orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag);
 
 				x[calibration_counter] = mag.x;
 				y[calibration_counter] = mag.y;

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -112,6 +112,10 @@ public:
 	 */
 	FixedwingEstimator();
 
+	/* we do not want people ever copying this class */
+	FixedwingEstimator(const FixedwingEstimator& that) = delete;
+	FixedwingEstimator operator=(const FixedwingEstimator&) = delete;
+
 	/**
 	 * Destructor, also kills the sensors task.
 	 */
@@ -371,9 +375,10 @@ FixedwingEstimator::FixedwingEstimator() :
 	_mag_offsets({}),
 
 	#ifdef SENSOR_COMBINED_SUB
-	_sensor_combined({}),
+	_sensor_combined{},
 	#endif
 
+	_pos_ref{},
 	_baro_ref(0.0f),
 	_baro_ref_offset(0.0f),
 	_baro_gps_offset(0.0f),
@@ -390,12 +395,18 @@ FixedwingEstimator::FixedwingEstimator() :
 /* states */
 	_baro_init(false),
 	_gps_initialized(false),
+	_gps_start_time(0),
+	_filter_start_time(0),
+	_last_sensor_timestamp(0),
+	_last_run(0),
 	_gyro_valid(false),
 	_accel_valid(false),
 	_mag_valid(false),
 	_ekf_logging(true),
 	_debug(0),
 	_mavlink_fd(-1),
+	_parameters{},
+	_parameter_handles{},
 	_ekf(nullptr),
 	_velocity_xy_filtered(0.0f),
 	_velocity_z_filtered(0.0f),
@@ -693,7 +704,7 @@ FixedwingEstimator::task_main()
 	/*
 	 * do subscriptions
 	 */
-	_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
+	_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -1041,7 +1052,7 @@ FixedwingEstimator::task_main()
 			orb_check(_baro_sub, &baro_updated);
 
 			if (baro_updated) {
-				orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
+				orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
 
 				_ekf->baroHgt = _baro.altitude;
 
@@ -1133,7 +1144,7 @@ FixedwingEstimator::task_main()
 					initVelNED[2] = _gps.vel_d_m_s;
 
 					// Set up height correctly
-					orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
+					orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
 					_baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame
 					_baro_gps_offset = _baro.altitude - gps_alt;
 					_ekf->baroHgt = _baro.altitude;

src/modules/ekf_att_pos_estimator/estimator_utilities.cpp

@@ -45,8 +45,11 @@ void Vector3f::zero(void)
     z = 0.0f;
 }
 
-Mat3f::Mat3f() {
-    identity();
+Mat3f::Mat3f() :
+    x{1.0f, 0.0f, 0.0f},
+    y{0.0f, 1.0f, 0.0f},
+    z{0.0f, 0.0f, 1.0f}
+{
 }
 
 void Mat3f::identity() {

src/modules/ekf_att_pos_estimator/module.mk

@@ -41,3 +41,5 @@ SRCS		= ekf_att_pos_estimator_main.cpp \
 		  ekf_att_pos_estimator_params.c \
 		  estimator_23states.cpp \
 		  estimator_utilities.cpp
+
+EXTRACXXFLAGS	= -Weffc++

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -529,7 +529,7 @@ FixedwingAttitudeControl::vehicle_accel_poll()
 	orb_check(_accel_sub, &accel_updated);
 
 	if (accel_updated) {
-		orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
+		orb_copy(ORB_ID(sensor_accel0), _accel_sub, &_accel);
 	}
 }
 
@@ -577,7 +577,7 @@ FixedwingAttitudeControl::task_main()
 	 */
 	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
+	_accel_sub = orb_subscribe(ORB_ID(sensor_accel0));
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));

src/modules/mavlink/mavlink_commands.cpp

@@ -40,9 +40,12 @@
 
 #include "mavlink_commands.h"
 
-MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel), _cmd_time(0)
+MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) :
+	_cmd_sub(mavlink->add_orb_subscription(ORB_ID(vehicle_command))),
+	_cmd{},
+	_channel(channel),
+	_cmd_time(0)
 {
-	_cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command));
 }
 
 void

src/modules/mavlink/mavlink_ftp.cpp

@@ -50,16 +50,20 @@ MavlinkFTP::getServer()
 	return _server;
 }
 
-MavlinkFTP::MavlinkFTP()
+MavlinkFTP::MavlinkFTP() :
+	_session_fds{},
+	_workBufs{},
+	_workFree{},
+	_lock{}
 {
 	// initialise the request freelist
 	dq_init(&_workFree);
 	sem_init(&_lock, 0, 1);
-    
-    // initialize session list
-    for (size_t i=0; i<kMaxSession; i++) {
-        _session_fds[i] = -1;
-    }
+
+	// initialize session list
+	for (size_t i=0; i<kMaxSession; i++) {
+		_session_fds[i] = -1;
+	}
 
 	// drop work entries onto the free list
 	for (unsigned i = 0; i < kRequestQueueSize; i++) {

src/modules/mavlink/mavlink_main.cpp

@@ -216,6 +216,7 @@ Mavlink::Mavlink() :
 	_device_name(DEFAULT_DEVICE_NAME),
 	_task_should_exit(false),
 	next(nullptr),
+	_instance_id(0),
 	_mavlink_fd(-1),
 	_task_running(false),
 	_hil_enabled(false),
@@ -231,17 +232,24 @@ Mavlink::Mavlink() :
 	_mission_pub(-1),
 	_mission_result_sub(-1),
 	_mode(MAVLINK_MODE_NORMAL),
+	_channel(MAVLINK_COMM_0),
+	_logbuffer{},
 	_total_counter(0),
+	_receive_thread{},
 	_verbose(false),
 	_forwarding_on(false),
 	_passing_on(false),
 	_ftp_on(false),
 	_uart_fd(-1),
+	_baudrate(57600),
+	_datarate(10000),
 	_mavlink_param_queue_index(0),
+	mavlink_link_termination_allowed(false),
 	_subscribe_to_stream(nullptr),
 	_subscribe_to_stream_rate(0.0f),
 	_flow_control_enabled(true),
-	_message_buffer({}),
+	_message_buffer{},
+	_message_buffer_mutex{},
 	_param_initialized(false),
 	_param_system_id(0),
 	_param_component_id(0),