diff --git a/conf/airframes/booz.xml b/conf/airframes/booz.xml
index bbfb74c32d..8c46471e03 100644
--- a/conf/airframes/booz.xml
+++ b/conf/airframes/booz.xml
@@ -33,6 +33,30 @@
   </rc_commands>
 
 
+ <section name="MICROMAG">
+
+    <define name="MM_SS_PIN" value="20"/>
+    <define name="MM_SS_IODIR" value="IO0DIR"/>
+    <define name="MM_SS_IOSET" value="IO0SET"/>
+    <define name="MM_SS_IOCLR" value="IO0CLR"/>
+
+    <define name="MM_RESET_PIN" value="21"/>
+    <define name="MM_RESET_IODIR" value="IO1DIR"/>
+    <define name="MM_RESET_IOSET" value="IO1SET"/>
+    <define name="MM_RESET_IOCLR" value="IO1CLR"/>
+
+    <define name="MM_DRDY_PIN" value="15"/>
+    <define name="MM_DRDY_PINSEL" value="PINSEL0"/>
+    <define name="MM_DRDY_PINSEL_BIT" value="30"/>
+    <define name="MM_DRDY_PINSEL_VAL" value="2"/>
+    <define name="MM_DRDY_EINT" value="2"/>
+
+  </section>
+
+
+
+
+
   <makefile>
 
 include $(PAPARAZZI_SRC)/conf/autopilot/conf_booz.makefile
diff --git a/conf/airframes/tl.xml b/conf/airframes/tl.xml
index 62a6e3932b..2074d4b83d 100644
--- a/conf/airframes/tl.xml
+++ b/conf/airframes/tl.xml
@@ -18,10 +18,10 @@
    </commands>
 
   <command_laws>
-          <let var="roll"     value="1.0  * @ROLL"/>
-          <let var="pitch"    value="1.0  * @PITCH"/>
-          <let var="yaw"      value="0.1 * @YAW"/>
-          <let var="throttle" value="1.0   * @THROTTLE"/>
+          <let var="roll"     value="1.0 * @ROLL"/>
+          <let var="pitch"    value="1.0 * @PITCH"/>
+          <let var="yaw"      value="1.0 * @YAW"/>
+          <let var="throttle" value="1.0 * @THROTTLE"/>
           <let var="right" value="$throttle + $yaw"/><!-- add trim term for Right -->
           <let var="left" value="$throttle - $yaw"/><!-- .06*thrsubtract trim term for Right -->  
           <set servo="AILERON" value="$roll"/>
@@ -39,6 +39,27 @@
     <set command="CAM"    value="@GAIN1"/>
    </rc_commands>
 
+
+  <section name="MICROMAG">
+
+    <define name="MM_SS_PIN" value="20"/>
+    <define name="MM_SS_IODIR" value="IO0DIR"/>
+    <define name="MM_SS_IOSET" value="IO0SET"/>
+    <define name="MM_SS_IOCLR" value="IO0CLR"/>
+
+    <define name="MM_RESET_PIN" value="29"/>
+    <define name="MM_RESET_IODIR" value="IO0DIR"/>
+    <define name="MM_RESET_IOSET" value="IO0SET"/>
+    <define name="MM_RESET_IOCLR" value="IO0CLR"/>
+
+    <define name="MM_DRDY_PINSEL" value="PINSEL1"/>
+    <define name="MM_DRDY_PINSEL_BIT" value="28"/>
+    <define name="MM_DRDY_PINSEL_VAL" value="2"/>
+    <define name="MM_DRDY_EINT" value="3"/>
+
+  </section>
+
+
   <section name="horizontal_control">
     <define name="NAV_GOTO_H_PGAIN" value="RadOfDeg(10)" unit="rad/m"/>
     <define name="NAV_GOTO_H_DGAIN" value="0"/>
@@ -80,10 +101,19 @@ ap.srcs += tl_autopilot.c tl_control.c tl_estimator.c
 ap.CFLAGS += -DGPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B38400 -DDOWNLINK_GPS_DEVICE=DOWNLINK_AP_DEVICE -DGPS_CONFIGURE -DUSER_GPS_CONFIGURE=\"tl_gps_configure.h\" -DGPS_BAUD=38400
 ap.srcs += gps_ubx.c gps.c latlong.c
 
