diff --git a/sw/airborne/arch/omap/mcu_periph/gpio_arch.h b/sw/airborne/arch/omap/mcu_periph/gpio_arch.h index 795f0e18f4..1f7c9a9487 100644 --- a/sw/airborne/arch/omap/mcu_periph/gpio_arch.h +++ b/sw/airborne/arch/omap/mcu_periph/gpio_arch.h @@ -34,16 +34,17 @@ /** * Set a gpio output to high level. */ -static inline void gpio_set(uint32_t port, uint16_t pin) { - -} +extern void gpio_set(uint32_t port, uint16_t pin); /** * Clear a gpio output to low level. */ -static inline void gpio_clear(uint32_t port, uint16_t pin) { +extern void gpio_clear(uint32_t port, uint16_t pin); -} +/** + * Read a gpio value. + */ +uint16_t gpio_get(uint32_t gpioport, uint16_t gpios); #endif /* GPIO_ARCH_H */ diff --git a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c index 5848b71643..d5fe722d09 100644 --- a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c +++ b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c @@ -30,7 +30,7 @@ #include "subsystems/actuators.h" #include "actuators_ardrone2_raw.h" -#include "gpio_ardrone.h" +#include "mcu_periph/gpio.h" #include /* Standard input/output definitions */ #include /* String function definitions */ @@ -52,6 +52,16 @@ */ int mot_fd; /**< File descriptor for the port */ +#define ARDRONE_GPIO_PORT 0 // Dummy for paparazzi compatibility + +#define ARDRONE_GPIO_PIN_MOTOR1 171 +#define ARDRONE_GPIO_PIN_MOTOR2 172 +#define ARDRONE_GPIO_PIN_MOTOR3 173 +#define ARDRONE_GPIO_PIN_MOTOR4 174 + +#define ARDRONE_GPIO_PIN_IRQ_FLIPFLOP 175 +#define ARDRONE_GPIO_PIN_IRQ_INPUT 176 + void actuators_ardrone_init(void) { //open mot port @@ -81,34 +91,34 @@ void actuators_ardrone_init(void) tcsetattr(mot_fd, TCSANOW, &options); //reset IRQ flipflop - on error 106 read 1, this code resets 106 to 0 - gpio_set_input(176); - gpio_set(175,0); - gpio_set(175,1); + gpio_setup_input(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_INPUT); + gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); //all select lines inactive - gpio_set(171,1); - gpio_set(172,1); - gpio_set(173,1); - gpio_set(174,1); + gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1); + gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR2); + gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR3); + gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR4); //configure motors uint8_t reply[256]; for(int m=0;m<4;m++) { - gpio_set(171+m,-1); + gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1 + m); actuators_ardrone_cmd(0xe0,reply,2); if(reply[0]!=0xe0 || reply[1]!=0x00) { printf("motor%d cmd=0x%02x reply=0x%02x\n",m+1,(int)reply[0],(int)reply[1]); } actuators_ardrone_cmd(m+1,reply,1); - gpio_set(171+m,1); + gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1 + m); } //all select lines active - gpio_set(171,0); - gpio_set(172,0); - gpio_set(173,0); - gpio_set(174,0); + gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1); + gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR2); + gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR3); + gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR4); //start multicast actuators_ardrone_cmd(0xa0,reply,1); @@ -118,8 +128,8 @@ void actuators_ardrone_init(void) actuators_ardrone_cmd(0xa0,reply,1); //reset IRQ flipflop - on error 176 reads 1, this code resets 176 to 0 - gpio_set(175,0); - gpio_set(175,1); + gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); // Left Red, Right Green actuators_ardrone_set_leds(MOT_LEDRED,MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDRED); @@ -136,7 +146,7 @@ void actuators_ardrone_motor_status(void); void actuators_ardrone_motor_status(void) { // If a motor IRQ lines is set - if (gpio_get(176) == 1) + if (gpio_get(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_IRQ_INPUT) == 1) { if (autopilot_motors_on) { @@ -144,8 +154,8 @@ void actuators_ardrone_motor_status(void) autopilot_set_motors_on(FALSE); // Toggle Flipflop reset so motors can be re-enabled - gpio_set(175,0); - gpio_set(175,1); + gpio_clear(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + gpio_set(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); } } } diff --git a/sw/airborne/boards/ardrone/gpio_ardrone.c b/sw/airborne/boards/ardrone/gpio_ardrone.c index b9172c7cdf..9222cd27ae 100644 --- a/sw/airborne/boards/ardrone/gpio_ardrone.c +++ b/sw/airborne/boards/ardrone/gpio_ardrone.c @@ -25,7 +25,8 @@ #include /* File control definitions */ #include /* Error number definitions */ #include -#include "gpio_ardrone.h" + +#include "mcu_periph/gpio.h" #define GPIO_MAGIC 'p' #define GPIO_DIRECTION _IOW(GPIO_MAGIC, 0, struct gpio_direction) @@ -49,22 +50,36 @@ struct gpio_direction { enum gpio_mode mode; }; -//val=0 -> set gpio output lo -//val=1 -> set gpio output hi -void gpio_set(int nr, int val) + +void gpio_set(uint32_t port, uint16_t pin) { struct gpio_data data; // Open the device if not open if (gpiofp == 0) - gpiofp = open("/dev/gpio",O_RDWR); + gpiofp = open("/dev/gpio",O_RDWR); // Read the GPIO value - data.pin = nr; - data.value = val; + data.pin = pin; + data.value = 1; ioctl(gpiofp, GPIO_WRITE, &data); } -void gpio_set_input(int nr) + +void gpio_clear(uint32_t port, uint16_t pin) +{ + struct gpio_data data; + // Open the device if not open + if (gpiofp == 0) + gpiofp = open("/dev/gpio",O_RDWR); + + // Read the GPIO value + data.pin = pin; + data.value = 0; + ioctl(gpiofp, GPIO_WRITE, &data); +} + + +void gpio_setup_input(uint32_t port, uint16_t pin) { struct gpio_direction dir; // Open the device if not open @@ -72,12 +87,28 @@ void gpio_set_input(int nr) gpiofp = open("/dev/gpio",O_RDWR); // Read the GPIO value - dir.pin = nr; + dir.pin = pin; dir.mode = GPIO_INPUT; ioctl(gpiofp, GPIO_DIRECTION, &dir); } -int gpio_get(int nr) + +void gpio_setup_output(uint32_t port, uint16_t pin) +{ + struct gpio_direction dir; + // Open the device if not open + if (gpiofp == 0) + gpiofp = open("/dev/gpio",O_RDWR); + + // Read the GPIO value + dir.pin = pin; + dir.mode = GPIO_OUTPUT_HIGH; + ioctl(gpiofp, GPIO_DIRECTION, &dir); +} + + + +uint16_t gpio_get(uint32_t gpioport, uint16_t pin) { struct gpio_data data; // Open the device if not open @@ -85,7 +116,7 @@ int gpio_get(int nr) gpiofp = open("/dev/gpio",O_RDWR); // Read the GPIO value - data.pin = nr; + data.pin = pin; ioctl(gpiofp, GPIO_READ, &data); return data.value; } diff --git a/sw/airborne/boards/ardrone/gpio_ardrone.h b/sw/airborne/boards/ardrone/gpio_ardrone.h deleted file mode 100644 index f455d33004..0000000000 --- a/sw/airborne/boards/ardrone/gpio_ardrone.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * Copyright (C) 2011 Hugo Perquin - http://blog.perquin.com - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, - * MA 02110-1301 USA. - */ - -/** - * @file boards/ardrone/gpio_ardrone.h - * ardrone GPIO driver - */ - -#ifndef GPIO_ARDRONE_H -#define GPIO_ARDRONE_H - -//val=0 -> set gpio output lo -//val=1 -> set gpio output hi -void gpio_set(int nr,int val); -void gpio_set_input(int nr); -int gpio_get(int nr); - -#endif /* GPIO_ARDRONE_H */