Merge branch 'local/c++_sensors' into px4dev_new_driver

Debug/NuttX

@@ -95,6 +95,7 @@ end
 document showfiles
 .    showfiles <TCB pointer>
 .        Prints the files opened by a task.
+end
 
 ################################################################################
 # Task display

apps/drivers/device/i2c.cpp

@@ -99,7 +99,7 @@ I2C::init()
 	}
 
 	// tell the world where we are
-	log("on bus %d at 0x%02x", _bus, _address);
+	log("on I2C bus %d at 0x%02x", _bus, _address);
 
 out:
 	return ret;

apps/drivers/device/spi.cpp

@@ -111,7 +111,7 @@ SPI::init()
 	}
 
 	// tell the workd where we are
-	log("on bus %d at %d", _bus, _device);
+	log("on SPI bus %d at %d", _bus, _device);
 
 out:
 	return ret;

apps/drivers/hmc5883/hmc5883.cpp

@@ -147,12 +147,9 @@ private:
 
 	orb_advert_t		_mag_topic;
 
-	unsigned		_reads;
-	unsigned		_measure_errors;
-	unsigned		_read_errors;
-	unsigned		_buf_overflows;
-
 	perf_counter_t		_sample_perf;
+	perf_counter_t		_comms_errors;
+	perf_counter_t		_buffer_overflows;
 
 	/**
 	 * Test whether the device supported by the driver is present at a
@@ -256,11 +253,9 @@ HMC5883::HMC5883(int bus) :
 	_oldest_report(0),
 	_reports(nullptr),
 	_mag_topic(-1),
-	_reads(0),
+	_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
-	_measure_errors(0),
+	_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
-	_read_errors(0),
+	_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows"))
-	_buf_overflows(0),
-	_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read"))
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -303,6 +298,12 @@ HMC5883::init()
 		goto out;
 	_oldest_report = _next_report = 0;
 
+	/* get a publish handle on the mag topic */
+	memset(&_reports[0], 0, sizeof(_reports[0]));
+	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_reports[0]);
+	if (_mag_topic < 0)
+		debug("failed to create sensor_mag object");
+
 	ret = OK;
 out:
 	return ret;
@@ -313,7 +314,7 @@ HMC5883::probe()
 {
 	uint8_t data[3] = {0, 0, 0};
 
-	_retries = 3;
+	_retries = 10;
 	if (read_reg(ADDR_ID_A, data[0]) ||
 	    read_reg(ADDR_ID_B, data[1]) ||
 	    read_reg(ADDR_ID_C, data[2]))
@@ -356,8 +357,6 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
 			}
 		}
 
-		_reads++;
-
 		/* if there was no data, warn the caller */
 		return ret ? ret : -EAGAIN;
 	}
@@ -385,7 +384,6 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
 		/* state machine will have generated a report, copy it out */
 		memcpy(buffer, _reports, sizeof(*_reports));
 		ret = sizeof(*_reports);
-		_reads++;
 
 	} while (0);
 
@@ -548,31 +546,13 @@ HMC5883::cycle_trampoline(void *arg)
 void
 HMC5883::cycle()
 {
-	/*
-	 * We have to publish the mag topic in the context of the workq
-	 * in order to ensure that the descriptor is valid when we go to publish.
-	 *
-	 * @bug	We can't really ever be torn down and restarted, since this
-	 *      descriptor will never be closed and on the restart we will be
-	 *      unable to re-advertise.
-	 */
-	if (_mag_topic == -1) {
-		struct mag_report m;
-
-		/* if this fails (e.g. no object in the system) we will cope */
-		memset(&m, 0, sizeof(m));
-		_mag_topic = orb_advertise(ORB_ID(sensor_mag), &m);
-
-		if (_mag_topic < 0)
-			debug("failed to create sensor_mag object");
-	}
-
 	/* collection phase? */
 	if (_collect_phase) {
 
 		/* perform collection */
 		if (OK != collect()) {
-			log("FATAL collection error - restarting\n");
+			log("collection error");
+			/* restart the measurement state machine */
 			start();
 			return;
 		}
@@ -596,10 +576,8 @@ HMC5883::cycle()
 	}
 
 	/* measurement phase */
-	if (OK != measure()) {
+	if (OK != measure())
-		log("FATAL measure error - restarting\n");
+		log("measure error");
-		start();
-	}
 
 	/* next phase is collection */
 	_collect_phase = true;
@@ -622,7 +600,7 @@ HMC5883::measure()
 	ret = write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE);
 
 	if (OK != ret)