+ap.srcs += tl_imu.c 
+
+ap.CFLAGS += -DADC -DUSE_ADC_5 -DADC_CHANNEL_GR=ADC_5
+
+ap.srcs += $(SRC_ARCH)/tl_baro.c
+
+ap.CFLAGS += -DUSE_MICROMAG
+ap.srcs += micromag.c $(SRC_ARCH)/micromag_hw.c
+
 ap.CFLAGS += -DNAV
 ap.srcs += common_nav.c tl_nav.c
 
-ap.CFLAGS += -D BARO_MS5534A_W1=0xABDC -D BARO_MS5534A_W2=0x849A -D BARO_MS5534A_W3=0X939E -D BARO_MS5534A_W4=0xB259
+ap.CFLAGS += -D TL_BARO_W1=0xABDC -D TL_BARO_W2=0x849A -D TL_BARO_W3=0X939E -D TL_BARO_W4=0xB259
 
 
   </makefile>
diff --git a/conf/messages.xml b/conf/messages.xml
index 5608188b98..4bb9a24dea 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -541,6 +541,12 @@
     <field name="ami_stat"   type="uint8"/>
   </message>
 
+  <message name="TL_ESTIMATOR" ID="246">
+      <field name="r"   type="float" unit="rad/s"/>
+      <field name="psi" type="float" unit="rad"/>
+      <field name="z_baro" type="float" unit="cm"/>
+  </message>
+
   <message name="ENOSE_STATUS" ID="250">
       <field name="val1" type="uint16"/>
       <field name="val2" type="uint16"/>
diff --git a/sw/airborne/arm7/baro_MS5534A.h b/sw/airborne/arm7/baro_MS5534A.h
index fdcf5d4857..fd1c44e201 100644
--- a/sw/airborne/arm7/baro_MS5534A.h
+++ b/sw/airborne/arm7/baro_MS5534A.h
@@ -34,14 +34,6 @@
 
 #ifdef USE_BARO_MS5534A
 
-#if ! defined USE_SPI
-#define USE_SPI
-#endif
-
-#if ! defined SPI_MASTER
-#define SPI_MASTER
-#endif
-
 extern bool_t baro_MS5534A_do_reset;
 extern bool_t baro_MS5534A_available;
 extern uint32_t baro_MS5534A_pressure;
diff --git a/sw/airborne/arm7/micromag_hw.c b/sw/airborne/arm7/micromag_hw.c
index e8649ee90e..bf5c8e7676 100644
--- a/sw/airborne/arm7/micromag_hw.c
+++ b/sw/airborne/arm7/micromag_hw.c
@@ -1,19 +1,26 @@
-/* PNI micromag3 connected on SPI1
+/* PNI micromag3 connected on SPI1 */
+/* 
+   Twisted Logic
+   SS    on P0.20
+   RESET on P0.29 
+   DRDY  on P0.30 ( EINT3 )
+*/
+
+/* 
+   IMU
    SS on P0.20
    RESET on P1.21
    DRDY on P0.15 ( EINT2 )
 */
+
 #include "micromag.h"
 
 
 volatile uint8_t micromag_cur_axe;
 
-#ifndef DISABLE_MAGNETOMETER
 static void EXTINT2_ISR(void) __attribute__((naked));
