diff --git a/conf/airframes/TU_Delft/yapa_xsens.xml b/conf/airframes/TU_Delft/yapa_xsens.xml
index d9e489ef66..7d1b565799 100644
--- a/conf/airframes/TU_Delft/yapa_xsens.xml
+++ b/conf/airframes/TU_Delft/yapa_xsens.xml
@@ -42,16 +42,21 @@
     <define name="BRAKE_PITCH" value="0.0f"/>
     <define name="MAX_BRAKE_RATE" value="150"/>
 
-    <define name="LIMIT(X,XL,XH)" value="(    ((X)>(XH))   ?    (XH)    :    ( ((X)>(XL)) ? (X) : (XL) )    )"/>
-  </section>
+    <define name="RATELIMIT(X,RATE)" value="( _rate_limited_value + Chop( (X) - _rate_limited_value, -(RATE), (RATE)  )); \
+       int16_t _rate_limited_value = 0;"/>
+
+</section>
 
   <command_laws>
     <!-- Brake Rate Limiter -->
-    <let var="brake_value_nofilt" value="LIMIT(-@BRAKE, 0, MAX_PPRZ)"/>
+    <let var="brake_value" value="Chop(-@BRAKE, 0, MAX_PPRZ)"/>
+    <!--<let var="brake_value" value="RATELIMIT(  $brake_value , MAX_BRAKE_RATE )"/>
+    
     <let var="test; \
   	      static int16_t _var_brake_value = 0; \
               _var_brake_value += LIMIT(_var_brake_value_nofilt - _var_brake_value,-MAX_BRAKE_RATE,MAX_BRAKE_RATE); \
               int verwaarloos_deze_warning_CDW" value="0"/>
+		-->
 
     <!-- Differential Aileron Depending on Brake Value -->
     <let var="aileron_up_rate" value="(AILERON_RATE_UP   * (MAX_PPRZ - $brake_value)) + (AILERON_RATE_UP_BRAKE   * $brake_value)"/>
@@ -83,6 +88,11 @@
     <define name="CATASTROPHIC_BAT_LEVEL" value="9.1" unit="V"/>
   </section>
 
+  <section name="INS" prefix="INS_">
+    <define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+    <define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+  </section>
+
   <section name="MISC">
     <define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
     <define name="CARROT" value="5." unit="s"/>
@@ -185,6 +195,7 @@
   </modules>
 
   <firmware name="fixedwing">
+
     <target name="ap" board="tiny_2.11">
       <define name="STRONG_WIND"/>
       <define name="WIND_INFO"/>
diff --git a/conf/modules/ins_xsens_MTiG_fixedwing.xml b/conf/modules/ins_xsens_MTiG_fixedwing.xml
index 27e67847cd..4990c68b8c 100644
--- a/conf/modules/ins_xsens_MTiG_fixedwing.xml
+++ b/conf/modules/ins_xsens_MTiG_fixedwing.xml
@@ -10,6 +10,7 @@
   <periodic fun="ins_periodic_task()" freq="60"/>
   <event fun="InsEventCheckAndHandle(handle_ins_msg())"/>
   <makefile>
+    <define name="AHRS_TYPE_H" value="\\\"modules/ins/ins_xsens.h\\\"" />
     <define name="USE_UART$(XSENS_UART_NR)"/>
     <define name="INS_LINK" value="Uart$(XSENS_UART_NR)"/>
     <define name="UART$(XSENS_UART_NR)_BAUD" value="B115200"/>
diff --git a/conf/settings/basic.xml b/conf/settings/basic.xml
index 35991b8d98..64147a71a1 100644
--- a/conf/settings/basic.xml
+++ b/conf/settings/basic.xml
@@ -25,7 +25,7 @@
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle">
       </dl_setting>
       <dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="downlink"/>
-      <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps_reset" module="gps_ubx" handler="Reset" shortname="GPS reset"/>
+      <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps.reset" module="subsystems/gps" handler="Reset" shortname="GPS reset"/>
     </dl_settings>
 
     <dl_settings NAME="Infrared">
diff --git a/conf/settings/lisa.xml b/conf/settings/lisa.xml
index 9facc3d61b..f717d6034f 100644
--- a/conf/settings/lisa.xml
+++ b/conf/settings/lisa.xml
@@ -25,7 +25,7 @@
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle">
       </dl_setting>
       <dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="downlink"/>
-      <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps_reset" module="gps_ubx" handler="Reset" shortname="GPS reset"/>
+      <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps.reset" module="subsystems/gps" handler="Reset" shortname="GPS reset"/>
     </dl_settings>
 
   </dl_settings>
