diff --git a/conf/airframes/tudelft/disco.xml b/conf/airframes/tudelft/disco.xml
index dd3f57cae1..a6533f37af 100644
--- a/conf/airframes/tudelft/disco.xml
+++ b/conf/airframes/tudelft/disco.xml
@@ -8,16 +8,24 @@
 
   <firmware name="fixedwing">
     <configure name="USE_MAGNETOMETER" value="TRUE"/>
+    <define name="USE_BAROMETER" value="TRUE"/>
 
     <target name="ap" board="disco">
       <configure name="PERIODIC_FREQUENCY" value="120"/>
       <module name="radio_control" type="sbus"/>
+      <configure name="MS5611_I2C_DEV" value="i2c1" />
     </target>
     <target name="sim" board="pc">
       <configure name="SYS_TIME_FREQUENCY" value="120"/>
       <configure name="PERIODIC_FREQUENCY" value="60"/>
       <module name="radio_control" type="ppm"/>
     </target>
+    <target name="nps" board="pc">
+      <!-- <configure name="SYS_TIME_FREQUENCY" value="120"/> -->
+      <configure name="PERIODIC_FREQUENCY" value="60"/>
+      <module name="radio_control" type="ppm"/>
+      <module name="fdm" type="jsbsim"/>
+    </target>
 
     <module name="telemetry" type="transparent_udp"/>
 
@@ -26,6 +34,9 @@
     <module name="gps" type="ublox"/>
     <module name="gps" type="ubx_ucenter"/>
     <module name="ins" type="alt_float"/>
+    <module name="baro" type="ms5611_i2c">
+      <define name="BB_MS5611_TYPE_MS5607" value="TRUE" />
+    </module>
     <module name="airspeed" type="ms45xx_i2c">
       <configure name="MS45XX_I2C_DEV" value="i2c1"/>
       <define name="MS45XX_PRESSURE_RANVE" value="0.05"/>
@@ -126,6 +137,12 @@
     <define name="APP_ANGLE" value="8" unit="deg"/>
   </section>
 
+
+
+  <section name="AIR_DATA" prefix="AIR_DATA_">
+    <define name="CALC_AMSL_BARO" value="TRUE"/>
+  </section>
+
   <section name="VERTICAL CONTROL" prefix="V_CTL_">
     <!-- outer loop proportional gain -->
     <define name="ALTITUDE_PGAIN" value="0.12"/>
@@ -184,15 +201,15 @@
     <define name="PITCH_MAX_SETPOINT" value="30." unit="deg"/>
     <define name="PITCH_MIN_SETPOINT" value="-30." unit="deg"/>
 
-    <define name="ROLL_ATTITUDE_GAIN" value="11000."/>
-    <define name="ROLL_RATE_GAIN" value="1000."/>
+    <define name="ROLL_ATTITUDE_GAIN" value="6000."/>
+    <define name="ROLL_RATE_GAIN" value="500."/>
     <define name="ROLL_IGAIN" value="100."/>
     <define name="ROLL_KFFA" value="0"/>
     <define name="ROLL_KFFD" value="0"/>
 
-    <define name="PITCH_PGAIN" value="17250"/>
+    <define name="PITCH_PGAIN" value="14250"/>
     <define name="PITCH_DGAIN" value="500."/>
-    <define name="PITCH_IGAIN" value="400"/>
+    <define name="PITCH_IGAIN" value="300"/>
     <define name="PITCH_KFFA" value="0."/>
     <define name="PITCH_KFFD" value="0."/>
 
diff --git a/sw/airborne/modules/ahrs/ahrs_float_dcm.c b/sw/airborne/modules/ahrs/ahrs_float_dcm.c
index a866c4712e..c23f2a9d10 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_dcm.c
+++ b/sw/airborne/modules/ahrs/ahrs_float_dcm.c
@@ -228,7 +228,6 @@ void ahrs_dcm_update_accel(struct FloatVect3 *accel)
 void ahrs_dcm_update_mag(struct FloatVect3 *mag)
 {
 #if USE_MAGNETOMETER
-MESSAGE("MAGNETOMETER FEEDBACK NOT TESTED YET")
 
   float cos_roll;
   float sin_roll;