[ardrone2] merge remaining ardrone2 support

closes #396

sw/airborne/boards/ardrone/actuators_ardrone2_raw.c

@@ -0,0 +1,169 @@
+/*
+ * Original Code from:
+ * Copyright (C) 2011 Hugo Perquin - http://blog.perquin.com
+ *
+ * Adapated for Paparazzi by:
+ * Copyright (C) 2012 Dino Hensen <dino.hensen@gmail.com>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi 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, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file boards/ardrone/actuators_ardrone2_raw.c
+ * Actuator driver for ardrone2-raw version
+ */
+
+#include "subsystems/actuators.h"
+#include "actuators_ardrone2_raw.h"
+#include "gpio.h"
+
+#include <stdio.h>   /* Standard input/output definitions */
+#include <string.h>  /* String function definitions */
+#include <unistd.h>  /* UNIX standard function definitions */
+#include <fcntl.h>   /* File control definitions */
+#include <errno.h>   /* Error number definitions */
+#include <termios.h> /* POSIX terminal control definitions */
+#include <stdint.h>
+
+/**
+ * Power consumption @ 11V all 4 motors running
+ * PWM   A
+ * 0    0.2
+ * 80   1.3
+ * 100  1.5
+ * 150  2.0
+ * 190  2.5
+ * 130  3.0
+ */
+int mot_fd; /**< File descriptor for the port */
+
+void actuators_ardrone_init(void)
+{
+  //open mot port
+  mot_fd = open("/dev/ttyO0", O_RDWR | O_NOCTTY | O_NDELAY);
+  if (mot_fd == -1)
+  {
+    perror("open_port: Unable to open /dev/ttyO0 - ");
+    return;
+  }
+  fcntl(mot_fd, F_SETFL, 0); //read calls are non blocking
+  fcntl(mot_fd, F_GETFL, 0);
+
+  //set port options
+  struct termios options;
+  //Get the current options for the port
+  tcgetattr(mot_fd, &options);
+  //Set the baud rates to 115200
+  cfsetispeed(&options, B115200);
+  cfsetospeed(&options, B115200);
+
+  options.c_cflag |= (CLOCAL | CREAD); //Enable the receiver and set local mode
+  options.c_iflag = 0; //clear input options
+  options.c_lflag=0; //clear local options
+  options.c_oflag &= ~OPOST; //clear output options (raw output)
+
+  //Set the new options for the port
+  tcsetattr(mot_fd, TCSANOW, &options);
+
+  //reset IRQ flipflop - on error 106 read 1, this code resets 106 to 0
+  gpio_set(106,-1);
+  gpio_set(107,0);
+  gpio_set(107,1);
+
+  //all select lines inactive
+  gpio_set(68,1);
+  gpio_set(69,1);
+  gpio_set(70,1);
+  gpio_set(71,1);
+
+  //configure motors
+  uint8_t reply[256];
+  for(int m=0;m<4;m++) {
+    gpio_set(68+m,-1);
+    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(68+m,1);
+  }
+
+  //all select lines active
+  gpio_set(68,-1);
+  gpio_set(69,-1);
+  gpio_set(70,-1);
+  gpio_set(71,-1);
+
+  //start multicast
+  actuators_ardrone_cmd(0xa0,reply,1);
+  actuators_ardrone_cmd(0xa0,reply,1);
+  actuators_ardrone_cmd(0xa0,reply,1);
+  actuators_ardrone_cmd(0xa0,reply,1);
+  actuators_ardrone_cmd(0xa0,reply,1);
+
+  //reset IRQ flipflop - on error 106 read 1, this code resets 106 to 0
+  gpio_set(106,-1);
+  gpio_set(107,0);
+  gpio_set(107,1);
+
+  //all leds green
+//  actuators_ardrone_set_leds(MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDGREEN);
+}
+
+int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen) {
+  write(mot_fd, &cmd, 1);
+  return read(mot_fd, reply, replylen);
+}
+
+void actuators_ardrone_commit(void)
+{
+  actuators_ardrone_set_pwm(actuators_pwm_values[0], actuators_pwm_values[1], actuators_pwm_values[2], actuators_pwm_values[3]);
+}
+
+/**
+ * Write motor speed command
+ * cmd = 001aaaaa aaaabbbb bbbbbccc ccccccdd ddddddd0
+ */
+void actuators_ardrone_set_pwm(uint16_t pwm0, uint16_t pwm1, uint16_t pwm2, uint16_t pwm3)
+{
+  uint8_t cmd[5];
+  cmd[0] = 0x20 | ((pwm0&0x1ff)>>4);
+  cmd[1] = ((pwm0&0x1ff)<<4) | ((pwm1&0x1ff)>>5);
+  cmd[2] = ((pwm1&0x1ff)<<3) | ((pwm2&0x1ff)>>6);
+  cmd[3] = ((pwm2&0x1ff)<<2) | ((pwm3&0x1ff)>>7);
+  cmd[4] = ((pwm3&0x1ff)<<1);
+  write(mot_fd, cmd, 5);
+}
+
+/**
+ * Write LED command
+ * cmd = 011grgrg rgrxxxxx (this is ardrone1 format, we need ardrone2 format)
+ */
+void actuators_ardrone_set_leds(uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3)
+{
+  uint8_t cmd[2];
+  cmd[0]=0x60 | ((led0&3)<<3) | ((led1&3)<<1) | ((led2&3)>>1);
+  cmd[1]=((led2&3)<<7) | ((led3&3)<<5);
+  write(mot_fd, cmd, 2);
+}
+
+void actuators_ardrone_close(void)
+{
+  close(mot_fd);
+}

sw/airborne/boards/ardrone/actuators_ardrone2_raw.h

@@ -0,0 +1,61 @@
+/*
+ * Original Code from:
+ * Copyright (C) 2011 Hugo Perquin - http://blog.perquin.com
+ *
+ * Adapated for Paparazzi by:
+ * Copyright (C) 2012 Dino Hensen <dino.hensen@gmail.com>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi 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, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file boards/ardrone/actuators_ardrone2_raw.h
+ * Actuator driver for ardrone2-raw version
+ */
+
+#ifndef ACTUATORS_ARDRONE2_RAW_H_
+#define ACTUATORS_ARDRONE2_RAW_H_
+
+#include <stdint.h>
+
+#ifndef ACTUATORS_ARDRONE_NB
+#define ACTUATORS_ARDRONE_NB 4
+#endif
+
+#define SERVOS_TICS_OF_USEC(_v) (_v)
+
+#define ActuatorArdroneSet(_i, _v) { actuators_pwm_values[_i] = _v; }
+#define ActuatorsArdroneCommit() actuators_ardrone_commit();
+#define ActuatorsArdroneInit() actuators_ardrone_init();
+
+#define MOT_LEDOFF 0
+#define MOT_LEDRED 1
+#define MOT_LEDGREEN 2
+#define MOT_LEDORANGE 3
+
+uint16_t actuators_pwm_values[ACTUATORS_ARDRONE_NB];
+
+extern void actuators_ardrone_commit(void);
+extern void actuators_ardrone_init(void);
+
+int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen);
+void actuators_ardrone_set_pwm(uint16_t pwm0, uint16_t pwm1, uint16_t pwm2, uint16_t pwm3);
+void actuators_ardrone_set_leds(uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3);
+void actuators_ardrone_close(void);
+
+#endif /* ACTUATORS_ARDRONE2_RAW_H_ */

sw/airborne/boards/ardrone/actuators_at.c

@@ -0,0 +1,63 @@
+/*
+ * Copyright (C) 2012-2013 Freek van Tienen
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi 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, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file boards/ardrone/actuators_at.c
+ * ardrone2-sdk actuators are driven by external software controller by AT-commands
+ */
+
+#include "subsystems/ahrs/ahrs_ardrone2.h"
+#include "actuators_at.h"
+#include "generated/airframe.h"
+#include "boards/ardrone/at_com.h"
+
+void actuators_init(void) {
+  init_at_com();
+}
+
+void actuators_set(pprz_t commands[]) {
+  //Calculate the thrus, roll, pitch and yaw from the PPRZ commands
+  float thrust = ((float)(commands[COMMAND_THRUST]-MAX_PPRZ/2) / (float)MAX_PPRZ)*2.0f;
+  float roll = ((float)commands[COMMAND_ROLL] / (float)MAX_PPRZ);
+  float pitch = ((float)commands[COMMAND_PITCH] / (float)MAX_PPRZ);
+  float yaw = ((float)commands[COMMAND_YAW] / (float)MAX_PPRZ);
+
+  //Starting engine
+  if(thrust > 0 && (ahrs_impl.control_state == CTRL_DEFAULT || ahrs_impl.control_state == CTRL_INIT || ahrs_impl.control_state == CTRL_LANDED))
+    at_com_send_ref(REF_TAKEOFF);
+
+  //Check emergency or stop engine
+  if((ahrs_impl.state & ARDRONE_EMERGENCY_MASK) != 0)
+    at_com_send_ref(REF_EMERGENCY);
+  else if(thrust < -0.9 && !(ahrs_impl.control_state == CTRL_DEFAULT || ahrs_impl.control_state == CTRL_INIT || ahrs_impl.control_state == CTRL_LANDED))
+    at_com_send_ref(0);
+
+  //Calibration
+  if((ahrs_impl.state & ARDRONE_MAGNETO_NEEDS_CALIB) != 0 && (ahrs_impl.control_state == CTRL_FLYING || ahrs_impl.control_state == CTRL_HOVERING))
+    at_com_send_calib(0);
+
+  //Moving
+  if((ahrs_impl.state & ARDRONE_MAGNETO_NEEDS_CALIB) == 0 && (ahrs_impl.control_state == CTRL_FLYING || ahrs_impl.control_state == CTRL_HOVERING))
+    at_com_send_pcmd(1, thrust, roll, pitch, yaw);
+
+  //Keep alive (FIXME)
+  at_com_send_config("general:navdata_demo", "FALSE");
+}

sw/airborne/boards/ardrone/actuators_at.h

@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2012-2013 Freek van Tienen
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi 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, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file boards/ardrone/actuators_at.h
+ * ardrone2-sdk actuators are driven by external software controller by AT-commands
+ */
+
+#ifndef BOARDS_ARDRONE_ACTUATORS_AT_H
+#define BOARDS_ARDRONE_ACTUATORS_AT_H
+
+#include "paparazzi.h"
+
+extern void actuators_init(void);
+extern void actuators_set(pprz_t commands[]);
+#define SetActuatorsFromCommands(commands, AP_MODE) actuators_set(commands)
+
+#endif /* BOARDS_ARDRONE_ACTUATORS_AT_H */

sw/airborne/boards/ardrone/at_com.c

