diff --git a/files/flightgear/Protocol/qgroundcontrol-fixed-wing.xml b/files/flightgear/Protocol/qgroundcontrol-fixed-wing.xml
index 94a60ded590e9c6d832220ba3c9639265702c04c..5fe0ad2b0c7832f49055f15962e6837604fe040f 100644
--- a/files/flightgear/Protocol/qgroundcontrol-fixed-wing.xml
+++ b/files/flightgear/Protocol/qgroundcontrol-fixed-wing.xml
@@ -141,6 +141,49 @@
      <node>/velocities/airspeed-kt</node>
      <factor>0.514444444444444</factor>      <!-- knots to mps -->
    </chunk>
+
+   <chunk>
+     <name>airspeed-indicated-mps</name>
+     <type>float</type>
+     <format>%.8f</format>
+     <node>/instrumentation/indicated-speed-kt</node>
+     <factor>0.514444444444444</factor>      <!-- knots to mps -->
+   </chunk>
+
+   <!-- Magnetometer -->
+   <chunk>
+    <name>Magnetic Variation (rad)</name>
+    <type>float</type>
+    <format>%.8f</format>
+    <node>/environment/magnetic-variation-deg</node>
+    <factor>0.01745329251994329576</factor>  <!-- degrees to radians -->
+   </chunk>
+
+   <chunk>
+    <name>Magnetic Dip (rad)</name>
+    <type>float</type>
+    <format>%.8f</format>
+    <node>/environment/magnetic-dip-deg</node>
+    <factor>0.01745329251994329576</factor>  <!-- degrees to radians -->
+   </chunk>
+
+   <!-- Temperature -->
+   <chunk>
+    <name>Temperature (deg C)</name>
+    <type>float</type>
+    <format>%.8f</format>
+    <node>/environment/temperature-degc</node>
+    <factor>1</factor>
+   </chunk>
+
+   <!-- Pressure -->
+   <chunk>
+    <name>Pressure (hPa)</name>
+    <type>float</type>
+    <format>%.8f</format>
+    <node>/environment/pressure-inhg</node>
+    <factor>33.86389</factor>                  <!-- inhg to hpa -->
+   </chunk>
    
   </output>
 
@@ -184,4 +227,4 @@
 
  </generic>
 
-</PropertyList>
\ No newline at end of file
+</PropertyList>
diff --git a/src/comm/QGCFlightGearLink.cc b/src/comm/QGCFlightGearLink.cc
index 61341b048839ba72b215c3a52f32b31a3cf27ddb..d8bc240ee37a962a29466bd2c310abf368004dd0 100644
--- a/src/comm/QGCFlightGearLink.cc
+++ b/src/comm/QGCFlightGearLink.cc
@@ -26,6 +26,7 @@ This file is part of the QGROUNDCONTROL project
  *   @brief Definition of UDP connection (server) for unmanned vehicles
  *   @see Flightgear Manual http://mapserver.flightgear.org/getstart.pdf
  *   @author Lorenz Meier <mavteam@student.ethz.ch>
+ *   @author Thomas Gubler <thomasgubler@student.ethz.ch>
  *
  */
 