-		_measure_errors++;
+		perf_count(_comms_errors);
 
 	return ret;
 }
@@ -661,6 +639,7 @@ HMC5883::collect()
 	ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report));
 
 	if (ret != OK) {
+		perf_count(_comms_errors);
 		debug("data/status read error");
 		goto out;
 	}
@@ -692,7 +671,7 @@ HMC5883::collect()
 
 	/* if we are running up against the oldest report, toss it */
 	if (_next_report == _oldest_report) {
-		_buf_overflows++;
+		perf_count(_buffer_overflows);
 		INCREMENT(_oldest_report, _num_reports);
 	}
 
@@ -737,10 +716,9 @@ HMC5883::meas_to_float(uint8_t in[2])
 void
 HMC5883::print_info()
 {
-	printf("reads:          %u\n", _reads);
+	perf_print_counter(_sample_perf);
-	printf("measure errors: %u\n", _measure_errors);
+	perf_print_counter(_comms_errors);
-	printf("read errors:    %u\n", _read_errors);
+	perf_print_counter(_buffer_overflows);
-	printf("read overflows: %u\n", _buf_overflows);
 	printf("poll interval:  %u ticks\n", _measure_ticks);
 	printf("report queue:   %u (%u/%u @ %p)\n",
 	       _num_reports, _oldest_report, _next_report, _reports);

apps/drivers/mpu6000/mpu6000.cpp

@@ -298,8 +298,8 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
 	_reads(0),
 	_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read"))
 {
-	// enable debug() calls
+	// disable debug() calls
-	_debug_enabled = true;
+	_debug_enabled = false;
 
 	// default accel scale factors
 	_accel_scale.x_offset = 0;
@@ -463,7 +463,7 @@ MPU6000::probe()
 	case MPU6000_REV_D8:
 	case MPU6000_REV_D9:
 	case MPU6000_REV_D10:
-		log("ID 0x%02x", _product);
+		debug("ID 0x%02x", _product);
 		return OK;
 	}
 

apps/drivers/ms5611/ms5611.cpp

@@ -132,12 +132,9 @@ private:
 
 	orb_advert_t		_baro_topic;
 
-	unsigned		_reads;
-	unsigned		_measure_errors;
-	unsigned		_read_errors;
-	unsigned		_buf_overflows;
-
 	perf_counter_t		_sample_perf;
+	perf_counter_t		_comms_errors;
+	perf_counter_t		_buffer_overflows;
 
 	/**
 	 * Test whether the device supported by the driver is present at a
@@ -252,11 +249,9 @@ MS5611::MS5611(int bus) :
 	_dT(0),
 	_temp64(0),
 	_baro_topic(-1),
-	_reads(0),
+	_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
-	_measure_errors(0),
+	_comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")),
-	_read_errors(0),
+	_buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows"))
-	_buf_overflows(0),
-	_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read"))
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -292,6 +287,12 @@ MS5611::init()
 
 	_oldest_report = _next_report = 0;
 
+	/* get a publish handle on the baro topic */
+	memset(&_reports[0], 0, sizeof(_reports[0]));
+	_baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]);
+	if (_baro_topic < 0)
+		debug("failed to create sensor_baro object");
+
 	ret = OK;
 out:
 	return ret;
@@ -300,7 +301,7 @@ out:
 int
 MS5611::probe()
 {
-	_retries = 3;
+	_retries = 10;
 	if((OK == probe_address(MS5611_ADDRESS_1)) ||
 	   (OK == probe_address(MS5611_ADDRESS_2))) {
 	   	_retries = 1;
@@ -358,8 +359,6 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
 			}
 		}
 
-		_reads++;
-
 		/* if there was no data, warn the caller */
 		return ret ? ret : -EAGAIN;
 	}
@@ -399,7 +398,6 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
 		/* state machine will have generated a report, copy it out */
 		memcpy(buffer, _reports, sizeof(*_reports));
 		ret = sizeof(*_reports);
-		_reads++;
 
 	} while (0);
 
