aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/drivers/airspeed/airspeed.cpp16
-rw-r--r--src/drivers/bma180/bma180.cpp22
-rw-r--r--src/drivers/ets_airspeed/ets_airspeed.cpp15
-rw-r--r--src/drivers/l3gd20/l3gd20.cpp30
-rw-r--r--src/drivers/lsm303d/lsm303d.cpp61
-rw-r--r--src/drivers/meas_airspeed/meas_airspeed.cpp15
-rw-r--r--src/drivers/mpu6000/mpu6000.cpp59
-rw-r--r--src/drivers/ms5611/ms5611.cpp80
8 files changed, 186 insertions, 112 deletions
diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 215d3792e..71c0b70f0 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/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)
- warnx("failed to create airspeed sensor object. Did you start uOrb?");
+ /* publication init */
+ 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 */
diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp
index df4e8f998..e43a34805 100644
--- a/src/drivers/bma180/bma180.cpp
+++ b/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 */
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index cdc70ac37..8bbef5cfa 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/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 (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
-
- 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");
- }
+ if (_airspeed_pub > 0 && !(_pub_blocked)) {
+ /* publish it */
+ orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
}
new_report(report);
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index e885b1bf9..90c3db9ae 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/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) {
- /* 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");
- }
-
+ if (_gyro_topic > 0 && !(_pub_blocked)) {
+ /* publish it */
+ orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
}
_read++;
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 3bd7a66f6..4dee7649b 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/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) {
- /* 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");
- }
-
+ if (_accel_topic > 0 && !(_pub_blocked)) {
+ /* publish it */
+ orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
}
_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) {
- /* 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");
- }
-
+ if (_mag->_mag_topic > 0 && !(_pub_blocked)) {
+ /* publish it */
+ orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
}
_mag_read++;
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index fee13f139..51a059e39 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/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 (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
-
- 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");
- }
+ if (_airspeed_pub > 0 && !(_pub_blocked)) {
+ /* publish it */
+ orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
}
new_report(report);
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 02fe6df4a..bf80c9cff 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/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) {
- /* 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 (_accel_topic > 0 && !(_pub_blocked)) {
+ /* publish it */
+ orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
}
- if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
-
- if (_gyro->_gyro_topic > 0) {
- /* 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");
- }
-
+ if (_gyro->_gyro_topic > 0 && !(_pub_blocked)) {
+ /* publish it */
+ orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
}
/* stop measuring */
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 089331566..0ef056273 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/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) {
- /* 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 (_baro_topic > 0 && !(_pub_blocked)) {
+ /* publish it */
+ orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
}
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))
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-11 11:21:47 +0000