Fixed setting mag queue depth

1 files changed, 27 insertions, 1 deletions

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index d9801f88f..7a65f644a 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -811,8 +811,34 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 			return SENSOR_POLLRATE_MANUAL;
 
 		return 1000000 / _call_mag_interval;
-	case SENSORIOCSQUEUEDEPTH:
+	
+	case SENSORIOCSQUEUEDEPTH: {
+			/* account for sentinel in the ring */
+			arg++;
+
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 2) || (arg > 100))
+				return -EINVAL;
+
+			/* allocate new buffer */
+			struct mag_report *buf = new struct mag_report[arg];
+
+			if (nullptr == buf)
+				return -ENOMEM;
+
+			/* reset the measurement state machine with the new buffer, free the old */
+			stop();
+			delete[] _mag_reports;
+			_num_mag_reports = arg;
+			_mag_reports = buf;
+			start();
+
+			return OK;
+		}
+
 	case SENSORIOCGQUEUEDEPTH:
+		return _num_mag_reports - 1;
+
 	case SENSORIOCRESET:
 		return ioctl(filp, cmd, arg);