diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-12 11:54:38 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-12 11:54:38 +0100 |
commit | 7af62bbe9e4baaa846a37176d6942e1893e42715 (patch) | |
tree | 2aaecc45856729d2e0ef819801f7d338aa208159 /src | |
parent | a34a14ce862c270192283514d8a1914fbe43bd48 (diff) | |
download | px4-firmware-7af62bbe9e4baaa846a37176d6942e1893e42715.tar.gz px4-firmware-7af62bbe9e4baaa846a37176d6942e1893e42715.tar.bz2 px4-firmware-7af62bbe9e4baaa846a37176d6942e1893e42715.zip |
Support for publication blocking: MPU6000, cleaned up device start
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 46 |
1 files changed, 26 insertions, 20 deletions
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index bbc595af4..02fe6df4a 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 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 @@ -443,7 +443,6 @@ int MPU6000::init() { int ret; - int gyro_ret; /* do SPI init (and probe) first */ ret = SPI::init(); @@ -488,16 +487,7 @@ MPU6000::init() return ret; } - /* fetch an initial set of measurements for advertisement */ - measure(); - _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); - if (_accel_class_instance == CLASS_DEVICE_PRIMARY) { - /* advertise accel topic */ - accel_report ar; - _accel_reports->get(&ar); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); - } out: return ret; @@ -1307,11 +1297,32 @@ MPU6000::measure() poll_notify(POLLIN); _gyro->parent_poll_notify(); - if (_accel_topic != -1) { - orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + if (_accel_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { + + if (_accel_topic > 0) { + /* publish it */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + } else { + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &arb); + + if (_accel_topic < 0) + debug("failed to create sensor_accel publication"); + } + } - if (_gyro->_gyro_topic != -1) { - orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); + + if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { + + if (_gyro->_gyro_topic > 0) { + /* publish it */ + orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); + } else { + _gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grb); + + if (_gyro->_gyro_topic < 0) + debug("failed to create sensor_gyro publication"); + } + } /* stop measuring */ @@ -1356,11 +1367,6 @@ MPU6000_gyro::init() } _gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH); - if (_gyro_class_instance == CLASS_DEVICE_PRIMARY) { - gyro_report gr; - memset(&gr, 0, sizeof(gr)); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); - } out: return ret; |