diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui index 5191788a0..42c7ed974 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui @@ -6,7 +6,7 @@ 0 0 - 587 + 738 379 diff --git a/ground/openpilotgcs/src/plugins/hitlnew/aerosimrcsimulator.cpp b/ground/openpilotgcs/src/plugins/hitlnew/aerosimrcsimulator.cpp index de26cb278..b238d0bda 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/aerosimrcsimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitlnew/aerosimrcsimulator.cpp @@ -90,7 +90,7 @@ void AeroSimRCSimulator::transmitUpdate() stream << armed << mode; // flight status stream << udpCounterASrecv; // packet counter - if (outSocket->writeDatagram(data, QHostAddress(settings.remoteHostAddress), settings.outPort) == -1) { + if (outSocket->writeDatagram(data, QHostAddress(settings.remoteAddress), settings.outPort) == -1) { qDebug() << "write failed: " << outSocket->errorString(); } @@ -125,7 +125,7 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data) return; } - float timeStep, + float delT, homeX, homeY, homeZ, WpHX, WpHY, WpLat, WpLon, posX, posY, posZ, // world @@ -138,7 +138,7 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data) rx, ry, rz, fx, fy, fz, ux, uy, uz, // matrix ch[8]; - stream >> timeStep; + stream >> delT; stream >> homeX >> homeY >> homeZ; stream >> WpHX >> WpHY >> WpLat >> WpLon; stream >> posX >> posY >> posZ; @@ -156,294 +156,50 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data) memset(&out, 0, sizeof(Output2OP)); -// struct Output2OP{ -// float latitude; -// float longitude; -// float altitude; -// float heading; -// float groundspeed; //[m/s] -// float calibratedAirspeed; //[m/s] -// float pitch; -// float roll; -// float pressure; -// float temperature; -// float velNorth; //[m/s] -// float velEast; //[m/s] -// float velDown; //[m/s] -// float dstN; //[m] -// float dstE; //[m] -// float dstD; //[m] -// float accX; //[m/s^2] -// float accY; //[m/s^2] -// float accZ; //[m/s^2] -// float rollRate; -// float pitchRate; -// float yawRate; -// }; + out.delT=delT; + /**********************************************************************************************/ + QMatrix4x4 mat; + mat = QMatrix4x4( fy, fx, -fz, 0.0, // model matrix + ry, rx, -rz, 0.0, // (X,Y,Z) -> (+Y,+X,-Z) + -uy, -ux, uz, 0.0, + 0.0, 0.0, 0.0, 1.0); + mat.optimize(); + + QQuaternion quat; // model quat + asMatrix2Quat(mat, quat); + + // rotate gravity + /*************************************************************************************/ + QVector3D acc = QVector3D(accY, accX, -accZ); // accel (X,Y,Z) -> (+Y,+X,-Z) + QVector3D gee = QVector3D(0.0, 0.0, -GEE); + QQuaternion qWorld = quat.conjugate(); + gee = qWorld.rotatedVector(gee); + acc += gee; + + out.rollRate = angY * RAD2DEG; // gyros (X,Y,Z) -> (+Y,+X,-Z) + out.pitchRate = angX * RAD2DEG; + out.yawRate = angZ * -RAD2DEG; + + out.accX = acc.x(); + out.accY = acc.y(); + out.accZ = acc.z(); + + /*************************************************************************************/ + QVector3D rpy; // model roll, pitch, yaw + asMatrix2RPY(mat, rpy); + + out.roll = rpy.x(); + out.pitch = rpy.y(); + out.heading = rpy.z(); /**********************************************************************************************/ - QTime currentTime = QTime::currentTime(); - /**********************************************************************************************/ -// static bool firstRun = true; -// if (settings.homeLocation) { -// if (firstRun) { -// HomeLocation::DataFields homeData; -// homeData = posHome->getData(); + out.altitude = posZ; + out.heading = yaw * RAD2DEG; + out.latitude = lat * 10e6; + out.longitude = lon * 10e6; + out.groundspeed = qSqrt(velX * velX + velY * velY); -// homeData.Latitude = WpLat * 10e6; -// homeData.Longitude = WpLon * 10e6; -// homeData.Altitude = homeZ; -// homeData.Set = HomeLocation::SET_TRUE; - -// posHome->setData(homeData); - -// firstRun = false; -// } -// if (settings.homeLocRate > 0) { -// static QTime homeLocTime = currentTime; -// if (homeLocTime.secsTo(currentTime) >= settings.homeLocRate) { -// firstRun = true; -// homeLocTime = currentTime; -// } -// } -// } - /**********************************************************************************************/ -// if (settings.attRaw || settings.attActual) { - QMatrix4x4 mat; - mat = QMatrix4x4( fy, fx, -fz, 0.0, // model matrix - ry, rx, -rz, 0.0, // (X,Y,Z) -> (+Y,+X,-Z) - -uy, -ux, uz, 0.0, - 0.0, 0.0, 0.0, 1.0); - mat.optimize(); - - QQuaternion quat; // model quat - asMatrix2Quat(mat, quat); - - // rotate gravity - QVector3D acc = QVector3D(accY, accX, -accZ); // accel (X,Y,Z) -> (+Y,+X,-Z) - QVector3D gee = QVector3D(0.0, 0.0, -GEE); - QQuaternion qWorld = quat.conjugate(); - gee = qWorld.rotatedVector(gee); - acc += gee; - - /*************************************************************************************/ -// if (settings.attRaw) { - out.rollRate = angY * RAD2DEG; // gyros (X,Y,Z) -> (+Y,+X,-Z) - out.pitchRate = angX * RAD2DEG; - out.yawRate = angZ * -RAD2DEG; - - out.accX = acc.x(); - out.accY = acc.y(); - out.accZ = acc.z(); -// } - /*************************************************************************************/ - QVector3D rpy; // model roll, pitch, yaw - asMatrix2RPY(mat, rpy); - - out.roll = rpy.x(); - out.pitch = rpy.y(); - out.heading = rpy.z(); - -// if (settings.attActHW) { -// // do nothing -// /*****************************************/ -// } else if (settings.attActSim) { -// // take all data from simulator -// AttitudeActual::DataFields attActData; -// attActData = attActual->getData(); - - -// attActData.Roll = rpy.x(); -// attActData.Pitch = rpy.y(); -// attActData.Yaw = rpy.z(); -// attActData.q1 = quat.scalar(); -// attActData.q2 = quat.x(); -// attActData.q3 = quat.y(); -// attActData.q4 = quat.z(); - -// attActual->setData(attActData); -// /*****************************************/ -// } else if (settings.attActCalc) { -// // calculate RPY with code from Attitude module -// AttitudeActual::DataFields attActData; -// attActData = attActual->getData(); - -// static float q[4] = {1, 0, 0, 0}; -// static float gyro_correct_int2 = 0; - -// float dT = timeStep; - -// AttitudeSettings::DataFields attSettData = attSettings->getData(); -// float accelKp = attSettData.AccelKp * 0.1666666666666667; -// float accelKi = attSettData.AccelKp * 0.1666666666666667; -// float yawBiasRate = attSettData.YawBiasRate; - -// // calibrate sensors on arming -// if (flightStatus->getData().Armed == FlightStatus::ARMED_ARMING) { -// accelKp = 2.0; -// accelKi = 0.9; -// } - -// float gyro[3] = {angY * RAD2DEG, angX * RAD2DEG, angZ * -RAD2DEG}; -// float attRawAcc[3] = {acc.x(), acc.y(), acc.z()}; - -// // code from Attitude module begin /////////////////////////////// -// float *accels = attRawAcc; -// float grot[3]; -// float accel_err[3]; - -// // Rotate gravity to body frame and cross with accels -// grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2])); -// grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1])); -// grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]); - -// // CrossProduct -// { -// accel_err[0] = accels[1]*grot[2] - grot[1]*accels[2]; -// accel_err[1] = grot[0]*accels[2] - accels[0]*grot[2]; -// accel_err[2] = accels[0]*grot[1] - grot[0]*accels[1]; -// } - -// // Account for accel magnitude -// float accel_mag = sqrt(accels[0] * accels[0] + accels[1] * accels[1] + accels[2] * accels[2]); -// accel_err[0] /= accel_mag; -// accel_err[1] /= accel_mag; -// accel_err[2] /= accel_mag; - -// // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s -// gyro_correct_int2 += -gyro[2] * yawBiasRate; - -// // Correct rates based on error, integral component dealt with in updateSensors -// gyro[0] += accel_err[0] * accelKp / dT; -// gyro[1] += accel_err[1] * accelKp / dT; -// gyro[2] += accel_err[2] * accelKp / dT + gyro_correct_int2; - -// // Work out time derivative from INSAlgo writeup -// // Also accounts for the fact that gyros are in deg/s -// float qdot[4]; -// qdot[0] = (-q[1] * gyro[0] - q[2] * gyro[1] - q[3] * gyro[2]) * dT * M_PI / 180 / 2; -// qdot[1] = (+q[0] * gyro[0] - q[3] * gyro[1] + q[2] * gyro[2]) * dT * M_PI / 180 / 2; -// qdot[2] = (+q[3] * gyro[0] + q[0] * gyro[1] - q[1] * gyro[2]) * dT * M_PI / 180 / 2; -// qdot[3] = (-q[2] * gyro[0] + q[1] * gyro[1] + q[0] * gyro[2]) * dT * M_PI / 180 / 2; - -// // Take a time step -// q[0] += qdot[0]; -// q[1] += qdot[1]; -// q[2] += qdot[2]; -// q[3] += qdot[3]; - -// if(q[0] < 0) { -// q[0] = -q[0]; -// q[1] = -q[1]; -// q[2] = -q[2]; -// q[3] = -q[3]; -// } - -// // Renomalize -// float qmag = sqrt((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3])); -// q[0] /= qmag; -// q[1] /= qmag; -// q[2] /= qmag; -// q[3] /= qmag; - -// // If quaternion has become inappropriately short or is nan reinit. -// // THIS SHOULD NEVER ACTUALLY HAPPEN -// if((fabs(qmag) < 1e-3) || (qmag != qmag)) { -// q[0] = 1; -// q[1] = 0; -// q[2] = 0; -// q[3] = 0; -// } - -// float rpy2[3]; -// // Quaternion2RPY -// { -// float q0s, q1s, q2s, q3s; -// q0s = q[0] * q[0]; -// q1s = q[1] * q[1]; -// q2s = q[2] * q[2]; -// q3s = q[3] * q[3]; - -// float R13, R11, R12, R23, R33; -// R13 = 2 * (q[1] * q[3] - q[0] * q[2]); -// R11 = q0s + q1s - q2s - q3s; -// R12 = 2 * (q[1] * q[2] + q[0] * q[3]); -// R23 = 2 * (q[2] * q[3] + q[0] * q[1]); -// R33 = q0s - q1s - q2s + q3s; - -// rpy2[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2 -// rpy2[2] = RAD2DEG * atan2f(R12, R11); -// rpy2[0] = RAD2DEG * atan2f(R23, R33); -// } - -// attActData.Roll = rpy2[0]; -// attActData.Pitch = rpy2[1]; -// attActData.Yaw = rpy2[2]; -// attActData.q1 = q[0]; -// attActData.q2 = q[1]; -// attActData.q3 = q[2]; -// attActData.q4 = q[3]; -// attActual->setData(attActData); -// /*****************************************/ -// } -// } - /**********************************************************************************************/ -// if (settings.gcsReciever) { -// static QTime gcsRcvrTime = currentTime; -// if (!settings.manualOutput || gcsRcvrTime.msecsTo(currentTime) >= settings.outputRate) { -// GCSReceiver::DataFields gcsRcvrData; -// gcsRcvrData = gcsReceiver->getData(); - -// for (int i = 0; i < 8; ++i) -// gcsRcvrData.Channel[i] = 1500 + (ch[i] * 500); - -// gcsReceiver->setData(gcsRcvrData); -// if (settings.manualOutput) -// gcsRcvrTime = currentTime; -// } -// } else if (settings.manualControl) { -// // not implemented yet -// } - /**********************************************************************************************/ -// if (settings.gpsPosition) { - static QTime gpsPosTime = currentTime; -// if (gpsPosTime.msecsTo(currentTime) >= settings.gpsPosRate) { - out.altitude = posZ; - out.heading = yaw * RAD2DEG; - out.latitude = lat * 10e6; - out.longitude = lon * 10e6; - out.groundspeed = qSqrt(velX * velX + velY * velY); -// gpsPosData.GeoidSeparation = 0.0; -// gpsPosData.Satellites = 8; -// gpsPosData.PDOP = 3.0; -// gpsPosData.Status = GPSPosition::STATUS_FIX3D; - -// gpsPosition->setData(gpsPosData); -// gpsPosTime = currentTime; -// } -// } - /**********************************************************************************************/ -// if (settings.sonarAltitude) { -// static QTime sonarAltTime = currentTime; -// if (sonarAltTime.msecsTo(currentTime) >= settings.sonarAltRate) { -// SonarAltitude::DataFields sonarAltData; -// sonarAltData = sonarAlt->getData(); - -// float sAlt = settings.sonarMaxAlt; -// // 0.35 rad ~= 20 degree -// if ((agl < (sAlt * 2.0)) && (roll < 0.35) && (pitch < 0.35)) { -// float x = agl * qTan(roll); -// float y = agl * qTan(pitch); -// float h = qSqrt(x*x + y*y + agl*agl); -// sAlt = qMin(h, sAlt); -// } - -// sonarAltData.Altitude = sAlt; -// sonarAlt->setData(sonarAltData); -// sonarAltTime = currentTime; -// } -// } /**********************************************************************************************/ out.dstN = posY * 100; out.dstE = posX * 100; @@ -453,6 +209,9 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data) out.velEast = velX * 100; out.velDown = velZ * 100; //WHY ISN'T THIS `-velZ`??? + updateUAVOs(out); + + #ifdef DBG_TIMERS static int cntRX = 0; if (cntRX >= 100) { diff --git a/ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp b/ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp index fec61c88d..1d60978a8 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp @@ -98,44 +98,44 @@ bool FGSimulator::setupProcess() QString cmdShell("bash"); #endif - // Start shell (Note: Could not start FG directly on Windows, only through terminal!) - simProcess->start(cmdShell); - if (simProcess->waitForStarted() == false) - { - emit processOutput("Error:" + simProcess->errorString()); - return false; - } + // Start shell (Note: Could not start FG directly on Windows, only through terminal!) + simProcess->start(cmdShell); + if (simProcess->waitForStarted() == false) + { + emit processOutput("Error:" + simProcess->errorString()); + return false; + } - // Setup arguments - // Note: The input generic protocol is set to update at a much higher rate than the actual updates are sent by the GCS. - // If this is not done then a lag will be introduced by FlightGear, likelly because the receive socket buffer builds up during startup. - QString args("--fg-root=\"" + settings.dataPath + "\" " + - "--timeofday=noon " + - "--httpd=5400 " + - "--enable-hud " + - "--in-air " + - "--altitude=3000 " + - "--vc=100 " + - "--log-level=alert " + - "--generic=socket,out,20," + settings.hostAddress + "," + QString::number(settings.inPort) + ",udp,opfgprotocol"); - if(!settings.manual) - { - args.append(" --generic=socket,in,400," + settings.remoteHostAddress + "," + QString::number(settings.outPort) + ",udp,opfgprotocol"); - } + // Setup arguments + // Note: The input generic protocol is set to update at a much higher rate than the actual updates are sent by the GCS. + // If this is not done then a lag will be introduced by FlightGear, likelly because the receive socket buffer builds up during startup. + QString args("--fg-root=\"" + settings.dataPath + "\" " + + "--timeofday=noon " + + "--httpd=5400 " + + "--enable-hud " + + "--in-air " + + "--altitude=3000 " + + "--vc=100 " + + "--log-level=alert " + + "--generic=socket,out,20," + settings.hostAddress + "," + QString::number(settings.inPort) + ",udp,opfgprotocol"); + if(!settings.manualControl) + { + args.append(" --generic=socket,in,400," + settings.remoteAddress + "," + QString::number(settings.outPort) + ",udp,opfgprotocol"); + } - // Start FlightGear - only if checkbox is selected in HITL options page - if (settings.startSim) - { - QString cmd("\"" + settings.binPath + "\" " + args + "\n"); - simProcess->write(cmd.toAscii()); - } - else - { - emit processOutput("Start Flightgear from the command line with the following arguments: \n\n" + args + "\n\n" + - "You can optionally run Flightgear from a networked computer.\n" + - "Make sure the computer running Flightgear can can ping your local interface adapter. ie." + settings.hostAddress + "\n" - "Remote computer must have the correct OpenPilot protocol installed."); - } + // Start FlightGear - only if checkbox is selected in HITL options page + if (settings.startSim) + { + QString cmd("\"" + settings.binPath + "\" " + args + "\n"); + simProcess->write(cmd.toAscii()); + } + else + { + emit processOutput("Start Flightgear from the command line with the following arguments: \n\n" + args + "\n\n" + + "You can optionally run Flightgear from a networked computer.\n" + + "Make sure the computer running Flightgear can can ping your local interface adapter. ie." + settings.hostAddress + "\n" + "Remote computer must have the correct OpenPilot protocol installed."); + } udpCounterGCSsend = 0; @@ -207,7 +207,7 @@ void FGSimulator::transmitUpdate() QByteArray data = cmd.toAscii(); - if(outSocket->writeDatagram(data, QHostAddress(settings.remoteHostAddress), settings.outPort) == -1) + if(outSocket->writeDatagram(data, QHostAddress(settings.remoteAddress), settings.outPort) == -1) { emit processOutput("Error sending UDP packet to FG: " + outSocket->errorString() + "\n"); } @@ -219,7 +219,7 @@ void FGSimulator::transmitUpdate() // V2.0 does not currently work with --generic-protocol } - if(!settings.manual) + if(!settings.manualControl) { actData.Roll = ailerons; actData.Pitch = -elevator; diff --git a/ground/openpilotgcs/src/plugins/hitlnew/hitlconfiguration.cpp b/ground/openpilotgcs/src/plugins/hitlnew/hitlconfiguration.cpp index 52d755857..bbee0db49 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/hitlconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/hitlnew/hitlconfiguration.cpp @@ -30,122 +30,62 @@ HITLConfiguration::HITLConfiguration(QString classId, QSettings* qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent) { - settings.simulatorId = ""; - settings.binPath = ""; - settings.dataPath = ""; - settings.manual = false; - settings.startSim = false; - settings.hostAddress = "127.0.0.1"; - settings.remoteHostAddress = "127.0.0.1"; - settings.outPort = 0; - settings.inPort = 0; - settings.latitude = ""; - settings.longitude = ""; - - //if a saved configuration exists load it - if(qSettings != 0) { - settings.simulatorId = qSettings->value("simulatorId").toString(); - settings.binPath = qSettings->value("binPath").toString(); - settings.dataPath = qSettings->value("dataPath").toString(); - settings.manual = qSettings->value("manual").toBool(); - settings.startSim = qSettings->value("startSim").toBool(); - settings.hostAddress = qSettings->value("hostAddress").toString(); - settings.remoteHostAddress = qSettings->value("remoteHostAddress").toString(); - settings.outPort = qSettings->value("outPort").toInt(); - settings.inPort = qSettings->value("inPort").toInt(); - settings.latitude = qSettings->value("latitude").toString(); - settings.longitude = qSettings->value("longitude").toString(); - } - bool homeLocation = true; - quint16 homeLocRate = 0; - - bool attRaw = true; - quint8 attRawRate = 20; - - bool attActual = true; - bool attActHW = false; - bool attActSim = true; - bool attActCalc = false; - - bool sonarAltitude = false; - float sonarMaxAlt = 5.0; - quint16 sonarAltRate = 50; - - bool gpsPosition = true; - quint16 gpsPosRate = 200; - - bool inputCommand = true; - bool gcsReciever = true; - bool manualControl = false; - - bool manualOutput = false; - quint8 outputRate = 20; - settings.simulatorId = ""; settings.binPath = ""; settings.dataPath = ""; - settings.manual = false; + settings.manualControl = false; settings.startSim = false; settings.addNoise = false; settings.hostAddress = "127.0.0.1"; - settings.remoteHostAddress = "127.0.0.1"; + settings.remoteAddress = "127.0.0.1"; settings.outPort = 0; settings.inPort = 0; settings.latitude = ""; settings.longitude = ""; - //if a saved configuration exists load it - if(qSettings != 0) { - settings.simulatorId = qSettings->value("simulatorId").toString(); - settings.binPath = qSettings->value("binPath").toString(); - settings.dataPath = qSettings->value("dataPath").toString(); - settings.manual = qSettings->value("manual").toBool(); - settings.addNoise = qSettings->value("noiseCheckBox").toBool(); - settings.startSim = qSettings->value("startSim").toBool(); - settings.hostAddress = qSettings->value("hostAddress").toString(); - settings.remoteHostAddress = qSettings->value("remoteHostAddress").toString(); - settings.outPort = qSettings->value("outPort").toInt(); - settings.inPort = qSettings->value("inPort").toInt(); - settings.latitude = qSettings->value("latitude").toString(); - settings.longitude = qSettings->value("longitude").toString(); - } - -#ifdef HRRRRRRRRRRR // if a saved configuration exists load it if (qSettings != 0) { - settings.simulatorId = qSettings->value("simulatorId", simulatorId).toString(); - settings.hostAddress = qSettings->value("hostAddress", hostAddress).toString(); - settings.inPort = qSettings->value("inPort", inPort).toInt(); - settings.remoteAddress = qSettings->value("remoteAddress", remoteAddress).toString(); - settings.outPort = qSettings->value("outPort", outPort).toInt(); - settings.binPath = qSettings->value("binPath", binPath).toString(); - settings.dataPath = qSettings->value("dataPath", dataPath).toString(); + settings.simulatorId = qSettings->value("simulatorId").toString(); + settings.hostAddress = qSettings->value("hostAddress").toString(); + settings.inPort = qSettings->value("inPort").toInt(); + settings.remoteAddress = qSettings->value("remoteAddress").toString(); + settings.outPort = qSettings->value("outPort").toInt(); - settings.homeLocation = qSettings->value("homeLocation", homeLocation).toBool(); - settings.homeLocRate = qSettings->value("homeLocRate", homeLocRate).toInt(); + settings.binPath = qSettings->value("binPath").toString(); + settings.dataPath = qSettings->value("dataPath").toString(); - settings.attRaw = qSettings->value("attRaw", attRaw).toBool(); - settings.attRawRate = qSettings->value("attRawRate", attRawRate).toInt(); + settings.latitude = qSettings->value("latitude").toString(); + settings.longitude = qSettings->value("longitude").toString(); + settings.startSim = qSettings->value("startSim").toBool(); + settings.addNoise = qSettings->value("noiseCheckBox").toBool(); - settings.attActual = qSettings->value("attActual", attActual).toBool(); - settings.attActHW = qSettings->value("attActHW", attActHW).toBool(); - settings.attActSim = qSettings->value("attActSim", attActSim).toBool(); - settings.attActCalc = qSettings->value("attActCalc", attActCalc).toBool(); + settings.homeLocation = qSettings->value("homeLocation").toBool(); + settings.homeLocRate = qSettings->value("homeLocRate").toInt(); - settings.sonarAltitude = qSettings->value("sonarAltitude", sonarAltitude).toBool(); - settings.sonarMaxAlt = qSettings->value("sonarMaxAlt", sonarMaxAlt).toFloat(); - settings.sonarAltRate = qSettings->value("sonarAltRate", sonarAltRate).toInt(); + settings.attRaw = qSettings->value("attRaw").toBool(); + settings.attRawRate = qSettings->value("attRawRate").toInt(); - settings.gpsPosition = qSettings->value("gpsPosition", gpsPosition).toBool(); - settings.gpsPosRate = qSettings->value("gpsPosRate", gpsPosRate).toInt(); + settings.attActual = qSettings->value("attActual").toBool(); + settings.attActHW = qSettings->value("attActHW").toBool(); + settings.attActSim = qSettings->value("attActSim").toBool(); + settings.attActCalc = qSettings->value("attActCalc").toBool(); - settings.inputCommand = qSettings->value("inputCommand", inputCommand).toBool(); - settings.gcsReciever = qSettings->value("gcsReciever", gcsReciever).toBool(); - settings.manualControl = qSettings->value("manualControl", manualControl).toBool(); - settings.manualOutput = qSettings->value("manualOutput", manualOutput).toBool(); - settings.outputRate = qSettings->value("outputRate", outputRate).toInt(); - } else { + settings.sonarAltitude = qSettings->value("sonarAltitude").toBool(); + settings.sonarMaxAlt = qSettings->value("sonarMaxAlt").toFloat(); + settings.sonarAltRate = qSettings->value("sonarAltRate").toInt(); + + settings.gpsPosition = qSettings->value("gpsPosition").toBool(); + settings.gpsPosRate = qSettings->value("gpsPosRate").toInt(); + + settings.inputCommand = qSettings->value("inputCommand").toBool(); + settings.gcsReciever = qSettings->value("gcsReciever").toBool(); + settings.manualControl = qSettings->value("manualControl").toBool(); + settings.manualOutput = qSettings->value("manualOutput").toBool(); + settings.outputRate = qSettings->value("outputRate").toInt(); + } +#ifdef HHRRRRRRRRRR + else { settings.simulatorId = simulatorId; settings.hostAddress = hostAddress; settings.inPort = inPort; @@ -195,25 +135,20 @@ IUAVGadgetConfiguration *HITLConfiguration::clone() */ void HITLConfiguration::saveConfig(QSettings* qSettings) const { qSettings->setValue("simulatorId", settings.simulatorId); -#ifdef HRRRRRRRRRRR - qSettings->setValue("hostAddress", settings.hostAddress); - qSettings->setValue("inPort", settings.inPort); - qSettings->setValue("remoteAddress", settings.remoteAddress); - qSettings->setValue("outPort", settings.outPort); -#endif qSettings->setValue("binPath", settings.binPath); qSettings->setValue("dataPath", settings.dataPath); - qSettings->setValue("manual", settings.manual); - qSettings->setValue("startSim", settings.startSim); + qSettings->setValue("hostAddress", settings.hostAddress); - qSettings->setValue("remoteHostAddress", settings.remoteHostAddress); + qSettings->setValue("remoteAddress", settings.remoteAddress); qSettings->setValue("outPort", settings.outPort); qSettings->setValue("inPort", settings.inPort); + qSettings->setValue("latitude", settings.latitude); qSettings->setValue("longitude", settings.longitude); + qSettings->setValue("addNoise", settings.addNoise); + qSettings->setValue("startSim", settings.startSim); + qSettings->setValue("manualControl", settings.manualControl); -#ifdef HRRRRRRRRRRR - //Added by hrrrr. Is it all used? qSettings->setValue("homeLocation", settings.homeLocation); qSettings->setValue("homeLocRate", settings.homeLocRate); qSettings->setValue("attRaw", settings.attRaw); @@ -232,6 +167,5 @@ void HITLConfiguration::saveConfig(QSettings* qSettings) const { qSettings->setValue("manualControl", settings.manualControl); qSettings->setValue("manualOutput", settings.manualOutput); qSettings->setValue("outputRate", settings.outputRate); -#endif } diff --git a/ground/openpilotgcs/src/plugins/hitlnew/hitloptionspage.cpp b/ground/openpilotgcs/src/plugins/hitlnew/hitloptionspage.cpp index dd9cadb63..b979e495d 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/hitloptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/hitlnew/hitloptionspage.cpp @@ -67,47 +67,49 @@ QWidget *HITLOptionsPage::createPage(QWidget *parent) m_optionsPage->dataPath->setPromptDialogTitle(tr("Choose flight simulator data directory")); // Restore the contents from the settings: - foreach(SimulatorCreator* creator, HITLPlugin::typeSimulators) - { - QString id = config->Settings().simulatorId; - if(creator->ClassId() == id) - m_optionsPage->chooseFlightSimulator->setCurrentIndex(HITLPlugin::typeSimulators.indexOf(creator)); - } + foreach(SimulatorCreator* creator, HITLPlugin::typeSimulators) + { + QString id = config->Settings().simulatorId; + if(creator->ClassId() == id) + m_optionsPage->chooseFlightSimulator->setCurrentIndex(HITLPlugin::typeSimulators.indexOf(creator)); + } - m_optionsPage->executablePath->setPath(config->Settings().binPath); - m_optionsPage->dataPath->setPath(config->Settings().dataPath); - m_optionsPage->manualControl->setChecked(config->Settings().manual); - m_optionsPage->startSim->setChecked(config->Settings().startSim); + m_optionsPage->executablePath->setPath(config->Settings().binPath); + m_optionsPage->dataPath->setPath(config->Settings().dataPath); + m_optionsPage->manualControl->setChecked(config->Settings().manualControl); + m_optionsPage->startSim->setChecked(config->Settings().startSim); + m_optionsPage->noiseCheckBox->setChecked(config->Settings().addNoise); - m_optionsPage->hostAddress->setText(config->Settings().hostAddress); - m_optionsPage->remoteHostAddress->setText(config->Settings().remoteHostAddress); - m_optionsPage->outputPort->setText(QString::number(config->Settings().outPort)); - m_optionsPage->inputPort->setText(QString::number(config->Settings().inPort)); - m_optionsPage->latitude->setText(config->Settings().latitude); - m_optionsPage->longitude->setText(config->Settings().longitude); + m_optionsPage->hostAddress->setText(config->Settings().hostAddress); + m_optionsPage->remoteAddress->setText(config->Settings().remoteAddress); + m_optionsPage->outputPort->setText(QString::number(config->Settings().outPort)); + m_optionsPage->inputPort->setText(QString::number(config->Settings().inPort)); + m_optionsPage->latitude->setText(config->Settings().latitude); + m_optionsPage->longitude->setText(config->Settings().longitude); return optionsPageWidget; } void HITLOptionsPage::apply() { - SimulatorSettings settings; - int i = m_optionsPage->chooseFlightSimulator->currentIndex(); + SimulatorSettings settings; + int i = m_optionsPage->chooseFlightSimulator->currentIndex(); - settings.simulatorId = m_optionsPage->chooseFlightSimulator->itemData(i).toString(); - settings.binPath = m_optionsPage->executablePath->path(); - settings.dataPath = m_optionsPage->dataPath->path(); - settings.manual = m_optionsPage->manualControl->isChecked(); - settings.startSim = m_optionsPage->startSim->isChecked(); - settings.hostAddress = m_optionsPage->hostAddress->text(); - settings.remoteHostAddress = m_optionsPage->remoteHostAddress->text(); + settings.simulatorId = m_optionsPage->chooseFlightSimulator->itemData(i).toString(); + settings.binPath = m_optionsPage->executablePath->path(); + settings.dataPath = m_optionsPage->dataPath->path(); + settings.manualControl = m_optionsPage->manualControl->isChecked(); + settings.startSim = m_optionsPage->startSim->isChecked(); + settings.addNoise = m_optionsPage->noiseCheckBox->isChecked(); + settings.hostAddress = m_optionsPage->hostAddress->text(); + settings.remoteAddress = m_optionsPage->remoteAddress->text(); - settings.inPort = m_optionsPage->inputPort->text().toInt(); - settings.outPort = m_optionsPage->outputPort->text().toInt(); - settings.longitude = m_optionsPage->longitude->text(); - settings.latitude = m_optionsPage->latitude->text(); + settings.inPort = m_optionsPage->inputPort->text().toInt(); + settings.outPort = m_optionsPage->outputPort->text().toInt(); + settings.longitude = m_optionsPage->longitude->text(); + settings.latitude = m_optionsPage->latitude->text(); - config->setSimulatorSettings(settings); + config->setSimulatorSettings(settings); } void HITLOptionsPage::finish() diff --git a/ground/openpilotgcs/src/plugins/hitlnew/hitloptionspage.ui b/ground/openpilotgcs/src/plugins/hitlnew/hitloptionspage.ui index 2120c94b5..17d050197 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/hitloptionspage.ui +++ b/ground/openpilotgcs/src/plugins/hitlnew/hitloptionspage.ui @@ -6,258 +6,968 @@ 0 0 - 590 - 367 + 885 + 741 - + 0 0 + + + 0 + 0 + + Form - - + + + 3 + + 0 - - - - Choose flight simulator: - - + + 0 + + + 0 + + + 3 + + + + + + + Choose flight simulator: + + + + + + + - - - - - - - Qt::Horizontal + + + + + 0 + 0 + - - - - - - Qt::Vertical - - + - 20 - 182 + 0 + 100 - - - - - - - 0 - 0 - + + + 350 + 16777215 + - - Local interface address (IP): - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + IP addresses + + + + 10 + 30 + 323 + 24 + + + + + + + Local host: + + + + + + + + 0 + 0 + + + + + 125 + 16777215 + + + + For communication with sim computer via network. Should be the IP address of one of the interfaces of the GCS computer. + + + + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + Port: + + + + + + + + 0 + 0 + + + + + 55 + 16777215 + + + + IP port for receiving data from sim + + + + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + + 10 + 60 + 322 + 24 + + + + + + + Remote host: + + + + + + + + 0 + 0 + + + + + 125 + 16777215 + + + + Only required if running simulator on remote machine. Should be the IP of the machine on which the simulator is running. + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + Port: + + + + + + + + 0 + 0 + + + + + 55 + 16777215 + + + + IP port for sending data to sim + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + layoutWidget + layoutWidget - - - - Latitude in degrees: + + + + + 0 + 180 + + + Program parameters + + + + + 120 + 60 + 401 + 16 + + + + + 0 + 0 + + + + + + + 10 + 60 + 94 + 16 + + + + + 0 + 0 + + + + + 110 + 16777215 + + + + Data directory: + + + + + + 120 + 34 + 401 + 16 + + + + + 0 + 0 + + + + + + + 10 + 34 + 104 + 16 + + + + + 0 + 0 + + + + + 110 + 16777215 + + + + Path executable: + + + + + + 10 + 120 + 229 + 20 + + + + Check this box to start the simulator on the local computer + + + Start simulator on local machine + + + + + + 10 + 150 + 89 + 20 + + + + Add noise to sensor simulation + + + Add noise + + + + + + 10 + 80 + 491 + 16 + + + + Qt::Horizontal + + + + + + 10 + 90 + 161 + 22 + + + + Initial latitude (decimal): + + + + + + 290 + 90 + 171 + 22 + + + + Initial longitude (decimal): + + + + + + 460 + 90 + 100 + 22 + + + + + 0 + 0 + + + + + 100 + 16777215 + + + + + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + 170 + 90 + 100 + 22 + + + + + 0 + 0 + + + + + 100 + 16777215 + + + + + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + - - - - Longitude in degrees: + + + + QFormLayout::AllNonFixedFieldsGrow - + + + + QFrame::StyledPanel + + + QFrame::Raised + + + + - - - - - 0 - 0 - - - - Data directory: - - - - - - - - 0 - 0 - - - - For receiving data from sim - - - - - - - - 1 - 0 - - - - - - - - - 0 - 0 - - - - Output Port: - - - - - - - - - - - 0 - 0 - - - - - - - - - 0 - 0 - - - - For sending data to sim - - - - - - - - 0 - 0 - - - - Manual aircraft control (can be used when hardware is not available) - - - Manual aircraft control (can be used when hardware is not available) - - - - - - - - 0 - 0 - - - - Path executable: - - - - - - - - 0 - 0 - - - - - - - - Input Port: - - - - - - - true - - - - 0 - 0 - - - - Check this box to start the simulator on the local computer - - - Start simulator on local machine - - - - - - - Remote interface address (IP): - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 0 - 0 - - - - Only required if running simulator on remote machine. Should be the IP of the machine on which the simulator is running. - - - - - - - - 0 - 0 - - - - For communication with sim computer via network. Should be the IP address of one of the interfaces of the GCS computer. - - + + + + + + Attitude data + + + + 3 + + + 3 + + + 3 + + + 3 + + + 0 + + + + + AttitudeRaw (gyro, accels) + + + true + + + true + + + false + + + + 3 + + + 3 + + + 0 + + + 0 + + + + + Refresh rate + + + + + + + false + + + ms + + + 10 + + + 100 + + + 20 + + + + + + + + + + AttitudeActual + + + true + + + true + + + false + + + + 3 + + + 3 + + + 0 + + + 0 + + + + + send raw data to board + + + + + + + + 75 + true + + + + + + + use values from simulator + + + true + + + + + + + + + + calculate from (noised) simulated sensor data + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + + + 0 + 275 + + + + Other data + + + + 3 + + + 3 + + + 3 + + + 3 + + + 0 + + + + + Ground truth position and velocity + + + true + + + true + + + false + + + + 3 + + + 3 + + + 0 + + + 0 + + + + + Refresh rate + + + + + + + false + + + 0 - update once, or every N seconds + + + sec + + + 10 + + + + + + + + + + GPS data + + + true + + + true + + + false + + + + 3 + + + 3 + + + 0 + + + 0 + + + + + Refresh rate + + + + + + + false + + + ms + + + 100 + + + 2000 + + + 500 + + + + + + + + + + false + + + SonarAltitude + + + true + + + true + + + false + + + + 3 + + + 6 + + + 3 + + + 0 + + + 0 + + + + + + + Range detectioon + + + + + + + m + + + 1 + + + 10 + + + 5 + + + + + + + Refresh rate + + + + + + + ms + + + 20 + + + 2000 + + + 10 + + + 50 + + + + + + + + + + + + false + + + Map command from simulator + + + true + + + true + + + false + + + + 3 + + + 3 + + + 0 + + + 0 + + + + + + 75 + true + + + + to GCSReciver + + + true + + + + + + + false + + + to ManualCtrl (not implemented) + + + + + + + + + + 6 + + + + + false + + + Maximum output rate + + + + + + + false + + + ms + + + 10 + + + 100 + + + 5 + + + 15 + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + @@ -269,6 +979,13 @@ 1 + + chooseFlightSimulator + hostAddress + inputPort + remoteAddress + outputPort + diff --git a/ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp b/ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp index 103677199..fb1b157d8 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp @@ -116,49 +116,53 @@ void Simulator::onDeleteSimulator(void) void Simulator::onStart() { - QMutexLocker locker(&lock); + QMutexLocker locker(&lock); - QThread* mainThread = QThread::currentThread(); - qDebug() << "Simulator Thread: "<< mainThread; + QThread* mainThread = QThread::currentThread(); - // Get required UAVObjects - ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager* objManager = pm->getObject(); - actDesired = ActuatorDesired::GetInstance(objManager); - manCtrlCommand = ManualControlCommand::GetInstance(objManager); - flightStatus = FlightStatus::GetInstance(objManager); - posHome = HomeLocation::GetInstance(objManager); - velActual = VelocityActual::GetInstance(objManager); - posActual = PositionActual::GetInstance(objManager); - altActual = BaroAltitude::GetInstance(objManager); - attActual = AttitudeActual::GetInstance(objManager); - accels = Accels::GetInstance(objManager); - gyros = Gyros::GetInstance(objManager); - gpsPos = GPSPosition::GetInstance(objManager); - telStats = GCSTelemetryStats::GetInstance(objManager); + qDebug() << "Simulator Thread: "<< mainThread; - // Listen to autopilot connection events - TelemetryManager* telMngr = pm->getObject(); - connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect())); - connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); - //connect(telStats, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(telStatsUpdated(UAVObject*))); + // Get required UAVObjects + ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager* objManager = pm->getObject(); + actDesired = ActuatorDesired::GetInstance(objManager); + manCtrlCommand = ManualControlCommand::GetInstance(objManager); + flightStatus = FlightStatus::GetInstance(objManager); + posHome = HomeLocation::GetInstance(objManager); + velActual = VelocityActual::GetInstance(objManager); + posActual = PositionActual::GetInstance(objManager); + baroAlt = BaroAltitude::GetInstance(objManager); + baroAirspeed = BaroAirspeed::GetInstance(objManager); + attActual = AttitudeActual::GetInstance(objManager); + attSettings = AttitudeSettings::GetInstance(objManager); + accels = Accels::GetInstance(objManager); + gyros = Gyros::GetInstance(objManager); + gpsPos = GPSPosition::GetInstance(objManager); + gpsVel = GPSVelocity::GetInstance(objManager); + telStats = GCSTelemetryStats::GetInstance(objManager); - // If already connect setup autopilot - GCSTelemetryStats::DataFields stats = telStats->getData(); - if ( stats.Status == GCSTelemetryStats::STATUS_CONNECTED ) - onAutopilotConnect(); + // Listen to autopilot connection events + TelemetryManager* telMngr = pm->getObject(); + connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect())); + connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); + //connect(telStats, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(telStatsUpdated(UAVObject*))); - inSocket = new QUdpSocket(); - outSocket = new QUdpSocket(); - setupUdpPorts(settings.hostAddress,settings.inPort,settings.outPort); + // If already connect setup autopilot + GCSTelemetryStats::DataFields stats = telStats->getData(); + if ( stats.Status == GCSTelemetryStats::STATUS_CONNECTED ) + onAutopilotConnect(); + + inSocket = new QUdpSocket(); + outSocket = new QUdpSocket(); + setupUdpPorts(settings.hostAddress,settings.inPort,settings.outPort); emit processOutput("\nLocal interface: " + settings.hostAddress + "\n" + \ - "Remote interface: " + settings.remoteHostAddress + "\n" + \ + "Remote interface: " + settings.remoteAddress + "\n" + \ "inputPort: " + QString::number(settings.inPort) + "\n" + \ "outputPort: " + QString::number(settings.outPort) + "\n"); qxtLog->info("\nLocal interface: " + settings.hostAddress + "\n" + \ - "Remote interface: " + settings.remoteHostAddress + "\n" + \ + "Remote interface: " + settings.remoteAddress + "\n" + \ "inputPort: " + QString::number(settings.inPort) + "\n" + \ "outputPort: " + QString::number(settings.outPort) + "\n"); @@ -297,3 +301,326 @@ void Simulator::telStatsUpdated(UAVObject* obj) } } + +void Simulator::updateUAVOs(Output2OP out){ + +// QTime currentTime = QTime::currentTime(); + + + Noise noise; + HitlNoiseGeneration noiseSource; + if(settings.addNoise){ + noise = noiseSource.generateNoise(); + } + else{ + memset(&noise, 0, sizeof(Noise)); + } + + HomeLocation::DataFields homeData = posHome->getData(); + if(!once) + { + // Upon startup, we reset the HomeLocation object to + // the plane's location: + memset(&homeData, 0, sizeof(HomeLocation::DataFields)); + // Update homelocation + homeData.Latitude = out.latitude; //Already in *10^7 integer format + homeData.Longitude = out.longitude; //Already in *10^7 integer format + homeData.Altitude = out.altitude; + double LLA[3]; + LLA[0]=out.latitude; + LLA[1]=out.longitude; + LLA[2]=out.altitude; + double ECEF[3]; + double RNE[9]; + Utils::CoordinateConversions().RneFromLLA(LLA,(double (*)[3])RNE); + Utils::CoordinateConversions().LLA2ECEF(LLA,ECEF); + homeData.Be[0]=0; + homeData.Be[1]=0; + homeData.Be[2]=0; + posHome->setData(homeData); + posHome->updated(); + + // Compute initial distance + initN = out.dstN; + initE = out.dstE; + initD = out.dstD; + + once=true; + } + + + // Update attActual object + AttitudeActual::DataFields attActualData; + attActualData = attActual->getData(); + + if (settings.attActHW) { + // do nothing + /*****************************************/ + } else if (settings.attActSim) { + // take all data from simulator + attActualData.Roll = out.roll + noise.attActualData.Roll; //roll; + attActualData.Pitch = out.pitch + noise.attActualData.Pitch; // pitch + attActualData.Yaw = out.heading + noise.attActualData.Yaw; // Yaw + float rpy[3]; + float quat[4]; + rpy[0] = attActualData.Roll; + rpy[1] = attActualData.Pitch; + rpy[2] = attActualData.Yaw; + Utils::CoordinateConversions().RPY2Quaternion(rpy,quat); + attActualData.q1 = quat[0]; + attActualData.q2 = quat[1]; + attActualData.q3 = quat[2]; + attActualData.q4 = quat[3]; + + //Set UAVO + attActual->setData(attActualData); + /*****************************************/ + } else if (settings.attActCalc) { + // calculate RPY with code from Attitude module + AttitudeActual::DataFields attActData; + + static float q[4] = {1, 0, 0, 0}; + static float gyro_correct_int2 = 0; + + float dT = out.delT; + + AttitudeSettings::DataFields attSettData = attSettings->getData(); + float accelKp = attSettData.AccelKp * 0.1666666666666667; + float accelKi = attSettData.AccelKp * 0.1666666666666667; + float yawBiasRate = attSettData.YawBiasRate; + + // calibrate sensors on arming + if (flightStatus->getData().Armed == FlightStatus::ARMED_ARMING) { + accelKp = 2.0; + accelKi = 0.9; + } + + float gyro[3] = {out.rollRate, out.pitchRate, out.yawRate}; + float attRawAcc[3] = {out.accX, out.accY, out.accZ}; + + // code from Attitude module begin /////////////////////////////// + float *accels = attRawAcc; + float grot[3]; + float accel_err[3]; + + // Rotate gravity to body frame and cross with accels + grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2])); + grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1])); + grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]); + + // CrossProduct + { + accel_err[0] = accels[1]*grot[2] - grot[1]*accels[2]; + accel_err[1] = grot[0]*accels[2] - accels[0]*grot[2]; + accel_err[2] = accels[0]*grot[1] - grot[0]*accels[1]; + } + + // Account for accel magnitude + float accel_mag = sqrt(accels[0] * accels[0] + accels[1] * accels[1] + accels[2] * accels[2]); + accel_err[0] /= accel_mag; + accel_err[1] /= accel_mag; + accel_err[2] /= accel_mag; + + // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s + gyro_correct_int2 += -gyro[2] * yawBiasRate; + + // Correct rates based on error, integral component dealt with in updateSensors + gyro[0] += accel_err[0] * accelKp / dT; + gyro[1] += accel_err[1] * accelKp / dT; + gyro[2] += accel_err[2] * accelKp / dT + gyro_correct_int2; + + // Work out time derivative from INSAlgo writeup + // Also accounts for the fact that gyros are in deg/s + float qdot[4]; + qdot[0] = (-q[1] * gyro[0] - q[2] * gyro[1] - q[3] * gyro[2]) * dT * M_PI / 180 / 2; + qdot[1] = (+q[0] * gyro[0] - q[3] * gyro[1] + q[2] * gyro[2]) * dT * M_PI / 180 / 2; + qdot[2] = (+q[3] * gyro[0] + q[0] * gyro[1] - q[1] * gyro[2]) * dT * M_PI / 180 / 2; + qdot[3] = (-q[2] * gyro[0] + q[1] * gyro[1] + q[0] * gyro[2]) * dT * M_PI / 180 / 2; + + // Take a time step + q[0] += qdot[0]; + q[1] += qdot[1]; + q[2] += qdot[2]; + q[3] += qdot[3]; + + if(q[0] < 0) { + q[0] = -q[0]; + q[1] = -q[1]; + q[2] = -q[2]; + q[3] = -q[3]; + } + + // Renomalize + float qmag = sqrt((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3])); + q[0] /= qmag; + q[1] /= qmag; + q[2] /= qmag; + q[3] /= qmag; + + // If quaternion has become inappropriately short or is nan reinit. + // THIS SHOULD NEVER ACTUALLY HAPPEN + if((fabs(qmag) < 1e-3) || (qmag != qmag)) { + q[0] = 1; + q[1] = 0; + q[2] = 0; + q[3] = 0; + } + + float rpy2[3]; + // Quaternion2RPY + { + float q0s, q1s, q2s, q3s; + q0s = q[0] * q[0]; + q1s = q[1] * q[1]; + q2s = q[2] * q[2]; + q3s = q[3] * q[3]; + + float R13, R11, R12, R23, R33; + R13 = 2 * (q[1] * q[3] - q[0] * q[2]); + R11 = q0s + q1s - q2s - q3s; + R12 = 2 * (q[1] * q[2] + q[0] * q[3]); + R23 = 2 * (q[2] * q[3] + q[0] * q[1]); + R33 = q0s - q1s - q2s + q3s; + + rpy2[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2 + rpy2[2] = RAD2DEG * atan2f(R12, R11); + rpy2[0] = RAD2DEG * atan2f(R23, R33); + } + + attActualData.Roll = rpy2[0]; + attActualData.Pitch = rpy2[1]; + attActualData.Yaw = rpy2[2]; + attActualData.q1 = q[0]; + attActualData.q2 = q[1]; + attActualData.q3 = q[2]; + attActualData.q4 = q[3]; + + //Set UAVO + attActual->setData(attActualData); + /*****************************************/ + } + + + if (settings.gcsReciever) { +// static QTime gcsRcvrTime = currentTime; +// if (!settings.manualOutput || gcsRcvrTime.msecsTo(currentTime) >= settings.outputRate) { +// GCSReceiver::DataFields gcsRcvrData; +// gcsRcvrData = gcsReceiver->getData(); + +// for (int i = 0; i < 8; ++i) +// gcsRcvrData.Channel[i] = 1500 + (ch[i] * 500); + +// gcsReceiver->setData(gcsRcvrData); +// if (settings.manualOutput) +// gcsRcvrTime = currentTime; +// } + } else if (settings.manualControl) { + // not implemented yet + } + + + if (settings.gpsPosition) { +// if (gpsPosTime.msecsTo(currentTime) >= settings.gpsPosRate) { + // Update GPS Position objects + GPSPosition::DataFields gpsPosData; + memset(&gpsPosData, 0, sizeof(GPSPosition::DataFields)); + gpsPosData.Altitude = out.altitude + noise.gpsPosData.Altitude; + gpsPosData.Heading = out.heading + noise.gpsPosData.Heading; + gpsPosData.Groundspeed = out.groundspeed + noise.gpsPosData.Groundspeed; + gpsPosData.Latitude = out.latitude + noise.gpsPosData.Latitude; //Already in *10^7 integer format + gpsPosData.Longitude = out.longitude + noise.gpsPosData.Longitude; //Already in *10^7 integer format + gpsPosData.GeoidSeparation = 0.0; + gpsPosData.PDOP = 3.0; + gpsPosData.VDOP = gpsPosData.PDOP*1.5; + gpsPosData.Satellites = 10; + gpsPosData.Status = GPSPosition::STATUS_FIX3D; + + gpsPos->setData(gpsPosData); + + // Update GPS Velocity.{North,East,Down} + GPSVelocity::DataFields gpsVelData; + memset(&gpsVelData, 0, sizeof(GPSVelocity::DataFields)); + gpsVelData.North = out.velNorth + noise.gpsVelData.North; + gpsVelData.East = out.velEast + noise.gpsVelData.East; + gpsVelData.Down = out.velDown + noise.gpsVelData.Down; + + gpsVel->setData(gpsVelData); + +// static QTime gpsPosTime = currentTime; +// gpsPosTime = currentTime; +// } + + } + + + // Update VelocityActual.{North,East,Down} + if (settings.groundTruth) { + VelocityActual::DataFields velocityActualData; + memset(&velocityActualData, 0, sizeof(VelocityActual::DataFields)); + velocityActualData.North = out.velNorth + noise.velocityActualData.North; + velocityActualData.East = out.velEast + noise.velocityActualData.East; + velocityActualData.Down = out.velDown + noise.velocityActualData.Down; + velActual->setData(velocityActualData); + + // Update PositionActual.{Nort,East,Down} + PositionActual::DataFields positionActualData; + memset(&positionActualData, 0, sizeof(PositionActual::DataFields)); + positionActualData.North = (out.dstN-initN) + noise.positionActualData.North; + positionActualData.East = (out.dstE-initE) + noise.positionActualData.East; + positionActualData.Down = (out.dstD/*-initD*/) + noise.positionActualData.Down; + posActual->setData(positionActualData); + } + + if (settings.sonarAltitude) { +// static QTime sonarAltTime = currentTime; +// if (sonarAltTime.msecsTo(currentTime) >= settings.sonarAltRate) { +// SonarAltitude::DataFields sonarAltData; +// sonarAltData = sonarAlt->getData(); + +// float sAlt = settings.sonarMaxAlt; +// // 0.35 rad ~= 20 degree +// if ((agl < (sAlt * 2.0)) && (roll < 0.35) && (pitch < 0.35)) { +// float x = agl * qTan(roll); +// float y = agl * qTan(pitch); +// float h = qSqrt(x*x + y*y + agl*agl); +// sAlt = qMin(h, sAlt); +// } + +// sonarAltData.Altitude = sAlt; +// sonarAlt->setData(sonarAltData); +// sonarAltTime = currentTime; +// } + } + + // Update BaroAltitude object + BaroAltitude::DataFields baroAltData; + memset(&baroAltData, 0, sizeof(BaroAltitude::DataFields)); + baroAltData.Altitude = out.altitude + noise.baroAltData.Altitude; + baroAltData.Temperature = out.temperature + noise.baroAltData.Temperature; + baroAltData.Pressure = out.pressure + noise.baroAltData.Pressure; + baroAlt->setData(baroAltData); + + // Update BaroAirspeed object + BaroAirspeed::DataFields baroAirspeedData; + memset(&baroAirspeedData, 0, sizeof(BaroAirspeed::DataFields)); + baroAirspeedData.CalibratedAirspeed = out.calibratedAirspeed + noise.baroAirspeed.CalibratedAirspeed; + baroAirspeed->setData(baroAirspeedData); + + if (settings.attRaw) { + //Update gyroscope sensor data + Gyros::DataFields gyroData; + memset(&gyroData, 0, sizeof(Gyros::DataFields)); + gyroData.x = out.rollRate + noise.gyroData.x; + gyroData.y = out.pitchRate + noise.gyroData.y; + gyroData.z = out.yawRate + noise.gyroData.z; + gyros->setData(gyroData); + + //Update accelerometer sensor data + Accels::DataFields accelData; + memset(&accelData, 0, sizeof(Accels::DataFields)); + accelData.x = out.accX + noise.accelData.x; + accelData.y = out.accY + noise.accelData.y; + accelData.z = out.accZ + noise.accelData.z; + accels->setData(accelData); + } +} diff --git a/ground/openpilotgcs/src/plugins/hitlnew/simulator.h b/ground/openpilotgcs/src/plugins/hitlnew/simulator.h index 8ca683654..ee2721fab 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/simulator.h +++ b/ground/openpilotgcs/src/plugins/hitlnew/simulator.h @@ -107,13 +107,42 @@ typedef struct _CONNECTION QString binPath; QString dataPath; QString hostAddress; - QString remoteHostAddress; + QString remoteAddress; int outPort; int inPort; - bool manual; bool startSim; QString latitude; QString longitude; + + + + //Added by Hhrrrr + bool homeLocation; + quint16 homeLocRate; + + bool attRaw; + quint8 attRawRate; + + bool attActual; + bool attActHW; + bool attActSim; + bool attActCalc; + + bool sonarAltitude; + float sonarMaxAlt; + quint16 sonarAltRate; + + bool groundTruth; + bool gpsPosition; + quint16 gpsPosRate; + + bool inputCommand; + bool gcsReciever; + bool manualControl; + bool manualOutput; + quint8 outputRate; + + } SimulatorSettings; @@ -140,6 +169,7 @@ struct Output2OP{ float rollRate; //[deg/s] float pitchRate; //[deg/s] float yawRate; //[deg/s] + float delT; }; @@ -214,6 +244,7 @@ protected: BaroAltitude* baroAlt; BaroAirspeed* baroAirspeed; AttitudeActual* attActual; + AttitudeSettings* attSettings; VelocityActual* velActual; GPSPosition* gpsPos; GPSVelocity* gpsVel; diff --git a/ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.cpp b/ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.cpp index 1546b862e..051e23013 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.cpp @@ -117,59 +117,59 @@ void XplaneSimulator::transmitUpdate() // 11th data settings (flight con: ail/elv/rud) buf.clear(); - code = 11; - //quint8 header[] = "DATA"; - /* - stream << *((quint32*)header) << - (quint8)0x30 << - code << - *((quint32*)&elevator) << - *((quint32*)&ailerons) << - *((quint32*)&rudder) << - none << - *((quint32*)&ailerons) << - none << - none << - none; - */ - buf.append("DATA0"); - buf.append(reinterpret_cast(&code), sizeof(code)); - buf.append(reinterpret_cast(&elevator), sizeof(elevator)); - buf.append(reinterpret_cast(&ailerons), sizeof(ailerons)); - buf.append(reinterpret_cast(&rudder), sizeof(rudder)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&rudder), sizeof(rudder)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&none), sizeof(none)); - TraceBuf(buf.data(),41); + code = 11; + //quint8 header[] = "DATA"; + /* + stream << *((quint32*)header) << + (quint8)0x30 << + code << + *((quint32*)&elevator) << + *((quint32*)&ailerons) << + *((quint32*)&rudder) << + none << + *((quint32*)&ailerons) << + none << + none << + none; + */ + buf.append("DATA0"); + buf.append(reinterpret_cast(&code), sizeof(code)); + buf.append(reinterpret_cast(&elevator), sizeof(elevator)); + buf.append(reinterpret_cast(&ailerons), sizeof(ailerons)); + buf.append(reinterpret_cast(&rudder), sizeof(rudder)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&rudder), sizeof(rudder)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); +// TraceBuf(buf.data(),41); - if(outSocket->writeDatagram(buf, QHostAddress(settings.remoteHostAddress), settings.outPort) == -1) - { - emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n"); - } - //outSocket->write(buf); + if(outSocket->writeDatagram(buf, QHostAddress(settings.remoteAddress), settings.outPort) == -1) + { + emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n"); + } + //outSocket->write(buf); - // 25th data settings (throttle command) - buf.clear(); - code = 25; - //stream << *((quint32*)header) << (quint8)0x30 << code << *((quint32*)&throttle) << none << none - // << none << none << none << none << none; - buf.append("DATA0"); - buf.append(reinterpret_cast(&code), sizeof(code)); - buf.append(reinterpret_cast(&throttle), sizeof(throttle)); - buf.append(reinterpret_cast(&throttle), sizeof(throttle)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&none), sizeof(none)); + // 25th data settings (throttle command) + buf.clear(); + code = 25; + //stream << *((quint32*)header) << (quint8)0x30 << code << *((quint32*)&throttle) << none << none + // << none << none << none << none << none; + buf.append("DATA0"); + buf.append(reinterpret_cast(&code), sizeof(code)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); - if(outSocket->writeDatagram(buf, QHostAddress(settings.remoteHostAddress), settings.outPort) == -1) - { - emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n"); - } + if(outSocket->writeDatagram(buf, QHostAddress(settings.remoteAddress), settings.outPort) == -1) + { + emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n"); + } //outSocket->write(buf); diff --git a/ground/openpilotgcs/src/plugins/plugins.pro b/ground/openpilotgcs/src/plugins/plugins.pro index 154335e3a..6add3aa77 100644 --- a/ground/openpilotgcs/src/plugins/plugins.pro +++ b/ground/openpilotgcs/src/plugins/plugins.pro @@ -147,12 +147,12 @@ plugin_hitlnew.depends += plugin_uavobjects plugin_hitlnew.depends += plugin_uavtalk SUBDIRS += plugin_hitlnew -#HITLNEW Simulation gadget v2 -plugin_hitl_v2.subdir = hitlv2 -plugin_hitl_v2.depends = plugin_coreplugin -plugin_hitl_v2.depends += plugin_uavobjects -plugin_hitl_v2.depends += plugin_uavtalk -SUBDIRS += plugin_hitl_v2 +##HITLNEW Simulation gadget v2 +#plugin_hitl_v2.subdir = hitlv2 +#plugin_hitl_v2.depends = plugin_coreplugin +#plugin_hitl_v2.depends += plugin_uavobjects +#plugin_hitl_v2.depends += plugin_uavtalk +#SUBDIRS += plugin_hitl_v2 # Export and Import GCS Configuration plugin_importexport.subdir = importexport