forked from Archive/PX4-Autopilot
Added IOCTL and command for sensor rail reset (does not yet re-initialize sensor drivers)
This commit is contained in:
parent
edc5b68499
commit
881cf61553
|
@ -81,6 +81,23 @@ __BEGIN_DECLS
|
|||
#define GPIO_EXTI_ACCEL_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN4)
|
||||
#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
|
||||
|
||||
/* Data ready pins off */
|
||||
#define GPIO_GYRO_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0)
|
||||
#define GPIO_MAG_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1)
|
||||
#define GPIO_ACCEL_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4)
|
||||
#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
|
||||
|
||||
/* SPI1 off */
|
||||
#define GPIO_SPI1_SCK_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5)
|
||||
#define GPIO_SPI1_MISO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN6)
|
||||
#define GPIO_SPI1_MOSI_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN7)
|
||||
|
||||
/* SPI1 chip selects off */
|
||||
#define GPIO_SPI_CS_GYRO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13)
|
||||
#define GPIO_SPI_CS_ACCEL_MAG_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15)
|
||||
#define GPIO_SPI_CS_BARO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7)
|
||||
#define GPIO_SPI_CS_MPU (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2)
|
||||
|
||||
/* SPI chip selects */
|
||||
#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
|
||||
#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue