diff --git a/conf/airframes/antenna.xml b/conf/airframes/antenna.xml
new file mode 100644
index 0000000000..648527c91e
--- /dev/null
+++ b/conf/airframes/antenna.xml
@@ -0,0 +1,44 @@
+
+
+
+
+ARCHI=avr
+ant.ARCHDIR = $(ARCHI)
+ant.ARCH = atmega128
+ant.TARGET = ant
+ant.TARGETDIR = ant
+
+
+# 16MHz resonator
+ant.LOW_FUSE = CF
+ant.HIGH_FUSE = 89
+ant.EXT_FUSE = ff
+ant.LOCK_FUSE = ff
+
+
+ant.CFLAGS += -DCONFIG=\"antenna.h\"
+ant.srcs = main_antenna.c sys_time.c $(SRC_ARCH)/sys_time_hw.c
+
+ant.CFLAGS += -DLED
+
+ant.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B38400
+ant.srcs += $(SRC_ARCH)/uart_hw.c
+
+ant.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
+ant.srcs += downlink.c pprz_transport.c
+
+ant.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart1
+ant.srcs += datalink.c
+
+#ant.CFLAGS +=
+ant.srcs += avr/ant_v2x.c avr/ant_spi.c
+
+ant.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3 -DUSE_AD0_4
+ant.srcs += $(SRC_ARCH)/adc_hw.c
+
+
+
+
+
+
+
diff --git a/conf/airframes/motor_bench.xml b/conf/airframes/motor_bench.xml
index 43dd2762f5..f8abf8ef31 100644
--- a/conf/airframes/motor_bench.xml
+++ b/conf/airframes/motor_bench.xml
@@ -1,4 +1,4 @@
-
+
diff --git a/conf/autopilot/antenna.h b/conf/autopilot/antenna.h
new file mode 100644
index 0000000000..f18fb43d86
--- /dev/null
+++ b/conf/autopilot/antenna.h
@@ -0,0 +1,13 @@
+#ifndef CONFIG_ANTENNA_H
+#define CONFIG_ANTENNA_H
+
+
+#define CLOCK 16
+
+#define LED_1_BANK C
+#define LED_1_PIN 0
+
+#define LED_2_BANK C
+#define LED_2_PIN 1
+
+#endif /* CONFIG_MOTOR_BENCH_H */
diff --git a/conf/messages.xml b/conf/messages.xml
index dc710d4b98..f1ca47f72a 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -344,6 +344,17 @@
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/avr/ant_spi.c b/sw/airborne/avr/ant_spi.c
new file mode 100644
index 0000000000..ec835643af
--- /dev/null
+++ b/sw/airborne/avr/ant_spi.c
@@ -0,0 +1,71 @@
+#include "ant_spi.h"
+
+#include
+#include "std.h"
+
+
+/*********************************Flush SPI*****************************************/
+
+
+void flush_SPI( void )
+{
+ if (bit_is_set(SPSR, SPIF))
+ {
+ uint8_t foo __attribute__ ((unused)) = SPDR;
+ }
+}
+
+
+/*****************************Initialisation SPI************************************/
+
+#define DDR_SPI DDRB
+#define PORT_SPI PORTB
+#define PIN_SS 0
+#define PIN_SCK 1
+#define PIN_MOSI 2
+#define PIN_SYNC 4
+
+void SPI_select_slave ( void )
+{
+ ClearBit(PORT_SPI, PIN_SS);
+}
+
+/***********************************************************************************/
+
+void SPI_unselect_slave ( void )
+{
+ SetBit(PORT_SPI, PIN_SS);
+}
+
+/***********************************************************************************/
+
+void SPI_master_init( void )
+{
+ /* Set SS, MOSI and SCK output, all others input */
+ DDR_SPI |= _BV(PIN_SS) | _BV(PIN_SCK) | _BV(PIN_MOSI);
+ /* unselect slave */
+ SPI_unselect_slave();
+ /* Enable SPI, Master, MSB first, clock idle low, sample on leading edge, clock rate fck/128 */
+ SPCR = ( _BV(SPE)| _BV(MSTR) | _BV(SPR1) | _BV(SPR0));
+}
+
+/***********************************************************************************/
+
+void SPI_start( void )
+{
+ SPI_select_slave();
+ if (bit_is_set(SPSR, SPIF)) {
+ uint8_t foo __attribute__ ((unused)) = SPDR;
+ }
+ /* enable interrupt */
+ SetBit(SPCR,SPIE);
+}
+
+/***********************************************************************************/
+
+void SPI_stop(void)
+{
+ SPI_unselect_slave ();
+ /* disable interrupt */
+ ClearBit(SPCR,SPIE);
+}
diff --git a/sw/airborne/avr/ant_spi.h b/sw/airborne/avr/ant_spi.h
new file mode 100644
index 0000000000..076f533d74
--- /dev/null
+++ b/sw/airborne/avr/ant_spi.h
@@ -0,0 +1,19 @@
+#ifndef ANT_SPI_H
+#define ANT_SPI_H
+
+#include "std.h"
+
+#include
+#include "std.h"
+
+void flush_SPI( void );
+void SPI_select_slave ( void );
+void SPI_unselect_slave ( void );
+void SPI_master_init( void );
+void SPI_start( void );
+void SPI_stop( void );
+
+#define SPI_transmit(c) { SPDR = c; }
+#define SPI_read() (SPDR)
+
+#endif /* ANT_SPI_H */
diff --git a/sw/airborne/avr/ant_v2x.c b/sw/airborne/avr/ant_v2x.c
new file mode 100644
index 0000000000..188d401919
--- /dev/null
+++ b/sw/airborne/avr/ant_v2x.c
@@ -0,0 +1,255 @@
+#include "ant_v2x.h"
+
+#include
+#include
+
+//#include "stdlib.h"
+#include "string.h"
+#include "std.h"
+//#include "systime.h"
+//#include "signalisation.h"
+//#include "utils.h"
+
+#include "ant_spi.h"
+
+//#include "downlink.h"
+
+volatile bool_t ant_v2x_data_available;
+struct Ant_V2xData ant_v2x_data;
+
+
+#define MAG_S_RESET 0
+#define MAG_S_UNINIT 1
+#define MAG_S_READY 2
+#define MAG_S_WAIT_MEAS 3
+volatile uint8_t ant_v2x_status;
+
+#define MAG_CS_IDLE 0
+#define MAG_CS_READING 1
+#define MAG_CS_WRITING 2
+volatile uint8_t ant_v2x_com_status;
+
+
+uint8_t ant_v2x_req[64];
+uint8_t ant_v2x_req_len;
+volatile uint8_t ant_v2x_req_idx;
+
+uint8_t ant_v2x_res[128];
+uint8_t ant_v2x_res_len;
+volatile uint8_t ant_v2x_res_idx;
+
+volatile uint8_t ant_v2x_periodic_count;
+
+/******************************Ant_V2x define************************************/
+
+#define ANT_V2X_DDR DDRB
+#define ANT_V2X_PORT PORTB
+#define ANT_V2X_PIN 4
+
+#define SYNC_FLAG 0xAA
+#define TERMINATOR 0x00
+
+#define GET_MODE_INFO 0x01
+#define MOD_INFO_RESP 0x02
+#define SET_DATA_COMPONENTS 0x03
+#define GET_DATA 0x04
+#define DATA_RESP 0x05
+#define SET_CONFIG 0x06
+#define GET_CONFIG 0x07
+#define CONFIG_RESP 0x08
+#define SAVE_CONFIG 0x09
+#define START_CAL 0x0A
+#define STOP_CAL 0x0B
+#define GET_CAL_DATA 0x0C
+#define CAL_DATA_RESP 0x0D
+#define SET_CAL_DATA 0x0E
+
+#define DATA_XRAW 0x01 // Slnt32 counts 32768 to 32767
+#define DATA_YRAW 0x02 // Slnt32 counts 32768 to 32767
+#define DATA_XCAL 0x03 // Float32 scaled to 1.0
+#define DATA_YCAL 0x04 // Float32 scaled to 1.0
+#define DATA_HEADING 0x05 // Float32 degrees 0.0 to 359.9
+#define DATA_MAGNITUDE 0x06 // Float32 scalebreak;
+#define DATA_TEMPERATURE 0x07 // Float32 Celsius
+#define DATA_DISTORTION 0x08 // Boolean
+#define DATA_CAL_STATUS 0x09 // Boolean
+
+
+extern void ant_v2x_periodic_initialise(void);
+extern void ant_v2x_send_req ( const uint8_t* req, uint8_t len);
+extern void ant_v2x_send_get_data ( void );
+
+
+/***************************Initialisation ant_v2x*******************************/
+
+void ant_v2x_init( void )
+{
+ SPI_master_init();
+ /* set sync as ouptut */
+ SetBit(ANT_V2X_DDR, ANT_V2X_PIN);
+ /* pull it down */
+ SetBit(ANT_V2X_PORT, ANT_V2X_PIN);
+
+ ant_v2x_data_available = FALSE;
+ ant_v2x_com_status = MAG_CS_IDLE;
+ ant_v2x_reset();
+}
+
+
+void ant_v2x_periodic_initialise( void ) {
+ static uint8_t init_status = 0;
+ if (ant_v2x_com_status != MAG_CS_IDLE)
+ return;
+ switch (init_status) {
+ case 0 : /* set data response format */
+ {
+ const uint8_t req[] = {SET_DATA_COMPONENTS, 0x08, DATA_XRAW, DATA_YRAW, DATA_XCAL, DATA_YCAL, DATA_HEADING, DATA_MAGNITUDE, DATA_DISTORTION, DATA_CAL_STATUS };
+ ant_v2x_send_req(req, sizeof(req));
+ }
+ break;
+ case 1 : /* set little endian */
+ {
+ const uint8_t req[] = {SET_CONFIG, 0x06, 0x00};
+ ant_v2x_send_req(req, sizeof(req));
+ }
+ break;
+ case 2 :
+ { /* set period */
+ const uint8_t req[] = {SET_CONFIG, 0x05, 0x07};
+ ant_v2x_send_req(req, sizeof(req));
+ }
+ break;
+ default:
+ ant_v2x_status = MAG_S_READY;
+ }
+ init_status++;
+}
+void ant_v2x_periodic( void ) { /* Run initialisation and communication request */
+ switch (ant_v2x_status) {
+ case MAG_S_RESET:
+ SetBit(ANT_V2X_PORT, ANT_V2X_PIN);
+ ant_v2x_status = MAG_S_UNINIT;
+ break;
+ case MAG_S_UNINIT:
+ ant_v2x_periodic_initialise();
+ break;
+ case MAG_S_READY: /* Ready to receive request */
+ /*GREEN_LED_ON();*/
+ /*YELLOW_LED_OFF();*/
+ // if (ant_v2x_data_available) return
+ ant_v2x_send_get_data();
+ ant_v2x_status = MAG_S_WAIT_MEAS;
+ ant_v2x_periodic_count = 0;
+ break;
+ case MAG_S_WAIT_MEAS: { /* Waiting for measures */
+ ant_v2x_periodic_count++;
+ if (ant_v2x_periodic_count > 5) {
+ ant_v2x_com_status = MAG_CS_READING;
+ SPI_start();
+ ant_v2x_res_idx = 0;
+ ant_v2x_res_len = 43;
+ SPI_transmit(0x00);
+ }
+ }
+ break;
+ }
+}
+
+/*****************************Ant_V2x reset****************************************/
+void ant_v2x_reset (void){
+ ClearBit(ANT_V2X_PORT, ANT_V2X_PIN);
+ ant_v2x_status = MAG_S_RESET;
+ ant_v2x_com_status = MAG_CS_IDLE;
+}
+
+/**************************Ant_V2x data communication******************************/
+void ant_v2x_send_get_data ( void ) {
+ const uint8_t req[] = {GET_DATA};
+ ant_v2x_send_req(req, sizeof(req));
+}
+
+/********************************Request procedure***********************************/
+void ant_v2x_send_req(const uint8_t* req, uint8_t len) {
+ memcpy(ant_v2x_req, req, len);
+ ant_v2x_req_len = len;
+ ant_v2x_req_idx = 0;
+ ant_v2x_com_status = MAG_CS_WRITING;
+ SPI_start();
+ SPI_transmit(SYNC_FLAG);/* transmit SYNC_FLAG first for every beginning of transmition */
+}
+
+void ant_v2x_read_data( void ) {
+ ant_v2x_data.xraw = *(int32_t*)&ant_v2x_res[4];
+ ant_v2x_data.yraw = *(int32_t*)&ant_v2x_res[9];
+ ant_v2x_data.xcal = *(float*)&ant_v2x_res[14];
+ ant_v2x_data.ycal = *(float*)&ant_v2x_res[19];
+ ant_v2x_data.heading = *(float*)&ant_v2x_res[24];
+ ant_v2x_data.magnitude = *(float*)&ant_v2x_res[29];
+ ant_v2x_data.temp = *(float*)&ant_v2x_res[34];
+ ant_v2x_data.distor = *(int8_t*)&ant_v2x_res[39];
+ ant_v2x_data.cal_status = *(int8_t*)&ant_v2x_res[41];
+}
+
+
+#define SPI_SIG_ON_WRITING() { \
+ uint8_t c __attribute__ ((unused)) = SPI_read(); \
+ if (ant_v2x_req_idx < ant_v2x_req_len) { \
+ SPI_transmit(ant_v2x_req[ant_v2x_req_idx]); \
+ } \
+ else if (ant_v2x_req_idx == ant_v2x_req_len) { \
+ SPI_transmit(TERMINATOR); \
+ } \
+ else { \
+ ant_v2x_com_status = MAG_CS_IDLE; \
+ SPI_stop(); \
+ } \
+ ant_v2x_req_idx++; \
+ }
+
+
+static uint8_t nb_retry = 0;
+#define MAX_RETRY 10
+
+#define SPI_SIG_ON_READING() { \
+ ant_v2x_res[ant_v2x_res_idx] = SPI_read(); \
+ if (ant_v2x_res_idx == 0) { \
+ if (nb_retry > MAX_RETRY) { \
+ ant_v2x_reset(); \
+ nb_retry = 0; \
+ /*YELLOW_LED_ON();*/ \
+ goto sig_exit; \
+ } \
+ if (ant_v2x_res[ant_v2x_res_idx] != SYNC_FLAG) { \
+ nb_retry++; \
+ SPI_transmit(0x00); \
+ goto sig_exit; \
+ } \
+ else { \
+ nb_retry = 0; \
+ } \
+ } \
+ ant_v2x_res_idx++; \
+ if (ant_v2x_res_idx < ant_v2x_res_len) { \
+ SPI_transmit(0x00); \
+ } \
+ else { \
+ ant_v2x_com_status = MAG_CS_IDLE; \
+ SPI_stop(); \
+ ant_v2x_status = MAG_S_READY; \
+ ant_v2x_data_available = TRUE; \
+ /*GREEN_LED_OFF();*/ \
+ } \
+ } \
+
+SIGNAL(SIG_SPI) {
+ switch ( ant_v2x_com_status) {
+ case MAG_CS_WRITING:
+ SPI_SIG_ON_WRITING();
+ break;
+ case MAG_CS_READING:
+ SPI_SIG_ON_READING();
+ }
+ sig_exit:
+ /*GREEN_LED_OFF();*/
+ asm("nop");
+}
diff --git a/sw/airborne/avr/ant_v2x.h b/sw/airborne/avr/ant_v2x.h
new file mode 100644
index 0000000000..6b696f5eda
--- /dev/null
+++ b/sw/airborne/avr/ant_v2x.h
@@ -0,0 +1,51 @@
+#ifndef ANT_V2X_H
+#define ANT_V2X_H
+
+#include "std.h"
+
+extern void ant_v2x_init( void );
+extern void ant_v2x_periodic(void);
+extern bool_t ant_v2x_is_in_calibration(void);
+extern void ant_v2x_reset( void );
+extern void ant_v2x_read_data( void );
+
+struct Ant_V2xConfig
+{
+ float declination;
+ uint8_t true_north;
+ uint8_t cal_sample_freq;
+ uint8_t sample_freq;
+ uint8_t period;
+ uint8_t big_idian;
+ uint8_t damping_size;
+};
+
+struct Ant_V2xData
+{
+ int32_t xraw;
+ int32_t yraw;
+ float xcal;
+ float ycal;
+ float heading;
+ float magnitude;
+ float temp;
+ uint8_t distor;
+ uint8_t cal_status;
+};
+
+struct Ant_V2xCal
+{
+ int8_t byte_count;
+ int32_t x_offset;
+ int32_t y_offset;
+ int32_t x_gain;
+ int32_t y_gain;
+ float phi;
+ float cal_magnitude;
+};
+
+extern volatile bool_t ant_v2x_data_available;
+extern struct Ant_V2xData ant_v2x_data;
+
+#endif /* ANT_V2X_H */
+
diff --git a/sw/airborne/main_antenna.c b/sw/airborne/main_antenna.c
new file mode 100644
index 0000000000..092f0d7a5b
--- /dev/null
+++ b/sw/airborne/main_antenna.c
@@ -0,0 +1,68 @@
+
+#include "std.h"
+#include "sys_time.h"
+#include "init_hw.h"
+#include "interrupt_hw.h"
+
+#include "led.h"
+
+#include "uart.h"
+#include "print.h"
+
+#include "adc.h"
+
+#include "ant_v2x.h"
+
+#include "messages.h"
+#include "downlink.h"
+
+static inline void main_init( void );
+static inline void main_periodic_task( void );
+static inline void main_event_task( void);
+
+int main( void ) {
+ main_init();
+ LED_ON(1);
+ LED_OFF(2);
+ while (1) {
+ if (sys_time_periodic())
+ main_periodic_task();
+ main_event_task();
+ }
+ return 0;
+}
+
+static inline void main_init( void ) {
+ hw_init();
+ sys_time_init();
+ led_init();
+ uart1_init_tx();
+ adc_init();
+ int_enable();
+}
+
+static inline void main_event_task( void ) {
+ if (ant_v2x_data_available) {
+ ant_v2x_read_data();
+ DOWNLINK_SEND_ANTENNA_DEBUG(&ant_v2x_data.xraw, &ant_v2x_data.yraw, \
+ &ant_v2x_data.xcal, &ant_v2x_data.ycal, \
+ &ant_v2x_data.heading, &ant_v2x_data.magnitude, \
+ &ant_v2x_data.temp, &ant_v2x_data.distor, \
+ &ant_v2x_data.cal_status);
+ ant_v2x_data_available = FALSE;
+ }
+}
+
+static inline void main_periodic_task( void ) {
+ static uint8_t cnt;
+ cnt++;
+ if (!(cnt%16)) {
+ LED_TOGGLE(2);
+ // uart1_transmit('#');
+ // Uart1PrintHex(cnt);
+ // uart1_transmit('\n');
+ }
+}
+
+
+