diff options
Diffstat (limited to 'src/drivers/l3gd20/l3gd20.cpp')
| -rw-r--r-- | src/drivers/l3gd20/l3gd20.cpp | 44 |
1 files changed, 33 insertions, 11 deletions
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 64d1a7e55..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; @@ -330,15 +331,22 @@ private: * @return 0 on success, 1 on failure */ int self_test(); + + /* this class does not allow copying */ + L3GD20(const L3GD20&); + L3GD20 operator=(const L3GD20&); }; L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */), + _call{}, _call_interval(0), _reports(nullptr), + _gyro_scale{}, _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), @@ -399,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; + } - /* advertise sensor topic, measure manually to initialize valid report */ - struct gyro_report grp; - _reports->get(&grp); + reset(); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp); + measure(); - if (_gyro_topic < 0) - debug("failed to create sensor_gyro publication"); + /* advertise sensor topic, measure manually to initialize valid report */ + struct gyro_report grp; + _reports->get(&grp); + _gyro_topic = orb_advertise(_orb_id, &grp); + + if (_gyro_topic < 0) { + debug("failed to create sensor_gyro publication"); } ret = OK; @@ -931,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++; @@ -990,6 +1009,9 @@ void info(); /** * Start the driver. + * + * This function call only returns once the driver + * started or failed to detect the sensor. */ void start(bool external_bus, enum Rotation rotation) |