-#endif /* DISABLE_MAGNETOMETER */
 
 void micromag_hw_init( void ) {
-#ifndef DISABLE_MAGNETOMETER
   /* configure SS pin */
   SetBit(MM_SS_IODIR, MM_SS_PIN); /* pin is output  */
   MmUnselect();                   /* pin idles high */
@@ -30,23 +37,23 @@ void micromag_hw_init( void ) {
   SetBit(EXTINT,MM_DRDY_EINT);   /* clear pending EINT */
   
   /* initialize interrupt vector */
-  VICIntSelect &= ~VIC_BIT( VIC_EINT2 );  /* select EINT2 as IRQ source */
-  VICIntEnable = VIC_BIT( VIC_EINT2 );    /* enable it */
-  VICVectCntl9 = VIC_ENABLE | VIC_EINT2;
+  VICIntSelect &= ~VIC_BIT( VIC_EINT3 );  /* select EINT2 as IRQ source */
+  VICIntEnable = VIC_BIT( VIC_EINT3 );    /* enable it */
+  VICVectCntl9 = VIC_ENABLE | VIC_EINT3;
   VICVectAddr9 = (uint32_t)EXTINT2_ISR;    // address of the ISR 
-#endif /* DISABLE_MAGNETOMETER */
 }
 
 void micromag_read( void ) {
-#ifndef DISABLE_MAGNETOMETER
-  MmSelect();
-  SpiEnable();
-  //  micromag_cur_axe = 0;
-  MmTriggerRead();
-#endif /* DISABLE_MAGNETOMETER */
+  if (micromag_status == MM_IDLE ) {
+    MmSelect();
+    SpiEnable();
+    SpiDisableRti();
+    micromag_cur_axe = 0;
+    micromag_status = MM_BUSY;
+    MmTriggerRead();
+  }
 }
 
-#ifndef DISABLE_MAGNETOMETER
 void EXTINT2_ISR(void) {
   ISR_ENTRY();
   /* read dummy control byte reply */
@@ -62,4 +69,4 @@ void EXTINT2_ISR(void) {
   VICVectAddr = 0x00000000;    /* clear this interrupt from the VIC */
   ISR_EXIT();
 }
-#endif /* DISABLE_MAGNETOMETER */
+
diff --git a/sw/airborne/arm7/micromag_hw.h b/sw/airborne/arm7/micromag_hw.h
index f4b5f8273f..2aa76589d6 100644
--- a/sw/airborne/arm7/micromag_hw.h
+++ b/sw/airborne/arm7/micromag_hw.h
@@ -6,24 +6,10 @@
 #include "interrupt_hw.h" 
 #include "spi_hw.h"
 
+#include "airframe.h"
 
 extern volatile uint8_t micromag_cur_axe;
 
-#define MM_SS_PIN 20
-#define MM_SS_IODIR IO0DIR
-#define MM_SS_IOSET IO0SET
-#define MM_SS_IOCLR IO0CLR
-
-#define MM_RESET_PIN 21
-#define MM_RESET_IODIR IO1DIR
-#define MM_RESET_IOSET IO1SET
-#define MM_RESET_IOCLR IO1CLR
-
-#define MM_DRDY_PIN 15
-#define MM_DRDY_PINSEL PINSEL0
-#define MM_DRDY_PINSEL_BIT 30
-#define MM_DRDY_PINSEL_VAL 2
-#define MM_DRDY_EINT 2
 
 #define MmSelect() SetBit(MM_SS_IOCLR,MM_SS_PIN)
 #define MmUnselect() SetBit(MM_SS_IOSET,MM_SS_PIN)
@@ -33,15 +19,13 @@ extern volatile uint8_t micromag_cur_axe;
 
 #define MmTriggerRead() {					\
     MmSet();							\
-    micromag_cur_axe++;						\
-    if (micromag_cur_axe == MM_NB_AXIS)				\
-      micromag_cur_axe = 0;					\
     MmReset();							\
     uint8_t control_byte = (micromag_cur_axe+1) << 0 | 4 << 4;	\
     SpiSend(control_byte);					\
   }
 
-#define MmOnSpiIt() { \
+#if 0
+#define MmOnSpiIt() {					\
     if (bit_is_set(SSPMIS, RTMIS)) {			\
       micromag_values[micromag_cur_axe] = SSPDR << 8;	\
       micromag_values[micromag_cur_axe] += SSPDR;	\
@@ -56,6 +40,24 @@ extern volatile uint8_t micromag_cur_axe;
       MmUnselect();					\
     }							\
 }
+#endif
 
