Added sensor rail reset IOCTL and command (fmu sensor_reset 10 resets for 10 ms)

1 files changed, 277 insertions, 179 deletions

diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 9433ab59f..d37c260f0 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -164,6 +164,7 @@ private:
 	static const unsigned	_ngpio;
 
 	void		gpio_reset(void);
+	void		sensor_reset(int ms);
 	void		gpio_set_function(uint32_t gpios, int function);
 	void		gpio_write(uint32_t gpios, int function);
 	uint32_t	gpio_read(void);
@@ -226,10 +227,10 @@ PX4FMU::PX4FMU() :
 	_armed(false),
 	_pwm_on(false),
 	_mixers(nullptr),
-	_failsafe_pwm({0}),
-	_disarmed_pwm({0}),
-	_num_failsafe_set(0),
-	_num_disarmed_set(0)
+	_failsafe_pwm( {0}),
+	       _disarmed_pwm( {0}),
+	       _num_failsafe_set(0),
+	       _num_disarmed_set(0)
 {
 	for (unsigned i = 0; i < _max_actuators; i++) {
 		_min_pwm[i] = PWM_DEFAULT_MIN;
@@ -293,11 +294,11 @@ PX4FMU::init()
 
 	/* start the IO interface task */
 	_task = task_spawn_cmd("fmuservo",
-			   SCHED_DEFAULT,
-			   SCHED_PRIORITY_DEFAULT,
-			   2048,
-			   (main_t)&PX4FMU::task_main_trampoline,
-			   nullptr);
+			       SCHED_DEFAULT,
+			       SCHED_PRIORITY_DEFAULT,
+			       2048,
+			       (main_t)&PX4FMU::task_main_trampoline,
+			       nullptr);
 
 	if (_task < 0) {
 		debug("task start failed: %d", errno);
@@ -326,7 +327,7 @@ PX4FMU::set_mode(Mode mode)
 	switch (mode) {
 	case MODE_2PWM:	// v1 multi-port with flow control lines as PWM
 		debug("MODE_2PWM");
-		
+
 		/* default output rates */
 		_pwm_default_rate = 50;
 		_pwm_alt_rate = 50;
@@ -340,7 +341,7 @@ PX4FMU::set_mode(Mode mode)
 
 	case MODE_4PWM: // v1 multi-port as 4 PWM outs
 		debug("MODE_4PWM");
-		
+
 		/* default output rates */
 		_pwm_default_rate = 50;
 		_pwm_alt_rate = 50;
@@ -354,7 +355,7 @@ PX4FMU::set_mode(Mode mode)
 
 	case MODE_6PWM: // v2 PWMs as 6 PWM outs
 		debug("MODE_6PWM");
-		
+
 		/* default output rates */
 		_pwm_default_rate = 50;
 		_pwm_alt_rate = 50;
@@ -396,6 +397,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
 
 			// get the channel mask for this rate group
 			uint32_t mask = up_pwm_servo_get_rate_group(group);
+
 			if (mask == 0)
 				continue;
 
@@ -409,6 +411,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
 					// not a legal map, bail
 					return -EINVAL;
 				}
+
 			} else {
 				// set it - errors here are unexpected
 				if (alt != 0) {
@@ -416,6 +419,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
 						warn("rate group set alt failed");
 						return -EINVAL;
 					}
+
 				} else {
 					if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) {
 						warn("rate group set default failed");
@@ -425,6 +429,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
 			}
 		}
 	}
+
 	_pwm_alt_rate_channels = rate_map;
 	_pwm_default_rate = default_rate;
 	_pwm_alt_rate = alt_rate;
@@ -471,7 +476,7 @@ PX4FMU::task_main()
 	memset(&controls_effective, 0, sizeof(controls_effective));
 	/* advertise the effective control inputs */
 	_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
-				   &controls_effective);
+					       &controls_effective);
 
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
@@ -503,6 +508,7 @@ PX4FMU::task_main()
 		 * We always mix at max rate; some channels may update slower.
 		 */
 		unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate;
