diff options
Diffstat (limited to 'src/drivers/mpu6000/mpu6000.cpp')
-rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 81 |
1 files changed, 61 insertions, 20 deletions
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 1b3a96a0d..6f5dae7ad 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -215,6 +215,7 @@ private: float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; + orb_id_t _accel_orb_id; int _accel_class_instance; RingBuffer *_gyro_reports; @@ -343,6 +344,9 @@ private: */ void _set_sample_rate(uint16_t desired_sample_rate_hz); + /* do not allow to copy this class due to pointer data members */ + MPU6000(const MPU6000&); + MPU6000 operator=(const MPU6000&); }; /** @@ -367,8 +371,12 @@ protected: private: MPU6000 *_parent; orb_advert_t _gyro_topic; + orb_id_t _gyro_orb_id; int _gyro_class_instance; + /* do not allow to copy this class due to pointer data members */ + MPU6000_gyro(const MPU6000_gyro&); + MPU6000_gyro operator=(const MPU6000_gyro&); }; /** driver 'main' command */ @@ -378,13 +386,17 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), _gyro(new MPU6000_gyro(this, path_gyro)), _product(0), + _call{}, _call_interval(0), _accel_reports(nullptr), + _accel_scale{}, _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), + _accel_orb_id(nullptr), _accel_class_instance(-1), _gyro_reports(nullptr), + _gyro_scale{}, _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _sample_rate(1000), @@ -498,33 +510,58 @@ MPU6000::init() 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); - /* advertise sensor topic, measure manually to initialize valid report */ - struct accel_report arp; - _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; - /* measurement will have generated a report, publish */ - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp); + case CLASS_DEVICE_TERTIARY: + _accel_orb_id = ORB_ID(sensor_accel2); + break; - if (_accel_topic < 0) - debug("failed to create sensor_accel publication"); + } + + _accel_topic = orb_advertise(_accel_orb_id, &arp); + if (_accel_topic < 0) { + warnx("ADVERT FAIL"); } - 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); + /* advertise sensor topic, measure manually to initialize valid report */ + struct gyro_report grp; + _gyro_reports->get(&grp); + + switch (_gyro->_gyro_class_instance) { + case CLASS_DEVICE_PRIMARY: + _gyro->_gyro_orb_id = ORB_ID(sensor_gyro0); + break; - _gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp); + case CLASS_DEVICE_SECONDARY: + _gyro->_gyro_orb_id = ORB_ID(sensor_gyro1); + break; - if (_gyro->_gyro_topic < 0) - debug("failed to create sensor_gyro publication"); + case CLASS_DEVICE_TERTIARY: + _gyro->_gyro_orb_id = ORB_ID(sensor_gyro2); + break; } + _gyro->_gyro_topic = orb_advertise(_gyro->_gyro_orb_id, &grp); + + if (_gyro->_gyro_topic < 0) { + warnx("ADVERT FAIL"); + } + out: return ret; } @@ -1345,14 +1382,14 @@ MPU6000::measure() poll_notify(POLLIN); _gyro->parent_poll_notify(); - if (_accel_topic > 0 && !(_pub_blocked)) { + if (!(_pub_blocked)) { /* publish it */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + orb_publish(_accel_orb_id, _accel_topic, &arb); } - if (_gyro->_gyro_topic > 0 && !(_pub_blocked)) { + if (!(_pub_blocked)) { /* publish it */ - orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); + orb_publish(_gyro->_gyro_orb_id, _gyro->_gyro_topic, &grb); } /* stop measuring */ @@ -1373,6 +1410,7 @@ MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) : CDev("MPU6000_gyro", path), _parent(parent), _gyro_topic(-1), + _gyro_orb_id(nullptr), _gyro_class_instance(-1) { } @@ -1437,6 +1475,9 @@ void usage(); /** * Start the driver. + * + * This function only returns if the driver is up and running + * or failed to detect the sensor. */ void start(bool external_bus, enum Rotation rotation) @@ -1507,7 +1548,7 @@ test(bool external_bus) int fd = open(path_accel, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)", + err(1, "%s open failed (try 'mpu6000 start')", path_accel); /* get the driver */ |