Improved LSM303D driver, plus some fixes to the HMC5883

1 files changed, 20 insertions, 5 deletions

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 198da9f0a..7ea1ae0f3 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -936,6 +936,7 @@ void
 Sensors::mag_init()
 {
 	int	fd;
+	int	ret;
 
 	fd = open(MAG_DEVICE_PATH, 0);
 
@@ -944,13 +945,27 @@ Sensors::mag_init()
 		errx(1, "FATAL: no magnetometer found");
 	}
 
-	/* set the mag internal poll rate to at least 150Hz */
-	ioctl(fd, MAGIOCSSAMPLERATE, 150);
+	/* try different mag sampling rates */
 
-	/* set the driver to poll at 150Hz */
-	ioctl(fd, SENSORIOCSPOLLRATE, 150);
+#if 0
+	ret = ioctl(fd, MAGIOCSSAMPLERATE, 150);
+	if (ret == OK) {
+		/* set the pollrate accordingly */
+		ioctl(fd, SENSORIOCSPOLLRATE, 150);
+	} else {
+		ret = ioctl(fd, MAGIOCSSAMPLERATE, 100);
+		/* if the slower sampling rate still fails, something is wrong */
+		if (ret == OK) {
+			/* set the driver to poll also at the slower rate */
+			ioctl(fd, SENSORIOCSPOLLRATE, 100);
+		} else {
+			errx(1, "FATAL: mag sampling rate could not be set");
+		}
+	}
+#endif
+	
 
-	int ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
+	ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
 	if (ret < 0)
 		errx(1, "FATAL: unknown if magnetometer is external or onboard");
 	else if (ret == 1)