Working HIL!

src/comm/MAVLinkSimulationMAV.cc

@@ -21,6 +21,9 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy
     roll(0.0),
     pitch(0.0),
     yaw(0.0),
+    rollspeed(0.0),
+    pitchspeed(0.0),
+    yawspeed(0.0),
     globalNavigation(true),
     firstWP(false),
     //    previousSPX(8.548056),
@@ -98,6 +101,8 @@ void MAVLinkSimulationMAV::mainloop()
 
         float zsign = (zm < 0) ? -1.0f : 1.0f;
 
+        if (!(sys_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL))
+        {
             if (!firstWP) {
                 //float trueyaw = atan2f(xm, ym);
 
@@ -126,14 +131,20 @@ void MAVLinkSimulationMAV::mainloop()
                 firstWP = false;
                 qDebug() << "INIT STEP";
             }
+        }
+        else
+        {
+            // FIXME Implement heading and altitude controller
+
+        }
 
 
         // GLOBAL POSITION
         mavlink_message_t msg;
         mavlink_global_position_int_t pos;
-        pos.alt = z*1000.0;
-        pos.lat = x*1E7;
-        pos.lon = y*1E7;
+        pos.alt = altitude*1000.0;
+        pos.lat = longitude*1E7;
+        pos.lon = longitude*1E7;
         pos.vx = sin(yaw)*10.0f*100.0f;
         pos.vy = 0;
         pos.vz = altPer100ms*10.0f*100.0f*zsign*-1.0f;
@@ -144,12 +155,12 @@ void MAVLinkSimulationMAV::mainloop()
         // ATTITUDE
         mavlink_attitude_t attitude;
         attitude.time_boot_ms = 0;
-        attitude.roll = 0.0f;
-        attitude.pitch = 0.0f;
+        attitude.roll = roll;
+        attitude.pitch = pitch;
         attitude.yaw = yaw;
-        attitude.rollspeed = 0.0f;
-        attitude.pitchspeed = 0.0f;
-        attitude.yawspeed = 0.0f;
+        attitude.rollspeed = rollspeed;
+        attitude.pitchspeed = pitchspeed;
+        attitude.yawspeed = yawspeed;
 
         mavlink_msg_attitude_encode(systemid, MAV_COMP_ID_IMU, &msg, &attitude);
         link->sendMAVLinkMessage(&msg);
@@ -157,11 +168,11 @@ void MAVLinkSimulationMAV::mainloop()
         // SYSTEM STATUS
         mavlink_sys_status_t status;
         status.load = 300;
-//        status.mode = sys_mode;
-//        status.nav_mode = nav_mode;
+        //        status.mode = sys_mode;
+        //        status.nav_mode = nav_mode;
         status.errors_comm = 0;
         status.voltage_battery = 10500;
-//        status.status = sys_state;
+        //        status.status = sys_state;
         status.battery_remaining = 90;
         mavlink_msg_sys_status_encode(systemid, MAV_COMP_ID_IMU, &msg, &status);
         link->sendMAVLinkMessage(&msg);
@@ -171,7 +182,7 @@ void MAVLinkSimulationMAV::mainloop()
         mavlink_vfr_hud_t hud;
         hud.airspeed = pos.vx/100.0f;
         hud.groundspeed = pos.vx/100.0f;
-        hud.alt = z;
+        hud.alt = altitude;
         hud.heading = static_cast<int>((yaw/M_PI)*180.0f+180.0f) % 360;
         hud.climb = pos.vz/100.0f;
         hud.throttle = 90;
