From 881cf61553f1acce6054db635e0d1af11476eb3e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Dec 2013 07:57:23 +0100 Subject: Added IOCTL and command for sensor rail reset (does not yet re-initialize sensor drivers) --- src/drivers/px4fmu/fmu.cpp | 78 +++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 77 insertions(+), 1 deletion(-) (limited to 'src/drivers/px4fmu/fmu.cpp') diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 0441566e9..9433ab59f 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1052,6 +1052,71 @@ PX4FMU::write(file *filp, const char *buffer, size_t len) return count * 2; } +void +PX4FMU::sensor_reset(int ms) +{ + 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_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_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_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); + + /* 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); + + /* wait a bit before starting SPI, different times didn't influence results */ + usleep(100); + + /* reconfigure the SPI pins */ +#ifdef CONFIG_STM32_SPI1 + stm32_configgpio(GPIO_SPI_CS_GYRO); + stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); + stm32_configgpio(GPIO_SPI_CS_BARO); + stm32_configgpio(GPIO_SPI_CS_MPU); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); +#endif +} + void PX4FMU::gpio_reset(void) { @@ -1154,6 +1219,10 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg) gpio_reset(); break; + case SENSOR_RESET: + sensor_reset(); + break; + case GPIO_SET_OUTPUT: case GPIO_SET_INPUT: case GPIO_SET_ALT_1: @@ -1489,11 +1558,18 @@ fmu_main(int argc, char *argv[]) if (!strcmp(verb, "fake")) fake(argc - 1, argv + 1); + if (!strcmp(verb, "sensor_reset")) + if (argc > 2) { + sensor_reset(strtol(argv[2], 0, 0)); + } else { + sensor_reset(0); + } + fprintf(stderr, "FMU: unrecognised command, try:\n"); #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n"); #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - fprintf(stderr, " mode_gpio, mode_pwm, test\n"); + fprintf(stderr, " mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n"); #endif exit(1); } -- cgit v1.2.3