diff --git a/conf/settings/tuning.xml b/conf/settings/tuning.xml
index 00e062b8b4..22ac1e1d7c 100644
--- a/conf/settings/tuning.xml
+++ b/conf/settings/tuning.xml
@@ -19,7 +19,7 @@
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle"/>
       <dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="downlink"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="downlink"/>
-      <!--dl_setting MAX="2" MIN="0" STEP="1" VAR="gps_reset" module="gps_ubx" handler="Reset" shortname="GPS reset"/-->
+      <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps.reset" module="subsystems/gps" handler="Reset" shortname="GPS reset"/>
 
       <dl_setting MAX="200" MIN="-200" STEP="10" VAR="nav_radius" module="subsystems/nav" handler="SetNavRadius">
         <strip_button icon="circle-right.png" name="Circle right" value="1"/>
diff --git a/conf/settings/tuningJH.xml b/conf/settings/tuningJH.xml
index f7ccad596f..911ed87aa4 100644
--- a/conf/settings/tuningJH.xml
+++ b/conf/settings/tuningJH.xml
@@ -18,7 +18,7 @@
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle">
       </dl_setting>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Ap" shortname="tele_AP"/>
-      <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps_reset" module="gps_ubx" handler="Reset" shortname="GPS reset"/>
+      <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps.reset" module="subsystems/gps" handler="Reset" shortname="GPS reset"/>
     </dl_settings>
 
     <dl_settings NAME="control">
diff --git a/conf/settings/tuning_basic_ins.xml b/conf/settings/tuning_basic_ins.xml
index b052c9be53..0d84278f26 100644
--- a/conf/settings/tuning_basic_ins.xml
+++ b/conf/settings/tuning_basic_ins.xml
@@ -19,7 +19,7 @@
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle"/>
       <dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="downlink"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="downlink"/>
-      <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps_reset" module="gps_ubx" handler="Reset" shortname="GPS reset"/>
+      <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps.reset" module="subsystems/gps" handler="Reset" shortname="GPS reset"/>
 
       <dl_setting MAX="200" MIN="-200" STEP="10" VAR="nav_radius" module="subsystems/nav" handler="SetNavRadius">
         <strip_button icon="circle-right.png" name="Circle right" value="1"/>
diff --git a/conf/settings/tuning_ctl_new.xml b/conf/settings/tuning_ctl_new.xml
index 06caf025ac..beafad6a5e 100644
--- a/conf/settings/tuning_ctl_new.xml
+++ b/conf/settings/tuning_ctl_new.xml
@@ -19,7 +19,7 @@
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle"/>
       <dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="downlink"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="downlink"/>
-      <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps_reset" module="gps_ubx" handler="Reset" shortname="GPS reset"/>
+      <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps.reset" module="subsystems/gps" handler="Reset" shortname="GPS reset"/>
 
       <dl_setting MAX="200" MIN="-200" STEP="10" VAR="nav_radius" module="subsystems/nav" handler="SetNavRadius">
         <strip_button icon="circle-right.png" name="Circle right" value="1" group="circle"/>
diff --git a/conf/settings/tuning_ins.xml b/conf/settings/tuning_ins.xml
index 08250f98b9..2cf8fe880b 100644
--- a/conf/settings/tuning_ins.xml
+++ b/conf/settings/tuning_ins.xml
@@ -19,7 +19,7 @@
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle"/>
       <dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="downlink"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="downlink"/>
-      <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps_reset" module="gps_ubx" handler="Reset" shortname="GPS reset"/>
+      <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps.reset" module="subsystems/gps" handler="Reset" shortname="GPS reset"/>
 
       <dl_setting MAX="200" MIN="-200" STEP="10" VAR="nav_radius" module="subsystems/nav" handler="SetNavRadius">
         <strip_button icon="circle-right.png" name="Circle right" value="1"/>
diff --git a/conf/settings/tuning_loiter.xml b/conf/settings/tuning_loiter.xml
index b2cd6984f4..eb6a88c2fe 100644
--- a/conf/settings/tuning_loiter.xml
+++ b/conf/settings/tuning_loiter.xml
@@ -19,7 +19,7 @@
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle"/>
       <dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="downlink"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="downlink"/>
-      <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps_reset" module="gps_ubx" handler="Reset" shortname="GPS reset"/>
+      <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps.reset" module="subsystems/gps" handler="Reset" shortname="GPS reset"/>
 
       <dl_setting MAX="200" MIN="-200" STEP="10" VAR="nav_radius" module="subsystems/nav" handler="SetNavRadius">
         <strip_button icon="circle-right.png" name="Circle right" value="1"/>