+
 		if (_current_update_rate != max_rate) {
 			_current_update_rate = max_rate;
 			int update_rate_in_ms = int(1000 / _current_update_rate);
@@ -511,6 +517,7 @@ PX4FMU::task_main()
 			if (update_rate_in_ms < 2) {
 				update_rate_in_ms = 2;
 			}
+
 			/* reject slower than 10 Hz updates */
 			if (update_rate_in_ms > 100) {
 				update_rate_in_ms = 100;
@@ -532,6 +539,7 @@ PX4FMU::task_main()
 			log("poll error %d", errno);
 			usleep(1000000);
 			continue;
+
 		} else if (ret == 0) {
 			/* timeout: no control data, switch to failsafe values */
 //			warnx("no PWM: failsafe");
@@ -553,12 +561,15 @@ PX4FMU::task_main()
 					case MODE_2PWM:
 						num_outputs = 2;
 						break;
+
 					case MODE_4PWM:
 						num_outputs = 4;
 						break;
+
 					case MODE_6PWM:
 						num_outputs = 6;
 						break;
+
 					default:
 						num_outputs = 0;
 						break;
@@ -572,9 +583,9 @@ PX4FMU::task_main()
 					for (unsigned i = 0; i < num_outputs; i++) {
 						/* last resort: catch NaN, INF and out-of-band errors */
 						if (i >= outputs.noutputs ||
-							!isfinite(outputs.output[i]) ||
-							outputs.output[i] < -1.0f ||
-							outputs.output[i] > 1.0f) {
+						    !isfinite(outputs.output[i]) ||
+						    outputs.output[i] < -1.0f ||
+						outputs.output[i] > 1.0f) {
 							/*
 							 * Value is NaN, INF or out of band - set to the minimum value.
 							 * This will be clearly visible on the servo status and will limit the risk of accidentally
@@ -592,6 +603,7 @@ PX4FMU::task_main()
 					for (unsigned i = 0; i < num_outputs; i++) {
 						controls_effective.control_effective[i] = (float)pwm_limited[i];
 					}
+
 					orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
 
 					/* output to the servos */
@@ -613,11 +625,13 @@ PX4FMU::task_main()
 
 				/* update the armed status and check that we're not locked down */
 				bool set_armed = aa.armed && !aa.lockdown;
+
 				if (_armed != set_armed)
 					_armed = set_armed;
 
 				/* update PWM status if armed or if disarmed PWM values are set */
 				bool pwm_on = (aa.armed || _num_disarmed_set > 0);
+
 				if (_pwm_on != pwm_on) {
 					_pwm_on = pwm_on;
 					up_pwm_servo_arm(pwm_on);
@@ -626,25 +640,31 @@ PX4FMU::task_main()
 		}
 
 #ifdef HRT_PPM_CHANNEL
+
 		// see if we have new PPM input data
 		if (ppm_last_valid_decode != rc_in.timestamp) {
 			// we have a new PPM frame. Publish it.
 			rc_in.channel_count = ppm_decoded_channels;
+
 			if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
 				rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
 			}
-			for (uint8_t i=0; i<rc_in.channel_count; i++) {
+
+			for (uint8_t i = 0; i < rc_in.channel_count; i++) {
 				rc_in.values[i] = ppm_buffer[i];
 			}
+
 			rc_in.timestamp = ppm_last_valid_decode;
 
 			/* lazily advertise on first publication */
 			if (to_input_rc == 0) {
 				to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in);
-			} else { 
+
+			} else {
 				orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
 			}
 		}
+
 #endif
 
 	}
@@ -753,142 +773,176 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		break;
 
 	case PWM_SERVO_SET_FAILSAFE_PWM: {
-		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
-		/* discard if too many values are sent */
-		if (pwm->channel_count > _max_actuators) {
-			ret = -EINVAL;
-			break;
-		}
-		for (unsigned i = 0; i < pwm->channel_count; i++) {
-			if (pwm->values[i] == 0) {
-				/* ignore 0 */
-			} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
-				_failsafe_pwm[i] = PWM_HIGHEST_MAX;
-			} else if (pwm->values[i] < PWM_LOWEST_MIN) {
-				_failsafe_pwm[i] = PWM_LOWEST_MIN;
-			} else {
-				_failsafe_pwm[i] = pwm->values[i];
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			/* discard if too many values are sent */
+			if (pwm->channel_count > _max_actuators) {
+				ret = -EINVAL;
+				break;
 			}
-		}
 
-		/*
-		 * update the counter
-		 * this is needed to decide if disarmed PWM output should be turned on or not
-		 */
-		_num_failsafe_set = 0;
-		for (unsigned i = 0; i < _max_actuators; i++) {
-			if (_failsafe_pwm[i] > 0)
-				_num_failsafe_set++;
+			for (unsigned i = 0; i < pwm->channel_count; i++) {
+				if (pwm->values[i] == 0) {
+					/* ignore 0 */
+				} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
+					_failsafe_pwm[i] = PWM_HIGHEST_MAX;
+
+				} else if (pwm->values[i] < PWM_LOWEST_MIN) {
+					_failsafe_pwm[i] = PWM_LOWEST_MIN;
+
+				} else {
+					_failsafe_pwm[i] = pwm->values[i];
+				}
+			}
+
+			/*
+			 * update the counter
+			 * this is needed to decide if disarmed PWM output should be turned on or not
+			 */
+			_num_failsafe_set = 0;
+
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				if (_failsafe_pwm[i] > 0)
+					_num_failsafe_set++;
+			}
+
+			break;
 		}
-		break;
-	}
+
 	case PWM_SERVO_GET_FAILSAFE_PWM: {
-		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
-		for (unsigned i = 0; i < _max_actuators; i++) {
-			pwm->values[i] = _failsafe_pwm[i];
-		}
-		pwm->channel_count = _max_actuators;
-		break;
-	}
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
 
