dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

This commit is contained in:
Lorenz Meier 2014-01-14 15:40:46 +01:00
parent 1f5eda37ab
commit d199710651
8 changed files with 186 additions and 112 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/drivers/airspeed/airspeed.cpp
View File

@ -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 */

22
src/drivers/bma180/bma180.cpp
View File

@ -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 */

15
src/drivers/ets_airspeed/ets_airspeed.cpp
View File

@ -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);

30
src/drivers/l3gd20/l3gd20.cpp
View File

@ -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++;

61
src/drivers/lsm303d/lsm303d.cpp
View File

@ -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++;

15
src/drivers/meas_airspeed/meas_airspeed.cpp
View File

@ -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);

59
src/drivers/mpu6000/mpu6000.cpp
View File

@ -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 */

80
src/drivers/ms5611/ms5611.cpp
View File

@ -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))