+#define MicromagOnSpiInt() {				\
+    if (bit_is_set(SSPMIS, RTMIS)) {			\
+      micromag_values[micromag_cur_axe] = SSPDR << 8;	\
+      micromag_values[micromag_cur_axe] += SSPDR;	\
+      SpiClearRti();					\
+      SpiDisableRti();					\
+      if (micromag_cur_axe == 2) {			\
+	micromag_status = MM_DATA_AVAILABLE;		\
+	SpiDisable();					\
+	MmUnselect();					\
+      }							\
+      else {						\
+	micromag_cur_axe++;				\
+	MmTriggerRead();				\
+      }							\
+    }							\
+  }
 
 #endif /* MICROMAG_HW_H */
diff --git a/sw/airborne/arm7/tl_baro.c b/sw/airborne/arm7/tl_baro.c
new file mode 100644
index 0000000000..fdb67a6aa5
--- /dev/null
+++ b/sw/airborne/arm7/tl_baro.c
@@ -0,0 +1,93 @@
+#include "tl_baro.h"
+
+#include "airframe.h"
+
+#define CMD_MEASUREMENT 0x0F
+#define CMD_PRESSURE    0x40
+#define CMD_TEMPERATURE 0x20
+
+
+uint16_t tl_baro_d[TL_BARO_NB_DATA];
+const uint8_t tl_baro_cmd[TL_BARO_NB_DATA] = {CMD_PRESSURE, CMD_TEMPERATURE};
+uint32_t tl_baro_pressure;
+uint16_t tl_baro_temp;
+uint8_t tl_baro_cur_data;
+
+static uint16_t c1, c2, c3, c4, ut1, c6;
+
+#define MS5534A_MCLK 32768
+#define PWM_PRESCALER 1
+#define PWM_PERIOD ((PCLK / PWM_PRESCALER) / MS5534A_MCLK)
+#define PWM_DUTY (PWM_PERIOD / 2)
+
+
+void tl_baro_init(void) {
+  
+  /* 32768Hz on PWM2 */
+  /* Configure P0.7 pin as PWM */
+  PINSEL0 &= ~(_BV(14));
+  PINSEL0 |= _BV(15);
+
+  /* No prescaler */
+  PWMPR = PWM_PRESCALER-1;
+
+  /* To get 32768Hz from a base frequency of 15MHz */
+  PWMMR0 = PWM_PERIOD;
+  PWMMR2 = PWM_DUTY;
+
+  PWMLER = PWMLER_LATCH0 | PWMLER_LATCH2;
+  PWMTCR = PWMTCR_COUNTER_ENABLE | PWMTCR_PWM_ENABLE;
+  PWMPCR = PWMPCR_ENA2;
+
+  /* setup slave select */
+  /* configure SS pin */
+  SetBit(TL_BARO_SS_IODIR, TL_BARO_SS_PIN); /* pin is output  */
+  TlBaroUnselect();                         /* pin idles high */
+
+  tl_baro_cur_data = TL_BARO_PRESSURE; 
+
+
+  /* End of init, configuration (page 11) */
+  c1 = TL_BARO_W1 >> 1;
+  c2 = ((TL_BARO_W3 & 0x3f) << 6) | (TL_BARO_W4 & 0x3f);
+  c3 = TL_BARO_W4 >> 6;
+  c4 = TL_BARO_W3 >> 6;
+  uint16_t c5 = ((TL_BARO_W1 & 0x1) << 10) | (TL_BARO_W2 >> 6);
+  c6 = TL_BARO_W2 & 0x3f;
+  
+  ut1 = (c5 << 3) + 20224;
+
+
+}
+
+void tl_baro_send_req(void) {
+  tl_baro_cur_data++; 
+  if (tl_baro_cur_data == TL_BARO_NB_DATA) tl_baro_cur_data = TL_BARO_PRESSURE;
+  TlBaroSelect();
+  //  SpiEnable(); 
+  //  SpiEnableRti();
+  SpiClrCPHA();
+  SSPDR = CMD_MEASUREMENT;
+  SSPDR = tl_baro_cmd[tl_baro_cur_data];
+
+}
+
+void tl_baro_read(void) {
+  TlBaroSelect();
+  SpiEnable();
+  SpiEnableRti();
+  SpiSetCPHA();
+  SSPDR = 0;
+  SSPDR = 0;
+
+}
+
+void tl_baro_compute(void) {
+  /* Compute pressure and temp (page 10) */
+  int16_t dT = tl_baro_d[1] - ut1;
+  tl_baro_temp = 200 + (dT*(c6+50)) / (1 << 10);
+  int16_t off = c2*4 + ((c4-512)*dT)/(1 << 12);
+  uint32_t sens = c1 + (c3*dT)/(1<<10) + 24576;
+  uint16_t x = (sens*(tl_baro_d[0]-7168))/(1<<14) - off;
+  tl_baro_pressure = ((x*100)>>5) + 250*100;
+}
diff --git a/sw/airborne/arm7/tl_baro.h b/sw/airborne/arm7/tl_baro.h
new file mode 100644
index 0000000000..03bc20dfed
--- /dev/null
+++ b/sw/airborne/arm7/tl_baro.h
@@ -0,0 +1,38 @@
+#ifndef TL_BARO_H
+#define TL_BARO_H
+
+#include "std.h"
+#include "LPC21xx.h"
+#include "spi_hw.h"
+#include "sys_time.h"
+
+#define TL_BARO_PRESSURE 0
+#define TL_BARO_TEMP     1
+#define TL_BARO_NB_DATA  2
+
+extern uint8_t tl_baro_cur_data;
+extern uint16_t tl_baro_d[];
+
+extern uint32_t tl_baro_pressure;
+extern uint16_t tl_baro_temp;
+
+
+/* P0.28 */
+#define TL_BARO_SS_IODIR IO0DIR
+#define TL_BARO_SS_PIN   28 
+#define TL_BARO_SS_IOCLR IO0CLR
+#define TL_BARO_SS_IOSET IO0SET
+
+#define TlBaroUnselect()   SetBit(TL_BARO_SS_IOCLR,TL_BARO_SS_PIN)
+#define TlBaroSelect() SetBit(TL_BARO_SS_IOSET,TL_BARO_SS_PIN)
+
+#define TlBaroOnSpiIntReading() { tl_baro_d[tl_baro_cur_data] = SSPDR<<8; tl_baro_d[tl_baro_cur_data] |= SSPDR; TlBaroUnselect(); }
+
+#define TlBaroOnSpiIntSending() { uint8_t foo = SSPDR; foo = SSPDR; TlBaroUnselect(); SpiDisable(); SpiDisableRti(); }
+
+
+extern void tl_baro_init(void);
+extern void tl_baro_send_req(void);
+extern void tl_baro_read(void);
+extern void tl_baro_compute(void);
+#endif /* TL_BARO_H */
diff --git a/sw/airborne/estimator.c b/sw/airborne/estimator.c
index 0ebf36494f..c27ea64866 100644
--- a/sw/airborne/estimator.c
+++ b/sw/airborne/estimator.c
@@ -99,24 +99,6 @@ void estimator_init( void ) {
 }
 
 