@@ -0,0 +1,194 @@
+/*
+ * Copyright (C) 2012-2013 Freek van Tienen
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi 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, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file boards/ardrone/at_com.c
+ * Sending and receiving of AT-commands specified by the ardrone API
+ */
+
+#include "at_com.h"
+#include "boards/ardrone2_sdk.h"
+#include "generated/airframe.h"
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <netdb.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <errno.h>
+
+int packet_seq = 1; //Packet sequence number
+
+int at_socket = -1, //AT socket connection
+    navdata_socket = -1; //Navdata socket connection
+
+struct sockaddr_in pc_addr, //Own pc address
+        drone_at, //Drone AT address
+        drone_nav, //Drone nav address
+        from; //From address
+
+bool_t at_com_ready = FALSE; //Status of the at communication
+char sessionId[9]; //THe config session ID
+
+void at_com_send(char* command);
+void init_at_config(void);
+
+//Init the at_com
+void init_at_com(void) {
+  //Check if already initialized
+  if (at_com_ready)
+    return;
+
+  //Create the at and navdata socket
+  if ((at_socket = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
+    printf("at_com: at_socket error (%s)\n", strerror(errno));
+  }
+  if ((navdata_socket = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
+    printf("at_com: navdata_socket error (%s)\n", strerror(errno));
+  }
+
+  //For recvfrom
+  pc_addr.sin_family = AF_INET;
+  pc_addr.sin_addr.s_addr = htonl(INADDR_ANY);
+  pc_addr.sin_port = htons(9800);
+
+  //For sendto AT
+  drone_at.sin_family = AF_INET;
+  drone_at.sin_addr.s_addr = inet_addr(ARDRONE_IP);
+  drone_at.sin_port = htons(ARDRONE_AT_PORT);
+
+  //For sendto navadata init
+  drone_nav.sin_family = AF_INET;
+  drone_nav.sin_addr.s_addr = inet_addr(ARDRONE_IP);
+  drone_nav.sin_port = htons(ARDRONE_NAVDATA_PORT);
+
+  //Bind the navdata socket
+  if (bind(navdata_socket, (struct sockaddr *) &pc_addr, sizeof(pc_addr)) < 0) {
+    printf("at_com: bind error (%s)\n", strerror(errno));
+  }
+
+  //Set unicast mode on
+  int one = 1;
+  sendto(navdata_socket, &one, 4, 0, (struct sockaddr *) &drone_nav,
+      sizeof(drone_nav));
+
+  //Init at config
+  init_at_config();
+
+  //Set at_com to ready
+  at_com_ready = TRUE;
+}
+
+//Init the at config
+void init_at_config(void) {
+  //Generate a session id
+  uint32_t binaryId = (uint32_t) rand();
+  binaryId = (0 != binaryId) ? binaryId : 1u;
+  snprintf(sessionId, 9, "%08x", binaryId);
+  sessionId[8] = '\0';
+
+  //Send session, application and user id:
+  at_com_send_config("custom:session_id", sessionId);
+  at_com_send_config("custom:application_id", "9D7BFD45");
+  at_com_send_config("custom:profile_id", "2BF07F58");
+
+  //Send config values
+  at_com_send_config("control:euler_angle_max", "0.52");
+  at_com_send_config("control:altitude_max", "20000");
+  at_com_send_config("control:control_vz_max", "2000");
+  at_com_send_config("control:control_yaw", "6.11");
+
+  //Send config values with the airframe.h
+#ifndef ARDRONE_FLIGHT_INDOOR
+  at_com_send_config("control:outdoor", "TRUE");
+#else
+  at_com_send_config("control:outdoor","FALSE");
+#endif
+#ifndef ARDRONE_WITHOUT_SHELL
+  at_com_send_config("control:flight_without_shell", "FALSE");
+#else
+  at_com_send_config("control:flight_without_shell","TRUE");
+#endif
+#ifdef ARDRONE_OWNER_MAC
+  at_com_send_config("network:owner_mac",ARDRONE_OWNER_MAC);
+#endif
+}
+
+//Recieve a navdata packet
+void at_com_recieve_navdata(unsigned char* buffer) {
+  int l;
+  recvfrom(navdata_socket, buffer, ARDRONE_NAVDATA_BUFFER_SIZE, 0x0,
+      (struct sockaddr *) &from, (socklen_t *) &l);
+}
+
+//Send an AT command
+void at_com_send(char* command) {
+  sendto(at_socket, command, strlen(command), 0, (struct sockaddr*) &drone_at,
+      sizeof(drone_at));
+}
+
+//Send a Config
+void at_com_send_config(char* key, char* value) {
+  char command[256];
+  sprintf(command, "AT*CONFIG_IDS=%d,\"%s\",\"2BF07F58\",\"9D7BFD45\"\r",
+      packet_seq++, sessionId);
+  at_com_send(command);
+  sprintf(command, "AT*CONFIG=%d,\"%s\",\"%s\"\r", packet_seq++, key, value);
+  at_com_send(command);
+}
+
+//Send a Flat trim
+void at_com_send_ftrim(void) {
+  char command[256];
+  sprintf(command, "AT*FTRIM=%d\r", packet_seq++);
+  at_com_send(command);
+}
+
+//Send a Ref
+void at_com_send_ref(int bits) {
+  char command[256];
+  sprintf(command, "AT*REF=%d,%d\r", packet_seq++, bits | REF_DEFAULT);
+  at_com_send(command);
+}
+
+//Send a Pcmd
+void at_com_send_pcmd(int mode, float thrust, float roll, float pitch,
+    float yaw) {
+  int f_thrust, f_roll, f_pitch, f_yaw;
+  char command[256];
+
+  //Change the floats to ints(dereferencing)
+  memcpy(&f_thrust, &thrust, sizeof thrust);
+  memcpy(&f_roll, &roll, sizeof roll);
+  memcpy(&f_pitch, &pitch, sizeof pitch);
+  memcpy(&f_yaw, &yaw, sizeof yaw);
+
+  sprintf(command, "AT*PCMD=%d,%d,%d,%d,%d,%d\r", packet_seq++, mode, f_roll,
+      f_pitch, f_thrust, f_yaw);
+  at_com_send(command);
+}
+
+//Send a Calib
+void at_com_send_calib(int device) {
+  char command[256];
+  sprintf(command, "AT*CALIB=%d,%d\r", packet_seq++, device);
+  at_com_send(command);
+}

sw/airborne/boards/ardrone/at_com.h

@@ -0,0 +1,161 @@
+/*
+ * Copyright (C) 2012-2013 Freek van Tienen
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi 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, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file boards/ardrone/at_com.h
+ * Sending and receiving of AT-commands specified by the ardrone API
+ */
+
+#include "math/pprz_algebra_float.h"
+
+#ifndef BOARDS_ARDRONE_AT_COM_H
+#define BOARDS_ARDRONE_AT_COM_H
+
+//Define the AT_REF bits
+typedef enum {
+  REF_TAKEOFF    = 1U << 9,
+  REF_EMERGENCY  = 1U << 8,
+  REF_DEFAULT    = 0x11540000
+} AT_REFS;
+
+//Define control states
+typedef enum {
+  CTRL_DEFAULT,
+  CTRL_INIT,
+  CTRL_LANDED,
+  CTRL_FLYING,
+  CTRL_HOVERING,
+  CTRL_TEST,
+  CTRL_TRANS_TAKEOFF,
+  CTRL_TRANS_GOTOFIX,
+  CTRL_TRANS_LANDING,
+  CTRL_TRANS_LOOPING,
+  CTRL_NUM_STATES
+} CTRL_STATES;
+
+//Define the AR.Drone states
+typedef enum {
+  ARDRONE_FLY_MASK            = 1U << 0,  /*!< FLY MASK : (0) ardrone is landed, (1) ardrone is flying */
+  ARDRONE_VIDEO_MASK          = 1U << 1,  /*!< VIDEO MASK : (0) video disable, (1) video enable */
+  ARDRONE_VISION_MASK         = 1U << 2,  /*!< VISION MASK : (0) vision disable, (1) vision enable */
+  ARDRONE_CONTROL_MASK        = 1U << 3,  /*!< CONTROL ALGO : (0) euler angles control, (1) angular speed control */
+  ARDRONE_ALTITUDE_MASK       = 1U << 4,  /*!< ALTITUDE CONTROL ALGO : (0) altitude control inactive (1) altitude control active */
+  ARDRONE_USER_FEEDBACK_START = 1U << 5,  /*!< USER feedback : Start button state */
+  ARDRONE_COMMAND_MASK        = 1U << 6,  /*!< Control command ACK : (0) None, (1) one received */
+  ARDRONE_CAMERA_MASK         = 1U << 7,  /*!< CAMERA MASK : (0) camera not ready, (1) Camera ready */
+  ARDRONE_TRAVELLING_MASK     = 1U << 8,  /*!< Travelling mask : (0) disable, (1) enable */
+  ARDRONE_USB_MASK            = 1U << 9,  /*!< USB key : (0) usb key not ready, (1) usb key ready */
+  ARDRONE_NAVDATA_DEMO_MASK   = 1U << 10, /*!< Navdata demo : (0) All navdata, (1) only navdata demo */
+  ARDRONE_NAVDATA_BOOTSTRAP   = 1U << 11, /*!< Navdata bootstrap : (0) options sent in all or demo mode, (1) no navdata options sent */
+  ARDRONE_MOTORS_MASK         = 1U << 12, /*!< Motors status : (0) Ok, (1) Motors problem */
+  ARDRONE_COM_LOST_MASK       = 1U << 13, /*!< Communication Lost : (1) com problem, (0) Com is ok */
+  ARDRONE_SOFTWARE_FAULT      = 1U << 14, /*!< Software fault detected - user should land as quick as possible (1) */
+  ARDRONE_VBAT_LOW            = 1U << 15, /*!< VBat low : (1) too low, (0) Ok */
+  ARDRONE_USER_EL             = 1U << 16, /*!< User Emergency Landing : (1) User EL is ON, (0) User EL is OFF*/
+  ARDRONE_TIMER_ELAPSED       = 1U << 17, /*!< Timer elapsed : (1) elapsed, (0) not elapsed */
+  ARDRONE_MAGNETO_NEEDS_CALIB = 1U << 18, /*!< Magnetometer calibration state : (0) Ok, no calibration needed, (1) not ok, calibration needed */
+  ARDRONE_ANGLES_OUT_OF_RANGE = 1U << 19, /*!< Angles : (0) Ok, (1) out of range */
+  ARDRONE_WIND_MASK           = 1U << 20, /*!< WIND MASK: (0) ok, (1) Too much wind */
+  ARDRONE_ULTRASOUND_MASK     = 1U << 21, /*!< Ultrasonic sensor : (0) Ok, (1) deaf */
+  ARDRONE_CUTOUT_MASK         = 1U << 22, /*!< Cutout system detection : (0) Not detected, (1) detected */
+  ARDRONE_PIC_VERSION_MASK    = 1U << 23, /*!< PIC Version number OK : (0) a bad version number, (1) version number is OK */
+  ARDRONE_ATCODEC_THREAD_ON   = 1U << 24, /*!< ATCodec thread ON : (0) thread OFF (1) thread ON */
+  ARDRONE_NAVDATA_THREAD_ON   = 1U << 25, /*!< Navdata thread ON : (0) thread OFF (1) thread ON */
+  ARDRONE_VIDEO_THREAD_ON     = 1U << 26, /*!< Video thread ON : (0) thread OFF (1) thread ON */
+  ARDRONE_ACQ_THREAD_ON       = 1U << 27, /*!< Acquisition thread ON : (0) thread OFF (1) thread ON */
+  ARDRONE_CTRL_WATCHDOG_MASK  = 1U << 28, /*!< CTRL watchdog : (1) delay in control execution (> 5ms), (0) control is well scheduled */
+  ARDRONE_ADC_WATCHDOG_MASK   = 1U << 29, /*!< ADC Watchdog : (1) delay in uart2 dsr (> 5ms), (0) uart2 is good */
+  ARDRONE_COM_WATCHDOG_MASK   = 1U << 30, /*!< Communication Watchdog : (1) com problem, (0) Com is ok */
+  ARDRONE_EMERGENCY_MASK      = 1U << 31  /*!< Emergency landing : (0) no emergency, (1) emergency */
+} ARDRONE_STATES;
+
+//Navdata option packet without data
+typedef struct _navdata_option_t {
+  uint16_t  tag;
+  uint16_t  size;
+  uint8_t   data[1];
+} __attribute__ ((packed)) navdata_option_t;
+
+//Main navdata packet
+typedef struct _navdata_t {
+  uint32_t    header;        /*!< Always set to NAVDATA_HEADER */
+  uint32_t    ardrone_state;    /*!< Bit mask built from def_ardrone_state_mask_t */
+  uint32_t    sequence;         /*!< Sequence number, incremented for each sent packet */
+  uint32_t    vision_defined;
+
+  navdata_option_t  options[1];
+} __attribute__ ((packed)) navdata_t;
+
+//Navdata checksum packet
+typedef struct _navdata_cks_t {
+  uint16_t  tag;
+  uint16_t  size;
+  uint32_t  cks;
+} __attribute__ ((packed)) navdata_cks_t;
+
+//Navdata demo option
+typedef struct _navdata_demo_t {
+  uint16_t      tag;          /*!< Navdata block ('option') identifier */
+  uint16_t      size;          /*!< set this to the size of this structure */
+  uint32_t      ctrl_state;             /*!< Flying state (landed, flying, hovering, etc.) defined in CTRL_STATES enum. */
+  uint32_t      vbat_flying_percentage; /*!< battery voltage filtered (mV) */
+  float        theta;                  /*!< UAV's pitch in milli-degrees */
+  float        phi;                    /*!< UAV's roll  in milli-degrees */
+  float        psi;                    /*!< UAV's yaw   in milli-degrees */
+  int32_t        altitude;               /*!< UAV's altitude in centimeters */
+  float        vx;                     /*!< UAV's estimated linear velocity */
+  float        vy;                     /*!< UAV's estimated linear velocity */
+  float        vz;                     /*!< UAV's estimated linear velocity */
+  uint32_t      num_frames;          /*!< streamed frame index */ // Not used -> To integrate in video stage.
+  // Camera parameters compute by detection
+  struct FloatMat33  detection_camera_rot;   /*!<  Deprecated ! Don't use ! */
+  struct FloatVect3  detection_camera_trans; /*!<  Deprecated ! Don't use ! */
+  uint32_t      detection_tag_index;    /*!<  Deprecated ! Don't use ! */
+  uint32_t        detection_camera_type;  /*!<  Type of tag searched in detection */
+  // Camera parameters compute by drone
+  struct FloatMat33  drone_camera_rot;    /*!<  Deprecated ! Don't use ! */
+  struct FloatVect3  drone_camera_trans;      /*!<  Deprecated ! Don't use ! */
+} __attribute__ ((packed)) navdata_demo_t;
+
+//Navdata physical measures option
+typedef struct _navdata_phys_measures_t {
+  uint16_t        tag;
+  uint16_t        size;
+
+  float          accs_temp;
+  uint16_t        gyro_temp;
+  struct FloatVect3    phys_accs;
+  struct FloatVect3    phys_gyros;
+  uint32_t        alim3V3;              // 3.3volt alim [LSB]
+  uint32_t        vrefEpson;            // ref volt Epson gyro [LSB]
+  uint32_t        vrefIDG;              // ref volt IDG gyro [LSB]
+} __attribute__ ((packed)) navdata_phys_measures_t;
+
+//External functions
+extern void init_at_com(void);
+extern void at_com_recieve_navdata(unsigned char* buffer);
+extern void at_com_send_config(char* key, char* value);
+extern void at_com_send_ftrim(void);
+extern void at_com_send_ref(int bits);
+extern void at_com_send_pcmd(int mode, float thrust, float roll, float pitch, float yaw);
+extern void at_com_send_calib(int device);
+
+#endif /* BOARDS_ARDRONE_AT_COM_H */

sw/airborne/boards/ardrone/baro_board.c

@@ -0,0 +1,54 @@
+/*
+ * Copyright (C) 2012 TU Delft Quatrotor Team 1
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi 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, or (at your option)
+ * any later version.
+ *
+ * Paparazzi 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 Paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file boards/ardrone/baro_board.c
+ * Paparazzi AR Drone 2 Baro Sensor implementation:.
+ *
+ * These functions are mostly empty because of the calibration and calculations
+ * done by the Parrot Navigation board.
+ */
+
+#include "subsystems/sensors/baro.h"
+#include "baro_board.h"
+
+struct Baro baro;
+
+void baro_init(void) {
+  baro.status = BS_UNINITIALIZED;
+  baro.absolute     = 0;
+  baro.differential = 0;
+  baro_data_available = 0;
+}
+
+void baro_periodic(void) {
+  baro.status = BS_RUNNING;
+  if(navdata_baro_available == 1) {
+    navdata_baro_available = 0;
+//    baro.absolute = navdata->pressure; // When this is un-commented the ardrone gets a pressure
+    // TODO do the right calculations for the right absolute pressure
+    baro.absolute = 0;
+    baro_data_available = TRUE;
+  }
+  else {
+    baro_data_available = FALSE;
+  }
+}

sw/airborne/boards/ardrone/baro_board.h

@@ -0,0 +1,51 @@
+/*
+ * Copyright (C) 2012 TU Delft Quatrotor Team 1
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi 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, or (at your option)
+ * any later version.
+ *
+ * Paparazzi 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 Paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file boards/ardrone/baro_board.h
+ * Paparazzi AR Drone 2 Baro Sensor implementation:.
+ *
+ * These functions are mostly empty because of the calibration and calculations
+ * done by the Parrot Navigation board.
+ */
+
+#ifndef BOARDS_ARDRONE2_BARO_H
+#define BOARDS_ARDRONE2_BARO_H
+
+#if BOARD_HAS_BARO
+#include "navdata.h"
+
+int baro_data_available;
+
+static inline void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void)){
+  if (baro_data_available) {
+    b_abs_handler();
+  }
+}
+
+#define BaroEvent(_b_abs_handler, _b_diff_handler) {\
+  baro_event(_b_abs_handler,_b_diff_handler);\
+}
+#else
+#define BaroEvent(_b_abs_handler, _b_diff_handler) {}
+#endif
+
+#endif /* BOARDS_ARDRONE2_BARO_H */

