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/l3gd20/l3gd20.cpp | |
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/l3gd20/l3gd20.cpp')
-rw-r--r-- | src/drivers/l3gd20/l3gd20.cpp | 30 |
1 files changed, 18 insertions, 12 deletions
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++; |