Merge pull request #520 from mavlink/pressure_offset

Added 2 kPA offset to pressure measurements to get HIL closer to reality

src/comm/MAVLinkSimulationLink.h

@@ -123,7 +123,7 @@ protected:
 
     void enqueue(uint8_t* stream, uint8_t* index, mavlink_message_t* msg);
 
-    static const uint8_t systemId = 220;
+    static const int8_t systemId = 220;
     static const uint8_t componentId = 200;
     static const uint16_t version = 1000;
 

src/comm/QGCFlightGearLink.cc

@@ -46,7 +46,8 @@ QGCFlightGearLink::QGCFlightGearLink(UASInterface* mav, QString startupArguments
     terraSync(NULL),
     flightGearVersion(0),
     startupArguments(startupArguments),
-    _sensorHilEnabled(true)
+    _sensorHilEnabled(true),
+    barometerOffsetkPa(0.0)
 {
     this->host = host;
     this->port = port+mav->getUASID();
@@ -279,7 +280,8 @@ void QGCFlightGearLink::readBytes()
     mag_dip = values.at(18).toFloat();
 
     temperature = values.at(19).toFloat();
-    abs_pressure = values.at(20).toFloat()*1e2; //convert to Pa from hPa
+    abs_pressure = values.at(20).toFloat() * 1e2f; //convert to Pa from hPa
+    abs_pressure += barometerOffsetkPa * 1e3f; //add offset, convert from kPa to Pa
 
     //calculate differential pressure
     const float air_gas_constant = 287.1f; // J/(kg * K)
@@ -736,3 +738,8 @@ void QGCFlightGearLink::setName(QString name)
     this->name = name;
     //    emit nameChanged(this->name);
 }
+
+void QGCFlightGearLink::setBarometerOffset(float barometerOffsetkPa)
+{
+    this->barometerOffsetkPa = barometerOffsetkPa;
+}

src/comm/QGCFlightGearLink.h

@@ -42,6 +42,7 @@ This file is part of the QGROUNDCONTROL project
 #include <configuration.h>
 #include "UASInterface.h"
 #include "QGCHilLink.h"
+#include <QGCHilFlightGearConfiguration.h>
 
 class QGCFlightGearLink : public QGCHilLink
 {
@@ -134,6 +135,7 @@ public slots:
     void printFgfsOutput();
     void printFgfsError();
     void setStartupArguments(QString startupArguments);
+    void setBarometerOffset(float barometerOffsetkPa);
 
 protected:
     QString name;
@@ -161,6 +163,7 @@ protected:
     unsigned int flightGearVersion;
     QString startupArguments;
     bool _sensorHilEnabled;
+    float barometerOffsetkPa;
 
     void setName(QString name);
 

src/comm/QGCHilLink.h

@@ -72,6 +72,8 @@ public slots:
 protected:
     virtual void setName(QString name) = 0;
 
+    static const unsigned int barometerOffsetkPa = 2;
+
 signals:
     /**
      * @brief This signal is emitted instantly when the link is connected

src/comm/QGCXPlaneLink.cc

@@ -667,8 +667,8 @@ void QGCXPlaneLink::readBytes()
             /* current pressure at MSL in kPa */
             double p1 = 1013.25 / 10.0;
 
-            /* measured pressure in hPa */
-            double p = abs_pressure / 10.0;
+            /* measured pressure in hPa, plus offset to simulate weather effects / offsets */
+            double p = abs_pressure / 10.0 + barometerOffsetkPa;
 
             /*
              * Solve:

src/uas/UAS.cc

@@ -3000,7 +3000,7 @@ bool UAS::emergencyKILL()
 /**
 * If enabled, connect the flight gear link.
 */
-void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil)
+void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObject * configuration)
 {
     QGCFlightGearLink* link = dynamic_cast<QGCFlightGearLink*>(simulation);
     if (!link || !simulation) {
@@ -3015,6 +3015,7 @@ void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil)
     link = dynamic_cast<QGCFlightGearLink*>(simulation);
     link->setStartupArguments(options);
     link->sensorHilEnabled(sensorHil);
+    QObject::connect(configuration, SIGNAL(barometerOffsetChanged(float)), link, SLOT(setBarometerOffset(float)));
     if (enable)
     {
         startHil();

src/uas/UAS.h

@@ -760,7 +760,7 @@ public slots:
     void go();
 
     /** @brief Enable / disable HIL */
-    void enableHilFlightGear(bool enable, QString options, bool sensorHil);
+    void enableHilFlightGear(bool enable, QString options, bool sensorHil, QObject * configuration);
     void enableHilJSBSim(bool enable, QString options);
     void enableHilXPlane(bool enable);
 

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, "", true);
+        mav->enableHilFlightGear(false, "", true, this);
         QGCHilFlightGearConfiguration* hfgconf = new QGCHilFlightGearConfiguration(mav, this);
         hfgconf->show();
         ui->simulatorConfigurationLayout->addWidget(hfgconf);

src/ui/QGCHilFlightGearConfiguration.cc

@@ -40,10 +40,16 @@ 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, ui->sensorHilCheckBox->isChecked());
+    mav->enableHilFlightGear(true,  options, ui->sensorHilCheckBox->isChecked(), this);
 }
 
 void QGCHilFlightGearConfiguration::on_stopButton_clicked()
 {
     mav->stopHil();
 }
