diff options
Diffstat (limited to 'src/drivers/mpu6000')
-rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 71 |
1 files changed, 50 insertions, 21 deletions
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 1b3a96a0d..25fd7d8a8 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -498,31 +498,44 @@ 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); - - /* measurement will have generated a report, publish */ - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &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_topic = orb_advertise(ORB_ID(sensor_accel0), &arp); + break; + + case CLASS_DEVICE_SECONDARY: + _accel_topic = orb_advertise(ORB_ID(sensor_accel1), &arp); + break; - if (_accel_topic < 0) - debug("failed to create sensor_accel publication"); + } + if (_accel_topic < 0) { + warnx("failed to create sensor_accel publication"); } - 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); - _gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp); + switch (_gyro->_gyro_class_instance) { + case CLASS_DEVICE_PRIMARY: + _gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro0), &grp); + break; + + case CLASS_DEVICE_SECONDARY: + _gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro1), &grp); + break; - if (_gyro->_gyro_topic < 0) - debug("failed to create sensor_gyro publication"); + } + if (_gyro->_gyro_topic < 0) { + warnx("failed to create sensor_gyro publication"); } out: @@ -1345,14 +1358,30 @@ 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); + switch (_accel_class_instance) { + case CLASS_DEVICE_PRIMARY: + orb_publish(ORB_ID(sensor_accel0), _accel_topic, &arb); + break; + + case CLASS_DEVICE_SECONDARY: + orb_publish(ORB_ID(sensor_accel1), _accel_topic, &arb); + break; + } } - if (_gyro->_gyro_topic > 0 && !(_pub_blocked)) { + if (!(_pub_blocked)) { /* publish it */ - orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); + switch (_gyro->_gyro_class_instance) { + case CLASS_DEVICE_PRIMARY: + orb_publish(ORB_ID(sensor_gyro0), _gyro->_gyro_topic, &grb); + break; + + case CLASS_DEVICE_SECONDARY: + orb_publish(ORB_ID(sensor_gyro1), _gyro->_gyro_topic, &grb); + break; + } } /* stop measuring */ |