LSM303D: Support for tertiary sensors

1 files changed, 23 insertions, 24 deletions

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index ff0eda60b..2750f8755 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -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)
 {
 }