diff --git a/conf/settings/tuning_tp_auto.xml b/conf/settings/tuning_tp_auto.xml
index ba8e605fe3..474c6bd9d7 100644
--- a/conf/settings/tuning_tp_auto.xml
+++ b/conf/settings/tuning_tp_auto.xml
@@ -18,7 +18,7 @@
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle">
       </dl_setting>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Ap" shortname="tele_AP"/>
-      <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps_reset" module="gps_ubx" handler="Reset" shortname="GPS reset"/>
+      <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps.reset" module="subsystems/gps" handler="Reset" shortname="GPS reset"/>
     </dl_settings>
 
     <dl_settings NAME="control">
diff --git a/conf/settings/ugv.xml b/conf/settings/ugv.xml
index 71eba1ade6..623d46e027 100644
--- a/conf/settings/ugv.xml
+++ b/conf/settings/ugv.xml
@@ -17,7 +17,7 @@
       </dl_setting>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle">
       </dl_setting>
-      <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps_reset" module="gps_ubx" handler="Reset" shortname="GPS reset"/>
+      <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps_reset" module="subsystems/gps" handler="Reset" shortname="GPS reset"/>
     </dl_settings>
   </dl_settings>
 </settings>
diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
index f20f690852..adb9d0513e 100644
--- a/sw/airborne/modules/ins/ins_xsens.c
+++ b/sw/airborne/modules/ins/ins_xsens.c
@@ -65,6 +65,14 @@ INS_FORMAT ins_mx;
 INS_FORMAT ins_my;
 INS_FORMAT ins_mz;
 
+float ins_pitch_neutral;
+float ins_roll_neutral;
+
+//////////////////////////////////////////////////////////////////////////////////////////
+//
+//	XSens Specific
+//
+
 volatile uint8_t ins_msg_received;
 
 #define XsensInitCheksum() { send_ck = 0; }
@@ -186,6 +194,9 @@ void ins_init( void ) {
 
   xsens_status = UNINIT;
 
+  ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
+  ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
+
   xsens_msg_status = 0;
   xsens_time_stamp = 0;
   xsens_output_mode = XSENS_OUTPUT_MODE;
diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h
index 446faabad8..96248cef20 100644
--- a/sw/airborne/modules/ins/ins_xsens.h
+++ b/sw/airborne/modules/ins/ins_xsens.h
@@ -31,6 +31,9 @@
 
 #include "std.h"
 
+extern float ins_pitch_neutral;
+extern float ins_roll_neutral;
+
 extern int8_t xsens_hour;
 extern int8_t xsens_min;
 extern int8_t xsens_sec;
diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h
index edee3f73fe..ace5586b3d 100644
--- a/sw/airborne/subsystems/gps.h
+++ b/sw/airborne/subsystems/gps.h
@@ -80,6 +80,7 @@ struct GpsState {
 
   uint8_t  lost_counter;         /* updated at 4Hz        */
   uint16_t last_msg_time;
+  uint16_t reset;                ///< hotstart, warmstart, coldstart
 };
 
 struct GpsTimeSync {
@@ -109,6 +110,21 @@ static inline void gps_periodic( void ) {
 #define GpsIsLost() (gps.lost_counter > 20) /* 4Hz -> 5s */
 
 
+//TODO
+// this still needs to call gps specific stuff
+
+/*
+ * GPS Reset
+ */
+
+#define CFG_RST_BBR_Hotstart  0x0000
+#define CFG_RST_BBR_Warmstart 0x0001
+#define CFG_RST_BBR_Coldstart 0xffff
+
+#define gps_Reset(_val) {                               \
+}
+
+
 
 #ifdef GPS_TIMESTAMP
 #ifndef PCLK
diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h
index 45e0392b86..321bd24810 100644
--- a/sw/airborne/subsystems/gps/gps_ubx.h
+++ b/sw/airborne/subsystems/gps/gps_ubx.h
@@ -42,7 +42,6 @@ struct GpsUbx {
   uint8_t msg_class;
 
   uint8_t status;
-  uint16_t reset;
   uint16_t len;
   uint8_t msg_idx;
   uint8_t ck_a, ck_b;
@@ -107,9 +106,6 @@ extern void gps_ubx_parse(uint8_t c);
 /*
  * GPS Reset
  */
-#define CFG_RST_BBR_Hotstart  0x0000
-#define CFG_RST_BBR_Warmstart 0x0001
-#define CFG_RST_BBR_Coldstart 0xffff
 
 #define CFG_RST_Reset_Hardware 0x00
 #define CFG_RST_Reset_Controlled 0x01