@@ -211,7 +222,7 @@ void MAVLinkSimulationMAV::mainloop()
         {
             mavlink_hil_controls_t hil;
             hil.roll_ailerons = 0.0;
-            hil.pitch_elevator = 0.0;
+            hil.pitch_elevator = 0.05;
             hil.yaw_rudder = 0.05;
             hil.throttle = 0.6;
             // Encode the data (adding header and checksums, etc.)
@@ -223,12 +234,12 @@ void MAVLinkSimulationMAV::mainloop()
         // Send actual controller outputs
         // This message just shows the direction
         // and magnitude of the control output
-//        mavlink_position_controller_output_t pos;
-//        pos.x = sin(yaw)*127.0f;
-//        pos.y = cos(yaw)*127.0f;
-//        pos.z = 0;
-//        mavlink_msg_position_controller_output_encode(systemid, MAV_COMP_ID_IMU, &ret, &pos);
-//        link->sendMAVLinkMessage(&ret);
+        //        mavlink_position_controller_output_t pos;
+        //        pos.x = sin(yaw)*127.0f;
+        //        pos.y = cos(yaw)*127.0f;
+        //        pos.z = 0;
+        //        mavlink_msg_position_controller_output_encode(systemid, MAV_COMP_ID_IMU, &ret, &pos);
+        //        link->sendMAVLinkMessage(&ret);
 
         // Send a named value with name "FLOAT" and 0.5f as value
 
@@ -395,31 +406,46 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
         if (systemid == mode.target_system) sys_mode = mode.base_mode;
     }
     break;
+    case MAVLINK_MSG_ID_HIL_STATE:
+    {
+        mavlink_hil_state_t state;
+        mavlink_msg_hil_state_decode(&msg, &state);
+        roll = state.roll;
+        pitch = state.pitch;
+        yaw = state.yaw;
+        rollspeed = state.rollspeed;
+        pitchspeed = state.pitchspeed;
+        yawspeed = state.yawspeed;
+        latitude = state.lat;
+        longitude = state.lon;
+        altitude = state.alt;
+    }
+    break;
     // FIXME MAVLINKV10PORTINGNEEDED
-//    case MAVLINK_MSG_ID_ACTION: {
-//        mavlink_action_t action;
-//        mavlink_msg_action_decode(&msg, &action);
-//        if (systemid == action.target && (action.target_component == 0 || action.target_component == MAV_COMP_ID_IMU)) {
-//            mavlink_action_ack_t ack;
-//            ack.action = action.action;
-////            switch (action.action) {
-////            case MAV_ACTION_TAKEOFF:
-////                flying = true;
-////                nav_mode = MAV_NAV_LIFTOFF;
-////                ack.result = 1;
-////                break;
-////            default: {
-////                ack.result = 0;
-////            }
-////            break;
-////            }
-
-//            // Give feedback about action
-//            mavlink_message_t r_msg;
-//            mavlink_msg_action_ack_encode(systemid, MAV_COMP_ID_IMU, &r_msg, &ack);
-//            link->sendMAVLinkMessage(&r_msg);
-//        }
-//    }
+    //    case MAVLINK_MSG_ID_ACTION: {
+    //        mavlink_action_t action;
+    //        mavlink_msg_action_decode(&msg, &action);
+    //        if (systemid == action.target && (action.target_component == 0 || action.target_component == MAV_COMP_ID_IMU)) {
+    //            mavlink_action_ack_t ack;
+    //            ack.action = action.action;
+    ////            switch (action.action) {
+    ////            case MAV_ACTION_TAKEOFF:
+    ////                flying = true;
+    ////                nav_mode = MAV_NAV_LIFTOFF;
+    ////                ack.result = 1;
+    ////                break;
+    ////            default: {
+    ////                ack.result = 0;
+    ////            }
+    ////            break;
+    ////            }
+
+    //            // Give feedback about action
+    //            mavlink_message_t r_msg;
+    //            mavlink_msg_action_ack_encode(systemid, MAV_COMP_ID_IMU, &r_msg, &ack);
+    //            link->sendMAVLinkMessage(&r_msg);
+    //        }
+    //    }
     break;
     case MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT: {
         mavlink_set_local_position_setpoint_t sp;

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;
+    // 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);