fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 04:47:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

LSM303D: Support for tertiary sensors

Browse Source
This commit is contained in:
Lorenz Meier
2014-07-15 23:32:03 +02:00
parent b6bac2c88d
commit 43bc2c3ef2
1 changed files with 23 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/lsm303d/lsm303d.cpp
+23 -24
View File
@@ -284,6 +284,7 @@ private:
unsigned _mag_samplerate;
orb_advert_t _accel_topic;
orb_id_t _accel_orb_id;
int _accel_class_instance;
unsigned _accel_read;
@@ -485,6 +486,7 @@ private:
LSM303D *_parent;
orb_advert_t _mag_topic;
orb_id_t _mag_orb_id;
int _mag_class_instance;
void measure();
@@ -508,6 +510,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_class_instance(-1),
_accel_read(0),
_mag_read(0),
@@ -618,16 +621,22 @@ LSM303D::init()
/* measurement will have generated a report, publish */
switch (_mag->_mag_class_instance) {
case CLASS_DEVICE_PRIMARY:
_mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag0), &mrp);
_mag->_mag_orb_id = ORB_ID(sensor_mag0);
break;
case CLASS_DEVICE_SECONDARY:
_mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag1), &mrp);
_mag->_mag_orb_id = ORB_ID(sensor_mag1);
break;
case CLASS_DEVICE_TERTIARY:
_mag->_mag_orb_id = ORB_ID(sensor_mag2);
break;
}
_mag->_mag_topic = orb_advertise(_mag->_mag_orb_id, &mrp);
if (_mag->_mag_topic < 0) {
warnx("failed to create sensor_mag publication");
warnx("ADVERT ERR");
}
_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
@@ -639,17 +648,22 @@ LSM303D::init()
/* measurement will have generated a report, publish */
switch (_accel_class_instance) {
case CLASS_DEVICE_PRIMARY:
_accel_topic = orb_advertise(ORB_ID(sensor_accel0), &arp);
_accel_orb_id = ORB_ID(sensor_accel0);
break;
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;
}
_accel_topic = orb_advertise(_accel_orb_id, &arp);
if (_accel_topic < 0) {
warnx("failed to create sensor_accel publication");
warnx("ADVERT ERR");
}
out:
@@ -1570,15 +1584,7 @@ LSM303D::measure()
if (!(_pub_blocked)) {
/* publish it */
switch (_accel_class_instance) {
case CLASS_DEVICE_PRIMARY:
orb_publish(ORB_ID(sensor_accel0), _accel_topic, &accel_report);
break;
case CLASS_DEVICE_SECONDARY:
orb_publish(ORB_ID(sensor_accel1), _accel_topic, &accel_report);
break;
}
orb_publish(_accel_orb_id, _accel_topic, &accel_report);
}
_accel_read++;
@@ -1655,15 +1661,7 @@ LSM303D::mag_measure()
if (!(_pub_blocked)) {
/* publish it */
switch (_mag->_mag_class_instance) {
case CLASS_DEVICE_PRIMARY:
orb_publish(ORB_ID(sensor_mag0), _mag->_mag_topic, &mag_report);
break;
case CLASS_DEVICE_SECONDARY:
orb_publish(ORB_ID(sensor_mag1), _mag->_mag_topic, &mag_report);
break;
}
orb_publish(_mag->_mag_orb_id, _mag->_mag_topic, &mag_report);
}
_mag_read++;
@@ -1757,6 +1755,7 @@ LSM303D_mag::LSM303D_mag(LSM303D *parent) :
CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG),
_parent(parent),
_mag_topic(-1),
_mag_orb_id(nullptr),
_mag_class_instance(-1)
{
}