-	case PWM_SERVO_SET_DISARMED_PWM: {
-		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
-		/* discard if too many values are sent */
-		if (pwm->channel_count > _max_actuators) {
-			ret = -EINVAL;
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				pwm->values[i] = _failsafe_pwm[i];
+			}
+
+			pwm->channel_count = _max_actuators;
 			break;
 		}
-		for (unsigned i = 0; i < pwm->channel_count; i++) {
-			if (pwm->values[i] == 0) {
-				/* ignore 0 */
-			} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
-				_disarmed_pwm[i] = PWM_HIGHEST_MAX;
-			} else if (pwm->values[i] < PWM_LOWEST_MIN) {
-				_disarmed_pwm[i] = PWM_LOWEST_MIN;
-			} else {
-				_disarmed_pwm[i] = pwm->values[i];
+
+	case PWM_SERVO_SET_DISARMED_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			/* discard if too many values are sent */
+			if (pwm->channel_count > _max_actuators) {
+				ret = -EINVAL;
+				break;
 			}
-		}
 
-		/*
-		 * update the counter
-		 * this is needed to decide if disarmed PWM output should be turned on or not
-		 */
-		_num_disarmed_set = 0;
-		for (unsigned i = 0; i < _max_actuators; i++) {
-			if (_disarmed_pwm[i] > 0)
-				_num_disarmed_set++;
+			for (unsigned i = 0; i < pwm->channel_count; i++) {
+				if (pwm->values[i] == 0) {
+					/* ignore 0 */
+				} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
+					_disarmed_pwm[i] = PWM_HIGHEST_MAX;
+
+				} else if (pwm->values[i] < PWM_LOWEST_MIN) {
+					_disarmed_pwm[i] = PWM_LOWEST_MIN;
+
+				} else {
+					_disarmed_pwm[i] = pwm->values[i];
+				}
+			}
+
+			/*
+			 * update the counter
+			 * this is needed to decide if disarmed PWM output should be turned on or not
+			 */
+			_num_disarmed_set = 0;
+
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				if (_disarmed_pwm[i] > 0)
+					_num_disarmed_set++;
+			}
+
+			break;
 		}
-		break;
-	}
+
 	case PWM_SERVO_GET_DISARMED_PWM: {
-		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
-		for (unsigned i = 0; i < _max_actuators; i++) {
-			pwm->values[i] = _disarmed_pwm[i];
-		}
-		pwm->channel_count = _max_actuators;
-		break;
-	}
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
 
-	case PWM_SERVO_SET_MIN_PWM: {
-		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
-		/* discard if too many values are sent */
-		if (pwm->channel_count > _max_actuators) {
-			ret = -EINVAL;
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				pwm->values[i] = _disarmed_pwm[i];
+			}
+
+			pwm->channel_count = _max_actuators;
 			break;
 		}
