moving booz from temporary google svn

2026-05-24 05:45:59 +08:00 · 2009-02-10 11:36:07 +00:00
parent 5bd613fec7
commit cd00d48253
90 changed files with 6563 additions and 320 deletions
@@ -1,6 +1,6 @@
 # asctec controllers
 ap.CFLAGS += -DACTUATORS=\"actuators_asctec_twi_blmc_hw.h\"
-ap.srcs += $(BOOZ_PRIV_ARCH)/actuators_asctec_twi_blmc_hw.c
+ap.srcs += $(SRC_BOOZ_ARCH)/actuators_asctec_twi_blmc_hw.c
 # on I2C0
 ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10
 ap.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
@@ -8,5 +8,5 @@ ap.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c

 # asctec controllers
 sim.CFLAGS += -DACTUATORS=\"actuators_asctec_twi_blmc_hw.h\"
-sim.srcs += $(BOOZ_PRIV_ARCH)/actuators_asctec_twi_blmc_hw.c actuators.c
+sim.srcs += $(SRC_BOOZ_ARCH)/actuators_asctec_twi_blmc_hw.c actuators.c

@@ -18,7 +18,7 @@

 #
 ap.CFLAGS += -DACTUATORS=\"actuators_buss_twi_blmc_hw.h\" -DUSE_BUSS_TWI_BLMC
-ap.srcs += $(BOOZ_PRIV_ARCH)/actuators_buss_twi_blmc_hw.c
+ap.srcs += $(SRC_BOOZ_ARCH)/actuators_buss_twi_blmc_hw.c
 # on I2C0
 ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10
 ap.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
@@ -26,6 +26,6 @@ ap.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c

 # Simulator
 sim.CFLAGS += -DACTUATORS=\"actuators_buss_twi_blmc_hw.h\" -DUSE_BUSS_TWI_BLMC
-sim.srcs += $(BOOZ_PRIV_SIM)/actuators_buss_twi_blmc_hw.c
+sim.srcs += $(SRC_BOOZ_SIM)/actuators_buss_twi_blmc_hw.c
 sim.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10
 sim.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
@@ -27,13 +27,9 @@ ARCHI=arm7
 FLASH_MODE = IAP
 BOARD_CFG = \"booz2_board_v1_0.h\"

-BOOZ=booz
-BOOZ_PRIV=booz_priv
-BOOZ_PRIV_ARCH=booz_priv/arm7
-BOOZ_PRIV_TEST=booz_priv/test
-BOOZ_ARCH=booz/arm7
-
-BOOZ_CFLAGS = -I$(BOOZ_PRIV) -I$(BOOZ_PRIV_ARCH)
+SRC_BOOZ=booz
+SRC_BOOZ_ARCH=booz/arm7
+SRC_BOOZ_TEST=booz/test

 ap.ARCHDIR = $(ARCHI)
 ap.ARCH = arm7tdmi
@@ -41,8 +37,9 @@ ap.TARGET = ap
 ap.TARGETDIR = ap


-ap.CFLAGS += -DCONFIG=$(BOARD_CFG) $(BOOZ_CFLAGS)
-ap.srcs += $(BOOZ_PRIV)/booz2_main.c
+ap.CFLAGS += -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH)
+ap.CFLAGS += -DCONFIG=$(BOARD_CFG)
+ap.srcs += $(SRC_BOOZ)/booz2_main.c
 ap.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
 # -DTIME_LED=1
 ap.CFLAGS += -DLED
@@ -52,88 +49,81 @@ ap.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 -DUART1_VIC_SLOT=6
 ap.srcs += $(SRC_ARCH)/uart_hw.c

 ap.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1 
-ap.srcs += $(BOOZ_PRIV)/booz2_telemetry.c \
+ap.srcs += $(SRC_BOOZ)/booz2_telemetry.c \
 	   downlink.c \
           pprz_transport.c

 ap.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart1
-ap.srcs += $(BOOZ_PRIV)/booz2_datalink.c
+ap.srcs += $(SRC_BOOZ)/booz2_datalink.c

-ap.srcs += $(BOOZ_PRIV)/booz2_commands.c
+ap.srcs += $(SRC_BOOZ)/booz2_commands.c

 ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA -DRC_LED=1
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c

-#ap.CFLAGS += -DACTUATORS=\"actuators_buss_twi_blmc_hw.h\" -DUSE_BUSS_TWI_BLMC
-#ap.srcs += $(BOOZ_PRIV_ARCH)/actuators_buss_twi_blmc_hw.c actuators.c
-#ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10
-#ap.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
+#
+# Actuator choice
+#
+# include booz2_actuators_buss.makefile
+# or
+# include booz2_actuators_asctec.makefile
+#

+#
+# IMU choice
+#
+# include booz2_imu_b2v1.makefile
+# or
+# include booz2_imu_crista.makefile
+#

-#ap.CFLAGS += -DBOOZ2_IMU_TYPE=IMU_V3
-#ap.srcs += $(BOOZ_PRIV)/booz2_imu_v3.c $(BOOZ_PRIV_ARCH)/booz2_imu_v3_hw.c
-
-#ap.CFLAGS += -DBOOZ2_IMU_TYPE=IMU_CRISTA
-#ap.srcs += $(BOOZ_PRIV)/booz2_imu_crista.c $(BOOZ_PRIV_ARCH)/booz2_imu_crista_hw.c
-
-#ap.CFLAGS += -DBOOZ2_IMU_TYPE=IMU_B2
-#ap.CFLAGS += -DSSP_VIC_SLOT=9
-#ap.srcs += $(BOOZ_PRIV)/booz2_imu_b2.c $(BOOZ_PRIV_ARCH)/booz2_imu_b2_hw.c
-#ap.CFLAGS += -DMAX1168_EOC_VIC_SLOT=8
-#ap.srcs += $(BOOZ_PRIV)/booz2_max1168.c $(BOOZ_PRIV_ARCH)/booz2_max1168_hw.c
-#ap.CFLAGS += -DUSE_I2C1  -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 -DI2C1_BUF_LEN=16
-##ap.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-#ap.CFLAGS += -DUSE_AMI601
-#ap.srcs += AMI601.c
-#ap.CFLAGS += -DFLOAT_T=float
-#ap.srcs += $(BOOZ_PRIV)/booz2_imu.c


 ap.CFLAGS += -DBOOZ2_ANALOG_BARO_LED=2 -DBOOZ2_ANALOG_BARO_PERIOD='SYS_TICS_OF_SEC((1./100.))'
-ap.srcs += $(BOOZ_PRIV)/booz2_analog_baro.c
+ap.srcs += $(SRC_BOOZ)/booz2_analog_baro.c

 ap.CFLAGS += -DBOOZ2_ANALOG_BATTERY_PERIOD='SYS_TICS_OF_SEC((1./10.))'
-ap.srcs += $(BOOZ_PRIV)/booz2_battery.c
+ap.srcs += $(SRC_BOOZ)/booz2_battery.c

 ap.CFLAGS += -DADC0_VIC_SLOT=2
 ap.CFLAGS += -DADC1_VIC_SLOT=3
-ap.srcs += $(BOOZ_PRIV)/booz2_analog.c $(BOOZ_PRIV_ARCH)/booz2_analog_hw.c
+ap.srcs += $(SRC_BOOZ)/booz2_analog.c $(SRC_BOOZ_ARCH)/booz2_analog_hw.c

-ap.srcs += $(BOOZ_PRIV)/booz2_gps.c
+ap.srcs += $(SRC_BOOZ)/booz2_gps.c
 ap.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400 -DUART0_VIC_SLOT=5
 ap.CFLAGS += -DGPS_LINK=Uart0 -DGPS_LED=4



-ap.srcs += $(BOOZ_PRIV)/booz2_autopilot.c
+ap.srcs += $(SRC_BOOZ)/booz2_autopilot.c

 ap.CFLAGS += -DFILTER_ALIGNER_LED=3
-ap.srcs += $(BOOZ_PRIV)/booz2_filter_aligner2.c
-ap.srcs += $(BOOZ_PRIV)/booz2_filter_attitude_cmpl_euler.c
-ap.srcs += $(BOOZ_PRIV)/booz_trig_int.c
-ap.srcs += $(BOOZ_PRIV)/booz2_stabilization.c
-ap.srcs += $(BOOZ_PRIV)/booz2_stabilization_rate.c
-ap.srcs += $(BOOZ_PRIV)/booz2_stabilization_attitude.c
+ap.srcs += $(SRC_BOOZ)/booz2_filter_aligner2.c
+ap.srcs += $(SRC_BOOZ)/booz2_filter_attitude_cmpl_euler.c
+ap.srcs += $(SRC_BOOZ)/booz_trig_int.c
+ap.srcs += $(SRC_BOOZ)/booz2_stabilization.c
+ap.srcs += $(SRC_BOOZ)/booz2_stabilization_rate.c
+ap.srcs += $(SRC_BOOZ)/booz2_stabilization_attitude.c

-ap.srcs += $(BOOZ_PRIV)/booz2_guidance_h.c
-ap.srcs += $(BOOZ_PRIV)/booz2_guidance_v.c
-ap.srcs += $(BOOZ_PRIV)/booz2_ins.c
+ap.srcs += $(SRC_BOOZ)/booz2_guidance_h.c
+ap.srcs += $(SRC_BOOZ)/booz2_guidance_v.c
+ap.srcs += $(SRC_BOOZ)/booz2_ins.c
 # vertical filter dummy complementary
 #ap.CFLAGS += -DUSE_VFD
 #  vertical filter float version
-ap.srcs += $(BOOZ_PRIV)/booz2_vf_float.c
+ap.srcs += $(SRC_BOOZ)/booz2_vf_float.c
 ap.CFLAGS += -DUSE_VFF -DDT_VFILTER="(1./512.)" -DFLOAT_T=float

-ap.srcs += $(BOOZ_PRIV)/booz2_navigation.c
+ap.srcs += $(SRC_BOOZ)/booz2_navigation.c



 ap.CFLAGS += -DHS_YAW

-ap.srcs += $(BOOZ_PRIV)/booz2_fms.c
+ap.srcs += $(SRC_BOOZ)/booz2_fms.c

 #ap.CFLAGS += -DBOOZ2_FMS_TYPE=BOOZ2_FMS_TYPE_DATALINK -DFMS_DATALINK_USE_COMMANDS_RAW
-#ap.srcs += $(BOOZ_PRIV)/booz2_fms_datalink.c
+#ap.srcs += $(SRC_BOOZ)/booz2_fms_datalink.c

 ap.CFLAGS += -DBOOZ2_FMS_TYPE=BOOZ2_FMS_TYPE_TEST_SIGNAL
-ap.srcs += $(BOOZ_PRIV)/booz2_fms_test_signal.c
+ap.srcs += $(SRC_BOOZ)/booz2_fms_test_signal.c
@@ -49,22 +49,24 @@
 # imu Booz2 v1
 ap.CFLAGS += -DBOOZ2_IMU_TYPE=\"booz2_imu_b2.h\"
 ap.CFLAGS += -DSSP_VIC_SLOT=9
-ap.srcs += $(BOOZ_PRIV)/booz2_imu_b2.c $(BOOZ_PRIV_ARCH)/booz2_imu_b2_hw.c
+ap.srcs += $(SRC_BOOZ)/booz2_imu_b2.c $(SRC_BOOZ_ARCH)/booz2_imu_b2_hw.c
 ap.CFLAGS += -DMAX1168_EOC_VIC_SLOT=8
-ap.srcs += $(BOOZ_PRIV)/booz2_max1168.c $(BOOZ_PRIV_ARCH)/booz2_max1168_hw.c
+ap.srcs += $(SRC_BOOZ)/booz2_max1168.c $(SRC_BOOZ_ARCH)/booz2_max1168_hw.c
 ap.CFLAGS += -DUSE_I2C1  -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 -DI2C1_BUF_LEN=16
 #ap.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
 ap.CFLAGS += -DUSE_AMI601
 ap.srcs += AMI601.c
 ap.CFLAGS += -DFLOAT_T=float
-ap.srcs += $(BOOZ_PRIV)/booz2_imu.c
+ap.srcs += $(SRC_BOOZ)/booz2_imu.c
+
+

 sim.CFLAGS += -DBOOZ2_IMU_TYPE=\"booz2_imu_b2.h\"
-sim.srcs += $(BOOZ_PRIV)/booz2_imu.c \
-	    $(BOOZ_PRIV)/booz2_imu_b2.c \
-            $(BOOZ_PRIV_SIM)/booz2_imu_b2_hw.c \
-            $(BOOZ_PRIV)/booz2_max1168.c \
-            $(BOOZ_PRIV_SIM)/booz2_max1168_sim.c
+sim.srcs += $(SRC_BOOZ)/booz2_imu.c \
+	    $(SRC_BOOZ)/booz2_imu_b2.c \
+            $(SRC_BOOZ_SIM)/booz2_imu_b2_hw.c \
+            $(SRC_BOOZ)/booz2_max1168.c \
+            $(SRC_BOOZ_SIM)/booz2_max1168_sim.c

 sim.CFLAGS += -DUSE_I2C1
 #  -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 -DI2C1_BUF_LEN=16
@@ -1,8 +1,11 @@

+
+
+
 ap.CFLAGS += -DBOOZ2_IMU_TYPE=\"booz2_imu_crista.h\"
-ap.srcs += $(BOOZ_PRIV)/booz2_imu_crista.c $(BOOZ_PRIV_ARCH)/booz2_imu_crista_hw.c
+ap.srcs += $(SRC_BOOZ)/booz2_imu_crista.c $(SRC_BOOZ_ARCH)/booz2_imu_crista_hw.c
 ap.CFLAGS += -DUSE_I2C1  -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 -DI2C1_BUF_LEN=16
 ap.CFLAGS += -DUSE_AMI601
 ap.srcs += AMI601.c
 ap.CFLAGS += -DFLOAT_T=float
-ap.srcs += $(BOOZ_PRIV)/booz2_imu.c
+ap.srcs += $(SRC_BOOZ)/booz2_imu.c
@@ -47,6 +47,9 @@
      <dl_setting var="booz2_guidance_h_igain" min="-400" step="1" max="0" module="booz2_guidance_h" shortname="ki"/>
   </dl_settings>

+   <dl_settings NAME="NAV">
+   </dl_settings>
+
    <dl_settings NAME="Filter">
       <dl_setting var="booz2_face_reinj_1" min="512" step="512" max="262144" module="booz2_filter_attitude_cmpl_euler" shortname="reinj_1"/>
    </dl_settings>
