From 012adc9e33bb9a92030174936546e67383b91a7a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 4 Dec 2013 09:25:07 +0100
Subject: [PATCH] Minor fixes to bus reset

---
 src/drivers/ms5611/ms5611.cpp |  7 +++++--
 src/drivers/px4fmu/fmu.cpp    | 14 ++++++++++----
 2 files changed, 15 insertions(+), 6 deletions(-)

diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 938786d3f1..87788824a9 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -420,8 +420,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return _reports->size();
 
 	case SENSORIOCRESET:
-		/* XXX implement this */
-		return -EINVAL;
+		/*
+		 * Since we are initialized, we do not need to do anything, since the
+		 * PROM is correctly read and the part does not need to be configured.
+		 */
+		return OK;
 
 	case BAROIOCSMSLPRESSURE:
 
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index d37c260f07..aab532514f 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -1151,7 +1151,8 @@ PX4FMU::sensor_reset(int ms)
 	stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);
 
 	/* wait for the sensor rail to reach GND */
-	usleep(ms * 000);
+	usleep(ms * 1000);
+	warnx("reset done, %d ms", ms);
 
 	/* re-enable power */
 
@@ -1289,7 +1290,7 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
 		break;
 
 	case GPIO_SENSOR_RAIL_RESET:
-		sensor_reset(20);
+		sensor_reset(arg);
 		break;
 
 	case GPIO_SET_OUTPUT:
@@ -1655,13 +1656,18 @@ fmu_main(int argc, char *argv[])
 	if (!strcmp(verb, "fake"))
 		fake(argc - 1, argv + 1);
 
-	if (!strcmp(verb, "sensor_reset"))
+	if (!strcmp(verb, "sensor_reset")) {
 		if (argc > 2) {
-			sensor_reset(strtol(argv[2], 0, 0));
+			int reset_time = strtol(argv[2], 0, 0);
+			sensor_reset(reset_time);
 
 		} else {
 			sensor_reset(0);
+			warnx("resettet default time");
 		}
+		exit(0);
+	}
+
 
 	fprintf(stderr, "FMU: unrecognised command, try:\n");
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)