Working HIL!

src/comm/MAVLinkSimulationMAV.h

@@ -36,6 +36,9 @@ protected:
     double roll;
     double pitch;
     double yaw;
+    double rollspeed;
+    double pitchspeed;
+    double yawspeed;
 
     bool globalNavigation;
     bool firstWP;

src/comm/QGCFlightGearLink.cc

@@ -24,6 +24,7 @@ This file is part of the QGROUNDCONTROL project
 /**
  * @file
  *   @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>
  *
  */
@@ -38,7 +39,9 @@ This file is part of the QGROUNDCONTROL project
 #include <QHostInfo>
 #include "MainWindow.h"
 
-QGCFlightGearLink::QGCFlightGearLink(UASInterface* mav, QString remoteHost, QHostAddress host, quint16 port)
+QGCFlightGearLink::QGCFlightGearLink(UASInterface* mav, QString remoteHost, QHostAddress host, quint16 port) :
+    process(NULL),
+    terraSync(NULL)
 {
     this->host = host;
     this->port = port+mav->getUASID();
@@ -282,8 +285,15 @@ bool QGCFlightGearLink::disconnectSimulation()
         delete process;
         process = NULL;
     }
+    if (terraSync)
+    {
+        terraSync->close();
+        delete terraSync;
+        terraSync = NULL;
+    }
     if (socket)
     {
+        socket->close();
         delete socket;
         socket = NULL;
     }
@@ -309,6 +319,7 @@ bool QGCFlightGearLink::connectSimulation()
     QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes()));
 
     process = new QProcess(this);
+    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(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)));
@@ -317,62 +328,145 @@ bool QGCFlightGearLink::connectSimulation()
     // Catch process error
     QObject::connect( process, SIGNAL(error(QProcess::ProcessError)),
                       this, SLOT(processError(QProcess::ProcessError)));
+    QObject::connect( terraSync, SIGNAL(error(QProcess::ProcessError)),
+                      this, SLOT(processError(QProcess::ProcessError)));
     // Start Flightgear
     QStringList processCall;
     QString processFgfs;
+    QString processTerraSync;
     QString fgRoot;
     QString fgScenery;
-    QString aircraft("Rascal110-JSBSim");
+    QString aircraft;
+
+    if (mav->getSystemType() == MAV_TYPE_FIXED_WING)
+    {
+        aircraft = "Rascal110-JSBSim";
+    }
+    else if (mav->getSystemType() == MAV_TYPE_QUADROTOR)
+    {
+        aircraft = "arducopter";
+    }
+    else
+    {
+        aircraft = "Rascal110-JSBSim";
+    }
 
 #ifdef Q_OS_MACX
     processFgfs = "/Applications/FlightGear.app/Contents/Resources/fgfs";
-    fgRoot = "--fg-root=/Applications/FlightGear.app/Contents/Resources/data";
-    fgScenery = "--fg-scenery=/Applications/FlightGear.app/Contents/Resources/data/Scenery:/Applications/FlightGear.app/Contents/Resources/data/Scenery-Terrasync";
+    processTerraSync = "/Applications/FlightGear.app/Contents/Resources/terrasync";
+    fgRoot = "/Applications/FlightGear.app/Contents/Resources/data";
+    //fgScenery = "/Applications/FlightGear.app/Contents/Resources/data/Scenery";
+    fgScenery = "/Applications/FlightGear.app/Contents/Resources/data/Scenery-TerraSync";
+    //   /Applications/FlightGear.app/Contents/Resources/data/Scenery:
 #endif
 
 #ifdef Q_OS_WIN32
     processFgfs = "C:\\Program Files (x86)\\FlightGear\\bin\\Win32\\fgfs";
-    fgRoot = "--fg-root=C:\\Program Files (x86)\\FlightGear\\data";
+    fgRoot = "C:\\Program Files (x86)\\FlightGear\\data";
+    fgScenery = "C:\\Program Files (x86)\\FlightGear\\data\\Scenery-Terrasync";
 #endif
 
 #ifdef Q_OS_LINUX
     processFgfs = "fgfs";
-    fgRoot = "--fg-root=/usr/share/flightgear/data";
+    fgRoot = "/usr/share/flightgear/data";
+    fgScenery = "/usr/share/flightgear/data/Scenery-Terrasync";
 #endif
 
