fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 08:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

mpu6000: Support for up to three accels / gyros

Browse Source
This commit is contained in:
Lorenz Meier
2014-07-15 23:14:50 +02:00
parent 5ef4e08c58
commit 32ed1eae80
1 changed files with 24 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/mpu6000/mpu6000.cpp
+24 -24
View File
@@ -215,6 +215,7 @@ private:
float _accel_range_scale; float _accel_range_scale;
float _accel_range_m_s2; float _accel_range_m_s2;
orb_advert_t _accel_topic; orb_advert_t _accel_topic;
orb_id_t _accel_orb_id;
int _accel_class_instance; int _accel_class_instance;
RingBuffer *_gyro_reports; RingBuffer *_gyro_reports;
@@ -367,6 +368,7 @@ protected:
private: private:
MPU6000 *_parent; MPU6000 *_parent;
orb_advert_t _gyro_topic; orb_advert_t _gyro_topic;
orb_id_t _gyro_orb_id;
int _gyro_class_instance; int _gyro_class_instance;
}; };
@@ -383,6 +385,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
_accel_range_scale(0.0f), _accel_range_scale(0.0f),
_accel_range_m_s2(0.0f), _accel_range_m_s2(0.0f),
_accel_topic(-1), _accel_topic(-1),
_accel_orb_id(nullptr),
_accel_class_instance(-1), _accel_class_instance(-1),
_gyro_reports(nullptr), _gyro_reports(nullptr),
_gyro_range_scale(0.0f), _gyro_range_scale(0.0f),
@@ -505,17 +508,23 @@ MPU6000::init()
/* measurement will have generated a report, publish */ /* measurement will have generated a report, publish */
switch (_accel_class_instance) { switch (_accel_class_instance) {
case CLASS_DEVICE_PRIMARY: case CLASS_DEVICE_PRIMARY:
_accel_topic = orb_advertise(ORB_ID(sensor_accel0), &arp); _accel_orb_id = ORB_ID(sensor_accel0);
break; break;
case CLASS_DEVICE_SECONDARY: case CLASS_DEVICE_SECONDARY:
_accel_topic = orb_advertise(ORB_ID(sensor_accel1), &arp); _accel_orb_id = ORB_ID(sensor_accel1);
break;
case CLASS_DEVICE_TERTIARY:
_accel_orb_id = ORB_ID(sensor_accel2);
break; break;
} }
_accel_topic = orb_advertise(_accel_orb_id, &arp);
if (_accel_topic < 0) { if (_accel_topic < 0) {
warnx("failed to create sensor_accel publication"); warnx("ADVERT FAIL");
} }
@@ -525,17 +534,23 @@ MPU6000::init()
switch (_gyro->_gyro_class_instance) { switch (_gyro->_gyro_class_instance) {
case CLASS_DEVICE_PRIMARY: case CLASS_DEVICE_PRIMARY:
_gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro0), &grp); _gyro->_gyro_orb_id = ORB_ID(sensor_gyro0);
break; break;
case CLASS_DEVICE_SECONDARY: case CLASS_DEVICE_SECONDARY:
_gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro1), &grp); _gyro->_gyro_orb_id = ORB_ID(sensor_gyro1);
break;
case CLASS_DEVICE_TERTIARY:
_gyro->_gyro_orb_id = ORB_ID(sensor_gyro2);
break; break;
} }
_gyro->_gyro_topic = orb_advertise(_gyro->_gyro_orb_id, &grp);
if (_gyro->_gyro_topic < 0) { if (_gyro->_gyro_topic < 0) {
warnx("failed to create sensor_gyro publication"); warnx("ADVERT FAIL");
} }
out: out:
@@ -1360,28 +1375,12 @@ MPU6000::measure()
if (!(_pub_blocked)) { if (!(_pub_blocked)) {
/* publish it */ /* publish it */
switch (_accel_class_instance) { orb_publish(_accel_orb_id, _accel_topic, &arb);
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 (!(_pub_blocked)) { if (!(_pub_blocked)) {
/* publish it */ /* publish it */
switch (_gyro->_gyro_class_instance) { orb_publish(_gyro->_gyro_orb_id, _gyro->_gyro_topic, &grb);
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 */ /* stop measuring */
@@ -1402,6 +1401,7 @@ MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
CDev("MPU6000_gyro", path), CDev("MPU6000_gyro", path),
_parent(parent), _parent(parent),
_gyro_topic(-1), _gyro_topic(-1),
_gyro_orb_id(nullptr),
_gyro_class_instance(-1) _gyro_class_instance(-1)
{ {
} }