-		for (unsigned i = 0; i < pwm->channel_count; i++) {
-			if (pwm->values[i] == 0) {
-				/* ignore 0 */
-			} else if (pwm->values[i] > PWM_HIGHEST_MIN) {
-				_min_pwm[i] = PWM_HIGHEST_MIN;
-			} else if (pwm->values[i] < PWM_LOWEST_MIN) {
-				_min_pwm[i] = PWM_LOWEST_MIN;
-			} else {
-				_min_pwm[i] = pwm->values[i];
+
+	case PWM_SERVO_SET_MIN_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			/* discard if too many values are sent */
+			if (pwm->channel_count > _max_actuators) {
+				ret = -EINVAL;
+				break;
 			}
+
+			for (unsigned i = 0; i < pwm->channel_count; i++) {
+				if (pwm->values[i] == 0) {
+					/* ignore 0 */
+				} else if (pwm->values[i] > PWM_HIGHEST_MIN) {
+					_min_pwm[i] = PWM_HIGHEST_MIN;
+
+				} else if (pwm->values[i] < PWM_LOWEST_MIN) {
+					_min_pwm[i] = PWM_LOWEST_MIN;
+
+				} else {
+					_min_pwm[i] = pwm->values[i];
+				}
+			}
+
+			break;
 		}
-		break;
-	}
+
 	case PWM_SERVO_GET_MIN_PWM: {
-		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
-		for (unsigned i = 0; i < _max_actuators; i++) {
-			pwm->values[i] = _min_pwm[i];
-		}
-		pwm->channel_count = _max_actuators;
-		arg = (unsigned long)&pwm;
-		break;
-	}
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
 
-	case PWM_SERVO_SET_MAX_PWM: {
-		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
-		/* discard if too many values are sent */
-		if (pwm->channel_count > _max_actuators) {
-			ret = -EINVAL;
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				pwm->values[i] = _min_pwm[i];
+			}
+
+			pwm->channel_count = _max_actuators;
+			arg = (unsigned long)&pwm;
 			break;
 		}
-		for (unsigned i = 0; i < pwm->channel_count; i++) {
-			if (pwm->values[i] == 0) {
-				/* ignore 0 */
-			} else if (pwm->values[i] < PWM_LOWEST_MAX) {
-				_max_pwm[i] = PWM_LOWEST_MAX;
-			} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
-				_max_pwm[i] = PWM_HIGHEST_MAX;
-			} else {
-				_max_pwm[i] = pwm->values[i];
+
+	case PWM_SERVO_SET_MAX_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			/* discard if too many values are sent */
+			if (pwm->channel_count > _max_actuators) {
+				ret = -EINVAL;
+				break;
 			}
+
+			for (unsigned i = 0; i < pwm->channel_count; i++) {
+				if (pwm->values[i] == 0) {
+					/* ignore 0 */
+				} else if (pwm->values[i] < PWM_LOWEST_MAX) {
+					_max_pwm[i] = PWM_LOWEST_MAX;
+
+				} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
+					_max_pwm[i] = PWM_HIGHEST_MAX;
+
+				} else {
+					_max_pwm[i] = pwm->values[i];
+				}
+			}
+
+			break;
 		}
-		break;
-	}
+
 	case PWM_SERVO_GET_MAX_PWM: {
-		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
-		for (unsigned i = 0; i < _max_actuators; i++) {
-			pwm->values[i] = _max_pwm[i];
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				pwm->values[i] = _max_pwm[i];
+			}
+
+			pwm->channel_count = _max_actuators;
+			arg = (unsigned long)&pwm;
+			break;
 		}
-		pwm->channel_count = _max_actuators;
-		arg = (unsigned long)&pwm;
-		break;
-	}
 
 	case PWM_SERVO_SET(5):
 	case PWM_SERVO_SET(4):
@@ -910,6 +964,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_SET(0):
 		if (arg <= 2100) {
 			up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
+
 		} else {
 			ret = -EINVAL;
 		}
@@ -946,18 +1001,21 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
 		break;
 
-	case PWM_SERVO_GET_COUNT:	
+	case PWM_SERVO_GET_COUNT:
 	case MIXERIOCGETOUTPUTCOUNT:
 		switch (_mode) {
 		case MODE_6PWM:
 			*(unsigned *)arg = 6;
 			break;
+
 		case MODE_4PWM:
 			*(unsigned *)arg = 4;
 			break;
+
 		case MODE_2PWM:
 			*(unsigned *)arg = 2;
 			break;
+
 		default:
 			ret = -EINVAL;
 			break;
@@ -1015,6 +1073,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 					ret = -EINVAL;
 				}
 			}