-    processCall << fgRoot;
-    processCall << fgScenery;
-    processCall << QString("--generic=socket,out,50,127.0.0.1,%1,udp,qgroundcontrol").arg(port);
-    processCall << QString("--generic=socket,in,50,127.0.0.1,%1,udp,qgroundcontrol").arg(currentPort);
+    // Sanity checks
+    bool sane = true;
+    QFileInfo executable(processFgfs);
+    if (!executable.isExecutable())
+    {
+        MainWindow::instance()->showCriticalMessage(tr("FlightGear Failed to Start"), tr("FlightGear was not found at %1").arg(processFgfs));
+        sane = false;
+    }
+
+    QFileInfo root(fgRoot);
+    if (!root.isDir())
+    {
+        MainWindow::instance()->showCriticalMessage(tr("FlightGear Failed to Start"), tr("FlightGear data directory was not found at %1").arg(fgRoot));
+        sane = false;
+    }
+
+    QFileInfo scenery(fgScenery);
+    if (!scenery.isDir())
+    {
+        MainWindow::instance()->showCriticalMessage(tr("FlightGear Failed to Start"), tr("FlightGear scenery directory was not found at %1").arg(fgScenery));
+        sane = false;
+    }
+
+    if (!sane) return false;
+
+    // --atlas=socket,out,1,localhost,5505,udp
+    // terrasync -p 5505 -S -d /usr/local/share/TerraSync
+
+    processCall << QString("--fg-root=%1").arg(fgRoot);
+    processCall << QString("--fg-scenery=%1").arg(fgScenery);
+    if (mav->getSystemType() == MAV_TYPE_QUADROTOR)
+    {
+        // FIXME ADD QUAD-Specific protocol here
+        processCall << QString("--generic=socket,out,50,127.0.0.1,%1,udp,qgroundcontrol").arg(port);
+        processCall << QString("--generic=socket,in,50,127.0.0.1,%1,udp,qgroundcontrol").arg(currentPort);
+    }
+    else
+    {
+        processCall << QString("--generic=socket,out,50,127.0.0.1,%1,udp,qgroundcontrol").arg(port);
+        processCall << QString("--generic=socket,in,50,127.0.0.1,%1,udp,qgroundcontrol").arg(currentPort);
+    }
+    processCall << "--atlas=socket,out,1,localhost,5505,udp";
     processCall << "--in-air";
+    processCall << "--roll=0";
+    processCall << "--pitch=0";
     processCall << "--vc=90";
     processCall << "--heading=300";
     processCall << "--timeofday=noon";
     processCall << "--disable-hud-3d";
     processCall << "--disable-fullscreen";
-//    processCall << "--control=mouse";
-//    processCall << "--disable-intro-music";
-//    processCall << "--disable-sound";
-//    processCall << "--disable-anti-alias-hud";
-//    processCall << "--disable-random-objects";
-//    processCall << "--disable-ai-models";
-//    processCall << "--wind=0@0";
+    processCall << "--geometry=400x300";
+    processCall << "--disable-anti-alias-hud";
+    processCall << "--wind=0@0";
+    processCall << "--turbulence=0.0";
+    processCall << "--prop:/sim/frame-rate-throttle-hz=30";
+    processCall << "--control=mouse";
+    processCall << "--disable-intro-music";
+    processCall << "--disable-sound";
+    processCall << "--disable-random-objects";
+    processCall << "--disable-ai-models";
+    processCall << "--shading-flat";
+    processCall << "--fog-disable";
+    processCall << "--disable-specular-highlight";
+    //processCall << "--disable-skyblend";
+    processCall << "--disable-random-objects";
+    processCall << "--disable-panel";
+    //processCall << "--disable-horizon-effect";
+    processCall << "--disable-clouds";
     processCall << "--fdm=jsb";
-    processCall << "--prop:/engines/engine/running=true";
     processCall << "--units-meters";
     if (mav->getSystemType() == MAV_TYPE_QUADROTOR)
     {
-        // Start the remaining three motors of the quad
+        // Start all engines of the quad
+        processCall << "--prop:/engines/engine[0]/running=true";
         processCall << "--prop:/engines/engine[1]/running=true";
         processCall << "--prop:/engines/engine[2]/running=true";
         processCall << "--prop:/engines/engine[3]/running=true";
     }
+    else
+    {
+        processCall << "--prop:/engines/engine/running=true";
+    }
     processCall << QString("--lat=%1").arg(UASManager::instance()->getHomeLatitude());
     processCall << QString("--lon=%1").arg(UASManager::instance()->getHomeLongitude());
     processCall << QString("--altitude=%1").arg(UASManager::instance()->getHomeAltitude());
     // Add new argument with this: processCall << "";
     processCall << QString("--aircraft=%2").arg(aircraft);
 
+
+    QStringList terraSyncArguments;
+    terraSyncArguments << "-p 5505";
+    terraSyncArguments << "-S";
+    terraSyncArguments << QString("-d=%1").arg(fgScenery);
+
+    terraSync->start(processTerraSync, terraSyncArguments);
     process->start(processFgfs, processCall);
 
 

src/comm/QGCFlightGearLink.h

@@ -109,6 +109,7 @@ protected:
     QTimer refreshTimer;
     UASInterface* mav;
     QProcess* process;
+    QProcess* terraSync;
 
     void setName(QString name);