ground/gcs/src/plugins/hitl/fgsimulator.cpp

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       flightgearbridge.h
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
   6  * @addtogroup GCSPlugins GCS Plugins
   7  * @{
   8  * @addtogroup HITLPlugin HITL Plugin
   9  * @{
  10  * @brief The Hardware In The Loop plugin
  11  *****************************************************************************/
  12 /*
  13  * This program is free software; you can redistribute it and/or modify
  14  * it under the terms of the GNU General Public License as published by
  15  * the Free Software Foundation; either version 3 of the License, or
  16  * (at your option) any later version.
  17  *
  18  * This program is distributed in the hope that it will be useful, but
  19  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  20  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  21  * for more details.
  22  *
  23  * You should have received a copy of the GNU General Public License along
  24  * with this program; if not, write to the Free Software Foundation, Inc.,
  25  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  26  */
  27
  28 #include "fgsimulator.h"
  29 #include "extensionsystem/pluginmanager.h"
  30 #include "coreplugin/icore.h"
  31 #include "coreplugin/threadmanager.h"
  32
  33 #ifndef M_PI
  34 #define M_PI 3.14159265358979323846
  35 #endif
  36
  37 // FGSimulator::FGSimulator(QString hostAddr, int outPort, int inPort, bool manual, QString binPath, QString dataPath) :
  38 // Simulator(hostAddr, outPort, inPort,  manual, binPath, dataPath),
  39 // fgProcess(NULL)
  40 // {
  41 //// Note: Only tested on windows 7
  42 // #if defined(Q_WS_WIN)
  43 // cmdShell = QString("c:/windows/system32/cmd.exe");
  44 // #else
  45 // cmdShell = QString("bash");
  46 // #endif
  47 // }
  48
  49 FGSimulator::FGSimulator(const SimulatorSettings & params) :
  50     Simulator(params)
  51 {
  52     udpCounterFGrecv  = 0;
  53     udpCounterGCSsend = 0;
  54 }
  55
  56 FGSimulator::~FGSimulator()
  57 {
  58     disconnect(simProcess, SIGNAL(readyReadStandardOutput()), this, SLOT(processReadyRead()));
  59 }
  60
  61 void FGSimulator::setupUdpPorts(const QString & host, int inPort, int outPort)
  62 {
  63     Q_UNUSED(outPort);
  64
  65     if (inSocket->bind(QHostAddress(host), inPort)) {
  66         emit processOutput("Successfully bound to address " + host + " on port " + QString::number(inPort) + "\n");
  67     } else {
  68         emit processOutput("Cannot bind to address " + host + " on port " + QString::number(inPort) + "\n");
  69     }
  70 }
  71
  72 bool FGSimulator::setupProcess()
  73 {
  74     QMutexLocker locker(&lock);
  75
  76     // Copy FlightGear generic protocol configuration file to the FG protocol directory
  77     // NOTE: Not working on Windows 7, if FG is installed in the "Program Files",
  78     // likelly due to permissions. The file should be manually copied to data/Protocol/opfgprotocol.xml
  79     // QFile xmlFile(":/flightgear/genericprotocol/opfgprotocol.xml");
  80     // xmlFile.open(QIODevice::ReadOnly | QIODevice::Text);
  81     // QString xml = xmlFile.readAll();
  82     // xmlFile.close();
  83     // QFile xmlFileOut(pathData + "/Protocol/opfgprotocol.xml");
  84     // xmlFileOut.open(QIODevice::WriteOnly | QIODevice::Text);
  85     // xmlFileOut.write(xml.toLatin1());
  86     // xmlFileOut.close();
  87
  88     Qt::HANDLE mainThread = QThread::currentThreadId();
  89
  90     qDebug() << "setupProcess Thread: " << mainThread;
  91
  92     simProcess = new QProcess();
  93     simProcess->setReadChannelMode(QProcess::MergedChannels);
  94     connect(simProcess, SIGNAL(readyReadStandardOutput()), this, SLOT(processReadyRead()));
  95     // Note: Only tested on windows 7
  96 #if defined(Q_WS_WIN)
  97     QString cmdShell("c:/windows/system32/cmd.exe");
  98 #else
  99     QString cmdShell("bash");
 100 #endif
 101
 102     // Start shell (Note: Could not start FG directly on Windows, only through terminal!)
 103     simProcess->start(cmdShell);
 104     if (simProcess->waitForStarted() == false) {
 105         emit processOutput("Error:" + simProcess->errorString());
 106         return false;
 107     }
 108
 109     // Setup arguments
 110     // Note: The input generic protocol is set to update at a much higher rate than the actual updates are sent by the GCS.
 111     // If this is not done then a lag will be introduced by FlightGear, likelly because the receive socket buffer builds up during startup.
 112     QString args("--fg-root=\"" + settings.dataPath + "\" " +
 113                  "--timeofday=noon " +
 114                  "--httpd=5400 " +
 115                  "--enable-hud " +
 116                  "--in-air " +
 117                  "--altitude=3000 " +
 118                  "--vc=100 " +
 119                  "--log-level=alert " +
 120                  "--generic=socket,out,20," + settings.hostAddress + "," + QString::number(settings.inPort) + ",udp,opfgprotocol");
 121     if (settings.manualControlEnabled) { // <--[BCH] What does this do? Why does it depend on ManualControl?
 122         args.append(" --generic=socket,in,400," + settings.remoteAddress + "," + QString::number(settings.outPort) + ",udp,opfgprotocol");
 123     }
 124
 125     // Start FlightGear - only if checkbox is selected in HITL options page
 126     if (settings.startSim) {
 127         QString cmd("\"" + settings.binPath + "\" " + args + "\n");
 128         simProcess->write(cmd.toLatin1());
 129     } else {
 130         emit processOutput("Start Flightgear from the command line with the following arguments: \n\n" + args + "\n\n" +
 131                            "You can optionally run Flightgear from a networked computer.\n" +
 132                            "Make sure the computer running Flightgear can can ping your local interface adapter. ie." + settings.hostAddress + "\n"
 133                            "Remote computer must have the correct OpenPilot protocol installed.");
 134     }
 135
 136     udpCounterGCSsend = 0;
 137
 138     return true;
 139 }
 140
 141 void FGSimulator::processReadyRead()
 142 {
 143     QByteArray bytes = simProcess->readAllStandardOutput();
 144     QString str(bytes);
 145
 146     if (!str.contains("Error reading data")) { // ignore error
 147         emit processOutput(str);
 148     }
 149 }
 150
 151 void FGSimulator::transmitUpdate()
 152 {
 153     ActuatorDesired::DataFields actData;
 154     FlightStatus::DataFields flightStatusData    = flightStatus->getData();
 155     ManualControlCommand::DataFields manCtrlData = manCtrlCommand->getData();
 156     float ailerons = -1;
 157     float elevator = -1;
 158     float rudder   = -1;
 159     float throttle = -1;
 160
 161     if (flightStatusData.FlightMode == FlightStatus::FLIGHTMODE_MANUAL) {
 162         // Read joystick input
 163         if (flightStatusData.Armed == FlightStatus::ARMED_ARMED) {
 164             // Note: Pitch sign is reversed in FG ?
 165             ailerons = manCtrlData.Roll;
 166             elevator = -manCtrlData.Pitch;
 167             rudder   = manCtrlData.Yaw;
 168             throttle = manCtrlData.Throttle;
 169         }
 170     } else {
 171         // Read ActuatorDesired from autopilot
 172         actData  = actDesired->getData();
 173
 174         ailerons = actData.Roll;
 175         elevator = -actData.Pitch;
 176         rudder   = actData.Yaw;
 177         throttle = actData.Thrust;
 178     }
 179
 180     int allowableDifference = 10;
 181
 182     // qDebug() << "UDP sent:" << udpCounterGCSsend << " - UDP Received:" << udpCounterFGrecv;
 183
 184     if (udpCounterFGrecv == udpCounterGCSsend) {
 185         udpCounterGCSsend = 0;
 186     }
 187
 188     if ((udpCounterGCSsend < allowableDifference) || (udpCounterFGrecv == 0)) { // FG udp queue is not delayed
 189         udpCounterGCSsend++;
 190
 191         // Send update to FlightGear
 192         QString cmd;
 193         cmd = QString("%1,%2,%3,%4,%5\n")
 194               .arg(ailerons) // ailerons
 195               .arg(elevator) // elevator
 196               .arg(rudder) // rudder
 197               .arg(throttle) // throttle
 198               .arg(udpCounterGCSsend); // UDP packet counter delay
 199
 200         QByteArray data = cmd.toLatin1();
 201
 202         if (outSocket->writeDatagram(data, QHostAddress(settings.remoteAddress), settings.outPort) == -1) {
 203             emit processOutput("Error sending UDP packet to FG: " + outSocket->errorString() + "\n");
 204         }
 205     } else {
 206         // don't send new packet. Flightgear cannot process UDP fast enough.
 207         // V1.9.1 reads udp packets at set frequency and will get delayed if packets are sent too fast
 208         // V2.0 does not currently work with --generic-protocol
 209     }
 210
 211     if (settings.manualControlEnabled) {
 212         actData.Roll   = ailerons;
 213         actData.Pitch  = -elevator;
 214         actData.Yaw    = rudder;
 215         actData.Thrust = throttle;
 216         // actData.NumLongUpdates = (float)udpCounterFGrecv;
 217         // actData.UpdateTime = (float)udpCounterGCSsend;
 218         actDesired->setData(actData);
 219     }
 220 }
 221
 222
 223 void FGSimulator::processUpdate(const QByteArray & inp)
 224 {
 225     // TODO: this does not use the FLIGHT_PARAM structure, it should!
 226     // Split
 227     QString data(inp);
 228     QStringList fields = data.split(",");
 229     // Get xRate (deg/s)
 230     // float xRate = fields[0].toFloat() * 180.0/M_PI;
 231     // Get yRate (deg/s)
 232     // float yRate = fields[1].toFloat() * 180.0/M_PI;
 233     // Get zRate (deg/s)
 234     // float zRate = fields[2].toFloat() * 180.0/M_PI;
 235     // Get xAccel (m/s^2)
 236     float xAccel    = fields[3].toFloat() * FT2M;
 237     // Get yAccel (m/s^2)
 238     float yAccel    = fields[4].toFloat() * FT2M;
 239     // Get xAccel (m/s^2)
 240     float zAccel    = fields[5].toFloat() * FT2M;
 241     // Get pitch (deg)
 242     float pitch     = fields[6].toFloat();
 243     // Get pitchRate (deg/s)
 244     float pitchRate = fields[7].toFloat();
 245     // Get roll (deg)
 246     float roll     = fields[8].toFloat();
 247     // Get rollRate (deg/s)
 248     float rollRate = fields[9].toFloat();
 249     // Get yaw (deg)
 250     float yaw       = fields[10].toFloat();
 251     // Get yawRate (deg/s)
 252     float yawRate   = fields[11].toFloat();
 253     // Get latitude (deg)
 254     float latitude  = fields[12].toFloat();
 255     // Get longitude (deg)
 256     float longitude = fields[13].toFloat();
 257     // Get heading (deg)
 258     // float heading      = fields[14].toFloat();
 259     // Get altitude (m)
 260     float altitude_msl = fields[15].toFloat() * FT2M;
 261     // Get altitudeAGL (m)
 262     float altitude_agl = fields[16].toFloat() * FT2M;
 263     // Get groundspeed (m/s)
 264     float groundspeed  = fields[17].toFloat() * KT2MPS;
 265     // Get airspeed (m/s)
 266     float airspeed     = fields[18].toFloat() * KT2MPS;
 267     // Get temperature (degC)
 268     float temperature  = fields[19].toFloat();
 269     // Get pressure (kpa)
 270     float pressure     = fields[20].toFloat() * INHG2KPA;
 271     // Get VelocityState Down (m/s)
 272     float velocityStateDown  = fields[21].toFloat() * FPS2CMPS * 1e-2f;
 273     // Get VelocityState East (m/s)
 274     float velocityStateEast  = fields[22].toFloat() * FPS2CMPS * 1e-2f;
 275     // Get VelocityState Down (m/s)
 276     float velocityStateNorth = fields[23].toFloat() * FPS2CMPS * 1e-2f;
 277
 278     // Get UDP packets received by FG
 279     int n = fields[24].toInt();
 280
 281     udpCounterFGrecv = n;
 282
 283     ///////
 284     // Output formatting
 285     ///////
 286     Output2Hardware out;
 287     memset(&out, 0, sizeof(Output2Hardware));
 288
 289     HomeLocation::DataFields homeData = posHome->getData();
 290     double HomeLLA[3] = { (double)homeData.Latitude * 1e-7, (double)homeData.Longitude * 1e-7, homeData.Altitude };
 291     double HomeECEF[3];
 292     float HomeRNE[3][3];
 293     double LLA[3]     = { latitude, longitude, altitude_msl };
 294     float NED[3];
 295     Utils::CoordinateConversions().RneFromLLA(HomeLLA, HomeRNE);
 296     Utils::CoordinateConversions().LLA2ECEF(HomeLLA, HomeECEF);
 297     Utils::CoordinateConversions().LLA2Base(LLA, HomeECEF, HomeRNE, NED);
 298
 299     // Update GPS Position objects
 300     out.latitude    = latitude * 1e7;
 301     out.longitude   = longitude * 1e7;
 302     out.altitude    = altitude_msl;
 303     out.agl = altitude_agl;
 304     out.groundspeed = groundspeed;
 305
 306     out.calibratedAirspeed = airspeed;
 307
 308
 309     // Update BaroSensor object
 310     out.temperature = temperature;
 311     out.pressure    = pressure;
 312
 313     // Update attState object
 314     out.roll      = roll;       // roll;
 315     out.pitch     = pitch;     // pitch
 316     out.heading   = yaw; // yaw
 317
 318     out.dstN      = NED[0];
 319     out.dstE      = NED[1];
 320     out.dstD      = NED[2];
 321
 322     // Update VelocityState.{North,East,Down}
 323     out.velNorth  = velocityStateNorth;
 324     out.velEast   = velocityStateEast;
 325     out.velDown   = velocityStateDown;
 326
 327     // Update gyroscope sensor data
 328     out.rollRate  = rollRate;
 329     out.pitchRate = pitchRate;
 330     out.yawRate   = yawRate;
 331
 332     // Update accelerometer sensor data
 333     out.accX      = xAccel;
 334     out.accY      = yAccel;
 335     out.accZ      = -zAccel;
 336
 337     updateUAVOs(out);
 338 }