@@ -44,7 +45,8 @@ QGCFlightGearLink::QGCFlightGearLink(UASInterface* mav, QString startupArguments
     process(NULL),
     terraSync(NULL),
     flightGearVersion(0),
-    startupArguments(startupArguments)
+    startupArguments(startupArguments),
+    _sensorHilEnabled(true)
 {
     this->host = host;
     this->port = port+mav->getUASID();
@@ -172,6 +174,7 @@ void QGCFlightGearLink::updateControls(uint64_t time, float rollAilerons, float
         QString state("%1\t%2\t%3\t%4\t%5\n");
         state = state.arg(rollAilerons).arg(pitchElevator).arg(yawRudder).arg(true).arg(throttle);
         writeBytes(state.toAscii().constData(), state.length());
+//        qDebug() << "updated controls" << rollAilerons << pitchElevator << yawRudder << throttle;
     }
     else
     {
@@ -227,49 +230,133 @@ void QGCFlightGearLink::readBytes()
 
     // Print string
     QString state(b);
-    //qDebug() << "FG LINK GOT:" << state;
+//    qDebug() << "FG LINK GOT:" << state;
 
     QStringList values = state.split("\t");
 
     // Check length
-    if (values.size() != 17)
+    if (values.size() != 22)
     {
-        qDebug() << "RETURN LENGTH MISMATCHING EXPECTED" << 17 << "BUT GOT" << values.size();
+        qDebug() << "RETURN LENGTH MISMATCHING EXPECTED" << 22 << "BUT GOT" << values.size();
         qDebug() << state;
         return;
     }
 
     // Parse string
     float roll, pitch, yaw, rollspeed, pitchspeed, yawspeed;
-    double lat, lon, alt;
+    double lat, lon, alt;   
+    float ind_airspeed;
+    float true_airspeed;
+    float vx, vy, vz, xacc, yacc, zacc;
+    float diff_pressure;
+    float temperature;
+    float abs_pressure;
+    float mag_variation, mag_dip, xmag_ned, ymag_ned, zmag_ned, xmag_body, ymag_body, zmag_body;
 
-    // XXX add
-    float ind_airspeed = 0.0f;
-    float true_airspeed = 0.0f;
-    double vx, vy, vz, xacc, yacc, zacc;
 
     lat = values.at(1).toDouble();
     lon = values.at(2).toDouble();
     alt = values.at(3).toDouble();
-    roll = values.at(4).toDouble();
-    pitch = values.at(5).toDouble();
-    yaw = values.at(6).toDouble();
-    rollspeed = values.at(7).toDouble();
-    pitchspeed = values.at(8).toDouble();
-    yawspeed = values.at(9).toDouble();
+    roll = values.at(4).toFloat();
+    pitch = values.at(5).toFloat();
+    yaw = values.at(6).toFloat();
+    rollspeed = values.at(7).toFloat();
+    pitchspeed = values.at(8).toFloat();
+    yawspeed = values.at(9).toFloat();
 
-    xacc = values.at(10).toDouble();
-    yacc = values.at(11).toDouble();
-    zacc = values.at(12).toDouble();
+    xacc = values.at(10).toFloat();
+    yacc = values.at(11).toFloat();
+    zacc = values.at(12).toFloat();
 
-    vx = values.at(13).toDouble();
-    vy = values.at(14).toDouble();
-    vz = values.at(15).toDouble();
+    vx = values.at(13).toFloat();
+    vy = values.at(14).toFloat();
+    vz = values.at(15).toFloat();
+
+    true_airspeed = values.at(16).toFloat();
+    ind_airspeed = values.at(17).toFloat();
+
+    mag_variation = values.at(18).toFloat();
+    mag_dip = values.at(19).toFloat();
+
+    temperature = values.at(20).toFloat();
+    abs_pressure = values.at(21).toFloat();
 
     // Send updated state
-    emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
+    //qDebug()  << "sensorHilEnabled: " << sensorHilEnabled;
+    if (_sensorHilEnabled)
+    {
+        quint16 fields_changed = 0xFFF; //set all 12 used bits
+
+        //calculate differential pressure
+        const float air_gas_constant = 287.1f; // J/(kg * K)
+        const float absolute_null_celsius = -273.15f; // °C
+        float density = abs_pressure / (air_gas_constant * (temperature - absolute_null_celsius));
+        diff_pressure = true_airspeed * true_airspeed * density / 2.0f;
+
+        float pressure_alt = alt;
+
+        xmag_ned = cosf(mag_variation);
+        ymag_ned = sinf(mag_variation);
+        zmag_ned = sinf(mag_dip);
+        float tempMagLength = sqrtf(xmag_ned*xmag_ned + ymag_ned*ymag_ned + zmag_ned*zmag_ned);
+        xmag_ned = xmag_ned / tempMagLength;
+        ymag_ned = ymag_ned / tempMagLength;
+        zmag_ned = zmag_ned / tempMagLength;
+
+        //transform magnetic measurement to body frame coordinates
+        double cosPhi = cos(roll);
+        double sinPhi = sin(roll);
+        double cosThe = cos(pitch);
+        double sinThe = sin(pitch);
+        double cosPsi = cos(yaw);
+        double sinPsi = sin(yaw);
+
+        float R_B_N[3][3];
+
+        R_B_N[0][0] = cosThe * cosPsi;
+        R_B_N[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
+        R_B_N[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
+
+		R_B_N[1][0] = cosThe * sinPsi;
+		R_B_N[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
+		R_B_N[1][2] = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
+
+		R_B_N[2][0] = -sinThe;
+		R_B_N[2][1] = sinPhi * cosThe;
+		R_B_N[2][2] = cosPhi * cosThe;
+
+        Eigen::Matrix3f R_B_N_M = Eigen::Map<Eigen::Matrix3f>((float*)R_B_N).eval();
+
+        Eigen::Vector3f mag_ned(xmag_ned, ymag_ned, zmag_ned);
+
+        Eigen::Vector3f mag_body = R_B_N_M * mag_ned;
+
+        xmag_body = mag_body(0);
+        ymag_body = mag_body(1);
+        zmag_body = mag_body(2);
+
+        emit sensorHilRawImuChanged(QGC::groundTimeUsecs(), xacc, yacc, zacc, rollspeed, pitchspeed, yawspeed,
+                                    xmag_body, ymag_body, zmag_body, abs_pressure, diff_pressure, pressure_alt, temperature, fields_changed);
+
+//        qDebug()  << "sensorHilRawImuChanged " << xacc  << yacc << zacc  << rollspeed << pitchspeed << yawspeed << xmag << ymag << zmag << abs_pressure << diff_pressure << pressure_alt << temperature;
+        int gps_fix_type = 3;
+        float eph = 0.3;
+        float epv = 0.6;
+        float vel = sqrt(vx*vx + vy*vy + vz*vz);
+        float cog = yaw;
+        int satellites = 8;
+
+        emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, vx, vy, vz, cog, satellites);
+
+//        qDebug()  << "sensorHilGpsChanged " << lat  << lon << alt  << vel;
+    } else {
+        emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
                          pitchspeed, yawspeed, lat, lon, alt,
-                         vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc);
+                         vx, vy, vz,
+                         ind_airspeed, true_airspeed,
+                         xacc, yacc, zacc);
+        //qDebug()  << "hilStateChanged " << (int32_t)lat << (int32_t)lon << (int32_t)alt;
+    }
 
     //    // Echo data for debugging purposes
     //    std::cerr << __FILE__ << __LINE__ << "Received datagram:" << std::endl;
@@ -303,7 +390,9 @@ bool QGCFlightGearLink::disconnectSimulation()
     disconnect(process, SIGNAL(error(QProcess::ProcessError)),
                this, SLOT(processError(QProcess::ProcessError)));
     disconnect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t)));
