diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-14 15:40:46 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-14 15:40:46 +0100 |
commit | d19971065140bdfbbe5972f2a394597504abef9e (patch) | |
tree | 47f4c08d75a3088806736e091b6338e4d1d48861 /src/drivers/mpu6000 | |
parent | 1f5eda37abffc10b51e4bcd94efa18c1dc76d21f (diff) | |
download | px4-firmware-d19971065140bdfbbe5972f2a394597504abef9e.tar.gz px4-firmware-d19971065140bdfbbe5972f2a394597504abef9e.tar.bz2 px4-firmware-d19971065140bdfbbe5972f2a394597504abef9e.zip |
Fixed up init sequence of all sensors - we can publish in interrupt context, but not advertise! All advertisements now contain valid data
Diffstat (limited to 'src/drivers/mpu6000')
-rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 59 |
1 files changed, 35 insertions, 24 deletions
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 */ |