diff --git a/conf/Makefile.omap b/conf/Makefile.omap
index 13e6a22336..361ea419ed 100644
--- a/conf/Makefile.omap
+++ b/conf/Makefile.omap
@@ -42,7 +42,8 @@ CFLAGS += -fno-short-enums
# CFLAGS += -malignment-traps
CFLAGS += -Wall -Wcast-qual -Wimplicit -Wcast-align
CFLAGS += -Wpointer-arith -Wswitch
-CFLAGS += -Wredundant-decls -Wreturn-type -Wshadow -Wunused -Wno-unused-result
+CFLAGS += -Wredundant-decls -Wreturn-type -Wshadow -Wunused
+#-Wno-unused-result
#CFLAGS += -Wa,-adhlns=$(OBJDIR)/$(notdir $(subst $(suffix $<),.lst,$<))
#CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
@@ -97,6 +98,7 @@ endif
echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/usbserial.ko /$(SUB_DIR)/usbserial.ko"; \
echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/pl2303.ko /$(SUB_DIR)/pl2303.ko"; \
echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/ftdi-sio.ko /$(SUB_DIR)/ftdi-sio.ko"; \
+ echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/cp210x.ko /$(SUB_DIR)/cp210x.ko"; \
echo "put $(OBJDIR)/$(TARGET).elf /$(SUB_DIR)/$(TARGET).elf"; \
echo "quit"; \
} | ftp -n $(HOST)
@@ -106,6 +108,7 @@ endif
echo "cd $(TARGET_DIR)"; \
echo "insmod cdc-acm.ko"; \
echo "insmod usbserial.ko"; \
+ echo "insmod cp210x.ko"; \
echo "insmod pl2303.ko"; \
echo "insmod ftdi-sio.ko"; \
echo "chmod 777 $(TARGET).elf"; \
diff --git a/conf/airframes/ardrone2_raw.xml b/conf/airframes/ardrone2_raw.xml
new file mode 100644
index 0000000000..d98337de97
--- /dev/null
+++ b/conf/airframes/ardrone2_raw.xml
@@ -0,0 +1,239 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/ardrone2_sdk.xml b/conf/airframes/ardrone2_sdk.xml
new file mode 100644
index 0000000000..b9bc1f7e91
--- /dev/null
+++ b/conf/airframes/ardrone2_sdk.xml
@@ -0,0 +1,154 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/examples/quadshot_asp21_spektrum.xml b/conf/airframes/examples/quadshot_asp21_spektrum.xml
index f2daafbfa3..8f583e899b 100644
--- a/conf/airframes/examples/quadshot_asp21_spektrum.xml
+++ b/conf/airframes/examples/quadshot_asp21_spektrum.xml
@@ -62,7 +62,7 @@ More information on the Quadshot can be found at transition-robotics.com -->
-
+
@@ -224,7 +224,7 @@ More information on the Quadshot can be found at transition-robotics.com -->
-
+
@@ -237,7 +237,7 @@ More information on the Quadshot can be found at transition-robotics.com -->
-
+
@@ -263,7 +263,7 @@ More information on the Quadshot can be found at transition-robotics.com -->
-
+
@@ -288,7 +288,7 @@ More information on the Quadshot can be found at transition-robotics.com -->
-
+
diff --git a/conf/boards/ardrone2_raw.makefile b/conf/boards/ardrone2_raw.makefile
new file mode 100644
index 0000000000..5ab2d5f7cc
--- /dev/null
+++ b/conf/boards/ardrone2_raw.makefile
@@ -0,0 +1,42 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# ardrone2_raw.makefile
+#
+# http://paparazzi.enac.fr/wiki/AR.Drone_2_-_Specifications
+#
+
+BOARD=ardrone
+BOARD_VERSION=2
+BOARD_TYPE=raw
+BOARD_CFG=\"boards/$(BOARD)$(BOARD_VERSION)_$(BOARD_TYPE).h\"
+
+ARCH=omap
+$(TARGET).ARCHDIR = $(ARCH)
+
+# -----------------------------------------------------------------------
+USER=foobar
+HOST=192.168.1.1
+SUB_DIR=raw
+FTP_DIR=/data/video
+TARGET_DIR=$(FTP_DIR)/$(SUB_DIR)
+# -----------------------------------------------------------------------
+
+# The GPS sensor is connected trough USB so we have to define the device
+GPS_PORT ?= UART1
+GPS_BAUD ?= B57600
+
+# Here we define what the UART1_DEV device mapping
+$(TARGET).CFLAGS += -DUART1_DEV=\"/dev/ttyUSB0\"
+#$(TARGET).CFLAGS += -DUART0_DEV=\"/dev/ttyO3\"
+
+# for distinction between RAW and SDK version
+$(TARGET).CFLAGS +=-DARDRONE2_RAW
+
+# -----------------------------------------------------------------------
+
+# default LED configuration
+RADIO_CONTROL_LED ?= none
+BARO_LED ?= none
+AHRS_ALIGNER_LED ?= none
+GPS_LED ?= none
+SYS_TIME_LED ?= none
diff --git a/conf/boards/ardrone2_sdk.makefile b/conf/boards/ardrone2_sdk.makefile
new file mode 100644
index 0000000000..46ba01c8c9
--- /dev/null
+++ b/conf/boards/ardrone2_sdk.makefile
@@ -0,0 +1,34 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# ardrone2_sdk.makefile
+#
+# http://paparazzi.enac.fr/wiki/AR.Drone_2_-_Specifications
+#
+
+BOARD=ardrone
+BOARD_VERSION=2
+BOARD_TYPE=sdk
+BOARD_CFG=\"boards/$(BOARD)$(BOARD_VERSION)_$(BOARD_TYPE).h\"
+
+ARCH=omap
+$(TARGET).ARCHDIR = $(ARCH)
+
+# -----------------------------------------------------------------------
+USER=foobar
+HOST=192.168.1.1
+SUB_DIR=sdk
+FTP_DIR=/data/video
+TARGET_DIR=$(FTP_DIR)/$(SUB_DIR)
+# -----------------------------------------------------------------------
+
+# The GPS sensor is connected trough USB so we have to define the device
+GPS_PORT ?= UART1
+GPS_BAUD ?= B57600
+
+# Here we define what the UART1_DEV device mapping
+$(TARGET).CFLAGS += -DUART1_DEV=\"/dev/ttyUSB0\"
+
+# for distinction between SDK and RAW version
+ap.CFLAGS +=-DARDRONE2_SDK
+
+# -----------------------------------------------------------------------
diff --git a/conf/conf.xml.example b/conf/conf.xml.example
index 55150448ae..d3d1944b63 100644
--- a/conf/conf.xml.example
+++ b/conf/conf.xml.example
@@ -125,6 +125,26 @@
settings=" settings/test_fixedwing_actuators.xml"
gui_color="blue"
/>
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile
index 9dce60427f..ee428e90fa 100644
--- a/conf/firmwares/rotorcraft.makefile
+++ b/conf/firmwares/rotorcraft.makefile
@@ -72,6 +72,10 @@ ifeq ($(ARCH), stm32)
ap.srcs += $(SRC_ARCH)/led_hw.c
endif
+ifeq ($(BOARD)$(BOARD_TYPE), ardroneraw)
+ap.srcs += $(SRC_BOARD)/gpio.c
+endif
+
# frequency of main periodic
PERIODIC_FREQUENCY ?= 512
ap.CFLAGS += -DPERIODIC_FREQUENCY=$(PERIODIC_FREQUENCY)
@@ -145,6 +149,12 @@ else ifeq ($(BOARD), lisa_l)
ap.CFLAGS += -DUSE_I2C2
ap.srcs += $(SRC_BOARD)/baro_board.c
+# Ardrone baro
+else ifeq ($(BOARD)$(BOARD_TYPE), ardroneraw)
+ap.srcs += $(SRC_BOARD)/baro_board.c
+else ifeq ($(BOARD)$(BOARD_TYPE), ardronesdk)
+ap.srcs += $(SRC_BOARD)/baro_board_dummy.c
+
# Lisa/M baro
else ifeq ($(BOARD), lisa_m)
# defaults to i2c baro bmp085 on the board
@@ -206,6 +216,10 @@ ap.CFLAGS += -DUSE_ADC
ap.CFLAGS += -DUSE_AD1 -DUSE_AD1_1 -DUSE_AD1_2 -DUSE_AD1_3 -DUSE_AD1_4
ap.srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c
ap.srcs += subsystems/electrical.c
+else ifeq ($(BOARD)$(BOARD_TYPE), ardronesdk)
+ap.srcs += $(SRC_BOARD)/electrical_dummy.c
+else ifeq ($(BOARD)$(BOARD_TYPE), ardroneraw)
+ap.srcs += $(SRC_ARCH)/subsystems/electrical/electrical_arch.c
endif
@@ -266,3 +280,8 @@ ap.srcs += subsystems/navigation/common_flight_plan.c
# or
# nothing
#
+ifeq ($(ARCH), omap)
+SRC_FMS=fms
+ap.CFLAGS += -I. -I$(SRC_FMS)
+ap.srcs += $(SRC_FMS)/fms_serial_port.c
+endif
diff --git a/conf/firmwares/subsystems/rotorcraft/ahrs_ardrone2.makefile b/conf/firmwares/subsystems/rotorcraft/ahrs_ardrone2.makefile
new file mode 100644
index 0000000000..020408e4fc
--- /dev/null
+++ b/conf/firmwares/subsystems/rotorcraft/ahrs_ardrone2.makefile
@@ -0,0 +1,16 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# Fixed point complementary filter using euler angles for attitude estimation
+#
+
+AHRS_CFLAGS = -DUSE_AHRS -DUSE_AHRS_ARDRONE2
+
+AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_ardrone2.h\"
+AHRS_SRCS += subsystems/ahrs.c
+AHRS_SRCS += subsystems/ahrs/ahrs_ardrone2.c
+
+ap.CFLAGS += $(AHRS_CFLAGS)
+ap.srcs += $(AHRS_SRCS)
+
+nps.CFLAGS += $(AHRS_CFLAGS)
+nps.srcs += $(AHRS_SRCS)
diff --git a/conf/firmwares/subsystems/rotorcraft/ins_ardrone2.makefile b/conf/firmwares/subsystems/rotorcraft/ins_ardrone2.makefile
new file mode 100644
index 0000000000..759efc1d21
--- /dev/null
+++ b/conf/firmwares/subsystems/rotorcraft/ins_ardrone2.makefile
@@ -0,0 +1,7 @@
+#
+# simple INS with float vertical filter
+#
+
+$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_ardrone2.h\" -DAUTOPILOT_DISABLE_AHRS_KILL
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_ardrone2.c
diff --git a/conf/firmwares/subsystems/shared/actuators_ardrone2.makefile b/conf/firmwares/subsystems/shared/actuators_ardrone2.makefile
new file mode 100644
index 0000000000..8273f4461d
--- /dev/null
+++ b/conf/firmwares/subsystems/shared/actuators_ardrone2.makefile
@@ -0,0 +1,11 @@
+
+# Actuator drivers for both ardrone versions are conditionally included here
+# The AT-command and RAW drivers are not interchangeble
+
+ifeq ($(BOARD_TYPE), sdk)
+ $(TARGET).srcs += $(SRC_BOARD)/actuators_at.c
+ $(TARGET).srcs += $(SRC_BOARD)/at_com.c
+else ifeq ($(BOARD_TYPE), raw)
+ $(TARGET).CFLAGS += -DACTUATORS
+ $(TARGET).srcs += $(SRC_BOARD)/actuators_ardrone2_raw.c
+endif
diff --git a/conf/firmwares/subsystems/shared/imu_ardrone2.makefile b/conf/firmwares/subsystems/shared/imu_ardrone2.makefile
new file mode 100644
index 0000000000..c574fd2d8a
--- /dev/null
+++ b/conf/firmwares/subsystems/shared/imu_ardrone2.makefile
@@ -0,0 +1,22 @@
+# imu AR.Drone2
+
+ifeq ($(BOARD_TYPE), sdk)
+imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_ardrone2_sdk.h\" -DUSE_IMU
+imu_srcs += $(SRC_SUBSYSTEMS)/imu.c
+imu_srcs += $(SRC_SUBSYSTEMS)/imu/imu_ardrone2_sdk.c
+else ifeq ($(BOARD_TYPE), raw)
+imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_ardrone2_raw.h\" -DUSE_IMU
+imu_srcs += $(SRC_SUBSYSTEMS)/imu.c
+imu_srcs += $(SRC_SUBSYSTEMS)/imu/imu_ardrone2_raw.c
+imu_srcs += $(SRC_BOARD)/navdata.c
+endif
+
+# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
+# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example
+ap.CFLAGS += $(imu_CFLAGS)
+ap.srcs += $(imu_srcs)
+
+#
+# Simulator
+#
+include $(CFG_SHARED)/imu_nps.makefile
diff --git a/conf/messages.xml b/conf/messages.xml
index d6c35e4e68..2ddf6a8999 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -19,7 +19,36 @@
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -1834,7 +1863,22 @@
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/telemetry/default_ardrone.xml b/conf/telemetry/default_ardrone.xml
new file mode 100644
index 0000000000..bc329e0112
--- /dev/null
+++ b/conf/telemetry/default_ardrone.xml
@@ -0,0 +1,147 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/tests_conf.xml b/conf/tests_conf.xml
index 422bc6bece..cca8a744c4 100644
--- a/conf/tests_conf.xml
+++ b/conf/tests_conf.xml
@@ -226,6 +226,26 @@
settings=" settings/fixedwing_basic.xml settings/control/ctl_basic.xml settings/estimation/ins_neutrals.xml settings/modules/cam.xml"
gui_color="blue"
/>
+
+
+ *
+ * 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 /* Standard input/output definitions */
+#include /* String function definitions */
+#include /* UNIX standard function definitions */
+#include /* File control definitions */
+#include /* Error number definitions */
+#include /* POSIX terminal control definitions */
+#include
+
+/**
+ * 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);
+}
diff --git a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.h b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.h
new file mode 100644
index 0000000000..2a66ba5dde
--- /dev/null
+++ b/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
+ *
+ * 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
+
+#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_ */
diff --git a/sw/airborne/boards/ardrone/actuators_at.c b/sw/airborne/boards/ardrone/actuators_at.c
new file mode 100644
index 0000000000..380a919a53
--- /dev/null
+++ b/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");
+}
diff --git a/sw/airborne/boards/ardrone/actuators_at.h b/sw/airborne/boards/ardrone/actuators_at.h
new file mode 100644
index 0000000000..b862a92c6c
--- /dev/null
+++ b/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 */
diff --git a/sw/airborne/boards/ardrone/at_com.c b/sw/airborne/boards/ardrone/at_com.c
new file mode 100644
index 0000000000..b2c11e4d0b
--- /dev/null
+++ b/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
+#include
+#include
+#include
+#include
+#include
+
+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);
+}
diff --git a/sw/airborne/boards/ardrone/at_com.h b/sw/airborne/boards/ardrone/at_com.h
new file mode 100644
index 0000000000..08605e9843
--- /dev/null
+++ b/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 */
diff --git a/sw/airborne/boards/ardrone/baro_board.c b/sw/airborne/boards/ardrone/baro_board.c
new file mode 100644
index 0000000000..7526ac485e
--- /dev/null
+++ b/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;
+ }
+}
diff --git a/sw/airborne/boards/ardrone/baro_board.h b/sw/airborne/boards/ardrone/baro_board.h
new file mode 100644
index 0000000000..7233f8b437
--- /dev/null
+++ b/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 */
diff --git a/sw/airborne/boards/ardrone/baro_board_dummy.c b/sw/airborne/boards/ardrone/baro_board_dummy.c
new file mode 100644
index 0000000000..8487675af7
--- /dev/null
+++ b/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) {
+}
diff --git a/sw/airborne/boards/ardrone/electrical_dummy.c b/sw/airborne/boards/ardrone/electrical_dummy.c
new file mode 100644
index 0000000000..0093d0a011
--- /dev/null
+++ b/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) { }
diff --git a/sw/airborne/boards/ardrone/gpio.c b/sw/airborne/boards/ardrone/gpio.c
new file mode 100644
index 0000000000..71879b526e
--- /dev/null
+++ b/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
+#include
+#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);
+}
diff --git a/sw/airborne/boards/ardrone/gpio.h b/sw/airborne/boards/ardrone/gpio.h
new file mode 100644
index 0000000000..3a445d5954
--- /dev/null
+++ b/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 */
diff --git a/sw/airborne/boards/ardrone/i2c-dev.h b/sw/airborne/boards/ardrone/i2c-dev.h
new file mode 100644
index 0000000000..79985d2a6b
--- /dev/null
+++ b/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
+ *
+ * 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
+#include
+
+/*
+ * 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 */
diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c
new file mode 100644
index 0000000000..8231957d70
--- /dev/null
+++ b/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
+#include
+#include // for O_RDWR, O_NOCTTY, O_NONBLOCK
+#include // for baud rates and options
+#include
+#include
+#include
+#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
+}
diff --git a/sw/airborne/boards/ardrone/navdata.h b/sw/airborne/boards/ardrone/navdata.h
new file mode 100644
index 0000000000..cf149b2196
--- /dev/null
+++ b/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
+
+#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_ */
diff --git a/sw/airborne/boards/ardrone2_raw.h b/sw/airborne/boards/ardrone2_raw.h
new file mode 100644
index 0000000000..6200042285
--- /dev/null
+++ b/sw/airborne/boards/ardrone2_raw.h
@@ -0,0 +1,14 @@
+#ifndef CONFIG_ARDRONE2_RAW
+#define CONFIG_ARDRONE2_RAW
+
+#define BOARD_ARDRONE2_RAW
+
+/* Default actuators driver */
+#define DEFAULT_ACTUATORS "boards/ardrone/actuators_ardrone2_raw.h"
+#define ActuatorDefaultSet(_x,_y) ActuatorArdroneSet(_x,_y)
+#define ActuatorsDefaultInit() ActuatorsArdroneInit()
+#define ActuatorsDefaultCommit() ActuatorsArdroneCommit()
+
+#define BOARD_HAS_BARO 1
+
+#endif /* CONFIG_ARDRONE2_RAW */
diff --git a/sw/airborne/boards/ardrone2_sdk.h b/sw/airborne/boards/ardrone2_sdk.h
new file mode 100644
index 0000000000..c325f3ec40
--- /dev/null
+++ b/sw/airborne/boards/ardrone2_sdk.h
@@ -0,0 +1,20 @@
+#ifndef CONFIG_ARDRONE2_SDK
+#define CONFIG_ARDRONE2_SDK
+
+#define BOARD_ARDRONE2_SDK
+
+/* Internal communication */
+#define ARDRONE_NAVDATA_PORT 5554
+#define ARDRONE_AT_PORT 5556
+#define ARDRONE_NAVDATA_BUFFER_SIZE 2048
+#define ARDRONE_IP "192.168.1.1"
+
+/* Default actuators driver */
+#define DEFAULT_ACTUATORS "boards/ardrone/actuators_at.h"
+#define ActuatorDefaultSet(_x,_y) {}
+#define ActuatorsDefaultInit() {}
+#define ActuatorsDefaultCommit() {}
+
+#define BOARD_HAS_BARO 0
+
+#endif /* CONFIG_ARDRONE2_SDK */
diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c
index eb40d61fa3..7650273a95 100644
--- a/sw/airborne/firmwares/rotorcraft/main.c
+++ b/sw/airborne/firmwares/rotorcraft/main.c
@@ -38,6 +38,9 @@
#include "subsystems/datalink/datalink.h"
#include "subsystems/settings.h"
#include "subsystems/datalink/xbee.h"
+#if DATALINK == WIFI
+#include "subsystems/datalink/wifi.h"
+#endif
#include "subsystems/commands.h"
#include "subsystems/actuators.h"
@@ -155,6 +158,10 @@ STATIC_INLINE void main_init( void ) {
xbee_init();
#endif
+#if DATALINK == WIFI
+ wifi_init();
+#endif
+
// register the timers for the periodic functions
main_periodic_tid = sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
modules_tid = sys_time_register_timer(1./MODULES_FREQUENCY, NULL);
diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h
index 9396b709de..39f06618a7 100644
--- a/sw/airborne/firmwares/rotorcraft/telemetry.h
+++ b/sw/airborne/firmwares/rotorcraft/telemetry.h
@@ -358,6 +358,28 @@
#define PERIODIC_SEND_FILTER(_trans, _dev) {}
#endif
+#if USE_AHRS_ARDRONE2
+#include "subsystems/ahrs/ahrs_ardrone2.h"
+#define PERIODIC_SEND_AHRS_ARDRONE2(_trans, _dev) { \
+ DOWNLINK_SEND_AHRS_ARDRONE2(_trans, _dev, \
+ &ahrs_impl.state, \
+ &ahrs_impl.control_state, \
+ &ahrs_impl.eulers.phi, \
+ &ahrs_impl.eulers.theta, \
+ &ahrs_impl.eulers.psi, \
+ &ahrs_impl.speed.x, \
+ &ahrs_impl.speed.y, \
+ &ahrs_impl.speed.z, \
+ &ahrs_impl.accel.x, \
+ &ahrs_impl.accel.y, \
+ &ahrs_impl.accel.z, \
+ &ahrs_impl.altitude, \
+ &ahrs_impl.battery); \
+ }
+#else
+#define PERIODIC_SEND_AHRS_ARDRONE2(_trans, _dev) {}
+#endif
+
#if USE_AHRS_CMPL_EULER || USE_AHRS_CMPL_QUAT
#define PERIODIC_SEND_AHRS_GYRO_BIAS_INT(_trans, _dev) { \
DOWNLINK_SEND_AHRS_GYRO_BIAS_INT(_trans, _dev, \
@@ -932,6 +954,42 @@
#define PERIODIC_SEND_CAM_TRACK(_trans, _dev) {}
#endif
+#ifdef ARDRONE2_RAW
+#include "navdata.h"
+#define PERIODIC_SEND_ARDRONE_NAVDATA(_trans, _dev) DOWNLINK_SEND_ARDRONE_NAVDATA(_trans, _dev, \
+ &navdata->taille, \
+ &navdata->nu_trame, \
+ &navdata->ax, \
+ &navdata->ay, \
+ &navdata->az, \
+ &navdata->vx, \
+ &navdata->vy, \
+ &navdata->vz, \
+ &navdata->temperature_acc, \
+ &navdata->temperature_gyro, \
+ &navdata->ultrasound, \
+ &navdata->us_debut_echo, \
+ &navdata->us_fin_echo, \
+ &navdata->us_association_echo, \
+ &navdata->us_distance_echo, \
+ &navdata->us_curve_time, \
+ &navdata->us_curve_value, \
+ &navdata->us_curve_ref, \
+ &navdata->nb_echo, \
+ &navdata->sum_echo, \
+ &navdata->gradient, \
+ &navdata->flag_echo_ini, \
+ &navdata->pressure, \
+ &navdata->temperature_pressure, \
+ &navdata->mx, \
+ &navdata->my, \
+ &navdata->mz, \
+ &navdata->chksum \
+ )
+#else
+#define PERIODIC_SEND_ARDRONE_NAVDATA(_trans, _dev) {}
+#endif
+
#include "generated/settings.h"
#define PERIODIC_SEND_DL_VALUE(_trans, _dev) PeriodicSendDlValue(_trans, _dev)
diff --git a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c
new file mode 100644
index 0000000000..95c865fb90
--- /dev/null
+++ b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c
@@ -0,0 +1,132 @@
+/*
+ * 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 subsystems/ahrs/ahrs_ardrone2.c
+ * AHRS implementation for ardrone2-sdk based on AT-commands.
+ *
+ * Uses AT-Commands to communicate with ardrone api to retrieve AHRS data
+ * and also sets battery level.
+ */
+
+#include "ahrs_ardrone2.h"
+#include "state.h"
+#include "math/pprz_algebra_float.h"
+#include "boards/ardrone/at_com.h"
+#include "subsystems/electrical.h"
+
+struct AhrsARDrone ahrs_impl;
+struct AhrsAligner ahrs_aligner;
+unsigned char buffer[2048]; //Packet buffer
+
+void ahrs_init(void) {
+ init_at_com();
+
+ //Set navdata_demo to FALSE and flat trim the ar drone
+ at_com_send_config("general:navdata_demo", "FALSE");
+ at_com_send_ftrim();
+
+ ahrs.status = AHRS_RUNNING;
+}
+
+void ahrs_align(void) {
+
+}
+
+void ahrs_propagate(void) {
+ //Recieve the main packet
+ at_com_recieve_navdata(buffer);
+ navdata_t* main_packet = (navdata_t*) &buffer;
+ ahrs_impl.state = main_packet->ardrone_state;
+
+ //Init the option
+ navdata_option_t* navdata_option = (navdata_option_t*)&(main_packet->options[0]);
+ bool_t full_read = FALSE;
+
+ //The possible packets
+ navdata_demo_t* navdata_demo;
+ navdata_phys_measures_t* navdata_phys_measures;
+
+ //Read the navdata until packet is fully readed
+ while(!full_read) {
+ //Check the tag for the right option
+ switch(navdata_option->tag) {
+ case 0: //NAVDATA_DEMO
+ navdata_demo = (navdata_demo_t*) navdata_option;
+
+ //Set the AHRS state
+ ahrs_impl.control_state = navdata_demo->ctrl_state >> 16;
+ ahrs_impl.eulers.phi = navdata_demo->phi;
+ ahrs_impl.eulers.theta = navdata_demo->theta;
+ ahrs_impl.eulers.psi = navdata_demo->psi;
+ ahrs_impl.speed.x = navdata_demo->vx / 1000;
+ ahrs_impl.speed.y = navdata_demo->vy / 1000;
+ ahrs_impl.speed.z = navdata_demo->vz / 1000;
+ ahrs_impl.altitude = navdata_demo->altitude / 10;
+ ahrs_impl.battery = navdata_demo->vbat_flying_percentage;
+
+ //Set the ned to body eulers
+ struct FloatEulers angles;
+ angles.theta = navdata_demo->theta/180000.*M_PI;
+ angles.psi = navdata_demo->psi/180000.*M_PI;
+ angles.phi = navdata_demo->phi/180000.*M_PI;
+ stateSetNedToBodyEulers_f(&angles);
+
+ //Update the electrical supply
+ electrical.vsupply = navdata_demo->vbat_flying_percentage;
+ break;
+ case 3: //NAVDATA_PHYS_MEASURES
+ navdata_phys_measures = (navdata_phys_measures_t*) navdata_option;
+
+ //Set the AHRS accel state
+ INT32_VECT3_SCALE_2(ahrs_impl.accel, navdata_phys_measures->phys_accs, 9.81, 1000)
+ break;
+ case 0xFFFF: //CHECKSUM
+ //TODO: Check the checksum
+ full_read = TRUE;
+ break;
+ default:
+ //printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size);
+ break;
+ }
+ navdata_option = (navdata_option_t*) ((int)navdata_option + navdata_option->size);
+ }
+}
+
+void ahrs_update_accel(void) {
+
+}
+
+void ahrs_update_mag(void) {
+
+}
+
+void ahrs_update_gps(void) {
+
+}
+
+void ahrs_aligner_init(void) {
+
+}
+
+void ahrs_aligner_run(void) {
+
+}
diff --git a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.h b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.h
new file mode 100644
index 0000000000..edd25a819d
--- /dev/null
+++ b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.h
@@ -0,0 +1,50 @@
+/*
+ * 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 subsystems/ahrs/ahrs_ardrone2.h
+ * AHRS implementation for ardrone2-sdk based on AT-commands.
+ *
+ * Uses AT-Commands to communicate with ardrone api to retrieve AHRS data
+ * and also sets battery level.
+ */
+
+#ifndef AHRS_ARDRONE2_H
+#define AHRS_ARDRONE2_H
+
+#include "subsystems/ahrs.h"
+#include "subsystems/ahrs/ahrs_aligner.h"
+#include "std.h"
+#include "math/pprz_algebra_int.h"
+
+struct AhrsARDrone {
+ uint32_t state; // ARDRONE_STATES
+ uint32_t control_state; // CTRL_STATES
+ struct FloatEulers eulers; // in radians
+ struct NedCoor_f speed; // in m/s
+ struct NedCoor_f accel; // in m/s^2
+ int32_t altitude; // in cm above ground
+ uint32_t battery; // in percentage
+ struct Int32Quat ltp_to_imu_quat;
+};
+extern struct AhrsARDrone ahrs_impl;
+
+#endif /* AHRS_ARDRONE2_H */
diff --git a/sw/airborne/subsystems/imu/imu_ardrone2_raw.c b/sw/airborne/subsystems/imu/imu_ardrone2_raw.c
new file mode 100644
index 0000000000..b43a6ab357
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_ardrone2_raw.c
@@ -0,0 +1,54 @@
+/*
+ * Copyright (C) 2012-2013 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 subsystems/imu/imu_ardrone2_raw.c
+ * IMU implementation for ardrone2-raw.
+ */
+
+#include "subsystems/imu.h"
+#include "navdata.h"
+#include "imu_ardrone2_raw.h"
+#include "mcu_periph/uart.h"
+
+void imu_impl_init(void) {
+ imu_data_available = FALSE;
+ navdata_init();
+}
+
+void imu_periodic(void) {
+ navdata_update();
+ //checks if the navboard has a new dataset ready
+ if (navdata_imu_available == TRUE) {
+ navdata_imu_available = FALSE;
+ RATES_ASSIGN(imu.gyro_unscaled, navdata->vx, navdata->vy, navdata->vz);
+ VECT3_ASSIGN(imu.accel_unscaled, navdata->ax, navdata->ay, navdata->az);
+ VECT3_ASSIGN(imu.mag_unscaled, navdata->mx, navdata->my, navdata->mz);
+ imu_data_available = TRUE;
+ }
+ else {
+ imu_data_available = FALSE;
+ }
+
+#ifdef USE_UART1
+ uart1_handler();
+#endif
+}
diff --git a/sw/airborne/subsystems/imu/imu_ardrone2_raw.h b/sw/airborne/subsystems/imu/imu_ardrone2_raw.h
new file mode 100644
index 0000000000..ce15a2c2d5
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_ardrone2_raw.h
@@ -0,0 +1,50 @@
+/*
+ * Copyright (C) 2012-2013 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 subsystems/imu/imu_ardrone2_raw.h
+ * IMU implementation for ardrone2-raw.
+ */
+
+#ifndef IMU_ARDRONE2_RAW_H_
+#define IMU_ARDRONE2_RAW_H_
+
+#include "subsystems/imu.h"
+#include "generated/airframe.h"
+#include "navdata.h"
+
+int imu_data_available;
+
+static inline void imu_ardrone2_event ( void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
+{
+ if (imu_data_available) {
+ imu_data_available = FALSE;
+ _gyro_handler();
+ _accel_handler();
+ _mag_handler();
+ }
+}
+
+#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
+ imu_ardrone2_event(_gyro_handler, _accel_handler, _mag_handler); \
+}
+
+#endif /* IMU_ARDRONE2_RAW_H_ */
diff --git a/sw/airborne/subsystems/imu/imu_ardrone2_sdk.c b/sw/airborne/subsystems/imu/imu_ardrone2_sdk.c
new file mode 100644
index 0000000000..837020778d
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_ardrone2_sdk.c
@@ -0,0 +1,37 @@
+/*
+ * 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 subsystems/imu/imu_ardrone2_sdk.c
+ * IMU implementation for ardrone2-sdk.
+ */
+
+#include "subsystems/imu.h"
+#include "imu_ardrone2_sdk.h"
+
+
+void imu_impl_init(void) {
+
+}
+
+void imu_periodic(void) {
+
+}
diff --git a/sw/airborne/subsystems/imu/imu_ardrone2_sdk.h b/sw/airborne/subsystems/imu/imu_ardrone2_sdk.h
new file mode 100644
index 0000000000..14fab1c40e
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_ardrone2_sdk.h
@@ -0,0 +1,39 @@
+/*
+ * 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 subsystems/imu/imu_ardrone2_sdk.h
+ * IMU implementation for ardrone2-sdk.
+ */
+
+#ifndef IMU_ARDRONE2_SDK_H_
+#define IMU_ARDRONE2_SDK_H_
+
+#include "subsystems/imu.h"
+#include "generated/airframe.h"
+
+static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) {
+ _gyro_handler();
+ _accel_handler();
+ _mag_handler();
+}
+
+#endif /* IMU_ARDRONE2_SDK_H_ */
diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.c b/sw/airborne/subsystems/ins/ins_ardrone2.c
new file mode 100644
index 0000000000..cdb8069e46
--- /dev/null
+++ b/sw/airborne/subsystems/ins/ins_ardrone2.c
@@ -0,0 +1,147 @@
+/*
+ * 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 subsystems/ins/ins_ardrone2.c
+ * INS implementation for ardrone2-sdk.
+ */
+
+#include "subsystems/ins/ins_ardrone2.h"
+#include "subsystems/ahrs.h"
+#include "subsystems/gps.h"
+#include "generated/airframe.h"
+#include "generated/flight_plan.h"
+#include "math/pprz_geodetic_int.h"
+
+#ifdef SITL
+#include "nps_fdm.h"
+#include
+#endif
+
+/* TODO: implement in state */
+int32_t ins_qfe;
+int32_t ins_baro_alt;
+
+//Keep track of gps pos and the init pos
+struct NedCoor_i ins_ltp_pos;
+struct LtpDef_i ins_ltp_def;
+
+// Keep track of INS LTP accel and speed
+struct NedCoor_f ins_ltp_accel;
+struct NedCoor_f ins_ltp_speed;
+
+bool_t ins_ltp_initialised;
+
+void ins_init() {
+#if USE_INS_NAV_INIT
+ struct LlaCoor_i llh_nav;
+
+ /** FIXME: should use the same code than MOVE_WP in firmwares/rotorcraft/datalink.c */
+ llh_nav.lat = INT32_RAD_OF_DEG(NAV_LAT0);
+ llh_nav.lon = INT32_RAD_OF_DEG(NAV_LON0);
+ llh_nav.alt = NAV_ALT0 + NAV_MSL0;
+
+ //Convert ltp
+ ltp_def_from_lla_i(&ins_ltp_def, &llh_nav);
+ ins_ltp_def.hmsl = NAV_ALT0;
+
+ //Set the ltp
+ stateSetLocalOrigin_i(&ins_ltp_def);
+
+ ins_ltp_initialised = TRUE;
+#else
+ ins_ltp_initialised = FALSE;
+#endif
+
+ ins.vf_realign = FALSE;
+ ins.hf_realign = FALSE;
+
+ INT32_VECT3_ZERO(ins_ltp_pos);
+
+ // TODO correct init
+ ins.status = INS_RUNNING;
+}
+
+void ins_periodic( void ) {
+
+}
+
+void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {
+
+}
+
+void ins_realign_v(float z __attribute__ ((unused))) {
+
+}
+
+void ins_propagate() {
+ /* untilt accels and speeds */
+ FLOAT_RMAT_VECT3_TRANSP_MUL(ins_ltp_accel, (*stateGetNedToBodyRMat_f()), ahrs_impl.accel);
+ FLOAT_RMAT_VECT3_TRANSP_MUL(ins_ltp_speed, (*stateGetNedToBodyRMat_f()), ahrs_impl.speed);
+
+ //Add g to the accelerations
+ ins_ltp_accel.z += 9.81;
+
+ //Save the accelerations and speeds
+ stateSetAccelNed_f(&ins_ltp_accel);
+ stateSetSpeedNed_f(&ins_ltp_speed);
+
+#if !USE_GPS_HEIGHT
+ //Set the height and save the position
+ ins_ltp_pos.z = -(ahrs_impl.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
+ stateSetPositionNed_i(&ins_ltp_pos);
+#endif
+}
+
+void ins_update_baro() {
+
+}
+
+
+void ins_update_gps(void) {
+#if USE_GPS
+ //Check for GPS fix
+ if (gps.fix == GPS_FIX_3D) {
+ //Set the initial coordinates
+ if(!ins_ltp_initialised) {
+ ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos);
+ ins_ltp_def.lla.alt = gps.lla_pos.alt;
+ ins_ltp_def.hmsl = gps.hmsl;
+ ins_ltp_initialised = TRUE;
+ stateSetLocalOrigin_i(&ins_ltp_def);
+ }
+
+ //Set the x and y and maybe z position in ltp and save
+ struct NedCoor_i ins_gps_pos_cm_ned;
+ ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos);
+#if USE_GPS_HEIGHT
+ INT32_VECT3_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
+#else
+ INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
+#endif
+ stateSetPositionNed_i(&ins_ltp_pos);
+ }
+#endif /* USE_GPS */
+}
+
+void ins_update_sonar() {
+
+}
diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.h b/sw/airborne/subsystems/ins/ins_ardrone2.h
new file mode 100644
index 0000000000..c14e362eea
--- /dev/null
+++ b/sw/airborne/subsystems/ins/ins_ardrone2.h
@@ -0,0 +1,45 @@
+/*
+ * 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 subsystems/ins/ins_ardrone2.h
+ * INS implementation for ardrone2-sdk.
+ */
+
+#ifndef INS_INT_H
+#define INS_INT_H
+
+#include "subsystems/ins.h"
+#include "std.h"
+#include "math/pprz_geodetic_int.h"
+#include "math/pprz_algebra_float.h"
+
+//TODO: implement in state
+extern int32_t ins_qfe;
+extern int32_t ins_baro_alt;
+
+extern struct NedCoor_i ins_ltp_pos;
+extern struct LtpDef_i ins_ltp_def;
+extern struct NedCoor_f ins_ltp_speed;
+extern struct NedCoor_f ins_ltp_accel;
+extern bool_t ins_ltp_initialised;
+
+#endif /* INS_INT_H */
diff --git a/sw/ext/ardrone2_drivers/cdc-acm.ko b/sw/ext/ardrone2_drivers/cdc-acm.ko
new file mode 100644
index 0000000000..b8db911440
Binary files /dev/null and b/sw/ext/ardrone2_drivers/cdc-acm.ko differ
diff --git a/sw/ext/ardrone2_drivers/cp210x.ko b/sw/ext/ardrone2_drivers/cp210x.ko
new file mode 100644
index 0000000000..0f811d1807
Binary files /dev/null and b/sw/ext/ardrone2_drivers/cp210x.ko differ
diff --git a/sw/ext/ardrone2_drivers/ftdi-sio.ko b/sw/ext/ardrone2_drivers/ftdi-sio.ko
new file mode 100644
index 0000000000..1553306fb9
Binary files /dev/null and b/sw/ext/ardrone2_drivers/ftdi-sio.ko differ
diff --git a/sw/ext/ardrone2_drivers/pl2303.ko b/sw/ext/ardrone2_drivers/pl2303.ko
new file mode 100644
index 0000000000..4d61b32426
Binary files /dev/null and b/sw/ext/ardrone2_drivers/pl2303.ko differ
diff --git a/sw/ext/ardrone2_drivers/usbserial.ko b/sw/ext/ardrone2_drivers/usbserial.ko
new file mode 100644
index 0000000000..cae655f71b
Binary files /dev/null and b/sw/ext/ardrone2_drivers/usbserial.ko differ