diff --git a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c
index 3a11e8c0b3..5848b71643 100644
--- a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c
+++ b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c
@@ -81,7 +81,7 @@ 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(176,-1);
+  gpio_set_input(176);
   gpio_set(175,0);
   gpio_set(175,1);
 
@@ -105,10 +105,10 @@ void actuators_ardrone_init(void)
   }
 
   //all select lines active
-  gpio_set(171,-1);
-  gpio_set(172,-1);
-  gpio_set(173,-1);
-  gpio_set(174,-1);
+  gpio_set(171,0);
+  gpio_set(172,0);
+  gpio_set(173,0);
+  gpio_set(174,0);
 
   //start multicast
   actuators_ardrone_cmd(0xa0,reply,1);
@@ -118,7 +118,6 @@ 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(176,-1);
   gpio_set(175,0);
   gpio_set(175,1);
 
diff --git a/sw/airborne/boards/ardrone/gpio_ardrone.c b/sw/airborne/boards/ardrone/gpio_ardrone.c
index 3f8e1ce40d..b9172c7cdf 100644
--- a/sw/airborne/boards/ardrone/gpio_ardrone.c
+++ b/sw/airborne/boards/ardrone/gpio_ardrone.c
@@ -22,121 +22,70 @@
  * ardrone GPIO driver
  */
 
-#include <stdio.h>
-#include <stdlib.h>
-#include "gpio_ardrone.h"
-
-//val=0 -> set gpio output lo
-//val=1 -> set gpio output hi
-//val=-1 -> set gpio as input (output hi-Z)
-int gpio_set(int nr,int val)
-{
-  char cmdline[200];
-  if(val<0) sprintf(cmdline,"/usr/sbin/gpio %d -d i",nr);
-  else if(val>0) sprintf(cmdline,"/usr/sbin/gpio %d -d ho 1",nr);
-  else sprintf(cmdline,"/usr/sbin/gpio %d -d ho 0",nr);
-  return system(cmdline);
-}
-
-// Option 1:
-//#define WE_HAVE_NO_CLUE_YET
-
-// Option 2:
-#define WE_MUST_TO_USE_THE_TERRIBLE_HACK
-
-// Option 3:
-//#define WE_KNOW_HOW_ARDRONE_IOCTL_WORKS_ON_DEV_GPIO
-
-
-
-#ifdef WE_HAVE_NO_CLUE_YET
-
-int gpio_get(int nr)
-{
-  return 0;
-}
-
-#endif
-
-
-
-
-#ifdef WE_MUST_TO_USE_THE_TERRIBLE_HACK
-
-FILE* ardrone_system_pipe = 0;
-
-int gpio_get(int nr)
-{
-
-  if (ardrone_system_pipe == 0)
-  {
-	  char cmdline[200];
-	  sprintf(cmdline,"/usr/sbin/gpio %d -r",nr);
-	  ardrone_system_pipe = popen(cmdline,"r");
-	  if (!ardrone_system_pipe)
-	  {
-		  return -1;
-	  }
-  }
-  else
-  {
-	  // TODO: we now call this with a large delay expecting that all data is present
-	  // if (!feof(pipe)) // Still busy
-
-	  char buff[128];
-	  char* ret = fgets(buff, 128, ardrone_system_pipe);
-	  ret = fgets(buff, 128, ardrone_system_pipe);
-	  pclose(ardrone_system_pipe);
-	  ardrone_system_pipe = 0;
-
-	  if (ret == NULL)
-	  {
-	    return -2;
-	  }
-
-	  int pin = ret[25] - '0';
-
-	  printf("GPIO_GET: %d '%d' \n", nr, pin);
-
-	  return pin;
-  }
-  return -3;
-}
-
-#endif
-
-
-
-
-
-#ifdef WE_KNOW_HOW_ARDRONE_IOCTL_WORKS_ON_DEV_GPIO
-
 #include <fcntl.h>   /* File control definitions */
 #include <errno.h>   /* Error number definitions */
 #include <sys/ioctl.h>
+#include "gpio_ardrone.h"
 
-
-#define GPIO_IOCTL_COUNT 0
-#define GPIO_IOCTL_GET 2
-
+#define GPIO_MAGIC 'p'
+#define GPIO_DIRECTION _IOW(GPIO_MAGIC, 0, struct gpio_direction)
+#define GPIO_READ _IOWR(GPIO_MAGIC, 1, struct gpio_data)
+#define GPIO_WRITE _IOW(GPIO_MAGIC, 2, struct gpio_data)
 int gpiofp = 0;
-int gpio_get(int nr)
-{
-	if (gpiofp == 0)
-	{
-		gpiofp = open("/dev/gpio",O_RDWR);
-		printf("GPIO open %d\n", gpiofp);
-		// printf("%s", errno() );
-	}
-	else
-	{
-		int gpio = nr;
-		int ret = ioctl(gpiofp, GPIO_IOCTL_GET, &gpio);
-		printf("GPIO_ %d = %d %d \n",nr,gpio, ret);
-	}
 
-	// We don't know yet
-	return 0;
+struct gpio_data {
+  int pin;
+  int value;
+};
+
+enum gpio_mode {
+    GPIO_INPUT = 0,             //!< Pin configured for input
+    GPIO_OUTPUT_LOW,            //!< Pin configured for output with low level
+    GPIO_OUTPUT_HIGH,           //!< Pin configured for output with high level
+};
+
+struct gpio_direction {
+    int pin;
+    enum gpio_mode mode;
+};
+
+//val=0 -> set gpio output lo
+//val=1 -> set gpio output hi
+void gpio_set(int nr, int val)
+{
+  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 = nr;
+  data.value = val;
+  ioctl(gpiofp, GPIO_WRITE, &data);
 }
 
-#endif
+void gpio_set_input(int nr)
+{
+  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 = nr;
+  dir.mode = GPIO_INPUT;
+  ioctl(gpiofp, GPIO_DIRECTION, &dir);
+}
+
+int gpio_get(int nr)
+{
+  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 = nr;
+  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
index 56dee38d43..f455d33004 100644
--- a/sw/airborne/boards/ardrone/gpio_ardrone.h
+++ b/sw/airborne/boards/ardrone/gpio_ardrone.h
@@ -27,8 +27,8 @@
 
 //val=0 -> set gpio output lo
 //val=1 -> set gpio output hi
-//val=-1 -> set gpio as input (output hi-Z)
-int gpio_set(int nr,int val);
+void gpio_set(int nr,int val);
+void gpio_set_input(int nr);
 int gpio_get(int nr);
 
 #endif /* GPIO_ARDRONE_H */