Add reset and samplerate ioctl to HMC5883 driver

1 files changed, 60 insertions, 47 deletions

diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 1afc16a9a..cb708db4a 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -192,6 +192,11 @@ private:
 	void			stop();
 
 	/**
+	 * Reset the device
+	 */
+	int			reset();
+
+	/**
 	 * Perform the on-sensor scale calibration routine.
 	 *
 	 * @note The sensor will continue to provide measurements, these
@@ -365,6 +370,9 @@ HMC5883::init()
 	if (I2C::init() != OK)
 		goto out;
 
+	/* reset the device configuration */
+	reset();
+
 	/* allocate basic report buffers */
 	_num_reports = 2;
 	_reports = new struct mag_report[_num_reports];
@@ -381,9 +389,6 @@ HMC5883::init()
 	if (_mag_topic < 0)
 		debug("failed to create sensor_mag object");
 
-	/* set range */
-	set_range(_range_ga);
-
 	ret = OK;
 	/* sensor is ok, but not calibrated */
 	_sensor_ok = true;
@@ -542,62 +547,61 @@ int
 HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
-
 	case SENSORIOCSPOLLRATE: {
-			switch (arg) {
+		switch (arg) {
 
-				/* switching to manual polling */
-			case SENSOR_POLLRATE_MANUAL:
-				stop();
-				_measure_ticks = 0;
-				return OK;
+			/* switching to manual polling */
+		case SENSOR_POLLRATE_MANUAL:
+			stop();
+			_measure_ticks = 0;
+			return OK;
 
-				/* external signalling (DRDY) not supported */
-			case SENSOR_POLLRATE_EXTERNAL:
+			/* external signalling (DRDY) not supported */
+		case SENSOR_POLLRATE_EXTERNAL:
 
-				/* zero would be bad */
-			case 0:
-				return -EINVAL;
+			/* zero would be bad */
+		case 0:
+			return -EINVAL;
 
-				/* set default/max polling rate */
-			case SENSOR_POLLRATE_MAX:
-			case SENSOR_POLLRATE_DEFAULT: {
-					/* do we need to start internal polling? */
-					bool want_start = (_measure_ticks == 0);
+			/* set default/max polling rate */
+		case SENSOR_POLLRATE_MAX:
+		case SENSOR_POLLRATE_DEFAULT: {
+				/* do we need to start internal polling? */
+				bool want_start = (_measure_ticks == 0);
 
-					/* set interval for next measurement to minimum legal value */
-					_measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
+				/* set interval for next measurement to minimum legal value */
+				_measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
 
-					/* if we need to start the poll state machine, do it */
-					if (want_start)
-						start();
+				/* if we need to start the poll state machine, do it */
+				if (want_start)
+					start();
 
-					return OK;
-				}
+				return OK;
+			}
 
-				/* adjust to a legal polling interval in Hz */
-			default: {
-					/* do we need to start internal polling? */
-					bool want_start = (_measure_ticks == 0);
+			/* adjust to a legal polling interval in Hz */
+		default: {
+				/* do we need to start internal polling? */
+				bool want_start = (_measure_ticks == 0);
 
-					/* convert hz to tick interval via microseconds */
-					unsigned ticks = USEC2TICK(1000000 / arg);
+				/* convert hz to tick interval via microseconds */
+				unsigned ticks = USEC2TICK(1000000 / arg);
 
-					/* check against maximum rate */
-					if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
-						return -EINVAL;
+				/* check against maximum rate */
+				if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
+					return -EINVAL;
 
-					/* update interval for next measurement */
-					_measure_ticks = ticks;
+				/* update interval for next measurement */
+				_measure_ticks = ticks;
 
-					/* if we need to start the poll state machine, do it */
-					if (want_start)
-						start();
+				/* if we need to start the poll state machine, do it */
+				if (want_start)
+					start();
 
-					return OK;
-				}
+				return OK;
 			}
 		}
+	}
 
 	case SENSORIOCGPOLLRATE:
 		if (_measure_ticks == 0)
@@ -633,13 +637,15 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return _num_reports - 1;
 
 	case SENSORIOCRESET:
-		/* XXX implement this */
-		return -EINVAL;
+		return reset();
 
 	case MAGIOCSSAMPLERATE:
+		/* same as pollrate because device is in single measurement mode*/
+		return ioctl(filp, SENSORIOCSPOLLRATE, arg);
+
 	case MAGIOCGSAMPLERATE:
-		/* not supported, always 1 sample per poll */
-		return -EINVAL;
+		/* same as pollrate because device is in single measurement mode*/
+		return ioctl(filp, SENSORIOCGPOLLRATE, 0);
 
 	case MAGIOCSRANGE:
 		return set_range(arg);
@@ -702,6 +708,13 @@ HMC5883::stop()
 	work_cancel(HPWORK, &_work);
 }
 
+int
+HMC5883::reset()
+{
+	/* set range */
+	return set_range(_range_ga);
+}
+
 void
 HMC5883::cycle_trampoline(void *arg)
 {