-    disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)));
+    disconnect(this, SIGNAL(hilStateChanged(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float)), mav, SLOT(sendHilState(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float)));
+    disconnect(this, SIGNAL(sensorHilGpsChanged(quint64, double, double, double, int, float, float, float, float, float, float, float, int)), mav, SLOT(sendHilGps(quint64, double, double, double, int, float, float, float, float, float, float, float, int)));
+    disconnect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)));
 
     if (process)
     {
@@ -350,8 +439,9 @@ bool QGCFlightGearLink::connectSimulation()
     terraSync = new QProcess(this);
 
     connect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t)));
-    connect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)));
-
+    connect(this, SIGNAL(hilStateChanged(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float)), mav, SLOT(sendHilState(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float)));
+    connect(this, SIGNAL(sensorHilGpsChanged(quint64, double, double, double, int, float, float, float, float, float, float, float, int)), mav, SLOT(sendHilGps(quint64, double, double, double, int, float, float, float, float, float, float, float, int)));
+    connect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)));
 
     UAS* uas = dynamic_cast<UAS*>(mav);
     if (uas)
diff --git a/src/comm/QGCFlightGearLink.h b/src/comm/QGCFlightGearLink.h
index 9bdda2e5d53c32b6aa635a321675f2c0a408afe2..d043d3c63ba9c9ab60b186b3c0a7a633fe82f747 100644
--- a/src/comm/QGCFlightGearLink.h
+++ b/src/comm/QGCFlightGearLink.h
@@ -83,6 +83,10 @@ public:
         return _sensorHilEnabled;
     }
 
+    void sensorHilEnabled(bool sensorHilEnabled) {
+        _sensorHilEnabled = sensorHilEnabled;
+    }
+
     void run();
 
 public slots:
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 1c727cd5bbc855c413b8c156ad00b89342972e1e..94d73f1edc6615f267852253216e457fce986ac6 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -2930,7 +2930,7 @@ bool UAS::emergencyKILL()
 /**
 * If enabled, connect the flight gear link.
 */
