diff options
Diffstat (limited to 'src/drivers/px4fmu/fmu.cpp')
| -rw-r--r-- | src/drivers/px4fmu/fmu.cpp | 71 |
1 files changed, 71 insertions, 0 deletions
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 3a9246bf2..7b09a4676 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -182,6 +182,7 @@ private: void gpio_reset(void); void sensor_reset(int ms); + void peripheral_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); @@ -1376,6 +1377,29 @@ PX4FMU::sensor_reset(int ms) #endif } +void +PX4FMU::peripheral_reset(int ms) +{ +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + + if (ms < 1) { + ms = 10; + } + + /* set the peripheral rails off */ + stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); + stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 1); + + /* wait for the peripheral rail to reach GND */ + usleep(ms * 1000); + warnx("reset done, %d ms", ms); + + /* re-enable power */ + + /* switch the peripheral rail back on */ + stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 0); +#endif +} void PX4FMU::gpio_reset(void) @@ -1488,6 +1512,10 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg) sensor_reset(arg); break; + case GPIO_PERIPHERAL_RAIL_RESET: + peripheral_reset(arg); + break; + case GPIO_SET_OUTPUT: case GPIO_SET_INPUT: case GPIO_SET_ALT_1: @@ -1529,6 +1557,7 @@ enum PortMode { PORT_GPIO_AND_SERIAL, PORT_PWM_AND_SERIAL, PORT_PWM_AND_GPIO, + PORT_PWM4, }; PortMode g_port_mode; @@ -1564,6 +1593,13 @@ fmu_new_mode(PortMode new_mode) #endif break; +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + case PORT_PWM4: + /* select 4-pin PWM mode */ + servo_mode = PX4FMU::MODE_4PWM; + break; +#endif + /* mixed modes supported on v1 board only */ #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) @@ -1667,6 +1703,24 @@ sensor_reset(int ms) } void +peripheral_reset(int ms) +{ + int fd; + + fd = open(PX4FMU_DEVICE_PATH, O_RDWR); + + if (fd < 0) { + errx(1, "open fail"); + } + + if (ioctl(fd, GPIO_PERIPHERAL_RAIL_RESET, ms) < 0) { + warnx("peripheral rail reset failed"); + } + + close(fd); +} + +void test(void) { int fd; @@ -1836,6 +1890,10 @@ fmu_main(int argc, char *argv[]) } else if (!strcmp(verb, "mode_pwm")) { new_mode = PORT_FULL_PWM; +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + } else if (!strcmp(verb, "mode_pwm4")) { + new_mode = PORT_PWM4; +#endif #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) } else if (!strcmp(verb, "mode_serial")) { @@ -1883,6 +1941,19 @@ fmu_main(int argc, char *argv[]) exit(0); } + if (!strcmp(verb, "peripheral_reset")) { + if (argc > 2) { + int reset_time = strtol(argv[2], 0, 0); + peripheral_reset(reset_time); + + } else { + peripheral_reset(0); + warnx("resettet default time"); + } + + exit(0); + } + if (!strcmp(verb, "i2c")) { if (argc > 3) { int bus = strtol(argv[2], 0, 0); |