+
 			break;
 		}
 
@@ -1049,55 +1108,58 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
 	for (uint8_t i = 0; i < count; i++) {
 		up_pwm_servo_set(i, values[i]);
 	}
+
 	return count * 2;
 }
 
 void
 PX4FMU::sensor_reset(int ms)
 {
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+
 	if (ms < 1) {
 		ms = 1;
 	}
 
 	/* disable SPI bus */
-        stm32_configgpio(GPIO_SPI_CS_GYRO_OFF);
-        stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF);
-        stm32_configgpio(GPIO_SPI_CS_BARO_OFF);
+	stm32_configgpio(GPIO_SPI_CS_GYRO_OFF);
+	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF);
+	stm32_configgpio(GPIO_SPI_CS_BARO_OFF);
 
-        stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0);
-        stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0);
-        stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0);
+	stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0);
+	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0);
+	stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0);
 
-        stm32_configgpio(GPIO_SPI1_SCK_OFF);
-        stm32_configgpio(GPIO_SPI1_MISO_OFF);
-        stm32_configgpio(GPIO_SPI1_MOSI_OFF);
+	stm32_configgpio(GPIO_SPI1_SCK_OFF);
+	stm32_configgpio(GPIO_SPI1_MISO_OFF);
+	stm32_configgpio(GPIO_SPI1_MOSI_OFF);
 
-        stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0);
-        stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0);
-        stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0);
+	stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0);
+	stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0);
+	stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0);
 
-        stm32_configgpio(GPIO_GYRO_DRDY_OFF);
-        stm32_configgpio(GPIO_MAG_DRDY_OFF);
-        stm32_configgpio(GPIO_ACCEL_DRDY_OFF);
+	stm32_configgpio(GPIO_GYRO_DRDY_OFF);
+	stm32_configgpio(GPIO_MAG_DRDY_OFF);
+	stm32_configgpio(GPIO_ACCEL_DRDY_OFF);
 
-        stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0);
-        stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0);
-        stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0);
+	stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0);
+	stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0);
+	stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0);
 
-        /* set the sensor rail off */
-        stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
-        stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);
+	/* set the sensor rail off */
+	stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
+	stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);
 
-        /* wait for the sensor rail to reach GND */
-        usleep(ms * 000);
+	/* wait for the sensor rail to reach GND */
+	usleep(ms * 000);
 
 	/* re-enable power */
 
 	/* switch the sensor rail back on */
-        stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1);
+	stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1);
 
-        /* wait a bit before starting SPI, different times didn't influence results */
-        usleep(100);
+	/* wait a bit before starting SPI, different times didn't influence results */
+	usleep(100);
 
 	/* reconfigure the SPI pins */
 #ifdef CONFIG_STM32_SPI1
@@ -1115,8 +1177,10 @@ PX4FMU::sensor_reset(int ms)
 	stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
 	stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
 #endif
+#endif
 }
 
+
 void
 PX4FMU::gpio_reset(void)
 {
@@ -1127,6 +1191,7 @@ PX4FMU::gpio_reset(void)
 	for (unsigned i = 0; i < _ngpio; i++) {
 		if (_gpio_tab[i].input != 0) {
 			stm32_configgpio(_gpio_tab[i].input);
+
 		} else if (_gpio_tab[i].output != 0) {
 			stm32_configgpio(_gpio_tab[i].output);
 		}
@@ -1143,6 +1208,7 @@ void
 PX4FMU::gpio_set_function(uint32_t gpios, int function)
 {
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+
 	/*
 	 * GPIOs 0 and 1 must have the same direction as they are buffered
 	 * by a shared 2-port driver.  Any attempt to set either sets both.
@@ -1154,6 +1220,7 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
 		if (GPIO_SET_OUTPUT == function)
 			stm32_gpiowrite(GPIO_GPIO_DIR, 1);
 	}
+
 #endif
 
 	/* configure selected GPIOs as required */
@@ -1178,9 +1245,11 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
 	}
 
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+
 	/* flip buffer to input mode if required */
 	if ((GPIO_SET_INPUT == function) && (gpios & 3))
 		stm32_gpiowrite(GPIO_GPIO_DIR, 0);
+
 #endif
 }
 
@@ -1219,8 +1288,8 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
 		gpio_reset();
 		break;
 
-	case SENSOR_RESET:
-		sensor_reset();
+	case GPIO_SENSOR_RAIL_RESET:
+		sensor_reset(20);
 		break;
 
 	case GPIO_SET_OUTPUT:
@@ -1296,8 +1365,9 @@ fmu_new_mode(PortMode new_mode)
 #endif
 		break;
 
-	/* mixed modes supported on v1 board only */
+		/* mixed modes supported on v1 board only */
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+
 	case PORT_FULL_SERIAL:
 		/* set all multi-GPIOs to serial mode */
 		gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
@@ -1320,6 +1390,7 @@ fmu_new_mode(PortMode new_mode)
 		servo_mode = PX4FMU::MODE_2PWM;
 		break;
 #endif
+
 	default:
 		return -1;
 	}
@@ -1374,14 +1445,30 @@ fmu_stop(void)
 }
 
 void
+sensor_reset(int ms)
+{
+	int	 fd;
+	int	 ret;
+
+	fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
+
+	if (fd < 0)
+		errx(1, "open fail");
+
+	if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0)
+		err(1, "servo arm failed");
+
+}
+
+void
 test(void)
 {
 	int	 fd;
-        unsigned servo_count = 0;
+	unsigned servo_count = 0;
 	unsigned pwm_value = 1000;
 	int	 direction = 1;
 	int	 ret;
-        
+
 	fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
 
 	if (fd < 0)
@@ -1389,9 +1476,9 @@ test(void)
 
 	if (ioctl(fd, PWM_SERVO_ARM, 0) < 0)       err(1, "servo arm failed");
 
-        if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) {
-            err(1, "Unable to get servo count\n");        
-        }
+	if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) {
+		err(1, "Unable to get servo count\n");
+	}
 
 	warnx("Testing %u servos", (unsigned)servo_count);
 
@@ -1404,32 +1491,38 @@ test(void)
 	for (;;) {
 		/* sweep all servos between 1000..2000 */
 		servo_position_t servos[servo_count];
+
 		for (unsigned i = 0; i < servo_count; i++)
 			servos[i] = pwm_value;
 
-                if (direction == 1) {
-                    // use ioctl interface for one direction
-                    for (unsigned i=0; i < servo_count;	i++) {
-                        if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
-                            err(1, "servo %u set failed", i);
-                        }
-                    }
-                } else {
-                    // and use write interface for the other direction
-                    ret = write(fd, servos, sizeof(servos));
-                    if (ret != (int)sizeof(servos))
-			err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
-                }
+		if (direction == 1) {
+			// use ioctl interface for one direction
+			for (unsigned i = 0; i < servo_count;	i++) {
+				if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
+					err(1, "servo %u set failed", i);
+				}
+			}
+
+		} else {
+			// and use write interface for the other direction
+			ret = write(fd, servos, sizeof(servos));
+
+			if (ret != (int)sizeof(servos))
+				err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
+		}
 
 		if (direction > 0) {
 			if (pwm_value < 2000) {
 				pwm_value++;
+
 			} else {
 				direction = -1;
 			}
+
 		} else {
 			if (pwm_value > 1000) {
 				pwm_value--;
+
 			} else {
 				direction = 1;
 			}
@@ -1441,6 +1534,7 @@ test(void)
 
 			if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value))
 				err(1, "error reading PWM servo %d", i);
+
 			if (value != servos[i])
 				errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
 		}
@@ -1448,12 +1542,14 @@ test(void)
 		/* Check if user wants to quit */
 		char c;
 		ret = poll(&fds, 1, 0);
+
 		if (ret > 0) {
 
 			read(0, &c, 1);
+
 			if (c == 0x03 || c == 0x63 || c == 'q') {
 				warnx("User abort\n");
-                                break;
+				break;
 			}
 		}
 	}
@@ -1526,6 +1622,7 @@ fmu_main(int argc, char *argv[])
 		new_mode = PORT_FULL_PWM;
 
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+
 	} else if (!strcmp(verb, "mode_serial")) {
 		new_mode = PORT_FULL_SERIAL;
 
@@ -1561,6 +1658,7 @@ fmu_main(int argc, char *argv[])
 	if (!strcmp(verb, "sensor_reset"))
 		if (argc > 2) {
 			sensor_reset(strtol(argv[2], 0, 0));
+
 		} else {
 			sensor_reset(0);
 		}