sw/airborne/boards/ardrone/baro_board_dummy.c

@@ -0,0 +1,40 @@
+/*
+ * Copyright (C) 2013 Dino Hensen
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi 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, or (at your option)
+ * any later version.
+ *
+ * Paparazzi 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 Paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file boards/ardrone/baro_board_dummy.c
+ * Dummy Baro Board.
+ *
+ * These functions are mostly empty because this is a dummy.
+ */
+
+#include "subsystems/sensors/baro.h"
+
+struct Baro baro;
+
+void baro_init(void) {
+  baro.status = BS_UNINITIALIZED;
+  baro.absolute     = 0;
+  baro.differential = 0;
+}
+
+void baro_periodic(void) {
+}

sw/airborne/boards/ardrone/electrical_dummy.c

@@ -0,0 +1,37 @@
+/*
+ *
+ * Copyright (C) 2009-2013 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi 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, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/**
+ * @file boards/ardrone/electrical_dummy.c
+ * dummy electrical status readings for ardrone-sdk version.
+ *
+ * Because ardrone2-sdk version does its battery updating in ahrs_adrone2.c.
+ */
+
+#include "subsystems/electrical.h"
+
+struct Electrical electrical;
+
+void electrical_init(void) { }
+
+void electrical_periodic(void) { }