-#ifdef IMU_3DMG
-#include "inter_mcu.h"
-void estimator_update_state_3DMG( void ) {
-  estimator_phi = from_fbw.from_fbw.euler[0];
-  estimator_psi = from_fbw.from_fbw.euler[1];
-  estimator_theta = from_fbw.from_fbw.euler[2];
-}
-#elif defined IMU_ANALOG && defined AHRS
-#include "ahrs.h"
-void estimator_update_state_ANALOG( void ) {
-//ahrs.h is in NED but estimator in NWU if i remember
-//perhaps this transform is not enough, i'm tired ;-) 
-  estimator_phi = ahrs_euler[0];
-  estimator_theta = -ahrs_euler[1];
-  estimator_psi = -ahrs_euler[2];
-}
-#else //NO_IMU
-#endif
 
 void estimator_propagate_state( void ) {
   
diff --git a/sw/airborne/micromag.c b/sw/airborne/micromag.c
index 4d10d6a437..901daa5fd9 100644
--- a/sw/airborne/micromag.c
+++ b/sw/airborne/micromag.c
@@ -1,6 +1,6 @@
 #include "micromag.h"
 
-volatile uint8_t micromag_data_available;
+volatile uint8_t micromag_status;
 volatile int16_t micromag_values[MM_NB_AXIS];
 
 void micromag_init( void ) {
@@ -10,5 +10,5 @@ void micromag_init( void ) {
   uint8_t i;
   for (i=0; i<MM_NB_AXIS; i++)
     micromag_values[i] = 0;
-  micromag_data_available = FALSE;
+  micromag_status = MM_IDLE;
 }
diff --git a/sw/airborne/micromag.h b/sw/airborne/micromag.h
index 929d341c42..820b0259ad 100644
--- a/sw/airborne/micromag.h
+++ b/sw/airborne/micromag.h
@@ -8,7 +8,20 @@
 extern void micromag_init( void );
 extern void micromag_read( void );
 
-extern volatile uint8_t micromag_data_available;
+#define MM_IDLE            0
+#define MM_BUSY            1
+#define MM_SENDING_REQ_X   1
+#define MM_SENDING_WAIT_X  2
+#define MM_READING_X       3
+#define MM_SENDING_REQ_Y   4
+#define MM_SENDING_WAIT_Y  5
+#define MM_READING_Y       6
+#define MM_SENDING_REQ_Z   7
+#define MM_SENDING_WAIT_Z  8
+#define MM_READING_Z       9
+#define MM_DATA_AVAILABLE 10
+
+extern volatile uint8_t micromag_status;
 extern volatile int16_t micromag_values[MM_NB_AXIS];
 
 extern void micromag_hw_init( void );
diff --git a/sw/airborne/tl_control.c b/sw/airborne/tl_control.c
index 77109683e9..78d60009a4 100644
--- a/sw/airborne/tl_control.c
+++ b/sw/airborne/tl_control.c
@@ -28,6 +28,7 @@ pprz_t tl_control_commands[COMMANDS_NB];
 float tl_control_attitude_phi_sp;
 float tl_control_attitude_theta_sp;
 float tl_control_attitude_psi_sp;
+
 /* float tl_control_attitude_phi_theta_pgain; */
 /* float tl_control_attitude_phi_theta_dgain; */
 /* float tl_control_attitude_psi_pgain; */
@@ -41,7 +42,7 @@ float tl_control_attitude_psi_sp;
 
 /* setpoints for max stick throw in degres */
 /* #define TL_CONTROL_ATTITUDE_PHI_THETA_MAX_SP 30. */
-/* #define TL_CONTROL_ATTITUDE_PSI_MAX_SP 45. */
+ #define TL_CONTROL_ATTITUDE_PSI_MAX_SP 45.
 
 
 void tl_control_init(void) {
@@ -72,8 +73,8 @@ void tl_control_rate_read_setpoints_from_rc(void) {
 
   tl_control_p_sp = -rc_values[RADIO_ROLL];
   tl_control_q_sp =  rc_values[RADIO_PITCH];
-  //  tl_control_r_sp = -rc_values[RADIO_YAW] * RadOfDeg(TL_CONTROL_RATE_R_MAX_SP)/MAX_PPRZ;
-  tl_control_r_sp = -rc_values[RADIO_YAW];
+  tl_control_r_sp = -rc_values[RADIO_YAW] * RadOfDeg(TL_CONTROL_RATE_R_MAX_SP)/MAX_PPRZ;
+  //tl_control_r_sp = -rc_values[RADIO_YAW];
   tl_control_power_sp = rc_values[RADIO_THROTTLE];
 
 }
@@ -84,21 +85,28 @@ void tl_control_rate_run(void) {
   const float cmd_p = tl_control_p_sp;
   const float cmd_q = tl_control_q_sp;
 
-  //  const float rate_err_r = tl_estimator_uf_r - tl_control_r_sp;
-  //  const float rate_d_err_r = rate_err_r - tl_control_rate_last_err_r;
-  //  tl_control_rate_last_err_r = rate_err_r;
-  //  const float cmd_r = tl_control_rate_r_pgain * ( rate_err_r + tl_control_rate_r_dgain * rate_d_err_r );
-  const float cmd_r = tl_control_r_sp;
+  const float rate_err_r = estimator_r - tl_control_r_sp;
+  const float rate_d_err_r = rate_err_r - tl_control_rate_last_err_r;
+  tl_control_rate_last_err_r = rate_err_r;
+  const float cmd_r = - tl_control_rate_r_pgain * ( rate_err_r + tl_control_rate_r_dgain * rate_d_err_r );
 
   tl_control_commands[COMMAND_ROLL]     = TRIM_PPRZ((int16_t)cmd_p);
   tl_control_commands[COMMAND_PITCH]    = TRIM_PPRZ((int16_t)cmd_q);
   tl_control_commands[COMMAND_YAW]      = TRIM_PPRZ((int16_t)cmd_r);
-  tl_control_commands[COMMAND_THROTTLE] = kill_throttle ? 0 : TRIM_PPRZ((int16_t) (tl_control_power_sp));
+  //  tl_control_commands[COMMAND_THROTTLE] = kill_throttle ? 0 : TRIM_PPRZ((int16_t) (tl_control_power_sp));
+  tl_control_commands[COMMAND_THROTTLE] = TRIM_PPRZ((int16_t) (tl_control_power_sp));
 
 }
 
 void tl_control_attitude_read_setpoints_from_rc(void) {
- 
+#if 0
+  if (!estimator_in_flight)
+    tl_control_attitude_psi_sp = estimator_psi;
+  else {
+    tl_control_attitude_psi_sp += rc_values[RADIO_YAW] * RadOfDeg(TL_CONTROL_ATTITUDE_PSI_MAX_SP)/MAX_PPRZ;
+    NormRadAngle(tl_control_attitude_psi_sp);
+  }
+#endif
 }
 
 void tl_control_attitude_run(void) {
diff --git a/sw/airborne/tl_estimator.c b/sw/airborne/tl_estimator.c
index 46fc4f7342..e275d790d0 100644
--- a/sw/airborne/tl_estimator.c
+++ b/sw/airborne/tl_estimator.c
@@ -1,5 +1,6 @@
 #include "tl_estimator.h"
 #include "gps.h"
+#include "tl_imu.h"
 #include "flight_plan.h"
 
 bool_t estimator_in_flight;
@@ -16,7 +17,9 @@ float estimator_speed; /* m/s */
 float estimator_climb; /* m/s */
 float estimator_course; /* rad, CCW */
 
+float estimator_r;
 float estimator_psi; /* rad, CCW */ 
+float estimator_z_baro;
 
 void tl_estimator_init(void) {
   tl_estimator_u = 0.;
@@ -37,6 +40,17 @@ void tl_estimator_use_gps(void) {
   estimator_course = RadOfDeg(gps_course / 10.);
 }
 
+void tl_estimator_use_gyro(void) {
+  estimator_r = tl_imu_r;
+}
+
+void tl_estimator_use_mag(void) {
+  estimator_psi = -atan2(tl_imu_hy, tl_imu_hx);
+  /* FIXME */
+  EstimatorSetAlt(tl_imu_z_baro);
+
+}
+
 void tl_estimator_to_body_frame(float east, float north,
 				float *front, float *right) {
   float c = cos(estimator_psi);
@@ -45,3 +59,4 @@ void tl_estimator_to_body_frame(float east, float north,
   *front = c * north + s * east;
   *right = - s * north + c * east;
 }
+
diff --git a/sw/airborne/tl_estimator.h b/sw/airborne/tl_estimator.h
index 7e73b31b51..f1dfbbd616 100644
--- a/sw/airborne/tl_estimator.h
+++ b/sw/airborne/tl_estimator.h
@@ -17,11 +17,17 @@ extern float estimator_speed; /* m/s */
 extern float estimator_climb; /* m/s */
 extern float estimator_course; /* rad, CCW */
 
-extern float estimator_psi; /* rad, CCW */ 
-
+extern float estimator_r;
+extern float estimator_psi;  /* rad, CCW */ 
+extern float estimator_z_baro;
 
 void tl_estimator_init(void);
 void tl_estimator_use_gps(void);
+void tl_estimator_use_gyro(void);
+void tl_estimator_use_mag(void);
+
+
+#define EstimatorSetAlt(_z) {estimator_z_baro = _z;}
 
 void tl_estimator_to_body_frame(float east, float north, float *front, float *right);
 
diff --git a/sw/airborne/tl_main.c b/sw/airborne/tl_main.c
index 07e2dd77f3..3d57346386 100644
--- a/sw/airborne/tl_main.c
+++ b/sw/airborne/tl_main.c
@@ -14,6 +14,7 @@
 #include "tl_telemetry.h"
 #include "datalink.h"
 #include "tl_autopilot.h"
+#include "tl_imu.h"
 #include "tl_estimator.h"
 #include "adc.h"
 #include "tl_bat.h"
@@ -49,8 +50,12 @@ static inline void tl_main_init( void ) {
   ppm_init();
   radio_control_init();
 
+  tl_imu_init();
+
   tl_estimator_init();
 
+  tl_control_init();
+
   uart0_init_tx();
   uart1_init_tx();
 
@@ -60,19 +65,23 @@ static inline void tl_main_init( void ) {
 }
 
 
-static inline void tl_main_periodic_task( void ) {  
+static inline void tl_main_periodic_task( void ) {
+
   tl_bat_periodic_task();
 
+  TlImuPeriodic();
+  tl_estimator_use_gyro();
+  
+  tl_autopilot_periodic_task();
+
   radio_control_periodic_task();
+
   if (rc_status != RC_OK)
      tl_autopilot_mode = TL_AP_MODE_FAILSAFE;
 
   /* 4Hz */
   RunOnceEvery(15, { common_nav_periodic_task_4Hz(); tl_nav_periodic_task(); });
   
-  /* run control loops */
-  tl_autopilot_periodic_task();
-
   tl_telemetry_periodic_task();
 
   SetActuatorsFromCommands(commands);
@@ -83,5 +92,8 @@ static inline void tl_main_event_task( void ) {
   RadioControlEventCheckAndHandle(tl_autopilot_on_rc_event);
 
   GpsEventCheckAndHandle(tl_estimator_use_gps, !estimator_in_flight);
+
   DlEventCheckAndHandle();
+
+  TlImuEventCheckAndHandle(tl_estimator_use_mag);
 }
diff --git a/sw/airborne/tl_telemetry.h b/sw/airborne/tl_telemetry.h
index a2209414a5..b8439ad6ce 100644
--- a/sw/airborne/tl_telemetry.h
+++ b/sw/airborne/tl_telemetry.h
@@ -10,11 +10,10 @@
 #include "actuators.h"
 #include "tl_bat.h"
 #include "tl_estimator.h"
+#include "tl_imu.h"
 #include "tl_nav.h"
 #include "tl_control.h"
 
-
-
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
 
 #define PERIODIC_SEND_ALIVE() DOWNLINK_SEND_ALIVE()
@@ -41,7 +40,20 @@
 #define PERIODIC_SEND_NAVIGATION() SEND_NAVIGATION()
 
 
+#define PERIODIC_SEND_IMU_GYRO() DOWNLINK_SEND_IMU_GYRO(&tl_imu_r, &tl_imu_r, &tl_imu_r)
+#define PERIODIC_SEND_IMU_MAG() DOWNLINK_SEND_IMU_MAG(&tl_imu_hx, &tl_imu_hy, &tl_imu_hz)
 
+#define PERIODIC_SEND_TL_ESTIMATOR() DOWNLINK_SEND_TL_ESTIMATOR(&estimator_r, &estimator_psi, &estimator_z_baro)
+
+#define PERIODIC_SEND_RATE_LOOP() DOWNLINK_SEND_BOOZ_RATE_LOOP(&estimator_r, &tl_control_r_sp, &estimator_r, &tl_control_r_sp, &estimator_r, &tl_control_r_sp)
+
+#if 0
+#define PERIODIC_SEND_TL_ESTIMATOR() {				\
+    float d1 = tl_baro_d[0];						\
+    float d2 = tl_baro_d[1];						\
+    DOWNLINK_SEND_TL_ESTIMATOR(&estimator_z_baro, &d1, &d2);		\
+  }
+#endif
 
 extern uint8_t telemetry_mode_Ap;