@@ -541,31 +539,14 @@ MS5611::cycle_trampoline(void *arg)
 void
 MS5611::cycle()
 {
-	/*
-	 * We have to publish the baro topic in the context of the workq
-	 * in order to ensure that the descriptor is valid when we go to publish.
-	 *
-	 * @bug	We can't really ever be torn down and restarted, since this
-	 *      descriptor will never be closed and on the restart we will be
-	 *      unable to re-advertise.
-	 */
-	if (_baro_topic == -1) {
-		struct baro_report b;
-
-		/* if this fails (e.g. no object in the system) we will cope */
-		memset(&b, 0, sizeof(b));
-		_baro_topic = orb_advertise(ORB_ID(sensor_baro), &b);
-
-		if (_baro_topic < 0)
-			debug("failed to create sensor_baro object");
-	}
 
 	/* collection phase? */
 	if (_collect_phase) {
 
 		/* perform collection */
 		if (OK != collect()) {
-			log("FATAL collection error - restarting\n");
+			log("collection error");
+			/* reset the collection state machine and try again */
 			start();
 			return;
 		}
@@ -592,10 +573,8 @@ MS5611::cycle()
 	}
 
 	/* measurement phase */
-	if (OK != measure()) {
+	if (OK != measure())
-		log("FATAL measure error - restarting\n");
+		log("measure error");
-		start();
-	}
 
 	/* next phase is collection */
 	_collect_phase = true;
@@ -623,7 +602,7 @@ MS5611::measure()
 	ret = transfer(&cmd_data, 1, nullptr, 0);
 
 	if (OK != ret)
-		_measure_errors++;
+		perf_count(_comms_errors);
 
 	return ret;
 }
@@ -643,7 +622,7 @@ MS5611::collect()
 	_reports[_next_report].timestamp = hrt_absolute_time();
 
 	if (OK != transfer(&cmd, 1, &data[0], 3)) {
-		_read_errors++;
+		perf_count(_comms_errors);
 		return -EIO;
 	}
 
@@ -691,7 +670,7 @@ MS5611::collect()
 
 		/* if we are running up against the oldest report, toss it */
 		if (_next_report == _oldest_report) {
-			_buf_overflows++;
+			perf_count(_buffer_overflows);
 			INCREMENT(_oldest_report, _num_reports);
 		}
 
@@ -774,10 +753,9 @@ MS5611::crc4(uint16_t *n_prom)
 void
 MS5611::print_info()
 {
-	printf("reads:          %u\n", _reads);
+	perf_print_counter(_sample_perf);
-	printf("measure errors: %u\n", _measure_errors);
+	perf_print_counter(_comms_errors);
-	printf("read errors:    %u\n", _read_errors);
+	perf_print_counter(_buffer_overflows);
-	printf("read overflows: %u\n", _buf_overflows);
 	printf("poll interval:  %u ticks\n", _measure_ticks);
 	printf("report queue:   %u (%u/%u @ %p)\n",
 	       _num_reports, _oldest_report, _next_report, _reports);

apps/sensors/Makefile

@@ -39,4 +39,7 @@ APPNAME		= sensors
 PRIORITY	= SCHED_PRIORITY_MAX-5
 STACKSIZE	= 4096
 
+CXXSRCS		= sensors.cpp
+CSRCS		= sensor_params.c
+
 include $(APPDIR)/mk/app.mk

apps/sensors/sensor_params.c

@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           @author Julian Oes <joes@student.ethz.ch>
+ *
+ * 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 sensor_params.c
+ *
+ * Parameters defined by the sensors task.
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+PARAM_DEFINE_FLOAT(SENSOR_GYRO_XOFF, 0.0f);
+PARAM_DEFINE_FLOAT(SENSOR_GYRO_YOFF, 0.0f);
+PARAM_DEFINE_FLOAT(SENSOR_GYRO_ZOFF, 0.0f);
+
+PARAM_DEFINE_FLOAT(SENSOR_MAG_XOFF, 0.0f);
+PARAM_DEFINE_FLOAT(SENSOR_MAG_YOFF, 0.0f);
+PARAM_DEFINE_FLOAT(SENSOR_MAG_ZOFF, 0.0f);
+
+PARAM_DEFINE_FLOAT(SENSOR_ACC_XOFF, 0.0f);
+PARAM_DEFINE_FLOAT(SENSOR_ACC_YOFF, 0.0f);
+PARAM_DEFINE_FLOAT(SENSOR_ACC_ZOFF, 0.0f);
+
+PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
+PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
+PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);
+PARAM_DEFINE_FLOAT(RC1_REV, 1.0f);
+
+PARAM_DEFINE_FLOAT(RC2_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC2_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC2_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC2_REV, 1.0f);
+
+PARAM_DEFINE_FLOAT(RC3_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC3_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC3_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC3_REV, 1.0f);
+
+PARAM_DEFINE_FLOAT(RC4_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC4_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC4_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC4_REV, 1.0f);
+
+PARAM_DEFINE_FLOAT(RC5_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC5_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC5_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC5_REV, 1.0f);
+
+PARAM_DEFINE_FLOAT(RC6_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC6_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC6_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC6_REV, 1.0f);
+
+PARAM_DEFINE_FLOAT(RC7_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC7_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC7_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC7_REV, 1.0f);
+
+PARAM_DEFINE_FLOAT(RC8_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC8_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC8_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC8_REV, 1.0f);
+
+PARAM_DEFINE_INT32(RC_TYPE, 1); // 1 = FUTABA
+
+/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
+PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));
+
+PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
+PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
+PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
+PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
+PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);