@@ -1,118 +0,0 @@
-#include "std.h"
-#include "init_hw.h"
-#include "interrupt_hw.h"
-#include "sys_time.h"
-
-#include "uart.h"
-#include "messages.h"
-#include "downlink.h"
-
-#include "LPC21xx.h"
-#include "6dof.h"
-#include "max1167.h"
-#include "micromag.h"
-#include "scp1000.h"
-
-#include "spi_hw.h"
-
-
-static inline void main_init(void);
-static inline void main_periodic(void);
-static inline void main_event(void);
-
-static void SPI1_ISR(void) __attribute__((naked));
-static void my_spi_init(void);
-
-uint16_t gyro_raw[AXIS_NB];
-
-uint32_t t0, t1, diff;
-
-int main( void ) {
-  main_init();
-  while (1) {
-    if (sys_time_periodic())
-      main_periodic();
-    main_event();
-  }
-  return 0;
-}
-
-
-static inline void main_init(void) {
-  hw_init();
-  sys_time_init();
-  uart1_init_tx();
-
-  my_spi_init();
-  max1167_init();
-  micromag_init(); 
-  scp1000_init();
-
-  
-
-  int_enable();
-}
-
-
-static inline void main_periodic(void) {
-  //  DOWNLINK_SEND_BOOT(&cpu_time_sec );
-  t0 = T0TC;
-  max1167_read();
-}
-
-
-static inline void main_event(void) {
-   if (max1167_status == STA_MAX1167_DATA_AVAILABLE) {
-     max1167_status = STA_MAX1167_IDLE;
-     gyro_raw[AXIS_P] = max1167_values[0];
-     gyro_raw[AXIS_Q] = max1167_values[1];
-     gyro_raw[AXIS_R] = max1167_values[2];
-     DOWNLINK_SEND_IMU_GYRO_RAW(&gyro_raw[AXIS_P], &gyro_raw[AXIS_Q], &gyro_raw[AXIS_R]);
-     t1 = T0TC;
-     diff = t1 - t0;
-
-     if (gyro_raw[AXIS_P] < 30000) DOWNLINK_SEND_BOOT(&cpu_time_sec );
-
-     //     DOWNLINK_SEND_TIME(&diff);
-   }
-}
-
-
-/* SSPCR0 settings */
-#define SSP_DDS  0x07 << 0  /* data size         : 8 bits        */
-#define SSP_FRF  0x00 << 4  /* frame format      : SPI           */
-#define SSP_CPOL 0x00 << 6  /* clock polarity    : data captured on first clock transition */  
-#define SSP_CPHA 0x00 << 7  /* clock phase       : SCK idles low */
-#define SSP_SCR  0x0F << 8  /* serial clock rate : divide by 16  */
-
-/* SSPCR1 settings */
-#define SSP_LBM  0x00 << 0  /* loopback mode     : disabled                  */
-#define SSP_SSE  0x00 << 1  /* SSP enable        : disabled                  */
-#define SSP_MS   0x00 << 2  /* master slave mode : master                    */
-#define SSP_SOD  0x00 << 3  /* slave output disable : don't care when master */
-
-
-static void my_spi_init(void) {
-
-  /* setup pins for SSP (SCK, MISO, MOSI) */
-  PINSEL1 |= 2 << 2 | 2 << 4 | 2 << 6;
-  
-  /* setup SSP */
-  SSPCR0 = SSP_DDS | SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR;
-  SSPCR1 = SSP_LBM | SSP_MS | SSP_SOD;
-  SSPCPSR = 0x2;
-  
-  /* initialize interrupt vector */
-  VICIntSelect &= ~VIC_BIT(VIC_SPI1);   // SPI1 selected as IRQ
-  VICIntEnable = VIC_BIT(VIC_SPI1);     // SPI1 interrupt enabled
-  VICVectCntl7 = VIC_ENABLE | VIC_SPI1;
-  VICVectAddr7 = (uint32_t)SPI1_ISR;    // address of the ISR 
-
-}
-
-static void SPI1_ISR(void) {
- ISR_ENTRY();
- Max1167OnSpiInt();
- VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
- ISR_EXIT();
-}
@@ -1,136 +0,0 @@
-#include "std.h"
-#include "init_hw.h"
-#include "interrupt_hw.h"
-#include "sys_time.h"
-
-#include "uart.h"
-#include "messages.h"
-#include "downlink.h"
-
-#include "LPC21xx.h"
-#include "spi_hw.h"
-#include "scp1000.h"
-
-static inline void main_init(void);
-static inline void main_periodic(void);
-static inline void main_event(void);
-
-static void SPI1_ISR(void) __attribute__((naked));
-static void my_spi_init(void);
-
-uint32_t t0, t1, diff;
-
-float pressure;
-float ground_pressure;
-float altitude;
-
-int main( void ) {
-  main_init();
-  while (1) {
-    if (sys_time_periodic())
-      main_periodic();
-    main_event();
-  }
-  return 0;
-}
-
-
-static inline void main_init(void) {
-  hw_init();
-  sys_time_init();
-  uart1_init_tx();
-
-  my_spi_init();
-  scp1000_init();
-
-  int_enable();
-}
-
-
-static inline void main_periodic(void) {
-  if (scp1000_status == SCP1000_STA_STOPPED)
-    Scp1000SendConfig();
-}
-
-
-static inline void main_event(void) {
-
-  if (scp1000_status == SCP1000_STA_GOT_EOC) {
-    Scp1000Read();
-  }
-
-  if (scp1000_status == SCP1000_STA_DATA_AVAILABLE) {
-    scp1000_status = SCP1000_STA_WAIT_EOC;
-    pressure = (float)scp1000_pressure*0.25;
-    if (ground_pressure == 0) ground_pressure = pressure;
-    altitude = 0.084 * (pressure - ground_pressure);
-    t1 = T0TC;
-    diff = t1 - t0;
-    DOWNLINK_SEND_TIME(&diff);
-    DOWNLINK_SEND_TL_IMU_PRESSURE(&pressure);
-    DOWNLINK_SEND_TL_IMU_RANGEMETER(&altitude);
-    t0 = t1;
-  }
-
-}
-
-
-/* set SSP input clock, PCLK / CPSDVSR = 7.5MHz */
-
-#if (PCLK == 15000000)
-#define CPSDVSR    2
-#else
-
-#if (PCLK == 30000000)
-#define CPSDVSR    4
-#else
-
-#if (PCLK == 60000000)
-#define CPSDVSR    8
-#else
-
-#error unknown PCLK frequency
-#endif
-#endif
-#endif
-
-/* SSPCR0 settings */
-#define SSP_DDS  0x07 << 0  /* data size         : 8 bits        */
-#define SSP_FRF  0x00 << 4  /* frame format      : SPI           */
-#define SSP_CPOL 0x00 << 6  /* clock polarity    : data captured on first clock transition */  
-#define SSP_CPHA 0x00 << 7  /* clock phase       : SCK idles low */
-#define SSP_SCR  0x0F << 8  /* serial clock rate : divide by 16  */
-
-/* SSPCR1 settings */
-#define SSP_LBM  0x00 << 0  /* loopback mode     : disabled                  */
-#define SSP_SSE  0x00 << 1  /* SSP enable        : disabled                  */
-#define SSP_MS   0x00 << 2  /* master slave mode : master                    */
-#define SSP_SOD  0x00 << 3  /* slave output disable : don't care when master */
-
-
-static void my_spi_init(void) {
-
-  /* setup pins for SSP (SCK, MISO, MOSI) */
-  PINSEL1 |= 2 << 2 | 2 << 4 | 2 << 6;
-  
-  /* setup SSP */
-  SSPCR0 = SSP_DDS | SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR;
-  SSPCR1 = SSP_LBM | SSP_MS | SSP_SOD;
-  SSPCPSR = CPSDVSR;
-  
-  /* initialize interrupt vector */
-  VICIntSelect &= ~VIC_BIT(VIC_SPI1);   // SPI1 selected as IRQ
-  VICIntEnable = VIC_BIT(VIC_SPI1);     // SPI1 interrupt enabled
-  VICVectCntl7 = VIC_ENABLE | VIC_SPI1;
-  VICVectAddr7 = (uint32_t)SPI1_ISR;    // address of the ISR 
-
-}
-
-static void SPI1_ISR(void) {
- ISR_ENTRY();
-
- Scp1000OnSpiIt();
-
- VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
- ISR_EXIT();
-}
@@ -0,0 +1,78 @@
+#include "actuators.h"
+
+#include "i2c.h"
+#include "led.h"
+
+bool_t  actuators_asctec_twi_blmc_command;
+uint8_t actuators_asctec_twi_blmc_command_type;
+
+#define MB_TWI_CONTROLLER_ASCTECH_ADDR_FRONT 0
+#define MB_TWI_CONTROLLER_ASCTECH_ADDR_BACK  1
+#define MB_TWI_CONTROLLER_ASCTECH_ADDR_LEFT  2
+#define MB_TWI_CONTROLLER_ASCTECH_ADDR_RIGHT 3
+uint8_t actuators_asctec_twi_blmc_addr;
+uint8_t actuators_asctec_twi_blmc_new_addr;
+
+#define ASCTEC_TWI_BLMC_NB 4
+
+int8_t asctec_twi_blmc_motor_power[ASCTEC_TWI_BLMC_NB];
+uint8_t twi_blmc_nb_err;
+uint8_t mb_twi_i2c_done;
+
+
+#define MB_TWI_CONTROLLER_MAX_CMD 200
+#define MB_TWI_CONTROLLER_ADDR 0x02
+
+void actuators_init(void) {
+  twi_blmc_nb_err = 0;
+  mb_twi_i2c_done = TRUE;
+  actuators_asctec_twi_blmc_command = MB_TWI_CONTROLLER_COMMAND_NONE;
+  actuators_asctec_twi_blmc_addr = MB_TWI_CONTROLLER_ASCTECH_ADDR_FRONT;
+}
+
+void asctec_twi_controller_send() {
+  if (mb_twi_i2c_done) {
+    if (actuators_asctec_twi_blmc_command != MB_TWI_CONTROLLER_COMMAND_NONE) {
+
+      switch (actuators_asctec_twi_blmc_command) {
+
+      case MB_TWI_CONTROLLER_COMMAND_TEST :
+	i2c_buf[0] = 251;
+	i2c_buf[1] = actuators_asctec_twi_blmc_addr;
+	i2c_buf[2] = 0;
+	i2c_buf[3] = 231 + actuators_asctec_twi_blmc_addr;
+	i2c_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
+      break;
+      
+      case MB_TWI_CONTROLLER_COMMAND_REVERSE :
+	i2c_buf[0] = 254;
+	i2c_buf[1] = actuators_asctec_twi_blmc_addr;
+	i2c_buf[2] = 0;
+	i2c_buf[3] = 234 + actuators_asctec_twi_blmc_addr;
+	i2c_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
+	break;
+   
+    case MB_TWI_CONTROLLER_COMMAND_SET_ADDR :
+	i2c_buf[0] = 250;
+	i2c_buf[1] = actuators_asctec_twi_blmc_addr;
+	i2c_buf[2] = actuators_asctec_twi_blmc_new_addr;
+	i2c_buf[3] = 230 + actuators_asctec_twi_blmc_addr + 
+	             actuators_asctec_twi_blmc_new_addr;
+	actuators_asctec_twi_blmc_addr = actuators_asctec_twi_blmc_new_addr;
+	i2c_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
+	break;
+
+      }
+      actuators_asctec_twi_blmc_command = MB_TWI_CONTROLLER_COMMAND_NONE;
+    }
+    else {
+      i2c_buf[0] = 100 + asctec_twi_blmc_motor_power[SERVO_PITCH];
+      i2c_buf[1] = 100 + asctec_twi_blmc_motor_power[SERVO_ROLL];
+      i2c_buf[2] = 100 + asctec_twi_blmc_motor_power[SERVO_YAW];
+      i2c_buf[3] = asctec_twi_blmc_motor_power[SERVO_THRUST];
+      i2c_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
+    }
+  }
+  else
+    twi_blmc_nb_err++;
+}
@@ -0,0 +1,64 @@
+#ifndef ACTUATORS_ASCTEC_TWI_BLMC_H
+#define ACTUATORS_ASCTEC_TWI_BLMC_H
+
+#include "std.h"
+#include "led.h"
+
+#include "airframe.h"
+
+extern void asctec_twi_controller_send(void);
+
+extern uint8_t twi_blmc_nb_err;
+extern int8_t asctec_twi_blmc_motor_power[];
+
+#define MB_TWI_CONTROLLER_COMMAND_NONE     0
+#define MB_TWI_CONTROLLER_COMMAND_TEST     1
+#define MB_TWI_CONTROLLER_COMMAND_REVERSE  2
+#define MB_TWI_CONTROLLER_COMMAND_SET_ADDR 3
+
+
+extern uint8_t actuators_asctec_twi_blmc_command;
+extern uint8_t actuators_asctec_twi_blmc_addr;
+extern uint8_t actuators_asctec_twi_blmc_new_addr;
+
+#define actuators_asctec_twi_blmc_hw_SetCommand(value) { \
+    actuators_asctec_twi_blmc_command = value;		 \
+  }
+
+#define actuators_asctec_twi_blmc_hw_SetAddr(value) {			\
+    actuators_asctec_twi_blmc_command = MB_TWI_CONTROLLER_COMMAND_SET_ADDR; \
+    actuators_asctec_twi_blmc_new_addr = value;				\
+  }
+
+#ifndef SetActuatorsFromCommands
+#ifdef KILL_MOTORS
+#define SetActuatorsFromCommands(_motors_on) {		      \
+    Actuator(SERVO_PITCH)  = 0;				      \
+    Actuator(SERVO_ROLL)   = 0;				      \
+    Actuator(SERVO_YAW)    = 0;				      \
+    Actuator(SERVO_THRUST) = 0;				      \
+    ActuatorsCommit();					      \
+  }
+#else
+#define SetActuatorsFromCommands(_motors_on) {				\
+    Bound(booz2_commands[COMMAND_PITCH],-100, 100);			\
+    Bound(booz2_commands[COMMAND_ROLL], -100, 100);			\
+    Bound(booz2_commands[COMMAND_YAW],  -100, 100);			\
+    if (_motors_on) {							\
+      Bound(booz2_commands[COMMAND_THRUST],  1, 200);			\
+    }									\
+    Actuator(SERVO_PITCH)  = -(uint8_t)booz2_commands[COMMAND_PITCH];	\
+    Actuator(SERVO_ROLL)   =  (uint8_t)booz2_commands[COMMAND_ROLL];	\
+    Actuator(SERVO_YAW)    = -(uint8_t)booz2_commands[COMMAND_YAW];	\
+    Actuator(SERVO_THRUST) =  (uint8_t)booz2_commands[COMMAND_THRUST];	\
+    ActuatorsCommit();							\
+  }
+#endif /* KILL_MOTORS              */
+#endif /* SetActuatorsFromCommands */
+
+#define Actuator(i) asctec_twi_blmc_motor_power[i]
+#define ActuatorsCommit() {			                \
+    asctec_twi_controller_send();				\
+  }
+
+#endif /* ACTUATORS_ASCTEC_TWI_BLMC_H */
@@ -0,0 +1,21 @@
+#include "actuators.h"
+
+#include CONFIG
+
+uint8_t twi_blmc_nb_err;
+
+uint8_t buss_twi_blmc_motor_power[BUSS_TWI_BLMC_NB];
+volatile bool_t  buss_twi_blmc_status;
+volatile bool_t  buss_twi_blmc_i2c_done;
+volatile uint8_t buss_twi_blmc_idx;
+
+const uint8_t buss_twi_blmc_addr[BUSS_TWI_BLMC_NB] = BUSS_BLMC_ADDR;
+
+void actuators_init ( void ) {
+  uint8_t i;
+  for (i=0; i<BUSS_TWI_BLMC_NB;i++)
+    buss_twi_blmc_motor_power[i] = 0;
+  buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_IDLE;
+  twi_blmc_nb_err = 0;
+  buss_twi_blmc_i2c_done = TRUE;
+}
@@ -0,0 +1,79 @@
+#ifndef ACTUATORS_BUSS_TWI_BLMC_HW_H
+#define ACTUATORS_BUSS_TWI_BLMC_HW_H
+
+
+
+#include <string.h>
+#include "std.h"
+#include "i2c.h"
+
+#include "airframe.h"
+#include "booz2_supervision.h"
+
+/*
+  We're not using the airframe file "mixer" facility
+  and instead explicitely call the "supervision" code
+  to do the mixing
+*/
+#ifndef SetActuatorsFromCommands
+#ifdef KILL_MOTORS
+#define SetActuatorsFromCommands(_motors_on) {		      \
+    Actuator(SERVO_FRONT) = 0;				      \
+    Actuator(SERVO_BACK)  = 0;				      \
+    Actuator(SERVO_RIGHT) = 0;				      \
+    Actuator(SERVO_LEFT)  = 0;				      \
+    ActuatorsCommit();					      \
+  }
+#else
+#define SetActuatorsFromCommands(_motors_on) {				\
+    pprz_t mixed_commands[SERVOS_NB];					\
+    BOOZ2_SUPERVISION_RUN(mixed_commands, booz2_commands, _motors_on);	\
+    Actuator(SERVO_FRONT) = (uint8_t)mixed_commands[SERVO_FRONT];	\
+    Actuator(SERVO_BACK)  = (uint8_t)mixed_commands[SERVO_BACK];	\
+    Actuator(SERVO_RIGHT) = (uint8_t)mixed_commands[SERVO_RIGHT];	\
+    Actuator(SERVO_LEFT)  = (uint8_t)mixed_commands[SERVO_LEFT];	\
+    ActuatorsCommit();							\
+  }
+#endif /* KILL_MOTORS              */
+#endif /* SetActuatorsFromCommands */
+
+#define ChopServo(x,a,b) ((x)>(b)?(b):(x))
+#define Actuator(i) buss_twi_blmc_motor_power[i]
+#define ActuatorsCommit() {			                \
+    if ( buss_twi_blmc_status == BUSS_TWI_BLMC_STATUS_IDLE) {	\
+      buss_twi_blmc_idx = 0;					\
+      buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_BUSY;		\
+      ActuatorsBussTwiBlmcSend();				\
+    }								\
+    else							\
+      twi_blmc_nb_err++;					\
+  }
+
+#define SERVOS_TICS_OF_USEC(s) ((uint8_t)(s)) 
+
+#define BUSS_TWI_BLMC_STATUS_IDLE 0
+#define BUSS_TWI_BLMC_STATUS_BUSY 1
+#define BUSS_TWI_BLMC_NB 4
+extern uint8_t buss_twi_blmc_motor_power[BUSS_TWI_BLMC_NB];
+extern volatile bool_t  buss_twi_blmc_status;
+extern uint8_t twi_blmc_nb_err;
+extern volatile bool_t  buss_twi_blmc_i2c_done;
+extern volatile uint8_t buss_twi_blmc_idx;
+extern const uint8_t buss_twi_blmc_addr[];
+
+#define ActuatorsBussTwiBlmcNext() {			\
+    buss_twi_blmc_idx++;				\
+    if (buss_twi_blmc_idx < BUSS_TWI_BLMC_NB) {		\
+      ActuatorsBussTwiBlmcSend();			\
+    }							\
+    else						\
+      buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_IDLE;	\
+  }
+
+#define ActuatorsBussTwiBlmcSend() {					             \
+    i2c_buf[0] = buss_twi_blmc_motor_power[buss_twi_blmc_idx];		             \
+    i2c_transmit(buss_twi_blmc_addr[buss_twi_blmc_idx], 1, &buss_twi_blmc_i2c_done); \
+  }
+
+
+#endif /* ACTUATORS_BUSS_TWI_BLMC_HW_H */
@@ -0,0 +1,91 @@
+#include "booz2_analog.h"
+
+#include "armVIC.h"
+#include "sys_time.h"
+
+#include "booz2_analog_baro.h"
+#include "booz2_battery.h"
+
+void ADC0_ISR ( void ) __attribute__((naked));
+void ADC1_ISR ( void ) __attribute__((naked));
+
+void booz2_analog_init_hw( void ) {
+
+  /* start ADC0 */
+  /* select P0.29 as AD0.2 for bat meas*/
+  PINSEL1 |=  0x01 << 26;
+  /* sample AD0.2 - PCLK/4 ( 3.75MHz) - ON */
+  AD0CR = 1 << 2 | 0x03 << 8 | 1 << 21;
+  /* AD0 selected as IRQ */
+  VICIntSelect &= ~VIC_BIT(VIC_AD0);
+  /* AD0 interrupt enabled */
+  VICIntEnable = VIC_BIT(VIC_AD0);
+  /* AD0 interrupt as VIC2 */
+  _VIC_CNTL(ADC0_VIC_SLOT) = VIC_ENABLE | VIC_AD0;
+  _VIC_ADDR(ADC0_VIC_SLOT) = (uint32_t)ADC0_ISR;
+  /* start convertion on T0M1 match */
+  AD0CR |= 4 << 24;
+
+
+ /* clear match 1 */                                     
+  T0EMR &= ~TEMR_EM1;                                   
+  /* set high on match 1 */
+  T0EMR |= TEMR_EMC1_2;
+  /* first match in a while */
+  T0MR1 = 1024;
+
+
+  /* start ADC1 */
+  /* select P0.10 as AD1.2 for baro*/
+  ANALOG_BARO_PINSEL |=  ANALOG_BARO_PINSEL_VAL << ANALOG_BARO_PINSEL_BIT;
+  /* sample AD1.2 - PCLK/4 ( 3.75MHz) - ON */
+  AD1CR = 1 << 2 | 0x03 << 8 | 1 << 21;
+  /* AD0 selected as IRQ */
+  VICIntSelect &= ~VIC_BIT(VIC_AD1);
+  /* AD0 interrupt enabled */
+  VICIntEnable = VIC_BIT(VIC_AD1);
+  /* AD0 interrupt as VIC2 */
+  _VIC_CNTL(ADC1_VIC_SLOT) = VIC_ENABLE | VIC_AD1;
+  _VIC_ADDR(ADC1_VIC_SLOT) = (uint32_t)ADC1_ISR;
+  /* start convertion on T0M3 match */
+  AD1CR |= 5 << 24;
+
+
+  /* clear match 2 */                                     
+  T0EMR &= ~TEMR_EM3;                                   
+  /* set high on match 2 */
+  T0EMR |= TEMR_EMC3_2;
+  /* first match in a while */
+  T0MR3 = 512;
+
+  /* turn on DAC pins */
+  PINSEL1 |= 2 << 18;
+}
+
+
+void ADC0_ISR ( void ) {
+  ISR_ENTRY();
+  uint32_t tmp = AD0GDR;
+  uint16_t tmp2 = (uint16_t)(tmp >> 6) & 0x03FF;
+  Booz2BatteryISRHandler(tmp2);
+  /* trigger next convertion */
+  T0MR1 += BOOZ2_ANALOG_BATTERY_PERIOD;
+  /* lower clock         */
+  T0EMR &= ~TEMR_EM1;   
+  VICVectAddr = 0x00000000;                 // clear this interrupt from the VIC
+  ISR_EXIT();                               // recover registers and return
+}
+
+void ADC1_ISR ( void ) {
+  ISR_ENTRY();
+  uint32_t tmp = AD1GDR;
+  uint16_t tmp2 = (uint16_t)(tmp >> 6) & 0x03FF;
+  Booz2BaroISRHandler(tmp2);
+  /* trigger next convertion */
+  T0MR3 += BOOZ2_ANALOG_BARO_PERIOD;
+  /* lower clock         */
+  T0EMR &= ~TEMR_EM3;   
+  VICVectAddr = 0x00000000;                 // clear this interrupt from the VIC
+  ISR_EXIT();                               // recover registers and return
+}
+
@@ -0,0 +1,10 @@
+#ifndef BOOZ2_ANALOG_HW_H
+#define BOOZ2_ANALOG_HW_H
+
+#include "LPC21xx.h"
+
+#define Booz2AnalogSetDAC(x) {  DACR = x << 6; }
+
+extern void booz2_analog_init_hw(void);
+
+#endif /* BOOZ2_ANALOG_HW_H */
@@ -0,0 +1,53 @@
+#include "booz2_imu_b2.h"
+
+
+static void SSP_ISR(void) __attribute__((naked));
+
+
+/* SSPCR0 settings */
+#define SSP_DDS  0x0F << 0  /* data size         : 16 bits        */
+#define SSP_FRF  0x00 << 4  /* frame format      : SPI           */
+#define SSP_CPOL 0x00 << 6  /* clock polarity    : data captured on first clock transition */  
+#define SSP_CPHA 0x00 << 7  /* clock phase       : SCK idles low */
+#define SSP_SCR  0x0F << 8  /* serial clock rate : divide by 16  */
+
+/* SSPCR1 settings */
+#define SSP_LBM  0x00 << 0  /* loopback mode     : disabled                  */
+#define SSP_SSE  0x00 << 1  /* SSP enable        : disabled                  */
+#define SSP_MS   0x00 << 2  /* master slave mode : master                    */
+#define SSP_SOD  0x00 << 3  /* slave output disable : don't care when master */
+
+#define SSPCR0_VAL (SSP_DDS |  SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR )
+#define SSPCR1_VAL (SSP_LBM |  SSP_SSE | SSP_MS | SSP_SOD )
+
+#define SSP_PINSEL1_SCK  (2<<2)
+#define SSP_PINSEL1_MISO (2<<4)
+#define SSP_PINSEL1_MOSI (2<<6)
+
+void booz2_imu_b2_hw_init(void) {
+  
+  /* setup pins for SSP (SCK, MISO, MOSI) */
+  PINSEL1 |= SSP_PINSEL1_SCK  | SSP_PINSEL1_MISO | SSP_PINSEL1_MOSI;
+  
+  /* setup SSP */
+  SSPCR0 = SSPCR0_VAL;;
+  SSPCR1 = SSPCR1_VAL;
+  SSPCPSR = 0x02;
+  
+  /* initialize interrupt vector */
+  VICIntSelect &= ~VIC_BIT( VIC_SPI1 );  /* SPI1 selected as IRQ */
+  VICIntEnable = VIC_BIT( VIC_SPI1 );    /* enable it            */
+  _VIC_CNTL(SSP_VIC_SLOT) = VIC_ENABLE | VIC_SPI1;
+  _VIC_ADDR(SSP_VIC_SLOT) = (uint32_t)SSP_ISR;      /* address of the ISR   */
+
+}
+
+
+static void SSP_ISR(void) {
+ ISR_ENTRY();
+ 
+ Max1168OnSpiInt();
+
+ VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
+ ISR_EXIT();
+}
@@ -0,0 +1,30 @@
+#ifndef BOOZ2_IMU_B2_HW_H
+#define BOOZ2_IMU_B2_HW_H
+
+/*
+
+  MAX1168 SPI ADC connected on SPI1 
+  SS on P0.20
+  EOC on P0.16 ( EINT0 )
+
+  PNI mag on same bus
+  SS on p1.28
+  EOC P0.30 ( EINT3 )
+  RESET P1.19
+
+*/
+
+#include "std.h"
+#include "LPC21xx.h"
+#include "interrupt_hw.h"  
+#include "spi_hw.h"
+
+//#include "booz2_debug.h"
+
+
+extern void booz2_imu_b2_hw_init(void);
+
+
+
+
+#endif /* BOOZ2_IMU_B2_HW_H */
@@ -0,0 +1,107 @@
+#include "booz2_imu_crista.h"
+
+#include "LPC21xx.h"
+#include "armVIC.h"
+#include CONFIG
+#include "led.h"
+#include "spi_hw.h"
+
+#define ADS8344_SS_IODIR IO0DIR
+#define ADS8344_SS_IOSET IO0SET
+#define ADS8344_SS_IOCLR IO0CLR
+#define ADS8344_SS_PIN   20 
+
+#define ADS8344Select()   SetBit(ADS8344_SS_IOCLR,ADS8344_SS_PIN)
+#define ADS8344Unselect() SetBit(ADS8344_SS_IOSET,ADS8344_SS_PIN)
+
+bool_t ADS8344_available;
+uint16_t ADS8344_values[ADS8344_NB_CHANNELS];
+
+#define POWER_MODE (1 << 1 | 1)
+#define SGL_DIF 1 // Single ended
+
+/* SSPCR0 settings */
+#define SSP_DSS  0x07 << 0  /* data size            : 8 bits   */
+#define SSP_FRF  0x00 << 4  /* frame format         : SPI      */
+#define SSP_CPOL 0x00 << 6  /* clock polarity       : idle low */  
+#define SSP_CPHA 0x00 << 7  /* clock phase          : 1        */
+#define SSP_SCR  0x09 << 8  /* serial clock rate    : 1MHz     */
+
+/* SSPCR1 settings */
+#define SSP_LBM  0x00 << 0  /* loopback mode        : disabled */
+#define SSP_SSE  0x00 << 1  /* SSP enable           : disabled */
+#define SSP_MS   0x00 << 2  /* master slave mode    : master   */
+#define SSP_SOD  0x00 << 3  /* slave output disable : disabled */
+
+static void SPI1_ISR(void) __attribute__((naked));
+static uint8_t channel;
+
+void booz2_imu_crista_hw_init(void) {
+  channel = 0;
+
+  /* setup pins for SSP (SCK, MISO, MOSI) */
+  PINSEL1 |= 2 << 2 | 2 << 4 | 2 << 6;
+  
+  /* setup SSP */
+  SSPCR0 = SSP_DSS | SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR;
+  SSPCR1 = SSP_LBM | SSP_MS | SSP_SOD;
+  SSPCPSR = 2; /* -> 50kHz */
+  
+  /* initialize interrupt vector */
+  VICIntSelect &= ~VIC_BIT(VIC_SPI1);   // SPI1 selected as IRQ
+  VICIntEnable = VIC_BIT(VIC_SPI1);     // SPI1 interrupt enabled
+  VICVectCntl7 = VIC_ENABLE | VIC_SPI1;
+  VICVectAddr7 = (uint32_t)SPI1_ISR;    // address of the ISR 
+
+  /* setup slave select */
+  /* configure SS pin */
+  SetBit( ADS8344_SS_IODIR,  ADS8344_SS_PIN);  /* pin is output  */
+  ADS8344Unselect();                           /* pin low        */
+}
+
+
+static inline void read_values( void ) {
+  uint8_t foo __attribute__ ((unused)) = SSPDR;
+  uint8_t msb = SSPDR;
+  uint8_t lsb = SSPDR;
+  uint8_t llsb = SSPDR;
+  ADS8344_values[channel] = (msb << 8 | lsb) << 1 | llsb >> 7;
+}
+
+static inline void send_request( void ) {
+  uint8_t control = 1 << 7 | channel << 4 | SGL_DIF << 2 | POWER_MODE;
+
+  SSPDR = control;
+  SSPDR = 0;
+  SSPDR = 0;
+  SSPDR = 0;
+}
+
+void ADS8344_start( void ) {
+  LED_ON(2);
+  ADS8344Select();
+  SpiClearRti();
+  SpiEnableRti();
+  SpiEnable();
+  send_request();
+}
+
+void SPI1_ISR(void) {
+ ISR_ENTRY();
+ read_values();
+ channel++;
+ if (channel > 7-1) {
+   channel = 0;
+   ADS8344_available = TRUE;
+   LED_OFF(2); 
+   ADS8344Unselect();    
+ }
+ else {
+   send_request();
+ }
+
+ SpiClearRti();
+
+ VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
+ ISR_EXIT();
+}
@@ -0,0 +1,17 @@
+#ifndef BOOZ_IMU_INT_HW_H
+#define BOOZ_IMU_INT_HW_H
+
+#include "std.h"
+
+extern void booz2_imu_crista_hw_init(void);
+
+
+
+#define Booz2ImuCristaHwPeriodic() {		\
+    ADS8344_start();				\
+  }
+
+extern void ADS8344_start( void );
+
+#endif /* BOOZ_IMU_INT_HW_H */
+
@@ -0,0 +1,95 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008  Antoine Drouin
+ *
+ * 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. 
+ *
+ */
+
+#include "booz2_max1168.h"
+
+static void EXTINT0_ISR(void) __attribute__((naked));
+
+void booz2_max1168_hw_init( void ) {
+  
+  /* SS pin is output */
+  SetBit(MAX1168_SS_IODIR, MAX1168_SS_PIN);
+  /* unselected max1168 */
+  Max1168Unselect();
+
+  /* connect P0.16 to extint0 (EOC) */
+  MAX1168_EOC_PINSEL |= MAX1168_EOC_PINSEL_VAL << MAX1168_EOC_PINSEL_BIT;
+  /* extint0 is edge trigered */
+  SetBit(EXTMODE, MAX1168_EOC_EINT);
+  /* extint0 is trigered on falling edge */
+  ClearBit(EXTPOLAR, MAX1168_EOC_EINT);
+  /* clear pending extint0 before enabling interrupts */
+  SetBit(EXTINT, MAX1168_EOC_EINT);
+
+   /* initialize interrupt vector */
+  VICIntSelect &= ~VIC_BIT( VIC_EINT0 );                     // EXTINT0 selected as IRQ
+  VICIntEnable = VIC_BIT( VIC_EINT0 );                       // EXTINT0 interrupt enabled
+  _VIC_CNTL(MAX1168_EOC_VIC_SLOT) = VIC_ENABLE | VIC_EINT0;
+  _VIC_ADDR(MAX1168_EOC_VIC_SLOT) = (uint32_t)EXTINT0_ISR;   // address of the ISR 
+}
+
+
+void booz2_max1168_read( void ) {
+  ASSERT((max1168_status == STA_MAX1168_IDLE),		\
+	 DEBUG_MAX_1168, MAX1168_ERR_READ_OVERUN);
+  /* select max1168 */ 
+  Max1168Select();
+  /* enable SPI */
+  SpiClearRti();
+  SpiDisableRti();
+  SpiEnable();
+  /* write control byte - wait EOC on extint */
+  //  const 
+  //uint16_t control_byte = ;
+  //  control_byte = control_byte << 8;
+  SSPDR = (1 << 0 | 1 << 3 | 7 << 5) << 8;
+  booz2_max1168_status = STA_MAX1168_SENDING_REQ;
+
+}
+
+void EXTINT0_ISR(void) {
+  ISR_ENTRY();
+  ASSERT((booz2_max1168_status == STA_MAX1168_SENDING_REQ),	\
+	 DEBUG_MAX_1168, MAX1168_ERR_SPURIOUS_EOC);
+  /* read dummy control byte reply */
+  uint16_t foo __attribute__ ((unused));
+  foo = SSPDR;
+  /* trigger 8 frames read */
+  SpiSend(0);
+  SpiSend(0);
+  SpiSend(0);
+  SpiSend(0);
+  SpiSend(0);
+  SpiSend(0);
+  SpiSend(0);
+  SpiSend(0);
+  SpiClearRti();
+  SpiEnableRti();
+  booz2_max1168_status = STA_MAX1168_READING_RES;
+  
+  SetBit(EXTINT, MAX1168_EOC_EINT);   /* clear extint0 */
+  VICVectAddr = 0x00000000;           /* clear this interrupt from the VIC */
+
+  ISR_EXIT();
+}
@@ -0,0 +1,81 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008  Antoine Drouin
+ *
+ * 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. 
+ *
+ */
+
+
+#ifndef BOOZ2_MAX1168_HW_H
+#define BOOZ2_MAX1168_HW_H
+
+extern void booz2_max1168_hw_init( void );
+
+/*
+  MAX1168 SPI ADC connected on SPI1 
+  SS on P0.20
+  EOC on P0.16 ( EINT0 )
+*/
+
+#include "std.h"
+#include "LPC21xx.h"
+#include "interrupt_hw.h"  
+#include "spi_hw.h"
+
+#include "booz2_debug.h"
+
+#define MAX1168_ERR_ISR_STATUS   0
+#define MAX1168_ERR_READ_OVERUN  1
+#define MAX1168_ERR_SPURIOUS_EOC 2
+
+#define MAX1168_SS_PIN 20
+#define MAX1168_SS_IODIR IO0DIR
+#define MAX1168_SS_IOSET IO0SET
+#define MAX1168_SS_IOCLR IO0CLR
+
+#define MAX1168_EOC_PIN 16
+#define MAX1168_EOC_PINSEL PINSEL1
+#define MAX1168_EOC_PINSEL_BIT 0
+#define MAX1168_EOC_PINSEL_VAL 1
+#define MAX1168_EOC_EINT 0
+
+#define Max1168Unselect() SetBit(MAX1168_SS_IOSET, MAX1168_SS_PIN)
+#define Max1168Select() SetBit(MAX1168_SS_IOCLR, MAX1168_SS_PIN)
+
+#define Max1168OnSpiInt() {						\
+    ASSERT((max1168_status == STA_MAX1168_READING_RES),			\
+	   DEBUG_MAX_1168, MAX1168_ERR_ISR_STATUS);			\
+     /* store convertion result */					\
+    booz2_max1168_values[0] = SSPDR;					\
+    booz2_max1168_values[1] = SSPDR;					\
+    booz2_max1168_values[2] = SSPDR;					\
+    booz2_max1168_values[3] = SSPDR;					\
+    booz2_max1168_values[4] = SSPDR;					\
+    booz2_max1168_values[5] = SSPDR;					\
+    booz2_max1168_values[6] = SSPDR;					\
+    booz2_max1168_values[7] = SSPDR;					\
+    SpiClearRti();							\
+    SpiDisableRti();							\
+    SpiDisable();							\
+    Max1168Unselect();							\
+    booz2_max1168_status = STA_MAX1168_DATA_AVAILABLE;			\
+  }
+
+#endif /* BOOZ2_MAX1168_HW_H */
@@ -0,0 +1,15 @@
+#include "booz2_analog.h"
+
+// battery on AD0.3 on P0.30
+// baro    on AD0.1 on P0.28
+
+#define CHAN_BAT  3
+#define CHAN_BARO 1
+
+
+void booz2_analog_init( void ) {
+
+  booz2_analog_init_hw();
+
+}
+
@@ -0,0 +1,8 @@
+#ifndef BOOZ2_ANALOG_H
+#define BOOZ2_ANALOG_H
+
+extern void booz2_analog_init( void );
+
+#include "booz2_analog_hw.h"
+
+#endif /* BOOZ2_ANALOG_H */
@@ -0,0 +1,58 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008  Antoine Drouin
+ *
+ * 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. 
+ *
+ */
+
+#include "booz2_analog_baro.h"
+
+#include "led.h"
+
+// pressure on AD0.1 on P0.28
+// offset on DAC on P0.25
+
+uint16_t booz2_analog_baro_status;
+uint16_t booz2_analog_baro_offset;
+uint16_t booz2_analog_baro_value;
+uint16_t booz2_analog_baro_value_filtered;
+bool_t   booz2_analog_baro_data_available;
+
+
+void booz2_analog_baro_init( void ) {
+
+  booz2_analog_baro_status = BOOZ2_ANALOG_BARO_UNINIT;
+
+  booz2_analog_baro_offset = 1023;
+  Booz2AnalogSetDAC(booz2_analog_baro_offset);
+
+  booz2_analog_baro_value = 0;
+  booz2_analog_baro_value_filtered = 0;
+  booz2_analog_baro_data_available = FALSE;
+#ifdef BOOZ2_ANALOG_BARO_LED
+  LED_OFF(BOOZ2_ANALOG_BARO_LED);
+#endif
+}
+
+
+
+
+
+
@@ -0,0 +1,87 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008  Antoine Drouin
+ *
+ * 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. 
+ *
+ */
+
+#ifndef BOOZ2_ANALOG_BARO_H
+#define BOOZ2_ANALOG_BARO_H
+
+#include "std.h"
+
+#include "booz2_analog.h"
+
+extern void booz2_analog_baro_init( void );
+
+#define BOOZ2_ANALOG_BARO_UNINIT  0
+#define BOOZ2_ANALOG_BARO_RUNNING 1
+
+extern uint16_t booz2_analog_baro_status;
+
+extern uint16_t booz2_analog_baro_offset;
+extern uint16_t booz2_analog_baro_value;
+extern uint16_t booz2_analog_baro_value_filtered;
+extern bool_t   booz2_analog_baro_data_available;
+
+#define Booz2AnalogBaroEvent(_handler) {	\
+    if (booz2_analog_baro_data_available) {	\
+      _handler();				\
+      booz2_analog_baro_data_available = FALSE;	\
+    }						\
+  }
+
+#define booz2_analog_baro_SetOffset(_o) {	\
+    booz2_analog_baro_offset = _o;		\
+    Booz2AnalogSetDAC(((uint16_t)_o));	\
+  }						
+
+#define Booz2BaroISRHandler(_val) {					\
+    booz2_analog_baro_value = _val;					\
+    booz2_analog_baro_value_filtered = (3*booz2_analog_baro_value_filtered + booz2_analog_baro_value)/4; \
+    if (booz2_analog_baro_status == BOOZ2_ANALOG_BARO_UNINIT)		\
+      Booz2AnalogBaroCalibrate();					\
+    /*  else */								\
+    booz2_analog_baro_data_available = TRUE;				\
+}
+
+/* decrease offset until adc reading is over a threshold */
+#define Booz2AnalogBaroCalibrate() {					\
+    RunOnceEvery(10, {							\
+	if (booz2_analog_baro_value_filtered < 850 && booz2_analog_baro_offset >= 1) { \
+	  if (booz2_analog_baro_value_filtered == 0)			\
+	    booz2_analog_baro_offset -= 15;				\
+	  else								\
+	    booz2_analog_baro_offset--;					\
+	  Booz2AnalogSetDAC(booz2_analog_baro_offset);			\
+	  /* #ifdef BOOZ2_ANALOG_BARO_LED */				\
+	    LED_TOGGLE(BOOZ2_ANALOG_BARO_LED);				\
+	    /* #endif */						\
+	    }								\
+	else {								\
+	  booz2_analog_baro_status = BOOZ2_ANALOG_BARO_RUNNING;		\
+	  /* #ifdef BOOZ2_ANALOG_BARO_LED */				\
+	  LED_ON(BOOZ2_ANALOG_BARO_LED);				\
+	    /* #endif */						\
+	    }								\
+      });								\
+  }									\
+
+#endif /* BOOZ2_ANALOG_BARO_H */
@@ -0,0 +1,221 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008  Antoine Drouin
+ *
+ * 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. 
+ *
+ */
+
+#include "booz2_autopilot.h"
+
+#include "radio_control.h"
+#include "booz2_commands.h"
+#include "booz2_navigation.h"
+#include "booz2_guidance_h.h"
+#include "booz2_guidance_v.h"
+#include "booz2_stabilization.h"
+
+uint8_t booz2_autopilot_mode;
+uint8_t booz2_autopilot_mode_auto2;
+bool_t  booz2_autopilot_motors_on;
+bool_t  booz2_autopilot_in_flight;
+uint32_t booz2_autopilot_motors_on_counter;
+uint32_t booz2_autopilot_in_flight_counter;
+
+#define BOOZ2_AUTOPILOT_MOTOR_ON_TIME     40
+#define BOOZ2_AUTOPILOT_IN_FLIGHT_TIME    40
+#define BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD (MAX_PPRZ / 20)
+#define BOOZ2_AUTOPILOT_YAW_TRESHOLD      (MAX_PPRZ * 19 / 20)
+
+void booz2_autopilot_init(void) {
+  booz2_autopilot_mode = BOOZ2_AP_MODE_FAILSAFE;
+  booz2_autopilot_motors_on = FALSE;
+  booz2_autopilot_in_flight = FALSE;
+  booz2_autopilot_motors_on_counter = 0;
+  booz2_autopilot_in_flight_counter = 0;
+  booz2_autopilot_mode_auto2 = BOOZ2_MODE_AUTO2;
+}
+
+
+#if 0
+#include "uart.h"
+#include "downlink.h"
+#include "messages.h"
+#endif
+
+void booz2_autopilot_periodic(void) {
+  
+  if ( !booz2_autopilot_motors_on ||
+       booz2_autopilot_mode == BOOZ2_AP_MODE_FAILSAFE ||
+       booz2_autopilot_mode == BOOZ2_AP_MODE_KILL ) {
+    SetCommands(booz2_commands_failsafe, 
+		booz2_autopilot_in_flight, booz2_autopilot_motors_on);
+  }
+  else {
+    RunOnceEvery(50, nav_periodic_task_10Hz())
+    booz2_guidance_v_run( booz2_autopilot_in_flight );
+    booz2_guidance_h_run( booz2_autopilot_in_flight );
+    SetCommands(booz2_stabilization_cmd, 
+		booz2_autopilot_in_flight, booz2_autopilot_motors_on);
+  }
+
+}
+
+
+void booz2_autopilot_set_mode(uint8_t new_autopilot_mode) {
+
+  if (new_autopilot_mode != booz2_autopilot_mode) {
+    /* horizontal mode */
+    switch (new_autopilot_mode) {
+    case BOOZ2_AP_MODE_FAILSAFE:
+    case BOOZ2_AP_MODE_KILL:
+      booz2_autopilot_motors_on = FALSE;
+      booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_KILL);
+      break;
+    case BOOZ2_AP_MODE_RATE_DIRECT:
+    case BOOZ2_AP_MODE_RATE_Z_HOLD:
+      booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_RATE);
+      break;
+    case BOOZ2_AP_MODE_ATTITUDE_DIRECT:
+    case BOOZ2_AP_MODE_ATTITUDE_Z_HOLD:
+      booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_ATTITUDE);
+      break;
+    case BOOZ2_AP_MODE_HOVER_DIRECT:
+    case BOOZ2_AP_MODE_HOVER_Z_HOLD:
+      booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_HOVER);
+      break;
+    case BOOZ2_AP_MODE_NAV:
+      booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_NAV);
+      break;
+    }
+    /* vertical mode */
+    switch (new_autopilot_mode) {
+    case BOOZ2_AP_MODE_FAILSAFE:
+    case BOOZ2_AP_MODE_KILL:
+      booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_KILL);
+      break;
+    case BOOZ2_AP_MODE_RATE_DIRECT:
+    case BOOZ2_AP_MODE_ATTITUDE_DIRECT:
+    case BOOZ2_AP_MODE_HOVER_DIRECT:
+      booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_DIRECT);
+      break;
+    case BOOZ2_AP_MODE_RATE_Z_HOLD:
+    case BOOZ2_AP_MODE_ATTITUDE_Z_HOLD:
+    case BOOZ2_AP_MODE_HOVER_Z_HOLD:
+    case BOOZ2_AP_MODE_NAV:
+      booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_HOVER);
+      break;
+    }
+    booz2_autopilot_mode = new_autopilot_mode;
+  } 
+  
+}
+
+#define BOOZ2_AUTOPILOT_CHECK_IN_FLIGHT() {				\
+    if (booz2_autopilot_in_flight) {					\
+      if (booz2_autopilot_in_flight_counter > 0) {			\
+	if (rc_values[RADIO_THROTTLE] < BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD) { \
+	  booz2_autopilot_in_flight_counter--;				\
+	  if (booz2_autopilot_in_flight_counter == 0) {			\
+	    booz2_autopilot_in_flight = FALSE;				\
+	  }								\
+	}								\
+	else {	/* rc throttle > threshold */				\
+	  booz2_autopilot_in_flight_counter = BOOZ2_AUTOPILOT_IN_FLIGHT_TIME; \
+        }								\
+      }									\
+    }									\
+    else { /* not in flight */						\
+      if (booz2_autopilot_in_flight_counter < BOOZ2_AUTOPILOT_IN_FLIGHT_TIME && \
+	  booz2_autopilot_motors_on) {					\
+	if (rc_values[RADIO_THROTTLE] > BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD) { \
+	  booz2_autopilot_in_flight_counter++;				\
+	  if (booz2_autopilot_in_flight_counter == BOOZ2_AUTOPILOT_IN_FLIGHT_TIME) \
+	    booz2_autopilot_in_flight = TRUE;				\
+	}								\
+	else { /*  rc throttle < threshold */				\
+	  booz2_autopilot_in_flight_counter = 0;			\
+	}								\
+      }									\
+    }									\
+  }
+
+#define BOOZ2_AUTOPILOT_CHECK_MOTORS_ON() {				\
+    if (booz2_autopilot_motors_on) {					\
+      if (rc_values[RADIO_THROTTLE] < BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD && \
+	  (rc_values[RADIO_YAW] > BOOZ2_AUTOPILOT_YAW_TRESHOLD ||	\
+	   rc_values[RADIO_YAW] < -BOOZ2_AUTOPILOT_YAW_TRESHOLD)) {	\
+	if ( booz2_autopilot_motors_on_counter > 0) {			\
+	  booz2_autopilot_motors_on_counter--;				\
+	  if (booz2_autopilot_motors_on_counter == 0)			\
+	    booz2_autopilot_motors_on = FALSE;				\
+	}								\
+      }									\
+      else { /* sticks not in the corner */				\
+	booz2_autopilot_motors_on_counter = BOOZ2_AUTOPILOT_MOTOR_ON_TIME; \
+      }									\
+    }									\
+    else { /* motors off */						\
+      if (rc_values[RADIO_THROTTLE] < BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD && \
+	  (rc_values[RADIO_YAW] > BOOZ2_AUTOPILOT_YAW_TRESHOLD ||	\
+	   rc_values[RADIO_YAW] < -BOOZ2_AUTOPILOT_YAW_TRESHOLD)) {	\
+	if ( booz2_autopilot_motors_on_counter <  BOOZ2_AUTOPILOT_MOTOR_ON_TIME) { \
+	  booz2_autopilot_motors_on_counter++;				\
+	  if (booz2_autopilot_motors_on_counter == BOOZ2_AUTOPILOT_MOTOR_ON_TIME) \
+	    booz2_autopilot_motors_on = TRUE;				\
+	}								\
+      }									\
+      else {								\
+	booz2_autopilot_motors_on_counter = 0;				\
+      }									\
+    }									\
+  }
+
+
+
+void booz2_autopilot_on_rc_event(void) {
+
+#if 0
+      DOWNLINK_SEND_BOOZ_DEBUG(&rc_values[RADIO_THROTTLE],	\
+			       &rc_values[RADIO_ROLL],		\
+			       &rc_values[RADIO_PITCH],		\
+			       &rc_values[RADIO_YAW]);		
+#endif
+
+  /* I think this should be hidden in rc code */
+  /* the ap gets a mode everytime - the rc filters it */
+  if (rc_values_contains_avg_channels) {
+    uint8_t new_autopilot_mode = 0;
+    BOOZ_AP_MODE_OF_PPRZ(rc_values[RADIO_MODE],new_autopilot_mode);
+    booz2_autopilot_set_mode(new_autopilot_mode);
+    rc_values_contains_avg_channels = FALSE;
+  }
+
+#ifdef KILL_SWITCH
+  if (rc_values[KILL_SWITCH] < 0)
+    booz2_autopilot_set_mode(BOOZ2_AP_MODE_KILL);
+#endif
+
+  BOOZ2_AUTOPILOT_CHECK_MOTORS_ON();
+  BOOZ2_AUTOPILOT_CHECK_IN_FLIGHT();
+
+  booz2_guidance_v_read_rc();
+  booz2_guidance_h_read_rc(booz2_autopilot_in_flight);
+
+}
@@ -0,0 +1,73 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008  Antoine Drouin
+ *
+ * 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. 
+ *
+ */
+
+#ifndef BOOZ2_AUTOPILOT_H
+#define BOOZ2_AUTOPILOT_H
+
+#include "std.h"
+
+#include "booz_geometry_int.h"
+#include "airframe.h"
+
+#define BOOZ2_AP_MODE_FAILSAFE         0
+#define BOOZ2_AP_MODE_KILL             1
+#define BOOZ2_AP_MODE_RATE_DIRECT      2
+#define BOOZ2_AP_MODE_ATTITUDE_DIRECT  3
+#define BOOZ2_AP_MODE_RATE_Z_HOLD      4
+#define BOOZ2_AP_MODE_ATTITUDE_Z_HOLD  5
+#define BOOZ2_AP_MODE_HOVER_DIRECT     6
+#define BOOZ2_AP_MODE_HOVER_Z_HOLD     7
+#define BOOZ2_AP_MODE_NAV              8
+
+extern uint8_t booz2_autopilot_mode;
+extern uint8_t booz2_autopilot_mode_auto2;
+extern bool_t  booz2_autopilot_motors_on;
+extern bool_t  booz2_autopilot_in_flight;
+
+extern void booz2_autopilot_init(void);
+extern void booz2_autopilot_periodic(void);
+extern void booz2_autopilot_on_rc_event(void);
+extern void booz2_autopilot_set_mode(uint8_t new_autopilot_mode);
+
+#ifndef BOOZ2_MODE_MANUAL
+#define BOOZ2_MODE_MANUAL BOOZ2_AP_MODE_RATE_DIRECT
+#endif
+#ifndef BOOZ2_MODE_AUTO1
+#define BOOZ2_MODE_AUTO1 BOOZ2_AP_MODE_ATTITUDE_DIRECT
+#endif
+#ifndef BOOZ2_MODE_AUTO2
+#define BOOZ2_MODE_AUTO2 BOOZ2_AP_MODE_ATTITUDE_Z_HOLD
+#endif
+
+
+#define TRESHOLD_1_PPRZ (MIN_PPRZ / 2)
+#define TRESHOLD_2_PPRZ (MAX_PPRZ/2)
+
+#define BOOZ_AP_MODE_OF_PPRZ(_rc, _booz_mode) {				\
+    if      (_rc > TRESHOLD_2_PPRZ) _booz_mode = booz2_autopilot_mode_auto2; \
+    else if (_rc > TRESHOLD_1_PPRZ) _booz_mode = BOOZ2_MODE_AUTO1;	\
+    else                            _booz_mode = BOOZ2_MODE_MANUAL;	\
+  }
+
+#endif /* BOOZ2_AUTOPILOT_H */
@@ -0,0 +1,7 @@
+#include "booz2_battery.h"
+
+uint8_t booz2_battery_voltage;
+
+extern void booz2_battery_init(void) {
+  booz2_battery_voltage = 0;
+}
@@ -0,0 +1,20 @@
+#ifndef BOOZ2_BATTERY_H
+#define BOOZ2_BATTERY_H
+
+#include "std.h"
+
+#include "airframe.h"
+
+/* decivolts */
+extern uint8_t booz2_battery_voltage;
+
+#define Booz2BatteryISRHandler(_val) {					\
+    uint32_t cal_v = (uint32_t)(_val) * BATTERY_SENS_NUM / BATTERY_SENS_DEN; \
+    uint32_t sum = (uint32_t)booz2_battery_voltage + cal_v;		\
+    booz2_battery_voltage = (uint8_t)(sum/2);				\
+  }
+
+
+extern void booz2_battery_init(void);
+
+#endif /* BOOZ2_BATTERY_H */
@@ -0,0 +1,28 @@
+/*  $Id$
+ *
+ * (c) 2009 Antoine Drouin
+ *
+ * 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.  
+ *
+ */
+
+#include "booz2_commands.h"
+
+pprz_t booz2_commands[COMMANDS_NB];
+const pprz_t booz2_commands_failsafe[COMMANDS_NB] = COMMANDS_FAILSAFE;
+
@@ -0,0 +1,40 @@
+/*  $Id$
+ *
+ * (c) 2009 Antoine Drouin
+ *
+ * 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.  
+ *
+ */
+#ifndef BOOZ2_COMMANDS_H
+#define BOOZ2_COMMANDS_H
+
+#include "paparazzi.h"
+#include "airframe.h"
+
+extern pprz_t booz2_commands[COMMANDS_NB];
+extern const pprz_t booz2_commands_failsafe[COMMANDS_NB];
+
+#define SetCommands(_in_cmd, _in_flight, _motors_on) {			\
+    booz2_commands[COMMAND_PITCH]  = _in_cmd[COMMAND_PITCH];		\
+    booz2_commands[COMMAND_ROLL]   = _in_cmd[COMMAND_ROLL];		\
+    booz2_commands[COMMAND_YAW]    = (_in_flight) ? _in_cmd[COMMAND_YAW] : 0; \
+    booz2_commands[COMMAND_THRUST] = (_motors_on) ? _in_cmd[COMMAND_THRUST] : 0; \
+  }
+
+#endif /* BOOZ2_COMMANDS_H */
+
@@ -0,0 +1,60 @@
+#define DATALINK_C
+#include "datalink.h"
+
+#include "settings.h"
+#include "downlink.h"
+#include "messages.h"
+#include "dl_protocol.h"
+#include "uart.h"
+
+#ifdef BOOZ2_FMS_TYPE
+#include "booz2_fms.h"
+#endif
+
+#include "booz2_navigation.h"
+
+#define IdOfMsg(x) (x[1])
+
+void dl_parse_msg(void) {
+  uint8_t msg_id = IdOfMsg(dl_buffer);
+  switch (msg_id) {
+  
+  case  DL_PING:
+    {
+      DOWNLINK_SEND_PONG();
+    }
+    break;
+    
+  case DL_SETTING :
+    {
+      uint8_t i = DL_SETTING_index(dl_buffer);
+      float var = DL_SETTING_value(dl_buffer);
+      DlSetting(i, var);
+      DOWNLINK_SEND_DL_VALUE(&i, &var);
+    }
+    break;
+
+#if defined BOOZ2_FMS_TYPE && BOOZ2_FMS_TYPE == BOOZ2_FMS_TYPE_DATALINK
+  case DL_BOOZ2_FMS_COMMAND :
+    {
+      if (DL_BOOZ2_FMS_COMMAND_ac_id(dl_buffer) != AC_ID) break;
+      BOOZ_FMS_PARSE_DATALINK(dl_buffer);
+    }
+    break;
+#endif
+
+  case DL_MOVE_WP :
+    {
+      uint8_t ac_id = DL_MOVE_WP_ac_id(dl_buffer);
+      if (ac_id != AC_ID) break;
+      uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
+      int32_t lat = DL_MOVE_WP_lat(dl_buffer);
+      int32_t lon = DL_MOVE_WP_lon(dl_buffer);
+      int32_t alt = DL_MOVE_WP_alt(dl_buffer);
+      PPRZ_INT32_LLA_ASSIGN(waypoints[wp_id], lat, lon, alt);
+      DOWNLINK_SEND_WP_MOVED_LLA(&wp_id, &lat, &lon, &alt);
+    }
+    break;
+
+  }
+}
@@ -0,0 +1,6 @@
+#include "booz2_debug.h"
+
+#ifdef BOOZ_DEBUG
+uint8_t booz_debug_mod;
+uint8_t booz_debug_err;
+#endif
@@ -0,0 +1,32 @@
+#ifndef BOOZ2_DEBUG_H
+#define BOOZ2_DEBUG_H
+
+#ifdef BOOZ_DEBUG
+
+#include "std.h"
+#include "uart.h"
+#include "messages.h"
+#include "downlink.h"
+
+extern uint8_t booz_debug_mod;
+extern uint8_t booz_debug_err;
+
+#define DEBUG_IMU          0
+#define DEBUG_MAX_1117     1
+#define DEBUG_SCP1000      2
+#define DEBUG_LINK_MCU_IMU 3
+
+
+#define ASSERT(cond, mod, err) {					\
+    if (!(cond)) {							\
+      booz_debug_mod = mod;						\
+      booz_debug_err = err;						\
+      DOWNLINK_SEND_BOOZ_ERROR(&booz_debug_mod, &booz_debug_err);	\
+    }									\
+  }
+#else
+#define ASSERT(cond, mod, err) {}
+#endif
+
+
+#endif /* BOOZ2_DEBUG_H */
@@ -0,0 +1,24 @@
+#ifndef BOOZ2_FILTER_ALIGNER_H
+#define BOOZ2_FILTER_ALIGNER_H
+
+#include "std.h"
+#include "booz_geometry_int.h"
+
+#define BOOZ2_FILTER_ALIGNER_UNINIT  0
+#define BOOZ2_FILTER_ALIGNER_RUNNING 1
+#define BOOZ2_FILTER_ALIGNER_LOCKED  2
+
+
+extern uint8_t booz2_filter_aligner_status;
+
+extern struct booz_ivect booz2_filter_aligner_lp_gyro;
+extern struct booz_ivect booz2_filter_aligner_lp_accel;
+extern struct booz_ivect booz2_filter_aligner_lp_mag;
+extern int32_t           booz2_filter_aligner_noise;
+extern int32_t           booz2_filter_aligner_low_noise_cnt;
+
+extern void booz2_filter_aligner_init(void);
+
+extern void booz2_filter_aligner_run(void);
+
+#endif /* BOOZ2_FILTER_ALIGNER_H */
@@ -0,0 +1,87 @@
+#include "booz2_filter_aligner.h"
+
+#include "booz2_imu.h"
+#include "led.h"
+
+uint8_t booz2_filter_aligner_status;
+
+struct booz_ivect booz2_filter_aligner_lp_gyro;
+struct booz_ivect booz2_filter_aligner_lp_accel;
+struct booz_ivect booz2_filter_aligner_lp_mag;
+int32_t booz2_filter_aligner_noise;
+int32_t booz2_filter_aligner_low_noise_cnt;
+
+#define SAMPLES_NB 512
+static struct booz_ivect gyro_sum;
+static struct booz_ivect accel_sum;
+static struct booz_ivect mag_sum;
+static int32_t ref_sensor_samples[SAMPLES_NB];
+static uint32_t samples_idx;
+
+void booz2_filter_aligner_init(void) {
+
+  booz2_filter_aligner_status = BOOZ2_FILTER_ALIGNER_RUNNING;
+  BOOZ_IVECT_ZERO(gyro_sum);
+  BOOZ_IVECT_ZERO(accel_sum);
+  BOOZ_IVECT_ZERO(mag_sum);
+  samples_idx = 0;
+  booz2_filter_aligner_noise = 0;
+  booz2_filter_aligner_low_noise_cnt = 0;
+}
+
+#define LOW_NOISE_THRESHOLD 90000
+#define LOW_NOISE_TIME          5
+
+void booz2_filter_aligner_run(void) {
+
+  BOOZ_IVECT_SUM(gyro_sum,  gyro_sum,  booz2_imu_gyro);
+  BOOZ_IVECT_SUM(accel_sum, accel_sum, booz2_imu_accel);
+  BOOZ_IVECT_SUM(mag_sum,   mag_sum   ,booz2_imu_mag);
+
+  ref_sensor_samples[samples_idx] = booz2_imu_accel.z;
+  samples_idx++;
+
+#ifdef FILTER_ALIGNER_LED
+  RunOnceEvery(50, {LED_TOGGLE(FILTER_ALIGNER_LED);});
+#endif
+
+  if (samples_idx >= SAMPLES_NB) {
+    int32_t avg_ref_sensor = accel_sum.z;
+    if ( avg_ref_sensor >= 0)
+      avg_ref_sensor += SAMPLES_NB / 2;
+    else
+      avg_ref_sensor -= SAMPLES_NB / 2;
+    avg_ref_sensor /= SAMPLES_NB;
+    
+    booz2_filter_aligner_noise = 0;
+    int i;
+    for (i=0; i<SAMPLES_NB; i++) {
+      int32_t diff = ref_sensor_samples[i] - avg_ref_sensor;
+      booz2_filter_aligner_noise += abs(diff);
+    }
+
+    BOOZ_IVECT_SDIV_ACC(booz2_filter_aligner_lp_gyro,  gyro_sum,  SAMPLES_NB);
+    BOOZ_IVECT_SDIV_ACC(booz2_filter_aligner_lp_accel, accel_sum, SAMPLES_NB);
+    BOOZ_IVECT_SDIV_ACC(booz2_filter_aligner_lp_mag,   mag_sum,   SAMPLES_NB);
+
+    BOOZ_IVECT_ZERO(gyro_sum);
+    BOOZ_IVECT_ZERO(accel_sum);
+    BOOZ_IVECT_ZERO(mag_sum);
+    samples_idx = 0;
+
+    if (booz2_filter_aligner_noise < LOW_NOISE_THRESHOLD)
+      booz2_filter_aligner_low_noise_cnt++;
+    else
+      if ( booz2_filter_aligner_low_noise_cnt > 0)
+	booz2_filter_aligner_low_noise_cnt--;
+
+    if (booz2_filter_aligner_low_noise_cnt > LOW_NOISE_TIME) {
+      booz2_filter_aligner_status = BOOZ2_FILTER_ALIGNER_LOCKED;
+#ifdef FILTER_ALIGNER_LED
+      LED_ON(FILTER_ALIGNER_LED);
+#endif
+    }
+  }
+
+}
+
@@ -0,0 +1,32 @@
+#ifndef BOOZ2_FILTER_ATTITUDE_H
+#define BOOZ2_FILTER_ATTITUDE_H
+
+#include "std.h"
+#include "booz_geometry_int.h"
+
+#define BOOZ2_FILTER_ATTITUDE_UNINIT  0
+#define BOOZ2_FILTER_ATTITUDE_RUNNING 1
+
+struct Booz_ahrs_state {
+  struct Pprz_int32_euler euler;
+  struct Pprz_int32_rate  rate;
+  uint8_t status;
+};
+
+extern struct Booz_ahrs_state booz_ahrs_state;
+
+extern struct booz_ieuler booz2_filter_attitude_euler_aligned;
+
+extern struct booz_ieuler booz2_filter_attitude_euler;
+extern struct booz_ivect  booz2_filter_attitude_rate;
+
+
+extern uint8_t booz2_filter_attitude_status;
+
+extern void booz2_filter_attitude_init(void);
+extern void booz2_filter_attitude_align(void);
+extern void booz2_filter_attitude_propagate(void);
+extern void booz2_filter_attitude_update(void);
+
+
+#endif /* BOOZ2_ATTITUDE_FILTER_H */
@@ -0,0 +1,205 @@
+#include "booz2_filter_attitude_cmpl_euler.h"
+
+#include "booz2_imu.h"
+#include "booz2_filter_aligner.h"
+
+#include "airframe.h"
+#include "booz_geometry_mixed.h"
+
+struct Booz_ahrs_state booz_ahrs_state;
+
+uint8_t booz2_filter_attitude_status;
+
+struct booz_ieuler booz2_filter_attitude_euler;
+struct booz_ivect  booz2_filter_attitude_rate;
+
+struct booz_ieuler booz2_filter_attitude_euler_aligned;
+
+
+struct booz_ivect  booz2_face_gyro_bias;
+struct booz_ieuler booz2_face_measure;
+struct booz_ieuler booz2_face_residual;
+struct booz_ieuler booz2_face_uncorrected;
+struct booz_ieuler booz2_face_corrected;
+
+int32_t booz2_face_reinj_1;
+
+/* 
+ * ACC_AMP
+ * 9.81 * 2^10 * sin(pi/4) * ACC_AMP = pi/4 * 2^12 * F_UPDATE
+ *
+ */
+#define ACC_AMP 226
+
+#define SAMPLES_NB 256
+//static struct booz_ivect lp_gyro_samples[SAMPLES_NB];
+//static struct booz_ivect lp_gyro_sum;
+static struct booz_ivect lp_accel_samples[SAMPLES_NB];
+static struct booz_ivect lp_accel_sum;
+static uint8_t samples_idx;
+
+
+static inline void apply_alignment(void);
+
+void booz2_filter_attitude_init(void) {
+  booz2_filter_attitude_status = BOOZ2_FILTER_ATTITUDE_UNINIT;
+  BOOZ_IEULER_ZERO(booz2_filter_attitude_euler);
+  BOOZ_IVECT_ZERO( booz2_filter_attitude_rate);
+  BOOZ_IVECT_ZERO( booz2_face_gyro_bias);
+  //  booz2_face_reinj_1 = 1024;
+  booz2_face_reinj_1 = 2048;
+  samples_idx = 0;
+}
+
+void booz2_filter_attitude_align(void) {
+
+  BOOZ_IVECT_COPY( booz2_face_gyro_bias, booz2_filter_aligner_lp_gyro);
+
+  booz2_filter_attitude_status = BOOZ2_FILTER_ATTITUDE_RUNNING;
+
+}
+
+
+#define F_UPDATE 512
+
+#define PI_INTEG_EULER         (PI_INT * F_UPDATE)
+#define TWO_PI_INTEG_EULER (TWO_PI_INT * F_UPDATE)
+#define INTEG_EULER_NORMALIZE(_a) {				\
+    while (_a >  PI_INTEG_EULER)  _a -= TWO_PI_INTEG_EULER;	\
+    while (_a < -PI_INTEG_EULER)  _a += TWO_PI_INTEG_EULER;	\
+  }
+
+
+#define PSI_OF_MAG(_psi, _mag, _phi_est, _theta_est) {			\
+									\
+    int32_t sphi;							\
+    BOOZ_ISIN(sphi, _phi_est);						\
+    int32_t cphi;							\
+    BOOZ_ICOS(cphi, _phi_est);						\
+    int32_t stheta;							\
+    BOOZ_ISIN(stheta, _theta_est);					\
+    int32_t ctheta;							\
+    BOOZ_ICOS(ctheta, _theta_est);					\
+									\
+    int32_t sphi_stheta = BOOZ_IMULT(sphi, stheta, ITRIG_RES );		\
+    int32_t cphi_stheta = BOOZ_IMULT(cphi, stheta, ITRIG_RES );		\
+    const int32_t mn =							\
+      ctheta      * _mag.x+						\
+      sphi_stheta * _mag.y+						\
+      cphi_stheta * _mag.z;						\
+    const int32_t me =							\
+      0           * _mag.x+						\
+      cphi        * _mag.y+						\
+      -sphi       * _mag.z;						\
+    float m_psi = -atan2(me, mn);					\
+    _psi = ((m_psi)*(FLOAT_T)(1<<(IANGLE_RES))*F_UPDATE);		\
+    									\
+  }
+
+/*
+ *
+ * fc = 1/(2*pi*tau)
+ *
+ * alpha = dt / ( tau + dt )
+ *
+ *
+ *  y(i) = alpha x(i) + (1-alpha) y(i-1)
+ *  or
+ *  y(i) = y(i-1) + alpha * (x(i) - y(i-1))
+ *
+ *
+ */
+
+
+
+void booz2_filter_attitude_propagate(void) {
+
+  /* low pass accels         */
+  BOOZ_IVECT_DIFF(lp_accel_sum, lp_accel_sum, lp_accel_samples[samples_idx]);
+  BOOZ_IVECT_COPY(lp_accel_samples[samples_idx], booz2_imu_accel);
+  BOOZ_IVECT_SUM(lp_accel_sum, lp_accel_sum, booz2_imu_accel);
+
+  /* unbias gyro             */
+  struct booz_ivect  uf_rate;
+  BOOZ_IVECT_DIFF(uf_rate, booz2_imu_gyro, booz2_face_gyro_bias);
+  /* low pass rate */  
+  BOOZ_IVECT_SUM(booz2_filter_attitude_rate, booz2_filter_attitude_rate, uf_rate);
+  BOOZ_IVECT_SDIV(booz2_filter_attitude_rate, booz2_filter_attitude_rate, 2);
+
+  /* dumb integrate eulers */
+  struct booz_ieuler euler_dot;
+  euler_dot.phi   = booz2_filter_attitude_rate.x;
+  euler_dot.theta = booz2_filter_attitude_rate.y;
+  euler_dot.psi   = booz2_filter_attitude_rate.z;
+  BOOZ_IEULER_SUM(booz2_face_uncorrected, booz2_face_uncorrected, euler_dot);
+  BOOZ_IEULER_SUM(booz2_face_corrected, booz2_face_corrected, euler_dot);
+
+
+  /* build a measurement */
+  struct booz_ieuler measurement;
+  measurement.phi   = -booz2_imu_accel.y * ACC_AMP;
+  measurement.theta =  booz2_imu_accel.x * ACC_AMP;
+  PSI_OF_MAG(measurement.psi, booz2_imu_mag, 
+	     booz2_filter_attitude_euler.phi, booz2_filter_attitude_euler.theta);
+
+  /* low pass it */
+  BOOZ_IEULER_SUM(booz2_face_measure, booz2_face_measure, measurement);
+  BOOZ_IEULER_SDIV(booz2_face_measure, booz2_face_measure, 2);
+
+  /* compute residual */
+  BOOZ_IEULER_DIFF(booz2_face_residual, booz2_face_measure, booz2_face_corrected);
+  INTEG_EULER_NORMALIZE(booz2_face_residual.psi);
+
+
+  struct booz_ieuler correction;
+  /* compute a correction */
+  BOOZ_IEULER_SDIV(correction, booz2_face_residual, booz2_face_reinj_1);
+  /* correct estimation */
+  BOOZ_IEULER_SUM(booz2_face_corrected, booz2_face_corrected, correction);
+  INTEG_EULER_NORMALIZE(booz2_face_corrected.psi);
+
+
+  /* scale our result      */
+  BOOZ_IEULER_SDIV(booz2_filter_attitude_euler, booz2_face_corrected, F_UPDATE);
+  /* convert to quaternion */
+  //  BOOZ_IQUAT_OF_EULER(booz2_filter_attitude_quat, booz2_filter_attitude_euler);
+
+  apply_alignment();
+
+}
+
+void booz2_filter_attitude_update(void) {
+
+
+}
+
+
+// FIXME
+
+static inline void apply_alignment(void) {
+  
+#if 0
+  booz2_filter_attitude_euler_aligned.phi = 
+    booz2_filter_attitude_euler.phi
+    + (FILTER_ALIGNMENT_DPSI   * booz2_filter_attitude_euler.theta) >>  IANGLE_RES
+    - (FILTER_ALIGNMENT_DTHETA * booz2_filter_attitude_euler.psi)   >>  IANGLE_RES;
+
+  booz2_filter_attitude_euler_aligned.theta = 
+    - (FILTER_ALIGNMENT_DPSI   * booz2_filter_attitude_euler.phi)   >>  IANGLE_RES
+    + booz2_filter_attitude_euler.theta
+    + (FILTER_ALIGNMENT_DPHI * booz2_filter_attitude_euler.psi)     >>  IANGLE_RES;
+
+  booz2_filter_attitude_euler_aligned.psi = 
+      (FILTER_ALIGNMENT_DTHETA * booz2_filter_attitude_euler.phi)    >>  IANGLE_RES
+    - (FILTER_ALIGNMENT_DPHI   * booz2_filter_attitude_euler.theta)  >>  IANGLE_RES
+    + booz2_filter_attitude_euler.psi;
+#else
+
+  booz2_filter_attitude_euler_aligned.phi   = booz2_filter_attitude_euler.phi - FILTER_ALIGNMENT_DPHI;
+  booz2_filter_attitude_euler_aligned.theta = booz2_filter_attitude_euler.theta - FILTER_ALIGNMENT_DTHETA;
+  booz2_filter_attitude_euler_aligned.psi   = booz2_filter_attitude_euler.psi;
+
+#endif
+
+
+}
@@ -0,0 +1,17 @@
+#ifndef BOOZ2_FILTER_ATTITUDE_CMPL_EULER_H
+#define BOOZ2_FILTER_ATTITUDE_CMPL_EULER_H
+
+#include "booz2_filter_attitude.h"
+#include "std.h"
+#include "booz_geometry_int.h"
+
+extern struct booz_ivect  booz2_face_gyro_bias;
+extern struct booz_ieuler booz2_face_measure;
+extern struct booz_ieuler booz2_face_residual;
+extern struct booz_ieuler booz2_face_uncorrected;
+extern struct booz_ieuler booz2_face_corrected;
+
+extern int32_t booz2_face_reinj_1;
+
+
+#endif /* BOOZ2_FILTER_ATTITUDE_CMPL_EULER_H */
@@ -0,0 +1,56 @@
+#include "booz2_fms.h"
+
+#include "booz2_imu.h"
+#include "booz2_gps.h"
+#include "booz2_filter_attitude.h"
+
+bool_t  booz_fms_on;
+bool_t  booz_fms_timeout;
+uint8_t booz_fms_last_msg;
+struct Booz_fms_info    booz_fms_info;
+struct Booz_fms_command booz_fms_input;
+
+#define BOOZ_FMS_TIMEOUT 100
+
+void booz_fms_init(void) {
+
+  booz_fms_on = FALSE;
+  booz_fms_timeout = TRUE;
+  booz_fms_last_msg = BOOZ_FMS_TIMEOUT;
+
+  booz_fms_input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE;
+  BOOZ_IEULER_ZERO(booz_fms_input.h_sp.attitude);
+  booz_fms_input.v_mode = BOOZ2_GUIDANCE_V_MODE_DIRECT;
+
+  booz_fms_impl_init();
+}
+
+void booz_fms_periodic(void) {
+  if (booz_fms_last_msg < BOOZ_FMS_TIMEOUT) 
+    booz_fms_last_msg++;
+  else {
+    booz_fms_timeout = TRUE;
+    booz_fms_input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE;
+    BOOZ_IEULER_ZERO(booz_fms_input.h_sp.attitude);
+  }
+  booz_fms_impl_periodic();
+}
+
+
+
+void booz_fms_update_info(void) {
+
+  PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.imu.gyro,  booz_imu_state.gyro);
+  PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.imu.accel, booz_imu_state.accel);
+  PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.imu.mag,   booz_imu_state.mag);
+
+  PPRZ_INT32_VECT3_COPY(booz_fms_info.gps.pos, booz_gps_state.pos);
+  PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.gps.speed, booz_gps_state.speed);
+  booz_fms_info.gps.pacc = booz_gps_state.pacc;
+  booz_fms_info.gps.num_sv = booz_gps_state.num_sv;
+  booz_fms_info.gps.fix = booz_gps_state.fix;
+
+  PPRZ_INT16_OF_INT32_EULER(booz_fms_info.ahrs.euler, booz_ahrs_state.euler)
+  PPRZ_INT16_OF_INT32_RATE (booz_fms_info.ahrs.rate,  booz_ahrs_state.rate)
+
+}
@@ -0,0 +1,114 @@
+/*
+ * $id$
+ * This is the "external interface" to the autopilot. It allows an external device to
+ * fetch the vehicle state and input commands at different levels. We should support
+ * different hardware peripherals like i2c, spi or uart.
+ * For now we only have an implementation using datalink messages. 
+ *
+ * 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#ifndef BOOZ2_FMS_H
+#define BOOZ2_FMS_H
+
+#include "std.h"
+#include "booz_geometry_int.h"
+#include "booz2_autopilot.h"
+#include "booz2_guidance_h.h"
+#include "booz2_guidance_v.h"
+
+
+struct Booz_fms_imu_info {
+  struct Pprz_int16_vect3 gyro;
+  struct Pprz_int16_vect3 accel;
+  struct Pprz_int16_vect3 mag;
+};
+
+struct Booz_fms_gps_info {
+  struct Pprz_int32_vect3 pos;
+  struct Pprz_int16_vect3 speed;
+  int32_t pacc;
+  uint8_t num_sv;
+  uint8_t fix;
+};
+
+struct Booz_fms_ahrs_info {
+  struct Pprz_int16_euler euler;
+  struct Pprz_int16_rate  rate;
+};
+
+struct Booz_fms_info {
+  struct Booz_fms_imu_info  imu;
+  struct Booz_fms_gps_info  gps;
+  struct Booz_fms_ahrs_info ahrs;
+//  struct Booz_fms_ins_info  ins;
+};
+
+struct Booz_fms_command {
+  union {
+    struct booz_ivect  rate;
+    struct booz_ieuler attitude;
+    struct booz_ivect2 speed;
+    struct booz_ivect pos; //FIXME Warning z is heading
+  } h_sp;
+  union {
+    int32_t direct;
+    int32_t climb;
+    int32_t height;
+  } v_sp;
+  uint8_t h_mode;
+  uint8_t v_mode;
+};
+
+extern bool_t  booz_fms_on;
+extern bool_t  booz_fms_timeout;
+extern uint8_t booz_fms_last_msg;
+
+extern struct Booz_fms_info    booz_fms_info;
+extern struct Booz_fms_command booz_fms_input;
+
+extern void booz_fms_init(void);
+extern void booz_fms_periodic(void);
+extern void booz_fms_update_info(void);
+
+
+#define BOOZ2_FMS_TYPE_DATALINK    0
+#define BOOZ2_FMS_TYPE_TEST_SIGNAL 1
+
+#if defined BOOZ2_FMS_TYPE
+#if BOOZ2_FMS_TYPE == BOOZ2_FMS_TYPE_DATALINK
+#include "booz2_fms_datalink.h"
+#elif BOOZ2_FMS_TYPE == BOOZ2_FMS_TYPE_TEST_SIGNAL
+#include "booz2_fms_test_signal.h"
+#else
+#error "booz2_fms.h: Unknown BOOZ2_FMS_TYPE"
+#endif
+#endif
+
+#define BOOZ2_FMS_SET_POS_SP(_pos_sp,_psi_sp) { \
+  _pos_sp.x = booz_fms_input.h_sp.pos.x; \
+  _pos_sp.y = booz_fms_input.h_sp.pos.y; \
+  /*_psi_sp = booz_fms_input.h_sp.pos.z;*/ \
+}
+
+#define BOOZ2_FMS_POS_INIT(_pos_sp,_psi_sp) { \
+  booz_fms_input.h_sp.pos.x = _pos_sp.x; \
+  booz_fms_input.h_sp.pos.y = _pos_sp.y; \
+  booz_fms_input.h_sp.pos.z = _psi_sp; \
+}
+
+#endif /* BOOZ2_FMS_H */
+
+
@@ -0,0 +1,9 @@
+#include "booz2_fms.h"
+
+void booz_fms_impl_init(void) {
+
+}
+
+void booz_fms_impl_periodic(void) {
+
+}
@@ -0,0 +1,85 @@
+/*
+ * $id$
+ * This is the implementation of the "external interface" to the autopilot.
+ * using datalink messages. 
+ *
+ * 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#ifndef BOOZ2_FMS_DATALINK_H
+#define BOOZ2_FMS_DATALINK_H
+
+extern void booz_fms_impl_init(void);
+extern void booz_fms_impl_periodic(void);
+
+#if defined FMS_DATALINK_USE_COMMANDS_RAW
+#define BOOZ_FMS_PARSE_DATALINK(_l, _cmds) {				\
+    if (_l >= 2) {							\
+      booz_fms_input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE;		\
+      booz_fms_input.h_sp.attitude.phi   = BOOZ2_JOYSTICK_SP_PHI_COEF   * _cmds[0]; \
+      booz_fms_input.h_sp.attitude.theta = BOOZ2_JOYSTICK_SP_THETA_COEF * _cmds[1]; \
+      booz_fms_input.h_sp.attitude.psi   = 0;				\
+      booz_fms_last_msg = 0;						\
+      booz_fms_timeout = FALSE;						\
+    }									\
+  }
+#else
+
+#ifdef BOOZ_FMS_PHI_THETA_MAX
+#define BOOZ_FMS_LIMIT_ATTITUDE(_fms_att) { \
+  BoundAbs(_fms_att.phi,BOOZ_FMS_PHI_THETA_MAX); \
+  BoundAbs(_fms_att.theta,BOOZ_FMS_PHI_THETA_MAX); \
+}
+#else
+#define BOOZ_FMS_LIMIT_ATTITUDE(_x) {}
+#endif
+
+#define BOOZ_FMS_PARSE_DATALINK(_dl_buffer) {				\
+    booz_fms_last_msg = 0;						\
+    booz_fms_input.h_mode = DL_BOOZ2_FMS_COMMAND_h_mode(_dl_buffer);	\
+    booz_fms_input.v_mode = DL_BOOZ2_FMS_COMMAND_v_mode(_dl_buffer);	\
+    switch (booz_fms_input.h_mode) {					\
+    case BOOZ2_GUIDANCE_H_MODE_KILL:					\
+    case BOOZ2_GUIDANCE_H_MODE_RATE :					\
+      break;								\
+    case BOOZ2_GUIDANCE_H_MODE_ATTITUDE :				\
+      {									\
+	  booz_fms_input.h_sp.attitude.phi   = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer);			\
+	  booz_fms_input.h_sp.attitude.theta = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer);			\
+	  booz_fms_input.h_sp.attitude.psi   = DL_BOOZ2_FMS_COMMAND_h_sp_3(_dl_buffer);			\
+        BOOZ_FMS_LIMIT_ATTITUDE(booz_fms_input.h_sp.attitude); \
+      }									\
+      break;  \
+    case BOOZ2_GUIDANCE_H_MODE_HOVER :					\
+      {									\
+	  booz_fms_input.h_sp.pos.x   = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer);			\
+	  booz_fms_input.h_sp.pos.y   = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer);			\
+      }									\
+      break;  \
+    case BOOZ2_GUIDANCE_H_MODE_NAV :					\
+      break;								\
+    }									\
+    switch (booz_fms_input.v_mode) {					\
+    case BOOZ2_GUIDANCE_V_MODE_KILL:					\
+    case BOOZ2_GUIDANCE_V_MODE_DIRECT:					\
+      break;								\
+    case BOOZ2_GUIDANCE_V_MODE_HOVER :					\
+      booz_fms_input.v_sp.height = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \
+      break;								\
+    }									\
+  }
+#endif /* FMS_USE_COMMANDS_RAW */
+
+#endif /* BOOZ2_FMS_DATALINK_H */
@@ -0,0 +1,65 @@
+#include "booz2_fms.h"
+
+#include "booz2_ins.h"
+#include "booz_geometry_mixed.h"
+
+#define BOOZ2_FMS_TEST_SIGNAL_DEFAULT_PERIOD    100;
+#define BOOZ2_FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE 20;
+#define BOOZ2_FMS_TEST_SIGNAL_DEFAULT_AXE  0
+
+uint8_t  booz_fms_test_signal_mode;
+
+uint32_t booz_fms_test_signal_period;
+uint32_t booz_fms_test_signal_amplitude;
+uint8_t  booz_fms_test_signal_axe;
+uint32_t booz_fms_test_signal_counter;
+
+uint32_t booz_fms_test_signal_start_z;
+
+void booz_fms_impl_init(void) {
+  booz_fms_test_signal_mode = BOOZ_FMS_TEST_SIGNAL_MODE_DISABLED;
+  booz_fms_test_signal_period = BOOZ2_FMS_TEST_SIGNAL_DEFAULT_PERIOD;
+  booz_fms_test_signal_amplitude = BOOZ2_FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE;
+  booz_fms_test_signal_axe = BOOZ2_FMS_TEST_SIGNAL_DEFAULT_AXE;
+  booz_fms_test_signal_counter = 0;
+  booz_fms_input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE;
+  booz_fms_input.v_mode = BOOZ2_GUIDANCE_V_MODE_DIRECT;
+}
+
+void booz_fms_impl_periodic(void) {
+
+  switch (booz_fms_test_signal_mode) {
+    
+  case BOOZ_FMS_TEST_SIGNAL_MODE_DISABLED: {
+    PPRZ_INT32_EULER_ASSIGN(booz_fms_input.h_sp.attitude, 0, 0, 0);
+  }
+    break;
+    
+  case BOOZ_FMS_TEST_SIGNAL_MODE_ATTITUDE: {
+    if (booz_fms_test_signal_counter < booz_fms_test_signal_period) {
+      PPRZ_INT32_EULER_ASSIGN(booz_fms_input.h_sp.attitude, booz_fms_test_signal_amplitude, 0, 0);
+    }
+    else {
+      PPRZ_INT32_EULER_ASSIGN(booz_fms_input.h_sp.attitude, -booz_fms_test_signal_amplitude, 0, 0);
+    }
+  }
+    break;
+    
+  case BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL: {
+    if (booz2_guidance_v_mode < BOOZ2_GUIDANCE_V_MODE_HOVER)
+      booz_fms_test_signal_start_z = booz_ins_position.z;
+    else {
+      booz_fms_input.v_sp.height = (booz_fms_test_signal_counter < booz_fms_test_signal_period) ?
+	booz_fms_test_signal_start_z : 
+	booz_fms_test_signal_start_z - 128;
+      //BOOZ_INT_OF_FLOAT(-0.5, IPOS_FRAC)
+    }
+  }
+  break;
+
+  }
+
+  booz_fms_test_signal_counter++;
+  if (booz_fms_test_signal_counter >= 2 * booz_fms_test_signal_period)
+    booz_fms_test_signal_counter = 0;
+}
@@ -0,0 +1,34 @@
+#ifndef BOOZ_FMS_TEST_SIGNAL_H
+#define BOOZ_FMS_TEST_SIGNAL_H
+
+#include "std.h"
+
+#define BOOZ_FMS_TEST_SIGNAL_MODE_DISABLED 0
+#define BOOZ_FMS_TEST_SIGNAL_MODE_ATTITUDE 1
+#define BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL 2
+
+extern uint8_t  booz_fms_test_signal_mode;
+
+extern uint32_t booz_fms_test_signal_period;
+extern uint32_t booz_fms_test_signal_amplitude;
+extern uint8_t  booz_fms_test_signal_axe;
+extern uint32_t booz_fms_test_signal_counter;
+
+extern void booz_fms_impl_init(void);
+extern void booz_fms_impl_periodic(void);
+
+#define booz2_fms_test_signal_SetPeriod(_val) { \
+    booz_fms_test_signal_period = _val;		\
+    booz_fms_test_signal_counter = 0;		\
+}
+
+#define booz2_fms_test_signal_SetTsMode(_val) {				\
+    booz_fms_test_signal_mode = _val;					\
+    if (booz_fms_test_signal_mode == BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL) \
+      booz_fms_input.v_mode = BOOZ2_GUIDANCE_V_MODE_HOVER;		\
+    else								\
+      booz_fms_input.v_mode = BOOZ2_GUIDANCE_V_MODE_DIRECT;		\
+}
+
+#endif /* BOOZ_FMS_TEST_SIGNAL_H */
+
@@ -0,0 +1,169 @@
+#include "booz2_gps.h"
+
+struct Booz_gps_state booz_gps_state;
+
+/* UBX NAV POSLLH */
+int32_t  booz2_gps_lon;
+int32_t  booz2_gps_lat;
+int32_t  booz2_gps_hmsl;
+uint32_t booz2_gps_hacc;
+uint32_t booz2_gps_vacc;
+
+/* UBX NAV SOL */
+//uint8_t  booz2_gps_fix;
+//int32_t  booz2_gps_pacc;
+//int32_t  booz2_gps_sacc;
+//uint8_t  booz2_gps_num_sv;
+
+
+/* UBX NAV VELNED */
+int32_t  booz2_gps_vel_n;
+int32_t  booz2_gps_vel_e;
+
+
+/* misc */
+volatile bool_t  booz2_gps_msg_received;
+volatile uint8_t booz2_gps_nb_ovrn;
+
+void booz2_gps_init(void) {
+
+  booz_gps_state.fix = BOOZ2_GPS_FIX_NONE;
+#ifdef GPS_LED
+  LED_ON(GPS_LED);
+#endif
+  ubx_init();
+
+}
+
+
+void booz2_gps_read_ubx_message(void) {
+
+  if (ubx_class == UBX_NAV_ID) {
+    if (ubx_id == UBX_NAV_POSLLH_ID) {
+      booz2_gps_lon = UBX_NAV_POSLLH_LON(ubx_msg_buf);
+      booz2_gps_lat = UBX_NAV_POSLLH_LAT(ubx_msg_buf);
+      booz2_gps_hmsl = UBX_NAV_POSLLH_HMSL(ubx_msg_buf);
+    }
+    else if (ubx_id == UBX_NAV_SOL_ID) {
+      booz_gps_state.fix     = UBX_NAV_SOL_GPSfix(ubx_msg_buf);
+      booz_gps_state.pos.x   = UBX_NAV_SOL_ECEF_X(ubx_msg_buf);
+      booz_gps_state.pos.y   = UBX_NAV_SOL_ECEF_Y(ubx_msg_buf);
+      booz_gps_state.pos.z   = UBX_NAV_SOL_ECEF_Z(ubx_msg_buf);
+      booz_gps_state.pacc    = UBX_NAV_SOL_Pacc(ubx_msg_buf);
+      booz_gps_state.speed.x = UBX_NAV_SOL_ECEFVX(ubx_msg_buf);
+      booz_gps_state.speed.y = UBX_NAV_SOL_ECEFVY(ubx_msg_buf);
+      booz_gps_state.speed.z = UBX_NAV_SOL_ECEFVZ(ubx_msg_buf);
+      booz_gps_state.sacc    = UBX_NAV_SOL_Sacc(ubx_msg_buf);
+      booz_gps_state.num_sv  = UBX_NAV_SOL_numSV(ubx_msg_buf);
+#ifdef GPS_LED
+      if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) {
+      	LED_OFF(GPS_LED);
+      }
+      else {
+	LED_TOGGLE(GPS_LED);
+      }
+#endif
+    }
+    else if (ubx_id == UBX_NAV_VELNED_ID) {
+      booz2_gps_vel_n = UBX_NAV_VELNED_VEL_N(ubx_msg_buf);
+      booz2_gps_vel_e = UBX_NAV_VELNED_VEL_E(ubx_msg_buf);
+    }
+  }
+
+}
+
+/* UBX parsing */
+
+bool_t  ubx_msg_available;
+#define UBX_MAX_PAYLOAD 255
+uint8_t ubx_msg_buf[UBX_MAX_PAYLOAD] __attribute__ ((aligned));
+uint8_t ubx_id;
+uint8_t ubx_class;
+
+#define UNINIT        0
+#define GOT_SYNC1     1
+#define GOT_SYNC2     2
+#define GOT_CLASS     3
+#define GOT_ID        4
+#define GOT_LEN1      5
+#define GOT_LEN2      6
+#define GOT_PAYLOAD   7
+#define GOT_CHECKSUM1 8
+
+static uint8_t  ubx_status;
+static uint16_t ubx_len;
+static uint8_t  ubx_msg_idx;
+static uint8_t  ck_a, ck_b;
+
+
+void ubx_init(void) {
+  ubx_status = UNINIT;
+  ubx_msg_available = FALSE;
+}
+
+void ubx_parse( uint8_t c ) {
+  if (ubx_status < GOT_PAYLOAD) {
+    ck_a += c;
+    ck_b += ck_a;
+  }
+  switch (ubx_status) {
+  case UNINIT:
+    if (c == UBX_SYNC1)
+      ubx_status++;
+    break;
+  case GOT_SYNC1:
+    if (c != UBX_SYNC2)
+      goto error;
+    ck_a = 0;
+    ck_b = 0;
+    ubx_status++;
+    break;
+  case GOT_SYNC2:
+    if (ubx_msg_available) {
+      /* Previous message has not yet been parsed: discard this one */
+      booz2_gps_nb_ovrn++;
+      goto error;
+    }
+    ubx_class = c;
+    ubx_status++;
+    break;
+  case GOT_CLASS:
+    ubx_id = c;
+    ubx_status++;
+    break;    
+  case GOT_ID:
+    ubx_len = c;
+    ubx_status++;
+    break;
+  case GOT_LEN1:
+    ubx_len |= (c<<8);
+    if (ubx_len > UBX_MAX_PAYLOAD)
+      goto error;
+    ubx_msg_idx = 0;
+    ubx_status++;
+    break;
+  case GOT_LEN2:
+    ubx_msg_buf[ubx_msg_idx] = c;
+    ubx_msg_idx++;
+    if (ubx_msg_idx >= ubx_len) {
+      ubx_status++;
+    }
+    break;
+  case GOT_PAYLOAD:
+    if (c != ck_a)
+      goto error;
+    ubx_status++;
+    break;
+  case GOT_CHECKSUM1:
+    if (c != ck_b)
+      goto error;
+    ubx_msg_available = TRUE;
+    goto restart;
+    break;
+  }
+  return;
+ error:  
+ restart:
+  ubx_status = UNINIT;
+  return;
+}
@@ -0,0 +1,92 @@
+#ifndef BOOZ2_GPS_H
+#define BOOZ2_GPS_H
+
+#include "std.h"
+#include "booz_geometry_int.h"
+#include "led.h"
+
+struct Booz_gps_state {
+  struct Pprz_int32_vect3 pos;   /* pos ECEF in cm     */
+  struct Pprz_int32_vect3 speed; /* speed ECEF in cm/s */
+  uint32_t pacc;                 /*                    */
+  uint32_t sacc;                 /*                    */
+  uint8_t num_sv;
+  uint8_t fix;
+};
+
+extern struct Booz_gps_state booz_gps_state;
+
+/* UBX NAV POSLLH */
+extern int32_t  booz2_gps_lon;
+extern int32_t  booz2_gps_lat;
+extern int32_t  booz2_gps_hmsl;
+extern uint32_t booz2_gps_hacc;
+extern uint32_t booz2_gps_vacc;
+
+/* UBX NAV SOL */
+#define  BOOZ2_GPS_FIX_NONE 0x00
+#define  BOOZ2_GPS_FIX_3D   0x03
+//extern uint8_t  booz2_gps_fix;
+//extern int32_t  booz2_gps_pacc;
+//extern int32_t  booz2_gps_sacc;
+//extern uint8_t  booz2_gps_num_sv;
+
+
+/* UBX NAV VELNED */
+extern int32_t  booz2_gps_vel_n;
+extern int32_t  booz2_gps_vel_e;
+
+#include "ubx_protocol.h"
+
+#ifdef SITL
+#define GPS_LINKChAvailable() (FALSE)
+#define GPS_LINKGetch() (TRUE)
+#define Booz2GpsEvent(_sol_available_callback) {	                \
+    _sol_available_callback();						\
+  }
+#else /* not SITL */
+#define Booz2GpsEvent(_sol_available_callback) {			\
+    if (GpsBuffer()) {							\
+      ReadGpsBuffer();							\
+    }									\
+    if (ubx_msg_available) {						\
+      booz2_gps_read_ubx_message();					\
+      if (ubx_class == UBX_NAV_ID && ubx_id == UBX_NAV_VELNED_ID) {	\
+	_sol_available_callback();					\
+      }									\
+      ubx_msg_available = FALSE;					\
+    }									\
+  }
+#endif
+
+
+extern void booz2_gps_init(void);
+extern void booz2_gps_read_ubx_message(void);
+
+
+#define __GpsLink(dev, _x) dev##_x
+#define _GpsLink(dev, _x)  __GpsLink(dev, _x)
+#define GpsLink(_x) _GpsLink(GPS_LINK, _x)
+
+#define GpsBuffer() GpsLink(ChAvailable())
+#define ReadGpsBuffer() {				\
+    while (GpsLink(ChAvailable())&&!ubx_msg_available)	\
+      ubx_parse(GpsLink(Getch()));			\
+  }
+
+/* UBX parsing - copied from gps_ubx.c */
+
+extern bool_t  ubx_msg_available;
+extern uint8_t ubx_msg_buf[];
+extern uint8_t ubx_id;
+extern uint8_t ubx_class;
+
+extern void ubx_parse( uint8_t c );
+extern void ubx_init(void);
+
+
+
+
+#endif /* BOOZ2_GPS_H */
+
+
@@ -0,0 +1,199 @@
+#include "booz2_guidance_h.h"
+
+#include "booz2_filter_attitude.h"
+#include "booz2_stabilization_rate.h"
+#include "booz2_stabilization_attitude.h"
+#include "booz2_stabilization_attitude_ref_traj_euler.h"
+#include "booz2_fms.h"
+#include "booz2_ins.h"
+#include "booz2_navigation.h"
+
+#include "airframe.h"
+#include "radio_control.h"
+
+uint8_t booz2_guidance_h_mode;
+
+struct booz_ivect2 booz2_guidance_h_pos_sp;
+
+struct booz_ivect2 booz2_guidance_h_pos_err;
+struct booz_ivect2 booz2_guidance_h_pos_err_sum;
+
+struct booz_ieuler booz2_guidance_h_rc_sp;
+struct booz_ivect2 booz2_guidance_h_command_earth;
+struct booz_ivect2 booz2_guidance_h_stick_earth_sp;
+struct booz_ieuler booz2_guidance_h_command_body;
+
+int32_t booz2_guidance_h_pgain;
+int32_t booz2_guidance_h_dgain;
+int32_t booz2_guidance_h_igain;
+
+
+static inline void booz2_guidance_h_hover_run(void);
+static inline void booz2_guidance_h_hover_enter(void);
+
+
+void booz2_guidance_h_init(void) {
+
+  booz2_guidance_h_mode = BOOZ2_GUIDANCE_H_MODE_KILL;
+  BOOZ_IVECT2_ZERO(booz2_guidance_h_pos_err_sum);
+  BOOZ_IEULER_ZERO(booz2_guidance_h_rc_sp);
+  BOOZ_IEULER_ZERO(booz2_guidance_h_command_body);
+  booz2_guidance_h_pgain = BOOZ2_GUIDANCE_H_PGAIN;
+  booz2_guidance_h_igain = BOOZ2_GUIDANCE_H_IGAIN;
+  booz2_guidance_h_dgain = BOOZ2_GUIDANCE_H_DGAIN;
+
+}
+
+
+void booz2_guidance_h_mode_changed(uint8_t new_mode) {
+  if (new_mode == booz2_guidance_h_mode)
+    return;
+
+  switch ( booz2_guidance_h_mode ) {
+	//      case BOOZ2_GUIDANCE_H_MODE_RATE:
+	//	booz_stabilization_rate_exit();
+	//	break;
+  }
+   
+  switch (new_mode) {
+
+  case BOOZ2_GUIDANCE_H_MODE_ATTITUDE:
+    booz2_stabilization_attitude_enter();
+    break;
+    
+  case BOOZ2_GUIDANCE_H_MODE_HOVER:
+  case BOOZ2_GUIDANCE_H_MODE_NAV:
+    booz2_guidance_h_hover_enter();
+    break;
+
+  }
+
+  booz2_guidance_h_mode = new_mode;
+  
+}
+
+void booz2_guidance_h_read_rc(bool_t  in_flight) {
+  
+  switch ( booz2_guidance_h_mode ) {
+
+  case BOOZ2_GUIDANCE_H_MODE_RATE:
+    booz2_stabilization_rate_read_rc();
+    break;
+    
+  case BOOZ2_GUIDANCE_H_MODE_ATTITUDE:
+    booz2_stabilization_attitude_read_rc(in_flight);
+    if (booz_fms_on)
+      BOOZ2_STABILIZATION_ATTITUDE_ADD_SP(booz_fms_input.h_sp.attitude);
+    break;
+  
+  case BOOZ2_GUIDANCE_H_MODE_HOVER:
+    if (booz_fms_on && booz_fms_input.h_mode >= BOOZ2_GUIDANCE_H_MODE_HOVER)
+      BOOZ2_FMS_SET_POS_SP(booz2_guidance_h_pos_sp,booz_stabilization_att_sp.psi);
+    BOOZ2_STABILIZATION_ATTITUDE_READ_RC(booz2_guidance_h_rc_sp, in_flight);
+    break;
+  
+  case BOOZ2_GUIDANCE_H_MODE_NAV:
+    BOOZ2_STABILIZATION_ATTITUDE_READ_RC(booz2_guidance_h_rc_sp, in_flight);
+    break;
+  }
+
+}
+
+
+void booz2_guidance_h_run(bool_t  in_flight) {
+  switch ( booz2_guidance_h_mode ) {
+
+  case BOOZ2_GUIDANCE_H_MODE_RATE:
+    booz2_stabilization_rate_run();
+    break;
+
+  case BOOZ2_GUIDANCE_H_MODE_ATTITUDE:
+    booz2_stabilization_attitude_run(in_flight);
+    break;
+    
+  case BOOZ2_GUIDANCE_H_MODE_HOVER:
+    booz2_guidance_h_hover_run();
+    booz2_stabilization_attitude_run(in_flight);
+    break;
+    
+  case BOOZ2_GUIDANCE_H_MODE_NAV:
+    PPRZ_INT32_VECT2_OF_LL(booz2_guidance_h_pos_sp, booz2_navigation_carrot);
+    booz2_guidance_h_hover_run();
+    booz2_stabilization_attitude_run(in_flight);
+    break;
+    
+  }
+
+
+
+}
+
+#define MAX_POS_ERR   4096
+#define MAX_SPEED_ERR 4096
+#define MAX_POS_ERR_SUM (4096 << 12)
+
+// 15 degres
+#define MAX_BANK 65536
+
+static inline void  booz2_guidance_h_hover_run(void) {
+
+  /* compute position error    */
+  BOOZ_IVECT2_DIFF(booz2_guidance_h_pos_err, booz_ins_position, booz2_guidance_h_pos_sp);
+  /* saturate it               */
+  BOOZ_IVECT2_STRIM(booz2_guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR);
+
+  /* compute speed error    */
+  struct booz_ivect2 booz2_guidance_h_speed_err;
+  BOOZ_IVECT2_COPY(booz2_guidance_h_speed_err, booz_ins_speed_earth);
+  /* saturate it               */
+  BOOZ_IVECT2_STRIM(booz2_guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR);
+  
+  /* update pos error integral */
+  BOOZ_IVECT2_ADD(booz2_guidance_h_pos_err_sum, booz2_guidance_h_pos_err);
+  /* saturate it               */
+  BOOZ_IVECT2_STRIM(booz2_guidance_h_pos_err_sum, -MAX_POS_ERR_SUM, MAX_POS_ERR_SUM);
+		    
+  /* run PID */
+  // cmd_earth < 15.17
+  booz2_guidance_h_command_earth.x = booz2_guidance_h_pgain * booz2_guidance_h_pos_err.x +
+                                      booz2_guidance_h_dgain * booz2_guidance_h_speed_err.x +
+                                      booz2_guidance_h_igain * (booz2_guidance_h_pos_err_sum.x >> 12); 
+  booz2_guidance_h_command_earth.y = booz2_guidance_h_pgain * booz2_guidance_h_pos_err.y +
+                                      booz2_guidance_h_dgain * booz2_guidance_h_speed_err.y +
+		                      booz2_guidance_h_igain * (booz2_guidance_h_pos_err_sum.y >> 12); 
+
+  BOOZ_IVECT2_STRIM(booz2_guidance_h_command_earth, -MAX_BANK, MAX_BANK);
+
+  /* Rotate to body frame */
+  int32_t s_psi, c_psi;
+  BOOZ_ISIN(s_psi, booz2_filter_attitude_euler_aligned.psi);	
+  BOOZ_ICOS(c_psi, booz2_filter_attitude_euler_aligned.psi);	
+
+
+  // ITRIG_RES - 2: 100mm erreur, gain 100 -> 10000 command | 2 degres = 36000, so multiply by 4
+  booz2_guidance_h_command_body.phi = 
+      ( - s_psi * booz2_guidance_h_command_earth.x + c_psi * booz2_guidance_h_command_earth.y)
+    >> (ITRIG_RES - 2);
+  booz2_guidance_h_command_body.theta = 
+    - ( c_psi * booz2_guidance_h_command_earth.x + s_psi * booz2_guidance_h_command_earth.y)
+    >> (ITRIG_RES - 2);
+
+
+  booz2_guidance_h_command_body.phi   += booz2_guidance_h_rc_sp.phi;
+  booz2_guidance_h_command_body.theta += booz2_guidance_h_rc_sp.theta;
+  booz2_guidance_h_command_body.psi   = booz2_guidance_h_rc_sp.psi;
+
+  BOOZ_IEULER_COPY(booz_stabilization_att_sp, booz2_guidance_h_command_body);
+
+}
+
+static inline void booz2_guidance_h_hover_enter(void) {
+
+  BOOZ_IVECT2_COPY(booz2_guidance_h_pos_sp, booz_ins_position);
+
+  BOOZ2_STABILIZATION_ATTITUDE_RESET_PSI_REF( booz2_guidance_h_rc_sp );
+
+  BOOZ_IVECT2_ZERO(booz2_guidance_h_pos_err_sum);
+
+  BOOZ2_FMS_POS_INIT(booz2_guidance_h_pos_sp,booz2_guidance_h_rc_sp.psi);
+}
@@ -0,0 +1,35 @@
+#ifndef BOOZ2_GUIDANCE_H_H
+#define BOOZ2_GUIDANCE_H_H
+
+
+#include "booz_geometry_int.h"
+
+#define BOOZ2_GUIDANCE_H_MODE_KILL      0
+#define BOOZ2_GUIDANCE_H_MODE_RATE      1
+#define BOOZ2_GUIDANCE_H_MODE_ATTITUDE  2
+#define BOOZ2_GUIDANCE_H_MODE_HOVER     3
+#define BOOZ2_GUIDANCE_H_MODE_NAV       4
+
+
+extern uint8_t booz2_guidance_h_mode;
+
+extern struct booz_ivect2 booz2_guidance_h_pos_sp;
+
+extern struct booz_ivect2 booz2_guidance_h_pos_err;
+extern struct booz_ivect2 booz2_guidance_h_pos_err_sum;
+
+extern struct booz_ieuler booz2_guidance_h_rc_sp;
+extern struct booz_ivect2 booz2_guidance_h_command_earth;
+extern struct booz_ieuler booz2_guidance_h_command_body;
+
+extern int32_t booz2_guidance_h_pgain;
+extern int32_t booz2_guidance_h_dgain;
+extern int32_t booz2_guidance_h_igain;
+
+
+extern void booz2_guidance_h_init(void);
+extern void booz2_guidance_h_mode_changed(uint8_t new_mode);
+extern void booz2_guidance_h_read_rc(bool_t  in_flight);
+extern void booz2_guidance_h_run(bool_t  in_flight);
+
+#endif /* BOOZ2_GUIDANCE_H_H */
@@ -0,0 +1,129 @@
+#include "booz2_guidance_v.h"
+
+#define B2_GUIDANCE_V_C
+#define B2_GUIDANCE_V_USE_REF
+#include "booz2_guidance_v_ref.h"
+
+#include "radio_control.h"
+#include "airframe.h"
+#include "booz2_stabilization.h"
+#include "booz2_fms.h"
+
+#include "booz2_ins.h"
+#include "booz_geometry_mixed.h"
+
+uint8_t booz2_guidance_v_mode;
+int32_t booz2_guidance_v_delta_t;
+int32_t booz2_guidance_v_rc_delta_t;
+
+
+int32_t booz2_guidance_v_z_sp;
+
+int32_t booz2_guidance_v_kp;
+int32_t booz2_guidance_v_kd;
+int32_t booz2_guidance_v_ki;
+
+int32_t booz2_guidance_v_z_sum_err;
+
+static inline void run_hover_loop(bool_t in_flight);
+
+
+void booz2_guidance_v_init(void) {
+
+  booz2_guidance_v_mode = BOOZ2_GUIDANCE_V_MODE_KILL;
+
+  booz2_guidance_v_kp = BOOZ2_GUIDANCE_V_HOVER_KP;
+  booz2_guidance_v_kd = BOOZ2_GUIDANCE_V_HOVER_KD;
+  booz2_guidance_v_ki = BOOZ2_GUIDANCE_V_HOVER_KI;
+
+  booz2_guidance_v_z_sum_err = 0;
+
+}
+
+
+void booz2_guidance_v_read_rc(void) {
+
+  booz2_guidance_v_rc_delta_t = (int32_t)rc_values[RADIO_THROTTLE] * 200 / MAX_PPRZ;
+
+  switch (booz2_guidance_v_mode) {
+    case BOOZ2_GUIDANCE_V_MODE_DIRECT:
+      booz2_guidance_v_z_sp = booz_ins_position.z;
+      break;
+    case BOOZ2_GUIDANCE_V_MODE_HOVER:
+      if (booz_fms_on && booz_fms_input.v_mode >= BOOZ2_GUIDANCE_V_MODE_HOVER)
+        booz2_guidance_v_z_sp = booz_fms_input.v_sp.height;
+      break;
+  }
+
+}
+
+void booz2_guidance_v_mode_changed(uint8_t new_mode) {
+  
+  if (new_mode == booz2_guidance_v_mode)
+    return;
+
+  //  switch ( booz2_guidance_v_mode ) {
+  //  
+  //  }
+   
+  switch (new_mode) {
+    
+  case BOOZ2_GUIDANCE_V_MODE_HOVER:
+    booz2_guidance_v_z_sum_err = 0;
+    booz2_guidance_v_z_sp = booz_ins_position.z;
+    break;
+  }
+  
+  booz2_guidance_v_mode = new_mode;
+
+}
+
+
+void booz2_guidance_v_run(bool_t in_flight) {
+
+  switch (booz2_guidance_v_mode) {
+  case BOOZ2_GUIDANCE_V_MODE_DIRECT:
+    booz2_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_rc_delta_t;
+    break;
+  case BOOZ2_GUIDANCE_V_MODE_HOVER:
+    run_hover_loop(in_flight);
+    if (booz2_guidance_v_delta_t < booz2_guidance_v_rc_delta_t)
+      booz2_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_delta_t;
+    else
+      booz2_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_rc_delta_t;
+    break;
+  }
+}
+
+
+#define MAX_Z_SUM_ERR (1<<23)
+
+static inline void run_hover_loop(bool_t in_flight) {
+
+#ifdef B2_GUIDANCE_V_USE_REF
+  b2_gv_update_ref();
+#else
+  b2_gv_set_ref(booz2_guidance_v_z_sp, 0, 0);
+#endif
+  int64_t ref_z = b2_gv_z_ref>>(B2_GV_Z_REF_FRAC - B2_GV_Z_SP_FRAC);
+  int32_t err_z =  booz_ins_position.z - (int32_t)ref_z;
+  int32_t ref_zd =  b2_gv_zd_ref<<(ISPEED_RES - B2_GV_ZD_REF_FRAC);
+  int32_t err_zd =  booz_ins_speed_earth.z - ref_zd;
+
+  if (in_flight) {
+    booz2_guidance_v_z_sum_err += err_z;
+    Bound(booz2_guidance_v_z_sum_err, -MAX_Z_SUM_ERR, MAX_Z_SUM_ERR);
+  }
+  else
+    booz2_guidance_v_z_sum_err = 0;
+
+  const int32_t hover_power = BOOZ2_GUIDANCE_V_HOVER_POWER;
+  
+  booz2_guidance_v_delta_t = hover_power + 
+    ((-booz2_guidance_v_kp * err_z) >> 12) + 
+    ((-booz2_guidance_v_kd * err_zd) >> 21) +
+    ((-booz2_guidance_v_ki * booz2_guidance_v_z_sum_err) >> 24);
+
+
+}
+
@@ -0,0 +1,28 @@
+#ifndef BOOZ2_GUIDANCE_V
+#define BOOZ2_GUIDANCE_V
+
+#include "std.h"
+
+#define BOOZ2_GUIDANCE_V_MODE_KILL   0
+#define BOOZ2_GUIDANCE_V_MODE_DIRECT 1
+#define BOOZ2_GUIDANCE_V_MODE_HOVER  2
+
+extern uint8_t booz2_guidance_v_mode;
+
+extern int32_t booz2_guidance_v_est_g;
+extern int32_t booz2_guidance_v_est_zd;
+extern int32_t booz2_guidance_v_z_sp;
+extern int32_t booz2_guidance_v_z_sum_err;
+extern int32_t booz2_guidance_v_delta_t;
+
+extern int32_t booz2_guidance_v_kp;
+extern int32_t booz2_guidance_v_kd;
+extern int32_t booz2_guidance_v_ki;
+
+
+extern void booz2_guidance_v_init(void);
+extern void booz2_guidance_v_read_rc(void);
+extern void booz2_guidance_v_mode_changed(uint8_t new_mode);
+extern void booz2_guidance_v_run(bool_t in_flight);
+
+#endif /* BOOZ2_GUIDANCE_V */
--- a/Show More
+++ b/Show More