diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2015-01-25 21:05:38 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-01-29 16:33:53 +0100 |
commit | d851a630d874f639e0861dc8411405f83ee23769 (patch) | |
tree | 2e51bab607145daab2a85c4561fe0825868d920d /src/drivers/lsm303d | |
parent | cc7a00b96e658f5d36763ef90ebac7fa813b55af (diff) | |
download | px4-firmware-d851a630d874f639e0861dc8411405f83ee23769.tar.gz px4-firmware-d851a630d874f639e0861dc8411405f83ee23769.tar.bz2 px4-firmware-d851a630d874f639e0861dc8411405f83ee23769.zip |
Move all drivers to multi pub/sub API
Diffstat (limited to 'src/drivers/lsm303d')
-rw-r--r-- | src/drivers/lsm303d/lsm303d.cpp | 42 |
1 files changed, 21 insertions, 21 deletions
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 6b65965b4..ff7068936 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -211,6 +211,12 @@ static const int ERROR = -1; #define LSM303D_ONE_G 9.80665f +#ifdef PX4_SPI_BUS_EXT +#define EXTERNAL_BUS PX4_SPI_BUS_EXT +#else +#define EXTERNAL_BUS 0 +#endif + extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } @@ -275,7 +281,7 @@ private: unsigned _mag_samplerate; orb_advert_t _accel_topic; - orb_id_t _accel_orb_id; + int _accel_orb_class_instance; int _accel_class_instance; unsigned _accel_read; @@ -330,6 +336,13 @@ private: void disable_i2c(); /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_bus == EXTERNAL_BUS); } + + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. * @@ -539,7 +552,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _mag_range_scale(0.0f), _mag_samplerate(0), _accel_topic(-1), - _accel_orb_id(nullptr), + _accel_orb_class_instance(-1), _accel_class_instance(-1), _accel_read(0), _mag_read(0), @@ -618,7 +631,6 @@ LSM303D::init() if (_accel_reports == nullptr) goto out; - /* advertise accel topic */ _mag_reports = new RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) @@ -641,7 +653,8 @@ LSM303D::init() _mag_reports->get(&mrp); /* measurement will have generated a report, publish */ - _mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); + _mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, + &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); if (_mag->_mag_topic < 0) { warnx("ADVERT ERR"); @@ -654,21 +667,8 @@ LSM303D::init() _accel_reports->get(&arp); /* measurement will have generated a report, publish */ - switch (_accel_class_instance) { - case CLASS_DEVICE_PRIMARY: - _accel_orb_id = ORB_ID(sensor_accel0); - break; - - case CLASS_DEVICE_SECONDARY: - _accel_orb_id = ORB_ID(sensor_accel1); - break; - - case CLASS_DEVICE_TERTIARY: - _accel_orb_id = ORB_ID(sensor_accel2); - break; - } - - _accel_topic = orb_advertise(_accel_orb_id, &arp); + _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_accel_topic < 0) { warnx("ADVERT ERR"); @@ -1556,7 +1556,7 @@ LSM303D::measure() if (!(_pub_blocked)) { /* publish it */ - orb_publish(_accel_orb_id, _accel_topic, &accel_report); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); } _accel_read++; |