sw/airborne/boards/ardrone/gpio.c

@@ -0,0 +1,39 @@
+/*
+ * 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.c
+ * ardrone GPIO driver
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "gpio.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);
+}

sw/airborne/boards/ardrone/gpio.h

@@ -0,0 +1,33 @@
+/*
+ * 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.h
+ * ardrone GPIO driver
+ */
+
+#ifndef GPIO_H
+#define GPIO_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);
+
+#endif /* GPIO_H */

sw/airborne/boards/ardrone/i2c-dev.h

@@ -0,0 +1,335 @@
+/*
+ * i2c-dev.h - i2c-bus driver, char device interface
+ *
+ * Copyright (C) 1995-97 Simon G. Vogl
+ * Copyright (C) 1998-99 Frodo Looijaard <frodol@dds.nl>
+ *
+ * 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.
+ */
+
+/* $Id: i2c-dev.h 5361 2008-10-19 09:47:02Z khali $ */
+
+/**
+ * @file boards/ardrone/i2c-dev.h
+ * I2C-bus driver
+ */
+
+#ifndef LIB_I2CDEV_H
+#define LIB_I2CDEV_H
+
+#include <linux/types.h>
+#include <sys/ioctl.h>
+
+/*
+ * I2C Message - used for pure i2c transaction, also from /dev interface
+ */
+struct i2c_msg {
+  __u16 addr;  /* slave address      */
+  unsigned short flags;
+#define I2C_M_TEN  0x10  /* we have a ten bit chip address  */
+#define I2C_M_RD  0x01
+#define I2C_M_NOSTART  0x4000
+#define I2C_M_REV_DIR_ADDR  0x2000
+#define I2C_M_IGNORE_NAK  0x1000
+#define I2C_M_NO_RD_ACK    0x0800
+  short len;    /* msg length        */
+  char *buf;    /* pointer to msg data      */
+};
+
+/* To determine what functionality is present */
+
+#define I2C_FUNC_I2C                    0x00000001
+#define I2C_FUNC_10BIT_ADDR             0x00000002
+#define I2C_FUNC_PROTOCOL_MANGLING      0x00000004 /* I2C_M_{REV_DIR_ADDR,NOSTART,..} */
+#define I2C_FUNC_SMBUS_PEC              0x00000008
+#define I2C_FUNC_SMBUS_BLOCK_PROC_CALL  0x00008000 /* SMBus 2.0 */
+#define I2C_FUNC_SMBUS_QUICK            0x00010000
+#define I2C_FUNC_SMBUS_READ_BYTE        0x00020000
+#define I2C_FUNC_SMBUS_WRITE_BYTE       0x00040000
+#define I2C_FUNC_SMBUS_READ_BYTE_DATA   0x00080000
+#define I2C_FUNC_SMBUS_WRITE_BYTE_DATA  0x00100000
+#define I2C_FUNC_SMBUS_READ_WORD_DATA   0x00200000
+#define I2C_FUNC_SMBUS_WRITE_WORD_DATA  0x00400000
+#define I2C_FUNC_SMBUS_PROC_CALL        0x00800000
+#define I2C_FUNC_SMBUS_READ_BLOCK_DATA  0x01000000
+#define I2C_FUNC_SMBUS_WRITE_BLOCK_DATA 0x02000000
+#define I2C_FUNC_SMBUS_READ_I2C_BLOCK   0x04000000 /* I2C-like block xfer  */
+#define I2C_FUNC_SMBUS_WRITE_I2C_BLOCK  0x08000000 /* w/ 1-byte reg. addr. */
+
+#define I2C_FUNC_SMBUS_BYTE (I2C_FUNC_SMBUS_READ_BYTE | \
+                             I2C_FUNC_SMBUS_WRITE_BYTE)
+#define I2C_FUNC_SMBUS_BYTE_DATA (I2C_FUNC_SMBUS_READ_BYTE_DATA | \
+                                  I2C_FUNC_SMBUS_WRITE_BYTE_DATA)
+#define I2C_FUNC_SMBUS_WORD_DATA (I2C_FUNC_SMBUS_READ_WORD_DATA | \
+                                  I2C_FUNC_SMBUS_WRITE_WORD_DATA)
+#define I2C_FUNC_SMBUS_BLOCK_DATA (I2C_FUNC_SMBUS_READ_BLOCK_DATA | \
+                                   I2C_FUNC_SMBUS_WRITE_BLOCK_DATA)
+#define I2C_FUNC_SMBUS_I2C_BLOCK (I2C_FUNC_SMBUS_READ_I2C_BLOCK | \
+                                  I2C_FUNC_SMBUS_WRITE_I2C_BLOCK)
+
+/* Old name, for compatibility */
+#define I2C_FUNC_SMBUS_HWPEC_CALC  I2C_FUNC_SMBUS_PEC
+
+/*
+ * Data for SMBus Messages
+ */
+#define I2C_SMBUS_BLOCK_MAX  32  /* As specified in SMBus standard */
+#define I2C_SMBUS_I2C_BLOCK_MAX  32  /* Not specified but we use same structure */
+union i2c_smbus_data {
+  __u8 byte;
+  __u16 word;
+  __u8 block[I2C_SMBUS_BLOCK_MAX + 2]; /* block[0] is used for length */
+                                              /* and one more for PEC */
+};
+
+/* smbus_access read or write markers */
+#define I2C_SMBUS_READ  1
+#define I2C_SMBUS_WRITE  0
+
+/* SMBus transaction types (size parameter in the above functions)
+   Note: these no longer correspond to the (arbitrary) PIIX4 internal codes! */
+#define I2C_SMBUS_QUICK             0
+#define I2C_SMBUS_BYTE              1
+#define I2C_SMBUS_BYTE_DATA         2
+#define I2C_SMBUS_WORD_DATA         3
+#define I2C_SMBUS_PROC_CALL         4
+#define I2C_SMBUS_BLOCK_DATA        5
+#define I2C_SMBUS_I2C_BLOCK_BROKEN  6
+#define I2C_SMBUS_BLOCK_PROC_CALL   7    /* SMBus 2.0 */
+#define I2C_SMBUS_I2C_BLOCK_DATA    8
+
+
+/* ----- commands for the ioctl like i2c_command call:
+ * note that additional calls are defined in the algorithm and hw
+ *  dependent layers - these can be listed here, or see the
+ *  corresponding header files.
+ */
+        /* -> bit-adapter specific ioctls  */
+#define I2C_RETRIES  0x0701  /* number of times a device address      */
+        /* should be polled when not            */
+                                /* acknowledging       */
+#define I2C_TIMEOUT  0x0702  /* set timeout - call with int     */
+
+
+/* this is for i2c-dev.c  */
+#define I2C_SLAVE    0x0703  /* Change slave address      */
+        /* Attn.: Slave address is 7 or 10 bits */
+#define I2C_SLAVE_FORCE  0x0706  /* Change slave address      */
+        /* Attn.: Slave address is 7 or 10 bits */
+        /* This changes the address, even if it */
+        /* is already taken!      */
+#define I2C_TENBIT   0x0704  /* 0 for 7 bit addrs, != 0 for 10 bit  */
+
+#define I2C_FUNCS    0x0705  /* Get the adapter functionality */
+#define I2C_RDWR     0x0707  /* Combined R/W transfer (one stop only)*/
+#define I2C_PEC      0x0708  /* != 0 for SMBus PEC                   */
+
+#define I2C_SMBUS    0x0720  /* SMBus-level access */
+
+/* -- i2c.h -- */
+
+
+/* Note: 10-bit addresses are NOT supported! */
+
+/* This is the structure as used in the I2C_SMBUS ioctl call */
+struct i2c_smbus_ioctl_data {
+  char read_write;
+  __u8 command;
+  int size;
+  union i2c_smbus_data *data;
+};
+
+/* This is the structure as used in the I2C_RDWR ioctl call */
+struct i2c_rdwr_ioctl_data {
+  struct i2c_msg *msgs;  /* pointers to i2c_msgs */
+  int nmsgs;    /* number of i2c_msgs */
+};
+
+
+static inline __s32 i2c_smbus_access(int file, char read_write, __u8 command,
+                                     int size, union i2c_smbus_data *data)
+{
+  struct i2c_smbus_ioctl_data args;
+
+  args.read_write = read_write;
+  args.command = command;
+  args.size = size;
+  args.data = data;
+  return ioctl(file,I2C_SMBUS,&args);
+}
+
+
+static inline __s32 i2c_smbus_write_quick(int file, __u8 value)
+{
+  return i2c_smbus_access(file,value,0,I2C_SMBUS_QUICK,NULL);
+}
+
+static inline __s32 i2c_smbus_read_byte(int file)
+{
+  union i2c_smbus_data data;
+  if (i2c_smbus_access(file,I2C_SMBUS_READ,0,I2C_SMBUS_BYTE,&data))
+    return -1;
+  else
+    return 0x0FF & data.byte;
+}
+
+static inline __s32 i2c_smbus_write_byte(int file, __u8 value)
+{
+  return i2c_smbus_access(file,I2C_SMBUS_WRITE,value,
+                          I2C_SMBUS_BYTE,NULL);
+}
+
+static inline __s32 i2c_smbus_read_byte_data(int file, __u8 command)
+{
+  union i2c_smbus_data data;
+  if (i2c_smbus_access(file,I2C_SMBUS_READ,command,
+                       I2C_SMBUS_BYTE_DATA,&data))
+    return -1;
+  else
+    return 0x0FF & data.byte;
+}
+
+static inline __s32 i2c_smbus_write_byte_data(int file, __u8 command,
+                                              __u8 value)
+{
+  union i2c_smbus_data data;
+  data.byte = value;
+  return i2c_smbus_access(file,I2C_SMBUS_WRITE,command,
+                          I2C_SMBUS_BYTE_DATA, &data);
+}
+
+static inline __s32 i2c_smbus_read_word_data(int file, __u8 command)
+{
+  union i2c_smbus_data data;
+  if (i2c_smbus_access(file,I2C_SMBUS_READ,command,
+                       I2C_SMBUS_WORD_DATA,&data))
+    return -1;
+  else
+    return 0x0FFFF & data.word;
+}
+
+static inline __s32 i2c_smbus_write_word_data(int file, __u8 command,
+                                              __u16 value)
+{
+  union i2c_smbus_data data;
+  data.word = value;
+  return i2c_smbus_access(file,I2C_SMBUS_WRITE,command,
+                          I2C_SMBUS_WORD_DATA, &data);
+}
+
+static inline __s32 i2c_smbus_process_call(int file, __u8 command, __u16 value)
+{
+  union i2c_smbus_data data;
+  data.word = value;
+  if (i2c_smbus_access(file,I2C_SMBUS_WRITE,command,
+                       I2C_SMBUS_PROC_CALL,&data))
+    return -1;
+  else
+    return 0x0FFFF & data.word;
+}
+
+
+/* Returns the number of read bytes */
+static inline __s32 i2c_smbus_read_block_data(int file, __u8 command,
+                                              __u8 *values)
+{
+  union i2c_smbus_data data;
+  int i;
+  if (i2c_smbus_access(file,I2C_SMBUS_READ,command,
+                       I2C_SMBUS_BLOCK_DATA,&data))
+    return -1;
+  else {
+    for (i = 1; i <= data.block[0]; i++)
+      values[i-1] = data.block[i];
+    return data.block[0];
+  }
+}
+
+static inline __s32 i2c_smbus_write_block_data(int file, __u8 command,
+                                               __u8 length, __u8 *values)
+{
+  union i2c_smbus_data data;
+  int i;
+  if (length > 32)
+    length = 32;
+  for (i = 1; i <= length; i++)
+    data.block[i] = values[i-1];
+  data.block[0] = length;
+  return i2c_smbus_access(file,I2C_SMBUS_WRITE,command,
+                          I2C_SMBUS_BLOCK_DATA, &data);
+}
+
+/* Returns the number of read bytes */
+/* Until kernel 2.6.22, the length is hardcoded to 32 bytes. If you
+   ask for less than 32 bytes, your code will only work with kernels
+   2.6.23 and later. */
+static inline __s32 i2c_smbus_read_i2c_block_data(int file, __u8 command,
+                                                  __u8 length, __u8 *values)
+{
+  union i2c_smbus_data data;
+  int i;
+
+  if (length > 32)
+    length = 32;
+  data.block[0] = length;
+  if (i2c_smbus_access(file,I2C_SMBUS_READ,command,
+                       length == 32 ? I2C_SMBUS_I2C_BLOCK_BROKEN :
+                        I2C_SMBUS_I2C_BLOCK_DATA,&data))
+    return -1;
+  else {
+    for (i = 1; i <= data.block[0]; i++)
+      values[i-1] = data.block[i];
+    return data.block[0];
+  }
+}
+
+static inline __s32 i2c_smbus_write_i2c_block_data(int file, __u8 command,
+                                               __u8 length, __u8 *values)
+{
+  union i2c_smbus_data data;
+  int i;
+  if (length > 32)
+    length = 32;
+  for (i = 1; i <= length; i++)
+    data.block[i] = values[i-1];
+  data.block[0] = length;
+  return i2c_smbus_access(file,I2C_SMBUS_WRITE,command,
+                          I2C_SMBUS_I2C_BLOCK_BROKEN, &data);
+}
+
+/* Returns the number of read bytes */
+static inline __s32 i2c_smbus_block_process_call(int file, __u8 command,
+                                                 __u8 length, __u8 *values)
+{
+  union i2c_smbus_data data;
+  int i;
+  if (length > 32)
+    length = 32;
+  for (i = 1; i <= length; i++)
+    data.block[i] = values[i-1];
+  data.block[0] = length;
+  if (i2c_smbus_access(file,I2C_SMBUS_WRITE,command,
+                       I2C_SMBUS_BLOCK_PROC_CALL,&data))
+    return -1;
+  else {
+    for (i = 1; i <= data.block[0]; i++)
+      values[i-1] = data.block[i];
+    return data.block[0];
+  }
+}
+
+
+#endif /* LIB_I2CDEV_H */