-void UAS::enableHilFlightGear(bool enable, QString options)
+void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil)
 {
     QGCFlightGearLink* link = dynamic_cast<QGCFlightGearLink*>(simulation);
     if (!link || !simulation) {
@@ -2944,6 +2944,7 @@ void UAS::enableHilFlightGear(bool enable, QString options)
     // Connect Flight Gear Link
     link = dynamic_cast<QGCFlightGearLink*>(simulation);
     link->setStartupArguments(options);
+    link->sensorHilEnabled(sensorHil);
     if (enable)
     {
         startHil();
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index 8a60d343ea7d0de0a6e46552f2ab69265216057b..e30946a67cfffbef2c173bbea7f9dfe651241ffa 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -730,7 +730,7 @@ public slots:
     void go();
 
     /** @brief Enable / disable HIL */
-    void enableHilFlightGear(bool enable, QString options);
+    void enableHilFlightGear(bool enable, QString options, bool sensorHil);
     void enableHilJSBSim(bool enable, QString options);
     void enableHilXPlane(bool enable);
 
diff --git a/src/ui/QGCHilConfiguration.cc b/src/ui/QGCHilConfiguration.cc
index 37467fb1378711a5229cda49248d367d7d340017..e501e8c6aaf7d16252fbd65b74e3c7599324953f 100644
--- a/src/ui/QGCHilConfiguration.cc
+++ b/src/ui/QGCHilConfiguration.cc
@@ -64,7 +64,7 @@ void QGCHilConfiguration::on_simComboBox_currentIndexChanged(int index)
     if(1 == index)
     {
         // Ensure the sim exists and is disabled
-        mav->enableHilFlightGear(false, "");
+        mav->enableHilFlightGear(false, "", true);
         QGCHilFlightGearConfiguration* hfgconf = new QGCHilFlightGearConfiguration(mav, this);
         hfgconf->show();
         ui->simulatorConfigurationLayout->addWidget(hfgconf);
diff --git a/src/ui/QGCHilFlightGearConfiguration.cc b/src/ui/QGCHilFlightGearConfiguration.cc
index 6a39a257e92c81343ae1741c1ba935aa8ca9affe..ddbccb7c7fa94edb6d6a0caa3dd08796e0f25e1a 100644
--- a/src/ui/QGCHilFlightGearConfiguration.cc
+++ b/src/ui/QGCHilFlightGearConfiguration.cc
@@ -40,7 +40,7 @@ void QGCHilFlightGearConfiguration::on_startButton_clicked()
     //XXX check validity of inputs
     QString options = ui->optionsPlainTextEdit->toPlainText();
     options.append(" --aircraft=" + ui->aircraftComboBox->currentText());
-    mav->enableHilFlightGear(true,  options);
+    mav->enableHilFlightGear(true,  options, ui->sensorHilCheckBox->isChecked());
 }
 
 void QGCHilFlightGearConfiguration::on_stopButton_clicked()
diff --git a/src/ui/QGCHilFlightGearConfiguration.ui b/src/ui/QGCHilFlightGearConfiguration.ui
index 2a4cb315b18d0b0ef530b02e9611c2f1a569694a..d06f174223349217bae9244cc3cabe096c03700b 100644
--- a/src/ui/QGCHilFlightGearConfiguration.ui
+++ b/src/ui/QGCHilFlightGearConfiguration.ui
@@ -25,8 +25,17 @@
   <property name="autoFillBackground">
    <bool>false</bool>
   </property>
-  <layout class="QGridLayout" name="gridLayout" rowminimumheight="0,0,0,0,0,0,0,0">
-   <property name="margin">
+  <layout class="QGridLayout" name="gridLayout">
+   <property name="leftMargin">
+    <number>0</number>
+   </property>
+   <property name="topMargin">
+    <number>0</number>
+   </property>
+   <property name="rightMargin">
+    <number>0</number>
+   </property>
+   <property name="bottomMargin">
     <number>0</number>
    </property>
    <property name="spacing">
@@ -52,14 +61,14 @@
      </property>
     </widget>
    </item>
-   <item row="2" column="0">
+   <item row="3" column="0">
     <widget class="QLabel" name="optionsLabel">
      <property name="text">
       <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Additional Options:&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
      </property>
     </widget>
    </item>
-   <item row="3" column="0" colspan="2">
+   <item row="4" column="0" colspan="2">
     <widget class="QPlainTextEdit" name="optionsPlainTextEdit">
      <property name="sizePolicy">
       <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
@@ -72,7 +81,7 @@
      </property>
     </widget>
    </item>
-   <item row="5" column="0">
+   <item row="6" column="0">
     <widget class="QPushButton" name="startButton">
      <property name="sizePolicy">
       <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
@@ -85,13 +94,23 @@
      </property>
     </widget>
    </item>
-   <item row="5" column="1">
+   <item row="6" column="1">
     <widget class="QPushButton" name="stopButton">
      <property name="text">
       <string>Stop</string>
      </property>
     </widget>
    </item>
+   <item row="2" column="0">
+    <widget class="QCheckBox" name="sensorHilCheckBox">
+     <property name="text">
+      <string>Sensor HIL</string>
+     </property>
+     <property name="checked">
+      <bool>true</bool>
+     </property>
+    </widget>
+   </item>
   </layout>
  </widget>
  <resources/>