apps/systemlib/err.c

@@ -155,7 +155,7 @@ warn(const char *fmt, ...)
 void
 vwarn(const char *fmt, va_list args) 
 {
-	warnerr_core(NOCODE, fmt, args);
+	warnerr_core(errno, fmt, args);
 }
 
 void

apps/systemlib/param/param.h

@@ -47,10 +47,13 @@
 
 #include <stdint.h>
 #include <stdbool.h>
+#include <sys/types.h>
 
 /** Maximum size of the parameter backing file */
 #define PARAM_FILE_MAXSIZE	4096
 
+__BEGIN_DECLS
+
 /**
  * Parameter types.
  */
@@ -192,6 +195,10 @@ __EXPORT void		param_foreach(void (*func)(void *arg, param_t param), void *arg,
  * Note that these structures are not known by name; they are
  * collected into a section that is iterated by the parameter
  * code.
+ *
+ * Note that these macros cannot be used in C++ code due to
+ * their use of designated initializers.  They should probably
+ * be refactored to avoid the use of a union for param_value_u.
  */
 
 /** define an int32 parameter */
@@ -199,9 +206,9 @@ __EXPORT void		param_foreach(void (*func)(void *arg, param_t param), void *arg,
 	static const					\
 	__attribute__((used, section("__param")))	\
 	struct param_info_s __param__##_name = {	\
-		.name = #_name,				\
+		#_name,					\
-			.type = PARAM_TYPE_INT32,	\
+		PARAM_TYPE_INT32,			\
-				.val.i = _default	\
+		.val.i = _default			\
 	}
 
 /** define a float parameter */
@@ -209,9 +216,9 @@ __EXPORT void		param_foreach(void (*func)(void *arg, param_t param), void *arg,
 	static const					\
 	__attribute__((used, section("__param")))	\
 	struct param_info_s __param__##_name = {	\
-		.name = #_name,				\
+		#_name,					\
-			.type = PARAM_TYPE_FLOAT,	\
+		PARAM_TYPE_FLOAT,			\
-				.val.f = _default	\
+		.val.f = _default			\
 	}
 
 /** define a parameter that points to a structure */
@@ -219,9 +226,9 @@ __EXPORT void		param_foreach(void (*func)(void *arg, param_t param), void *arg,
 	static const					\
 	__attribute__((used, section("__param")))	\
 	struct param_info_s __param__##_name = {	\
-		.name = #_name,				\
+		#_name,					\
-			.type = PARAM_TYPE_STRUCT + sizeof(_default), \
+		PARAM_TYPE_STRUCT + sizeof(_default),	\
-				.val.p = &_default;	\
+		.val.p = &_default;			\
 	}
 
 /**
@@ -245,4 +252,6 @@ struct param_info_s {
 	union param_value_u val;
 };
 
+__END_DECLS
+
 #endif

apps/uORB/topics/rc_channels.h

@@ -96,7 +96,7 @@ struct rc_channels_s {
   uint8_t chan_count;                 /**< maximum number of valid channels */
 
   /*String array to store the names of the functions*/
-  const char function_name[RC_CHANNELS_FUNCTION_MAX][20];
+  char function_name[RC_CHANNELS_FUNCTION_MAX][20];
   uint8_t function[RC_CHANNELS_FUNCTION_MAX];
   uint8_t rssi;                       /**< Overall receive signal strength */
 }; /**< radio control channels. */

nuttx/configs/px4fmu/nsh/defconfig

@@ -543,7 +543,7 @@ CONFIG_DEV_CONSOLE=y
 CONFIG_DEV_LOWCONSOLE=n
 CONFIG_MUTEX_TYPES=n
 CONFIG_PRIORITY_INHERITANCE=y
-CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SEM_PREALLOCHOLDERS=8
 CONFIG_SEM_NNESTPRIO=8
 CONFIG_FDCLONE_DISABLE=n
 CONFIG_FDCLONE_STDIO=y