From 3990332745f3e9c810da2e17331ce0b38f2eb29b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 17 Aug 2015 12:02:03 +1000 Subject: [PATCH] HAL_PX4: prevent error on GPIO line on change of pinMode thanks to Michael for noticing this --- libraries/AP_HAL_PX4/GPIO.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL_PX4/GPIO.cpp b/libraries/AP_HAL_PX4/GPIO.cpp index 46132ad73f..9eb0229567 100644 --- a/libraries/AP_HAL_PX4/GPIO.cpp +++ b/libraries/AP_HAL_PX4/GPIO.cpp @@ -69,7 +69,17 @@ void PX4GPIO::pinMode(uint8_t pin, uint8_t output) { switch (pin) { case PX4_GPIO_FMU_SERVO_PIN(0) ... PX4_GPIO_FMU_SERVO_PIN(5): - ioctl(_gpio_fmu_fd, output?GPIO_SET_OUTPUT:GPIO_SET_INPUT, 1U<<(pin-PX4_GPIO_FMU_SERVO_PIN(0))); + uint32_t pinmask = 1U<<(pin-PX4_GPIO_FMU_SERVO_PIN(0)); + if (output) { + uint8_t old_value = read(pin); + if (old_value) { + ioctl(_gpio_fmu_fd, GPIO_SET_OUTPUT_HIGH, pinmask); + } else { + ioctl(_gpio_fmu_fd, GPIO_SET_OUTPUT_LOW, pinmask); + } + } else { + ioctl(_gpio_fmu_fd, GPIO_SET_INPUT, pinmask); + } break; } }