Fixed up init sequence of all sensors - we can publish in interrupt context, but not advertise! All advertisements now contain valid data

src/drivers/airspeed/airspeed.cpp

@@ -133,8 +133,20 @@ Airspeed::init()
 	/* register alternate interfaces if we have to */
 	_class_instance = register_class_devname(AIRSPEED_DEVICE_PATH);
 
-	if (_airspeed_pub < 0)
+	/* publication init */
-		warnx("failed to create airspeed sensor object. Did you start uOrb?");
+	if (_class_instance == CLASS_DEVICE_PRIMARY) {
+
+		/* advertise sensor topic, measure manually to initialize valid report */
+		struct differential_pressure_s arp;
+		measure();
+		_reports->get(&arp);
+
+		/* measurement will have generated a report, publish */
+		_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);
+
+		if (_airspeed_pub < 0)
+			warnx("failed to create airspeed sensor object. uORB started?");
+	}
 
 	ret = OK;
 	/* sensor is ok, but we don't really know if it is within range */

src/drivers/bma180/bma180.cpp

@@ -153,6 +153,7 @@ private:
 	float			_accel_range_scale;
 	float			_accel_range_m_s2;
 	orb_advert_t		_accel_topic;
+	int			_class_instance;
 
 	unsigned		_current_lowpass;
 	unsigned		_current_range;
@@ -238,6 +239,7 @@ BMA180::BMA180(int bus, spi_dev_e device) :
 	_accel_range_scale(0.0f),
 	_accel_range_m_s2(0.0f),
 	_accel_topic(-1),
+	_class_instance(-1),
 	_current_lowpass(0),
 	_current_range(0),
 	_sample_perf(perf_alloc(PC_ELAPSED, "bma180_read"))
@@ -282,11 +284,6 @@ BMA180::init()
 	if (_reports == nullptr)
 		goto out;
 
-	/* advertise sensor topic */
-	struct accel_report zero_report;
-	memset(&zero_report, 0, sizeof(zero_report));
-	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
-
 	/* perform soft reset (p48) */
 	write_reg(ADDR_RESET, SOFT_RESET);
 
@@ -322,6 +319,19 @@ BMA180::init()
 		ret = ERROR;
 	}
 
+	_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
+
+	/* advertise sensor topic, measure manually to initialize valid report */
+	measure();
+
+	if (_class_instance == CLASS_DEVICE_PRIMARY) {
+		struct accel_report arp;
+		_reports->get(&arp);
+
+		/* measurement will have generated a report, publish */
+		_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
+	}
+
 out:
 	return ret;
 }
@@ -723,7 +733,7 @@ BMA180::measure()
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	if !(_pub_blocked)
+	if (_accel_topic > 0 && !(_pub_blocked))
 		orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
 
 	/* stop the perf counter */

src/drivers/ets_airspeed/ets_airspeed.cpp

@@ -184,18 +184,9 @@ ETSAirspeed::collect()
 	report.voltage = 0;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
 
-	/* announce the airspeed if needed, just publish else */
+	if (_airspeed_pub > 0 && !(_pub_blocked)) {
-	if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
+		/* publish it */
-
+		orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
-		if (_airspeed_pub > 0) {
-			/* publish it */
-			orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
-		} else {
-			_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &report);
-
-			if (_airspeed_pub < 0)
-				debug("failed to create differential_pressure publication");
-		}
 	}
 
 	new_report(report);

src/drivers/l3gd20/l3gd20.cpp

@@ -382,6 +382,21 @@ L3GD20::init()
 
 	reset();
 
+	measure();
+
+	if (_class_instance == CLASS_DEVICE_PRIMARY) {
+
+		/* 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");
+
+	}
+
 	ret = OK;
 out:
 	return ret;
@@ -888,18 +903,9 @@ L3GD20::measure()
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
+	if (_gyro_topic > 0 && !(_pub_blocked)) {
-
+		/* publish it */
-		if (_gyro_topic > 0) {
+		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
-			/* publish it */
-			orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
-		} else {
-			_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &report);
-
-			if (_gyro_topic < 0)
-				debug("failed to create sensor_gyro publication");
-		}
-
 	}
 
 	_read++;

src/drivers/lsm303d/lsm303d.cpp

@@ -597,8 +597,39 @@ LSM303D::init()
 		goto out;
 	}
 
+	/* 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);
+
+		/* measurement will have generated a report, publish */
+		_mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mrp);
+
+		if (_mag->_mag_topic < 0)
+			debug("failed to create sensor_mag publication");
+
+	}
+
 	_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);
+
+		/* 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");
+
+	}
+
 out:
 	return ret;
 }
@@ -1510,18 +1541,9 @@ LSM303D::measure()
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
-	if (_accel_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
+	if (_accel_topic > 0 && !(_pub_blocked)) {
-
+		/* publish it */
-		if (_accel_topic > 0) {
+		orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
-			/* publish it */
-			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
-		} else {
-			_accel_topic = orb_advertise(ORB_ID(sensor_accel), &accel_report);
-
-			if (_accel_topic < 0)
-				debug("failed to create sensor_accel publication");
-		}
-
 	}
 
 	_accel_read++;
@@ -1593,18 +1615,9 @@ LSM303D::mag_measure()
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
-	if (_mag->_mag_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
+	if (_mag->_mag_topic > 0 && !(_pub_blocked)) {
-
+		/* publish it */
-		if (_mag->_mag_topic > 0) {
+		orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
-			/* publish it */
-			orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
-		} else {
-			_mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mag_report);
-
-			if (_mag->_mag_topic < 0)
-				debug("failed to create sensor_mag publication");
-		}
-
 	}
 
 	_mag_read++;

src/drivers/meas_airspeed/meas_airspeed.cpp

@@ -216,18 +216,9 @@ MEASAirspeed::collect()
 	report.voltage = 0;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
 
-	/* announce the airspeed if needed, just publish else */
+	if (_airspeed_pub > 0 && !(_pub_blocked)) {
-	if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
+		/* publish it */
-
+		orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
-		if (_airspeed_pub > 0) {
-			/* publish it */
-			orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
-		} else {
-			_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &report);
-
-			if (_airspeed_pub < 0)
-				debug("failed to create differential_pressure publication");
-		}
 	}
 
 	new_report(report);

src/drivers/mpu6000/mpu6000.cpp

@@ -489,6 +489,35 @@ MPU6000::init()
 
 	_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
 
+	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);
+
+		/* 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");
+
+	}
+
+	if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY) {
+
+		/* advertise sensor topic, measure manually to initialize valid report */
+		struct gyro_report grp;
+		_gyro_reports->get(&grp);
+
+		_gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp);
+
+		if (_gyro->_gyro_topic < 0)
+			debug("failed to create sensor_gyro publication");
+
+	}
+
 out:
 	return ret;
 }
@@ -1297,32 +1326,14 @@ MPU6000::measure()
 	poll_notify(POLLIN);
 	_gyro->parent_poll_notify();
 
-	if (_accel_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
+	if (_accel_topic > 0 && !(_pub_blocked)) {
-
+		/* publish it */
-		if (_accel_topic > 0) {
+		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
-			/* publish it */
-			orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
-		} else {
-			_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arb);
-
-			if (_accel_topic < 0)
-				debug("failed to create sensor_accel publication");
-		}
-
 	}
 
-	if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
+	if (_gyro->_gyro_topic > 0 && !(_pub_blocked)) {
-
+		/* publish it */
-		if (_gyro->_gyro_topic > 0) {
+		orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
-			/* publish it */
-			orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
-		} else {
-			_gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grb);
-
-			if (_gyro->_gyro_topic < 0)
-				debug("failed to create sensor_gyro publication");
-		}
-
 	}
 
 	/* stop measuring */

src/drivers/ms5611/ms5611.cpp

@@ -90,6 +90,7 @@ static const int ERROR = -1;
 /* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */
 #define MS5611_CONVERSION_INTERVAL	10000	/* microseconds */
 #define MS5611_MEASUREMENT_RATIO	3	/* pressure measurements per temperature measurement */
+#define MS5611_BARO_DEVICE_PATH		"/dev/ms5611"
 
 class MS5611 : public device::CDev
 {
@@ -194,7 +195,7 @@ protected:
 extern "C" __EXPORT int ms5611_main(int argc, char *argv[]);
 
 MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
-	CDev("MS5611", BARO_DEVICE_PATH),
+	CDev("MS5611", MS5611_BARO_DEVICE_PATH),
 	_interface(interface),
 	_prom(prom_buf.s),
 	_measure_ticks(0),
@@ -222,7 +223,7 @@ MS5611::~MS5611()
 	stop_cycle();
 
 	if (_class_instance != -1)
-		unregister_class_devname(BARO_DEVICE_PATH, _class_instance);
+		unregister_class_devname(MS5611_BARO_DEVICE_PATH, _class_instance);
 
 	/* free any existing reports */
 	if (_reports != nullptr)
@@ -260,7 +261,54 @@ MS5611::init()
 	/* register alternate interfaces if we have to */
 	_class_instance = register_class_devname(BARO_DEVICE_PATH);
 
-	ret = OK;
+	struct baro_report brp;
+	/* do a first measurement cycle to populate reports with valid data */
+	_measure_phase = 0;
+	_reports->flush();
+
+	/* this do..while is goto without goto */
+	do {
+		/* do temperature first */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		usleep(MS5611_CONVERSION_INTERVAL);
+
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* now do a pressure measurement */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		usleep(MS5611_CONVERSION_INTERVAL);
+
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* state machine will have generated a report, copy it out */
+		_reports->get(&brp);
+
+		ret = OK;
+
+		if (_class_instance == CLASS_DEVICE_PRIMARY) {
+
+			_baro_topic = orb_advertise(ORB_ID(sensor_baro), &brp);
+
+			if (_baro_topic < 0)
+				debug("failed to create sensor_baro publication");
+		}
+
+	} while (0);
+
 out:
 	return ret;
 }
@@ -668,17 +716,9 @@ MS5611::collect()
 		report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
 
 		/* publish it */
-		if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
+		if (_baro_topic > 0 && !(_pub_blocked)) {
-
+			/* publish it */
-			if (_baro_topic > 0) {
+			orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
-				/* publish it */
-				orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
-			} else {
-				_baro_topic = orb_advertise(ORB_ID(sensor_baro), &report);
-
-				if (_baro_topic < 0)
-					debug("failed to create sensor_baro publication");
-			}
 		}
 
 		if (_reports->force(&report)) {
@@ -821,7 +861,7 @@ start()
 		goto fail;
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(BARO_DEVICE_PATH, O_RDONLY);
+	fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
 	if (fd < 0) {
 		warnx("can't open baro device");
 		goto fail;
@@ -855,10 +895,10 @@ test()
 	ssize_t sz;
 	int ret;
 
-	int fd = open(BARO_DEVICE_PATH, O_RDONLY);
+	int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
-		err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
+		err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH);
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
@@ -914,7 +954,7 @@ test()
 void
 reset()
 {
-	int fd = open(BARO_DEVICE_PATH, O_RDONLY);
+	int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "failed ");
@@ -953,10 +993,10 @@ calibrate(unsigned altitude)
 	float	pressure;
 	float	p1;
 
-	int fd = open(BARO_DEVICE_PATH, O_RDONLY);
+	int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
-		err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
+		err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH);
 
 	/* start the sensor polling at max */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX))