diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-17 07:33:50 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-17 07:33:50 +0200 |
commit | f89062fe3bf56aef23c2ea1a29ae3468694344fa (patch) | |
tree | f6d2ef6e950d21aa1a3baa1fdf58f19a79f41a82 /src/drivers/l3gd20/l3gd20.cpp | |
parent | 65c952e134daa7c505026ddf2139148fe3092161 (diff) | |
parent | 7705a24f7227035d5932a1288e26ce75cec07fdf (diff) | |
download | px4-firmware-f89062fe3bf56aef23c2ea1a29ae3468694344fa.tar.gz px4-firmware-f89062fe3bf56aef23c2ea1a29ae3468694344fa.tar.bz2 px4-firmware-f89062fe3bf56aef23c2ea1a29ae3468694344fa.zip |
Merge pull request #1186 from PX4/logging
Multi-instance handling for sensors
Diffstat (limited to 'src/drivers/l3gd20/l3gd20.cpp')
-rw-r--r-- | src/drivers/l3gd20/l3gd20.cpp | 35 |
1 files changed, 24 insertions, 11 deletions
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index f72db82c0..cfae8761c 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -213,6 +213,7 @@ private: float _gyro_range_scale; float _gyro_range_rad_s; orb_advert_t _gyro_topic; + orb_id_t _orb_id; int _class_instance; unsigned _current_rate; @@ -345,6 +346,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _gyro_topic(-1), + _orb_id(nullptr), _class_instance(-1), _current_rate(0), _orientation(SENSOR_BOARD_ROTATION_DEFAULT), @@ -405,21 +407,32 @@ L3GD20::init() _class_instance = register_class_devname(GYRO_DEVICE_PATH); - reset(); + switch (_class_instance) { + case CLASS_DEVICE_PRIMARY: + _orb_id = ORB_ID(sensor_gyro0); + break; - measure(); + case CLASS_DEVICE_SECONDARY: + _orb_id = ORB_ID(sensor_gyro1); + break; - if (_class_instance == CLASS_DEVICE_PRIMARY) { + case CLASS_DEVICE_TERTIARY: + _orb_id = ORB_ID(sensor_gyro2); + break; + } + + reset(); - /* advertise sensor topic, measure manually to initialize valid report */ - struct gyro_report grp; - _reports->get(&grp); + measure(); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp); + /* advertise sensor topic, measure manually to initialize valid report */ + struct gyro_report grp; + _reports->get(&grp); - if (_gyro_topic < 0) - debug("failed to create sensor_gyro publication"); + _gyro_topic = orb_advertise(_orb_id, &grp); + if (_gyro_topic < 0) { + debug("failed to create sensor_gyro publication"); } ret = OK; @@ -937,9 +950,9 @@ L3GD20::measure() poll_notify(POLLIN); /* publish for subscribers */ - if (_gyro_topic > 0 && !(_pub_blocked)) { + if (!(_pub_blocked)) { /* publish it */ - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); + orb_publish(_orb_id, _gyro_topic, &report); } _read++; |