sw/airborne/boards/ardrone/navdata.c

@@ -0,0 +1,218 @@
+/*
+ * Copyright (C) 2012 Dino Hensen, Vincent van Hoek
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi 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, or (at your option)
+ * any later version.
+ *
+ * Paparazzi 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 Paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file boards/ardrone/navdata.c
+ * ardrone2 navdata aquisition driver.
+ *
+ * The ardrone2 provides a navdata stream of packets
+ * containing info about all sensors at a rate of 200Hz.
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <fcntl.h>     // for O_RDWR, O_NOCTTY, O_NONBLOCK
+#include <termios.h>   // for baud rates and options
+#include <unistd.h>
+#include <string.h>
+#include <math.h>
+#include "navdata.h"
+
+int nav_fd;
+
+int navdata_init()
+{
+  port = malloc(sizeof(navdata_port));
+
+  nav_fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY | O_NONBLOCK);
+  if (nav_fd == -1)
+  {
+    perror("navdata_init: Unable to open /dev/ttyO1 - ");
+    return 1;
+  } else {
+    port->isOpen = 1;
+  }
+
+  fcntl(nav_fd, F_SETFL, 0); //read calls are non blocking
+  //set port options
+  struct termios options;
+  //Get the current options for the port
+  tcgetattr(nav_fd, &options);
+  //Set the baud rates to 460800
+  cfsetispeed(&options, B460800);
+  cfsetospeed(&options, B460800);
+
+  options.c_cflag |= (CLOCAL | CREAD); //Enable the receiver and set local mode
+  options.c_iflag = 0; //clear input options
+  options.c_lflag = 0; //clear local options
+  options.c_oflag &= ~OPOST; //clear output options (raw output)
+
+  //Set the new options for the port
+  tcsetattr(nav_fd, TCSANOW, &options);
+
+  // stop acquisition
+  uint8_t cmd=0x02;
+  write(nav_fd, &cmd, 1);
+
+  // start acquisition
+  cmd=0x01;
+  write(nav_fd, &cmd, 1);
+
+  navdata = malloc(sizeof(measures_t));
+  navdata_imu_available = 0;
+  navdata_baro_available = 0;
+
+  port->bytesRead = 0;
+  port->totalBytesRead = 0;
+  port->packetsRead = 0;
+  port->isInitialized = 1;
+
+  previousUltrasoundHeight = 0;
+
+  return 0;
+}
+
+void navdata_close()
+{
+  port->isOpen = 0;
+  close(nav_fd);
+}
+
+void navdata_read()
+{
+  int newbytes = 0;
+
+  if (port->isInitialized != 1)
+    navdata_init();
+
+  if (port->isOpen != 1)
+    return;
+
+  newbytes = read(nav_fd, port->buffer+port->bytesRead, NAVDATA_BUFFER_SIZE-port->bytesRead);
+
+  // because non-blocking read returns -1 when no bytes available
+  if (newbytes > 0)
+  {
+    port->bytesRead += newbytes;
+    port->totalBytesRead += newbytes;
+  }
+
+}
+
+void navdata_update()
+{
+  navdata_read();
+
+  // while there is something interesting to do...
+  while (port->bytesRead >= 60)
+  {
+    if (port->buffer[0] == NAVDATA_START_BYTE)
+    {
+      // if checksum is OK
+      if ( 1 ) // we dont know how to calculate the checksum
+//      if ( navdata_checksum() == 0 )
+      {
+        memcpy(navdata, port->buffer, NAVDATA_PACKET_SIZE);
+        navdata_imu_available = 1;
+        navdata_baro_available = 1;
+        port->packetsRead++;
+//        printf("CCRC=%d, GCRC=%d, error=%d\n", crc, navdata->chksum, abs(crc-navdata->chksum));
+        navdata_getHeight();
+      }
+      navdata_CropBuffer(60);
+    }
+    else
+    {
+      // find start byte, copy all data from startbyte to buffer origin, update bytesread
+      uint8_t * pint;
+      pint = memchr(port->buffer, NAVDATA_START_BYTE, port->bytesRead);
+
+      if (pint != NULL) {
+        port->bytesRead -= pint - port->buffer;
+        navdata_CropBuffer(pint - port->buffer);
+      } else {
+        // if the start byte was not found, it means there is junk in the buffer
+        port->bytesRead = 0;
+      }
+    }
+  }
+}
+
+void navdata_CropBuffer(int cropsize)
+{
+  if (port->bytesRead - cropsize < 0) {
+    // TODO think about why the amount of bytes read minus the cropsize gets below zero
+    printf("BytesRead - Cropsize may not be below zero...");
+    return;
+  }
+
+  memmove(port->buffer, port->buffer+cropsize, NAVDATA_BUFFER_SIZE-cropsize);
+  port->bytesRead -= cropsize;
+}
+
+int16_t navdata_getHeight() {
+
+  if (navdata->ultrasound > 10000) {
+    return previousUltrasoundHeight;
+  }
+
+  int16_t ultrasoundHeight = 0;
+
+  ultrasoundHeight = (navdata->ultrasound - 880) / 26.553;
+
+  previousUltrasoundHeight = ultrasoundHeight;
+
+  return ultrasoundHeight;
+}
+
+// The checksum should be calculated here: we don't know the algorithm
+uint16_t navdata_checksum() {
+  navdata_cks = 0;
+  navdata_cks += navdata->nu_trame;
+  navdata_cks += navdata->ax;
+  navdata_cks += navdata->ay;
+  navdata_cks += navdata->az;
+  navdata_cks += navdata->vx;
+  navdata_cks += navdata->vy;
+  navdata_cks += navdata->vz;
+  navdata_cks += navdata->temperature_acc;
+  navdata_cks += navdata->temperature_gyro;
+  navdata_cks += navdata->ultrasound;
+  navdata_cks += navdata->us_debut_echo;
+  navdata_cks += navdata->us_fin_echo;
+  navdata_cks += navdata->us_association_echo;
+  navdata_cks += navdata->us_distance_echo;
+  navdata_cks += navdata->us_curve_time;
+  navdata_cks += navdata->us_curve_value;
+  navdata_cks += navdata->us_curve_ref;
+  navdata_cks += navdata->nb_echo;
+  navdata_cks += navdata->sum_echo;
+  navdata_cks += navdata->gradient;
+  navdata_cks += navdata->flag_echo_ini;
+  navdata_cks += navdata->pressure;
+  navdata_cks += navdata->temperature_pressure;
+  navdata_cks += navdata->mx;
+  navdata_cks += navdata->my;
+  navdata_cks += navdata->mz;
+//  navdata_cks += navdata->chksum;
+
+  return 0; // we dont know how to calculate the checksum
+}

sw/airborne/boards/ardrone/navdata.h

@@ -0,0 +1,109 @@
+/*
+ * Copyright (C) 2012 Dino Hensen, Vincent van Hoek
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi 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, or (at your option)
+ * any later version.
+ *
+ * Paparazzi 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 Paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file boards/ardrone/navdata.h
+ * ardrone2 navdata aquisition driver.
+ *
+ * The ardrone2 provides a navdata stream of packets
+ * containing info about all sensors at a rate of 200Hz.
+ */
+
+#ifndef NAVDATA_H_
+#define NAVDATA_H_
+
+#include <stdint.h>
+
+#define NAVDATA_PACKET_SIZE 60
+#define NAVDATA_BUFFER_SIZE 80
+#define NAVDATA_START_BYTE 0x3a
+
+typedef struct {
+	uint8_t isInitialized;
+	uint8_t isOpen;
+	uint16_t bytesRead;
+	uint32_t totalBytesRead;
+	uint32_t packetsRead;
+	uint8_t buffer[NAVDATA_BUFFER_SIZE];
+} navdata_port;
+
+typedef struct
+{
+  uint16_t taille;
+  uint16_t nu_trame;
+
+  uint16_t ax;
+  uint16_t ay;
+  uint16_t az;
+
+  int16_t vx;
+  int16_t vy;
+  int16_t vz;
+  uint16_t temperature_acc;
+  uint16_t temperature_gyro;
+
+  uint16_t ultrasound;
+
+  uint16_t us_debut_echo;
+  uint16_t us_fin_echo;
+  uint16_t us_association_echo;
+  uint16_t us_distance_echo;
+
+  uint16_t us_curve_time;
+  uint16_t us_curve_value;
+  uint16_t us_curve_ref;
+
+  uint16_t nb_echo;
+
+  uint32_t sum_echo; //unsigned long
+  int16_t gradient;
+
+  uint16_t flag_echo_ini;
+
+  int32_t pressure; //long
+  int16_t temperature_pressure;
+
+  int16_t mx;
+  int16_t my;
+  int16_t mz;
+
+  uint16_t chksum;
+
+} __attribute__ ((packed)) measures_t;
+
+measures_t* navdata;
+navdata_port* port;
+uint16_t navdata_cks;
+uint8_t navdata_imu_available;
+uint8_t navdata_baro_available;
+int16_t previousUltrasoundHeight;
+
+int navdata_init(void);
+void navdata_close(void);
+
+void navdata_read(void);
+void navdata_update(void);
+void navdata_CropBuffer(int cropsize);
+
+uint16_t navdata_checksum(void);
+int16_t navdata_getHeight(void);
+
+#endif /* NAVDATA_H_ */