+
+void QGCHilFlightGearConfiguration::on_barometerOffsetLineEdit_textChanged(const QString& baroOffset)
+{
+    emit barometerOffsetChanged(baroOffset.toFloat());
+}
+

src/ui/QGCHilFlightGearConfiguration.h

@@ -25,9 +25,13 @@ protected:
 private slots:
     void on_startButton_clicked();
     void on_stopButton_clicked();
+    void on_barometerOffsetLineEdit_textChanged(const QString& baroOffset);
 
 private:
     Ui::QGCHilFlightGearConfiguration *ui;
+
+signals:
+     void barometerOffsetChanged(float barometerOffsetkPa);
 };
 
 #endif // QGCHILFLIGHTGEARCONFIGURATION_H

src/ui/QGCHilFlightGearConfiguration.ui

@@ -32,13 +32,6 @@
    <property name="spacing">
     <number>6</number>
    </property>
-   <item row="0" column="0">
-    <widget class="QLabel" name="aircraftPlaintextInfoLabel">
-     <property name="text">
-      <string>Airframe:</string>
-     </property>
-    </widget>
-   </item>
    <item row="1" column="0" colspan="2">
     <widget class="QComboBox" name="aircraftComboBox">
      <property name="sizePolicy">
@@ -52,14 +45,21 @@
      </property>
     </widget>
    </item>
-   <item row="3" column="0">
+   <item row="4" 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="4" column="0" colspan="2">
+   <item row="0" column="0">
+    <widget class="QLabel" name="aircraftPlaintextInfoLabel">
+     <property name="text">
+      <string>Airframe:</string>
+     </property>
+    </widget>
+   </item>
+   <item row="5" column="0" colspan="2">
     <widget class="QPlainTextEdit" name="optionsPlainTextEdit">
      <property name="sizePolicy">
       <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
@@ -72,7 +72,7 @@
      </property>
     </widget>
    </item>
-   <item row="6" column="0">
+   <item row="7" column="0">
     <widget class="QPushButton" name="startButton">
      <property name="sizePolicy">
       <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
@@ -85,7 +85,7 @@
      </property>
     </widget>
    </item>
-   <item row="6" column="1">
+   <item row="7" column="1">
     <widget class="QPushButton" name="stopButton">
      <property name="text">
       <string>Stop</string>
@@ -102,6 +102,20 @@
      </property>
     </widget>
    </item>
+   <item row="3" column="0">
+    <widget class="QLabel" name="barometerOffsetLabel">
+     <property name="text">
+      <string>Barometer Offset [kPa]:</string>
+     </property>
+    </widget>
+   </item>
+   <item row="3" column="1">
+    <widget class="QLineEdit" name="barometerOffsetLineEdit">
+     <property name="text">
+      <string>0</string>
+     </property>
+    </widget>
+   </item>
   </layout>
  </widget>
  <resources/>