diff -rubN paparazzi/sw/airborne/ap_downlink.h paparazzi.xsense/sw/airborne/ap_downlink.h
--- paparazzi/sw/airborne/ap_downlink.h	2009-03-10 14:13:24.000000000 +0300
+++ paparazzi.xsense/sw/airborne/ap_downlink.h	2009-04-27 14:17:30.000000000 +0400
@@ -168,6 +168,11 @@
 #define PERIODIC_SEND_DebugChao() DOWNLINK_SEND_DebugChao(&ugear_debug1, &ugear_debug2, &ugear_debug3, &ugear_debug4, &ugear_debug5, &ugear_debug6)
 #endif
 
+#if defined XSENS
+#define PERIODIC_SEND_GPS() DOWNLINK_SEND_GPS(&gps_mode, &gps_utm_east, &gps_utm_north, &gps_course, &gps_alt, &gps_gspeed,&gps_climb, &gps_itow, &gps_utm_zone, &gps_nb_ovrn)
+#define PERIODIC_SEND_GPS_SOL() DOWNLINK_SEND_GPS_SOL(&gps_Pacc, &gps_Sacc, &gps_PDOP, &gps_numSV)
+#endif
+
 #ifdef USE_BARO_MS5534A
 #include "baro_MS5534A.h"
 #define PERIODIC_SEND_BARO_MS5534A() DOWNLINK_SEND_BARO_MS5534A(&baro_MS5534A_pressure, &baro_MS5534A_temp, &baro_MS5534A_z)
diff -rubN paparazzi/sw/airborne/ins_xsens.c paparazzi.xsense/sw/airborne/ins_xsens.c
--- paparazzi/sw/airborne/ins_xsens.c	2009-03-10 14:13:24.000000000 +0300
+++ paparazzi.xsense/sw/airborne/ins_xsens.c	2009-04-27 14:41:28.000000000 +0400
@@ -272,6 +272,7 @@
         gps_alt = ins_z * 100;
 #endif
         offset += XSENS_DATA_Position_LENGTH;
+        gps_pos_available = TRUE;
       }
       if (XSENS_MASK_Velocity(xsens_output_mode)) {
         ins_vx = XSENS_DATA_Velocity_vx(xsens_msg_buf,offset);
diff -rubN paparazzi/sw/airborne/main_ap.c paparazzi.xsense/sw/airborne/main_ap.c
--- paparazzi/sw/airborne/main_ap.c	2009-03-16 12:20:34.000000000 +0300
+++ paparazzi.xsense/sw/airborne/main_ap.c	2009-04-27 14:37:31.000000000 +0400
@@ -130,7 +130,7 @@
 #endif 
 
 #ifdef XSENS 
-#include "xsens_ins.h"
+#include "ins.h"
 #endif
 /*code added by Haiyang Chao ends*/
 
@@ -170,6 +170,10 @@
 static uint8_t  mcu1_ppm_cpt;
 #endif
 
+#ifndef INFRARED
+#define PERIODIC_SEND_IR_SENSORS()
+#endif
+
 bool_t kill_throttle = FALSE;
 
 float slider_1_val, slider_2_val;
@@ -786,6 +790,29 @@
 	}
 #endif /* UGEAR*/
 
+#ifdef XSENS
+        if (InsBuffer()) {
+                ReadInsBuffer();
+        }
+
+        if (ins_msg_received) {
+                parse_ins_msg();
+                ins_msg_received = FALSE;
+
+                if (gps_pos_available){
+                        gps_verbose_downlink = !launch;
+                        if (gps_mode == 3) {
+                                last_gps_msg_t = cpu_time_sec;
+                                estimator_update_state_gps();
+                        }
+                        gps_downlink();
+                        gps_pos_available = FALSE;
+		}
+
+                estimator_phi   = (float)ins_phi;
+  		estimator_theta = (float)ins_theta;
+        }
+#else
 #ifdef GPS
 #ifndef HITL /** else comes through the datalink */
   if (GpsBuffer()) {
@@ -808,6 +835,7 @@
     }
   }
 #endif /** GPS */
+#endif
 
 #if defined DATALINK