rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-05 21:52:10 +01:00
Code Issues Releases Activity

Finished port, but still untested.

Browse Source
This commit is contained in:
Kenz Dale 2012-08-06 18:39:56 +02:00
parent d4a1fb28c7
commit cf9b04d0ac
10 changed files with 1550 additions and 780 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui
View File

@ -6,7 +6,7 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>587</width> <width>738</width>
<height>379</height> <height>379</height>
</rect> </rect>
</property> </property>

333
ground/openpilotgcs/src/plugins/hitlnew/aerosimrcsimulator.cpp
View File

@ -90,7 +90,7 @@ void AeroSimRCSimulator::transmitUpdate()
stream << armed << mode; // flight status stream << armed << mode; // flight status
stream << udpCounterASrecv; // packet counter 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(); qDebug() << "write failed: " << outSocket->errorString();
} }
@ -125,7 +125,7 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data)
return; return;
} }
float timeStep, float delT,
homeX, homeY, homeZ, homeX, homeY, homeZ,
WpHX, WpHY, WpLat, WpLon, WpHX, WpHY, WpLat, WpLon,
posX, posY, posZ, // world posX, posY, posZ, // world
@ -138,7 +138,7 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data)
rx, ry, rz, fx, fy, fz, ux, uy, uz, // matrix rx, ry, rz, fx, fy, fz, ux, uy, uz, // matrix
ch[8]; ch[8];
stream >> timeStep; stream >> delT;
stream >> homeX >> homeY >> homeZ; stream >> homeX >> homeY >> homeZ;
stream >> WpHX >> WpHY >> WpLat >> WpLon; stream >> WpHX >> WpHY >> WpLat >> WpLon;
stream >> posX >> posY >> posZ; stream >> posX >> posY >> posZ;
@ -156,294 +156,50 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data)
memset(&out, 0, sizeof(Output2OP)); memset(&out, 0, sizeof(Output2OP));
// struct Output2OP{ out.delT=delT;
// float latitude; /**********************************************************************************************/
// float longitude; QMatrix4x4 mat;
// float altitude; mat = QMatrix4x4( fy, fx, -fz, 0.0, // model matrix
// float heading; ry, rx, -rz, 0.0, // (X,Y,Z) -> (+Y,+X,-Z)
// float groundspeed; //[m/s] -uy, -ux, uz, 0.0,
// float calibratedAirspeed; //[m/s] 0.0, 0.0, 0.0, 1.0);
// float pitch; mat.optimize();
// float roll;
// float pressure; QQuaternion quat; // model quat
// float temperature; asMatrix2Quat(mat, quat);
// float velNorth; //[m/s]
// float velEast; //[m/s] // rotate gravity
// float velDown; //[m/s] /*************************************************************************************/
// float dstN; //[m] QVector3D acc = QVector3D(accY, accX, -accZ); // accel (X,Y,Z) -> (+Y,+X,-Z)
// float dstE; //[m] QVector3D gee = QVector3D(0.0, 0.0, -GEE);
// float dstD; //[m] QQuaternion qWorld = quat.conjugate();
// float accX; //[m/s^2] gee = qWorld.rotatedVector(gee);
// float accY; //[m/s^2] acc += gee;
// float accZ; //[m/s^2]
// float rollRate; out.rollRate = angY * RAD2DEG; // gyros (X,Y,Z) -> (+Y,+X,-Z)
// float pitchRate; out.pitchRate = angX * RAD2DEG;
// float yawRate; 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(); out.altitude = posZ;
/**********************************************************************************************/ out.heading = yaw * RAD2DEG;
// static bool firstRun = true; out.latitude = lat * 10e6;
// if (settings.homeLocation) { out.longitude = lon * 10e6;
// if (firstRun) { out.groundspeed = qSqrt(velX * velX + velY * velY);
// HomeLocation::DataFields homeData;
// homeData = posHome->getData();
// 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.dstN = posY * 100;
out.dstE = posX * 100; out.dstE = posX * 100;
@ -453,6 +209,9 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data)
out.velEast = velX * 100; out.velEast = velX * 100;
out.velDown = velZ * 100; //WHY ISN'T THIS `-velZ`??? out.velDown = velZ * 100; //WHY ISN'T THIS `-velZ`???
updateUAVOs(out);
#ifdef DBG_TIMERS #ifdef DBG_TIMERS
static int cntRX = 0; static int cntRX = 0;
if (cntRX >= 100) { if (cntRX >= 100) {

76
ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp
View File

@ -98,44 +98,44 @@ bool FGSimulator::setupProcess()
QString cmdShell("bash"); QString cmdShell("bash");
#endif #endif
// Start shell (Note: Could not start FG directly on Windows, only through terminal!) // Start shell (Note: Could not start FG directly on Windows, only through terminal!)
simProcess->start(cmdShell); simProcess->start(cmdShell);
if (simProcess->waitForStarted() == false) if (simProcess->waitForStarted() == false)
{ {
emit processOutput("Error:" + simProcess->errorString()); emit processOutput("Error:" + simProcess->errorString());
return false; return false;
} }
// Setup arguments // 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. // 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. // 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 + "\" " + QString args("--fg-root=\"" + settings.dataPath + "\" " +
"--timeofday=noon " + "--timeofday=noon " +
"--httpd=5400 " + "--httpd=5400 " +
"--enable-hud " + "--enable-hud " +
"--in-air " + "--in-air " +
"--altitude=3000 " + "--altitude=3000 " +
"--vc=100 " + "--vc=100 " +
"--log-level=alert " + "--log-level=alert " +
"--generic=socket,out,20," + settings.hostAddress + "," + QString::number(settings.inPort) + ",udp,opfgprotocol"); "--generic=socket,out,20," + settings.hostAddress + "," + QString::number(settings.inPort) + ",udp,opfgprotocol");
if(!settings.manual) if(!settings.manualControl)
{ {
args.append(" --generic=socket,in,400," + settings.remoteHostAddress + "," + QString::number(settings.outPort) + ",udp,opfgprotocol"); 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 // Start FlightGear - only if checkbox is selected in HITL options page
if (settings.startSim) if (settings.startSim)
{ {
QString cmd("\"" + settings.binPath + "\" " + args + "\n"); QString cmd("\"" + settings.binPath + "\" " + args + "\n");
simProcess->write(cmd.toAscii()); simProcess->write(cmd.toAscii());
} }
else else
{ {
emit processOutput("Start Flightgear from the command line with the following arguments: \n\n" + args + "\n\n" + 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" + "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" "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."); "Remote computer must have the correct OpenPilot protocol installed.");
} }
udpCounterGCSsend = 0; udpCounterGCSsend = 0;
@ -207,7 +207,7 @@ void FGSimulator::transmitUpdate()
QByteArray data = cmd.toAscii(); 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"); 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 // V2.0 does not currently work with --generic-protocol
} }
if(!settings.manual) if(!settings.manualControl)
{ {
actData.Roll = ailerons; actData.Roll = ailerons;
actData.Pitch = -elevator; actData.Pitch = -elevator;

150
ground/openpilotgcs/src/plugins/hitlnew/hitlconfiguration.cpp
View File

@ -30,122 +30,62 @@
HITLConfiguration::HITLConfiguration(QString classId, QSettings* qSettings, QObject *parent) : HITLConfiguration::HITLConfiguration(QString classId, QSettings* qSettings, QObject *parent) :
IUAVGadgetConfiguration(classId, 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.simulatorId = "";
settings.binPath = ""; settings.binPath = "";
settings.dataPath = ""; settings.dataPath = "";
settings.manual = false; settings.manualControl = false;
settings.startSim = false; settings.startSim = false;
settings.addNoise = false; settings.addNoise = false;
settings.hostAddress = "127.0.0.1"; settings.hostAddress = "127.0.0.1";
settings.remoteHostAddress = "127.0.0.1"; settings.remoteAddress = "127.0.0.1";
settings.outPort = 0; settings.outPort = 0;
settings.inPort = 0; settings.inPort = 0;
settings.latitude = ""; settings.latitude = "";
settings.longitude = ""; 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 a saved configuration exists load it
if (qSettings != 0) { if (qSettings != 0) {
settings.simulatorId = qSettings->value("simulatorId", simulatorId).toString(); settings.simulatorId = qSettings->value("simulatorId").toString();
settings.hostAddress = qSettings->value("hostAddress", hostAddress).toString(); settings.hostAddress = qSettings->value("hostAddress").toString();
settings.inPort = qSettings->value("inPort", inPort).toInt(); settings.inPort = qSettings->value("inPort").toInt();
settings.remoteAddress = qSettings->value("remoteAddress", remoteAddress).toString(); settings.remoteAddress = qSettings->value("remoteAddress").toString();
settings.outPort = qSettings->value("outPort", outPort).toInt(); settings.outPort = qSettings->value("outPort").toInt();
settings.binPath = qSettings->value("binPath", binPath).toString();
settings.dataPath = qSettings->value("dataPath", dataPath).toString();
settings.homeLocation = qSettings->value("homeLocation", homeLocation).toBool(); settings.binPath = qSettings->value("binPath").toString();
settings.homeLocRate = qSettings->value("homeLocRate", homeLocRate).toInt(); settings.dataPath = qSettings->value("dataPath").toString();
settings.attRaw = qSettings->value("attRaw", attRaw).toBool(); settings.latitude = qSettings->value("latitude").toString();
settings.attRawRate = qSettings->value("attRawRate", attRawRate).toInt(); 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.homeLocation = qSettings->value("homeLocation").toBool();
settings.attActHW = qSettings->value("attActHW", attActHW).toBool(); settings.homeLocRate = qSettings->value("homeLocRate").toInt();
settings.attActSim = qSettings->value("attActSim", attActSim).toBool();
settings.attActCalc = qSettings->value("attActCalc", attActCalc).toBool();
settings.sonarAltitude = qSettings->value("sonarAltitude", sonarAltitude).toBool(); settings.attRaw = qSettings->value("attRaw").toBool();
settings.sonarMaxAlt = qSettings->value("sonarMaxAlt", sonarMaxAlt).toFloat(); settings.attRawRate = qSettings->value("attRawRate").toInt();
settings.sonarAltRate = qSettings->value("sonarAltRate", sonarAltRate).toInt();
settings.gpsPosition = qSettings->value("gpsPosition", gpsPosition).toBool(); settings.attActual = qSettings->value("attActual").toBool();
settings.gpsPosRate = qSettings->value("gpsPosRate", gpsPosRate).toInt(); 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.sonarAltitude = qSettings->value("sonarAltitude").toBool();
settings.gcsReciever = qSettings->value("gcsReciever", gcsReciever).toBool(); settings.sonarMaxAlt = qSettings->value("sonarMaxAlt").toFloat();
settings.manualControl = qSettings->value("manualControl", manualControl).toBool(); settings.sonarAltRate = qSettings->value("sonarAltRate").toInt();
settings.manualOutput = qSettings->value("manualOutput", manualOutput).toBool();
settings.outputRate = qSettings->value("outputRate", outputRate).toInt(); settings.gpsPosition = qSettings->value("gpsPosition").toBool();
} else { 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.simulatorId = simulatorId;
settings.hostAddress = hostAddress; settings.hostAddress = hostAddress;
settings.inPort = inPort; settings.inPort = inPort;
@ -195,25 +135,20 @@ IUAVGadgetConfiguration *HITLConfiguration::clone()
*/ */
void HITLConfiguration::saveConfig(QSettings* qSettings) const { void HITLConfiguration::saveConfig(QSettings* qSettings) const {
qSettings->setValue("simulatorId", settings.simulatorId); 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("binPath", settings.binPath);
qSettings->setValue("dataPath", settings.dataPath); qSettings->setValue("dataPath", settings.dataPath);
qSettings->setValue("manual", settings.manual);
qSettings->setValue("startSim", settings.startSim);
qSettings->setValue("hostAddress", settings.hostAddress); qSettings->setValue("hostAddress", settings.hostAddress);
qSettings->setValue("remoteHostAddress", settings.remoteHostAddress); qSettings->setValue("remoteAddress", settings.remoteAddress);
qSettings->setValue("outPort", settings.outPort); qSettings->setValue("outPort", settings.outPort);
qSettings->setValue("inPort", settings.inPort); qSettings->setValue("inPort", settings.inPort);
qSettings->setValue("latitude", settings.latitude); qSettings->setValue("latitude", settings.latitude);
qSettings->setValue("longitude", settings.longitude); 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("homeLocation", settings.homeLocation);
qSettings->setValue("homeLocRate", settings.homeLocRate); qSettings->setValue("homeLocRate", settings.homeLocRate);
qSettings->setValue("attRaw", settings.attRaw); qSettings->setValue("attRaw", settings.attRaw);
@ -232,6 +167,5 @@ void HITLConfiguration::saveConfig(QSettings* qSettings) const {
qSettings->setValue("manualControl", settings.manualControl); qSettings->setValue("manualControl", settings.manualControl);
qSettings->setValue("manualOutput", settings.manualOutput); qSettings->setValue("manualOutput", settings.manualOutput);
qSettings->setValue("outputRate", settings.outputRate); qSettings->setValue("outputRate", settings.outputRate);
#endif
} }

62
ground/openpilotgcs/src/plugins/hitlnew/hitloptionspage.cpp
View File

@ -67,47 +67,49 @@ QWidget *HITLOptionsPage::createPage(QWidget *parent)
m_optionsPage->dataPath->setPromptDialogTitle(tr("Choose flight simulator data directory")); m_optionsPage->dataPath->setPromptDialogTitle(tr("Choose flight simulator data directory"));
// Restore the contents from the settings: // Restore the contents from the settings:
foreach(SimulatorCreator* creator, HITLPlugin::typeSimulators) foreach(SimulatorCreator* creator, HITLPlugin::typeSimulators)
{ {
QString id = config->Settings().simulatorId; QString id = config->Settings().simulatorId;
if(creator->ClassId() == id) if(creator->ClassId() == id)
m_optionsPage->chooseFlightSimulator->setCurrentIndex(HITLPlugin::typeSimulators.indexOf(creator)); m_optionsPage->chooseFlightSimulator->setCurrentIndex(HITLPlugin::typeSimulators.indexOf(creator));
} }
m_optionsPage->executablePath->setPath(config->Settings().binPath); m_optionsPage->executablePath->setPath(config->Settings().binPath);
m_optionsPage->dataPath->setPath(config->Settings().dataPath); m_optionsPage->dataPath->setPath(config->Settings().dataPath);
m_optionsPage->manualControl->setChecked(config->Settings().manual); m_optionsPage->manualControl->setChecked(config->Settings().manualControl);
m_optionsPage->startSim->setChecked(config->Settings().startSim); m_optionsPage->startSim->setChecked(config->Settings().startSim);
m_optionsPage->noiseCheckBox->setChecked(config->Settings().addNoise);
m_optionsPage->hostAddress->setText(config->Settings().hostAddress); m_optionsPage->hostAddress->setText(config->Settings().hostAddress);
m_optionsPage->remoteHostAddress->setText(config->Settings().remoteHostAddress); m_optionsPage->remoteAddress->setText(config->Settings().remoteAddress);
m_optionsPage->outputPort->setText(QString::number(config->Settings().outPort)); m_optionsPage->outputPort->setText(QString::number(config->Settings().outPort));
m_optionsPage->inputPort->setText(QString::number(config->Settings().inPort)); m_optionsPage->inputPort->setText(QString::number(config->Settings().inPort));
m_optionsPage->latitude->setText(config->Settings().latitude); m_optionsPage->latitude->setText(config->Settings().latitude);
m_optionsPage->longitude->setText(config->Settings().longitude); m_optionsPage->longitude->setText(config->Settings().longitude);
return optionsPageWidget; return optionsPageWidget;
} }
void HITLOptionsPage::apply() void HITLOptionsPage::apply()
{ {
SimulatorSettings settings; SimulatorSettings settings;
int i = m_optionsPage->chooseFlightSimulator->currentIndex(); int i = m_optionsPage->chooseFlightSimulator->currentIndex();
settings.simulatorId = m_optionsPage->chooseFlightSimulator->itemData(i).toString(); settings.simulatorId = m_optionsPage->chooseFlightSimulator->itemData(i).toString();
settings.binPath = m_optionsPage->executablePath->path(); settings.binPath = m_optionsPage->executablePath->path();
settings.dataPath = m_optionsPage->dataPath->path(); settings.dataPath = m_optionsPage->dataPath->path();
settings.manual = m_optionsPage->manualControl->isChecked(); settings.manualControl = m_optionsPage->manualControl->isChecked();
settings.startSim = m_optionsPage->startSim->isChecked(); settings.startSim = m_optionsPage->startSim->isChecked();
settings.hostAddress = m_optionsPage->hostAddress->text(); settings.addNoise = m_optionsPage->noiseCheckBox->isChecked();
settings.remoteHostAddress = m_optionsPage->remoteHostAddress->text(); settings.hostAddress = m_optionsPage->hostAddress->text();
settings.remoteAddress = m_optionsPage->remoteAddress->text();
settings.inPort = m_optionsPage->inputPort->text().toInt(); settings.inPort = m_optionsPage->inputPort->text().toInt();
settings.outPort = m_optionsPage->outputPort->text().toInt(); settings.outPort = m_optionsPage->outputPort->text().toInt();
settings.longitude = m_optionsPage->longitude->text(); settings.longitude = m_optionsPage->longitude->text();
settings.latitude = m_optionsPage->latitude->text(); settings.latitude = m_optionsPage->latitude->text();
config->setSimulatorSettings(settings); config->setSimulatorSettings(settings);
} }
void HITLOptionsPage::finish() void HITLOptionsPage::finish()

1169
ground/openpilotgcs/src/plugins/hitlnew/hitloptionspage.ui
View File

@ -6,258 +6,968 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>590</width> <width>885</width>
<height>367</height> <height>741</height>
</rect> </rect>
</property> </property>
<property name="sizePolicy"> <property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Expanding"> <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch> <horstretch>0</horstretch>
<verstretch>0</verstretch> <verstretch>0</verstretch>
</sizepolicy> </sizepolicy>
</property> </property>
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="windowTitle"> <property name="windowTitle">
<string>Form</string> <string>Form</string>
</property> </property>
<layout class="QGridLayout" name="gridLayout"> <layout class="QVBoxLayout" name="verticalLayout">
<property name="margin"> <property name="spacing">
<number>3</number>
</property>
<property name="leftMargin">
<number>0</number> <number>0</number>
</property> </property>
<item row="1" column="1" colspan="2"> <property name="topMargin">
<widget class="QLabel" name="label_3"> <number>0</number>
<property name="text"> </property>
<string>Choose flight simulator:</string> <property name="rightMargin">
</property> <number>0</number>
</widget> </property>
<property name="bottomMargin">
<number>3</number>
</property>
<item>
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
<widget class="QLabel" name="chooseFlightSimulatorLabel">
<property name="text">
<string>Choose flight simulator:</string>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="chooseFlightSimulator"/>
</item>
</layout>
</item> </item>
<item row="1" column="3" colspan="2"> <item>
<widget class="QComboBox" name="chooseFlightSimulator"/> <widget class="QGroupBox" name="groupBox_3">
</item> <property name="sizePolicy">
<item row="2" column="1" colspan="4"> <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<widget class="Line" name="line"> <horstretch>0</horstretch>
<property name="orientation"> <verstretch>0</verstretch>
<enum>Qt::Horizontal</enum> </sizepolicy>
</property> </property>
</widget> <property name="minimumSize">
</item>
<item row="11" column="2">
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size> <size>
<width>20</width> <width>0</width>
<height>182</height> <height>100</height>
</size> </size>
</property> </property>
</spacer> <property name="maximumSize">
</item> <size>
<item row="3" column="1" colspan="2"> <width>350</width>
<widget class="QLabel" name="label_6"> <height>16777215</height>
<property name="sizePolicy"> </size>
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property> </property>
<property name="text"> <property name="title">
<string>Local interface address (IP):</string> <string>IP addresses</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property> </property>
<widget class="QWidget" name="layoutWidget">
<property name="geometry">
<rect>
<x>10</x>
<y>30</y>
<width>323</width>
<height>24</height>
</rect>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_7">
<item>
<widget class="QLabel" name="label_6">
<property name="text">
<string>Local host: </string>
</property>
</widget>
</item>
<item>
<widget class="QLineEdit" name="hostAddress">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>125</width>
<height>16777215</height>
</size>
</property>
<property name="toolTip">
<string>For communication with sim computer via network. Should be the IP address of one of the interfaces of the GCS computer.</string>
</property>
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_5">
<property name="text">
<string>Port:</string>
</property>
</widget>
</item>
<item>
<widget class="QLineEdit" name="inputPort">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>55</width>
<height>16777215</height>
</size>
</property>
<property name="toolTip">
<string>IP port for receiving data from sim</string>
</property>
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QWidget" name="layoutWidget">
<property name="geometry">
<rect>
<x>10</x>
<y>60</y>
<width>322</width>
<height>24</height>
</rect>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_8">
<item>
<widget class="QLabel" name="label_9">
<property name="text">
<string>Remote host:</string>
</property>
</widget>
</item>
<item>
<widget class="QLineEdit" name="remoteAddress">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>125</width>
<height>16777215</height>
</size>
</property>
<property name="toolTip">
<string>Only required if running simulator on remote machine. Should be the IP of the machine on which the simulator is running.</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_4">
<property name="text">
<string>Port:</string>
</property>
</widget>
</item>
<item>
<widget class="QLineEdit" name="outputPort">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>55</width>
<height>16777215</height>
</size>
</property>
<property name="toolTip">
<string>IP port for sending data to sim</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
</layout>
</widget>
<zorder>layoutWidget</zorder>
<zorder>layoutWidget</zorder>
</widget> </widget>
</item> </item>
<item row="6" column="1"> <item>
<widget class="QLabel" name="label_7"> <widget class="QGroupBox" name="groupBox_4">
<property name="text"> <property name="minimumSize">
<string>Latitude in degrees:</string> <size>
<width>0</width>
<height>180</height>
</size>
</property> </property>
<property name="title">
<string>Program parameters</string>
</property>
<widget class="Utils::PathChooser" name="dataPath" native="true">
<property name="geometry">
<rect>
<x>120</x>
<y>60</y>
<width>401</width>
<height>16</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
</widget>
<widget class="QLabel" name="label_2">
<property name="geometry">
<rect>
<x>10</x>
<y>60</y>
<width>94</width>
<height>16</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>110</width>
<height>16777215</height>
</size>
</property>
<property name="text">
<string>Data directory:</string>
</property>
</widget>
<widget class="Utils::PathChooser" name="executablePath" native="true">
<property name="geometry">
<rect>
<x>120</x>
<y>34</y>
<width>401</width>
<height>16</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
</widget>
<widget class="QLabel" name="label">
<property name="geometry">
<rect>
<x>10</x>
<y>34</y>
<width>104</width>
<height>16</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>110</width>
<height>16777215</height>
</size>
</property>
<property name="text">
<string>Path executable:</string>
</property>
</widget>
<widget class="QCheckBox" name="startSim">
<property name="geometry">
<rect>
<x>10</x>
<y>120</y>
<width>229</width>
<height>20</height>
</rect>
</property>
<property name="toolTip">
<string>Check this box to start the simulator on the local computer</string>
</property>
<property name="text">
<string>Start simulator on local machine</string>
</property>
</widget>
<widget class="QCheckBox" name="noiseCheckBox">
<property name="geometry">
<rect>
<x>10</x>
<y>150</y>
<width>89</width>
<height>20</height>
</rect>
</property>
<property name="toolTip">
<string>Add noise to sensor simulation</string>
</property>
<property name="text">
<string>Add noise</string>
</property>
</widget>
<widget class="Line" name="line">
<property name="geometry">
<rect>
<x>10</x>
<y>80</y>
<width>491</width>
<height>16</height>
</rect>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
<widget class="QLabel" name="label_13">
<property name="geometry">
<rect>
<x>10</x>
<y>90</y>
<width>161</width>
<height>22</height>
</rect>
</property>
<property name="text">
<string>Initial latitude (decimal):</string>
</property>
</widget>
<widget class="QLabel" name="label_14">
<property name="geometry">
<rect>
<x>290</x>
<y>90</y>
<width>171</width>
<height>22</height>
</rect>
</property>
<property name="text">
<string>Initial longitude (decimal):</string>
</property>
</widget>
<widget class="QLineEdit" name="longitude">
<property name="geometry">
<rect>
<x>460</x>
<y>90</y>
<width>100</width>
<height>22</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>100</width>
<height>16777215</height>
</size>
</property>
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
<widget class="QLineEdit" name="latitude">
<property name="geometry">
<rect>
<x>170</x>
<y>90</y>
<width>100</width>
<height>22</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>100</width>
<height>16777215</height>
</size>
</property>
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</widget> </widget>
</item> </item>
<item row="6" column="3"> <item>
<widget class="QLabel" name="label_8"> <layout class="QFormLayout" name="formLayout">
<property name="text"> <property name="fieldGrowthPolicy">
<string>Longitude in degrees:</string> <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
</property> </property>
</widget> <item row="0" column="0">
<widget class="QFrame" name="frame">
<property name="frameShape">
<enum>QFrame::StyledPanel</enum>
</property>
<property name="frameShadow">
<enum>QFrame::Raised</enum>
</property>
</widget>
</item>
</layout>
</item> </item>
<item row="8" column="1"> <item>
<widget class="QLabel" name="label_2"> <layout class="QHBoxLayout" name="horizontalLayout_3">
<property name="sizePolicy"> <item>
<sizepolicy hsizetype="Preferred" vsizetype="Fixed"> <widget class="QGroupBox" name="groupBox">
<horstretch>0</horstretch> <property name="title">
<verstretch>0</verstretch> <string>Attitude data</string>
</sizepolicy> </property>
</property> <layout class="QVBoxLayout" name="verticalLayout_2">
<property name="text"> <property name="spacing">
<string>Data directory:</string> <number>3</number>
</property> </property>
</widget> <property name="leftMargin">
</item> <number>3</number>
<item row="5" column="2"> </property>
<widget class="QLineEdit" name="inputPort"> <property name="topMargin">
<property name="sizePolicy"> <number>3</number>
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed"> </property>
<horstretch>0</horstretch> <property name="rightMargin">
<verstretch>0</verstretch> <number>3</number>
</sizepolicy> </property>
</property> <property name="bottomMargin">
<property name="toolTip"> <number>0</number>
<string>For receiving data from sim</string> </property>
</property> <item>
</widget> <widget class="QGroupBox" name="attRawCheckbox">
</item> <property name="title">
<item row="7" column="2" colspan="3"> <string>AttitudeRaw (gyro, accels)</string>
<widget class="Utils::PathChooser" name="executablePath" native="true"> </property>
<property name="sizePolicy"> <property name="flat">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred"> <bool>true</bool>
<horstretch>1</horstretch> </property>
<verstretch>0</verstretch> <property name="checkable">
</sizepolicy> <bool>true</bool>
</property> </property>
</widget> <property name="checked">
</item> <bool>false</bool>
<item row="5" column="3"> </property>
<widget class="QLabel" name="label_4"> <layout class="QHBoxLayout" name="horizontalLayout_4">
<property name="sizePolicy"> <property name="spacing">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred"> <number>3</number>
<horstretch>0</horstretch> </property>
<verstretch>0</verstretch> <property name="topMargin">
</sizepolicy> <number>3</number>
</property> </property>
<property name="text"> <property name="rightMargin">
<string>Output Port:</string> <number>0</number>
</property> </property>
</widget> <property name="bottomMargin">
</item> <number>0</number>
<item row="8" column="2" colspan="3"> </property>
<widget class="Utils::PathChooser" name="dataPath" native="true"/> <item>
</item> <widget class="QLabel" name="label_8">
<item row="6" column="4"> <property name="text">
<widget class="QLineEdit" name="longitude"> <string>Refresh rate</string>
<property name="sizePolicy"> </property>
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed"> </widget>
<horstretch>0</horstretch> </item>
<verstretch>0</verstretch> <item>
</sizepolicy> <widget class="QSpinBox" name="attRawRate">
</property> <property name="enabled">
</widget> <bool>false</bool>
</item> </property>
<item row="5" column="4"> <property name="suffix">
<widget class="QLineEdit" name="outputPort"> <string>ms</string>
<property name="sizePolicy"> </property>
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed"> <property name="minimum">
<horstretch>0</horstretch> <number>10</number>
<verstretch>0</verstretch> </property>
</sizepolicy> <property name="maximum">
</property> <number>100</number>
<property name="toolTip"> </property>
<string>For sending data to sim</string> <property name="value">
</property> <number>20</number>
</widget> </property>
</item> </widget>
<item row="9" column="1" colspan="4"> </item>
<widget class="QCheckBox" name="manualControl"> </layout>
<property name="sizePolicy"> </widget>
<sizepolicy hsizetype="Preferred" vsizetype="Fixed"> </item>
<horstretch>0</horstretch> <item>
<verstretch>0</verstretch> <widget class="QGroupBox" name="attActualCheckbox">
</sizepolicy> <property name="title">
</property> <string>AttitudeActual</string>
<property name="toolTip"> </property>
<string>Manual aircraft control (can be used when hardware is not available)</string> <property name="flat">
</property> <bool>true</bool>
<property name="text"> </property>
<string>Manual aircraft control (can be used when hardware is not available)</string> <property name="checkable">
</property> <bool>true</bool>
</widget> </property>
</item> <property name="checked">
<item row="7" column="1"> <bool>false</bool>
<widget class="QLabel" name="label"> </property>
<property name="sizePolicy"> <layout class="QVBoxLayout" name="verticalLayout_4">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed"> <property name="spacing">
<horstretch>0</horstretch> <number>3</number>
<verstretch>0</verstretch> </property>
</sizepolicy> <property name="topMargin">
</property> <number>3</number>
<property name="text"> </property>
<string>Path executable:</string> <property name="rightMargin">
</property> <number>0</number>
</widget> </property>
</item> <property name="bottomMargin">
<item row="6" column="2"> <number>0</number>
<widget class="QLineEdit" name="latitude"> </property>
<property name="sizePolicy"> <item>
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed"> <widget class="QRadioButton" name="attActHW">
<horstretch>0</horstretch> <property name="text">
<verstretch>0</verstretch> <string>send raw data to board</string>
</sizepolicy> </property>
</property> </widget>
</widget> </item>
</item> <item>
<item row="5" column="1"> <widget class="QRadioButton" name="attActSim">
<widget class="QLabel" name="label_5"> <property name="font">
<property name="text"> <font>
<string>Input Port:</string> <weight>75</weight>
</property> <bold>true</bold>
</widget> </font>
</item> </property>
<item row="10" column="1" colspan="2"> <property name="toolTip">
<widget class="QCheckBox" name="startSim"> <string/>
<property name="enabled"> </property>
<bool>true</bool> <property name="text">
</property> <string>use values from simulator</string>
<property name="sizePolicy"> </property>
<sizepolicy hsizetype="Preferred" vsizetype="Fixed"> <property name="checked">
<horstretch>0</horstretch> <bool>true</bool>
<verstretch>0</verstretch> </property>
</sizepolicy> </widget>
</property> </item>
<property name="toolTip"> <item>
<string>Check this box to start the simulator on the local computer</string> <widget class="QRadioButton" name="attActCalc">
</property> <property name="toolTip">
<property name="text"> <string/>
<string>Start simulator on local machine</string> </property>
</property> <property name="text">
</widget> <string>calculate from (noised) simulated sensor data</string>
</item> </property>
<item row="4" column="1" colspan="2"> </widget>
<widget class="QLabel" name="label_9"> </item>
<property name="text"> <item>
<string>Remote interface address (IP):</string> <spacer name="verticalSpacer_2">
</property> <property name="orientation">
<property name="alignment"> <enum>Qt::Vertical</enum>
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set> </property>
</property> <property name="sizeHint" stdset="0">
</widget> <size>
</item> <width>20</width>
<item row="4" column="3"> <height>40</height>
<widget class="QLineEdit" name="remoteHostAddress"> </size>
<property name="sizePolicy"> </property>
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed"> </spacer>
<horstretch>0</horstretch> </item>
<verstretch>0</verstretch> </layout>
</sizepolicy> </widget>
</property> </item>
<property name="toolTip"> </layout>
<string>Only required if running simulator on remote machine. Should be the IP of the machine on which the simulator is running.</string> </widget>
</property> </item>
</widget> <item>
</item> <widget class="QGroupBox" name="groupBox_2">
<item row="3" column="3"> <property name="minimumSize">
<widget class="QLineEdit" name="hostAddress"> <size>
<property name="sizePolicy"> <width>0</width>
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed"> <height>275</height>
<horstretch>0</horstretch> </size>
<verstretch>0</verstretch> </property>
</sizepolicy> <property name="title">
</property> <string>Other data</string>
<property name="toolTip"> </property>
<string>For communication with sim computer via network. Should be the IP address of one of the interfaces of the GCS computer.</string> <layout class="QVBoxLayout" name="verticalLayout_3">
</property> <property name="spacing">
</widget> <number>3</number>
</property>
<property name="leftMargin">
<number>3</number>
</property>
<property name="topMargin">
<number>3</number>
</property>
<property name="rightMargin">
<number>3</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
<widget class="QGroupBox" name="groundTruthCheckbox">
<property name="title">
<string>Ground truth position and velocity</string>
</property>
<property name="flat">
<bool>true</bool>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<property name="checked">
<bool>false</bool>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_2">
<property name="spacing">
<number>3</number>
</property>
<property name="topMargin">
<number>3</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
<widget class="QLabel" name="label_7">
<property name="text">
<string>Refresh rate</string>
</property>
</widget>
</item>
<item>
<widget class="QSpinBox" name="groundTruthRateSpinbox">
<property name="enabled">
<bool>false</bool>
</property>
<property name="toolTip">
<string>0 - update once, or every N seconds</string>
</property>
<property name="suffix">
<string>sec</string>
</property>
<property name="maximum">
<number>10</number>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="gpsPosition">
<property name="title">
<string>GPS data</string>
</property>
<property name="flat">
<bool>true</bool>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<property name="checked">
<bool>false</bool>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_5">
<property name="spacing">
<number>3</number>
</property>
<property name="topMargin">
<number>3</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
<widget class="QLabel" name="label_10">
<property name="text">
<string>Refresh rate</string>
</property>
</widget>
</item>
<item>
<widget class="QSpinBox" name="gpsPosRateSpinbox">
<property name="enabled">
<bool>false</bool>
</property>
<property name="suffix">
<string>ms</string>
</property>
<property name="minimum">
<number>100</number>
</property>
<property name="maximum">
<number>2000</number>
</property>
<property name="value">
<number>500</number>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="baroAltitude">
<property name="enabled">
<bool>false</bool>
</property>
<property name="title">
<string>SonarAltitude</string>
</property>
<property name="flat">
<bool>true</bool>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<property name="checked">
<bool>false</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout_6">
<property name="spacing">
<number>3</number>
</property>
<property name="leftMargin">
<number>6</number>
</property>
<property name="topMargin">
<number>3</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
<layout class="QGridLayout" name="gridLayout_2">
<item row="0" column="0">
<widget class="QLabel" name="label_11">
<property name="text">
<string>Range detectioon</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QSpinBox" name="sonarMaxAlt">
<property name="suffix">
<string>m</string>
</property>
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>10</number>
</property>
<property name="value">
<number>5</number>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_12">
<property name="text">
<string>Refresh rate</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QSpinBox" name="sonarAltRate">
<property name="suffix">
<string>ms</string>
</property>
<property name="minimum">
<number>20</number>
</property>
<property name="maximum">
<number>2000</number>
</property>
<property name="singleStep">
<number>10</number>
</property>
<property name="value">
<number>50</number>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="inputCommand">
<property name="enabled">
<bool>false</bool>
</property>
<property name="title">
<string>Map command from simulator</string>
</property>
<property name="flat">
<bool>true</bool>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<property name="checked">
<bool>false</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout_5">
<property name="spacing">
<number>3</number>
</property>
<property name="topMargin">
<number>3</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
<widget class="QRadioButton" name="gcsReciever">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>to GCSReciver</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QRadioButton" name="manualControl">
<property name="enabled">
<bool>false</bool>
</property>
<property name="text">
<string>to ManualCtrl (not implemented)</string>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_6">
<property name="spacing">
<number>6</number>
</property>
<item>
<widget class="QCheckBox" name="manualOutput">
<property name="enabled">
<bool>false</bool>
</property>
<property name="text">
<string>Maximum output rate</string>
</property>
</widget>
</item>
<item>
<widget class="QSpinBox" name="outputRate">
<property name="enabled">
<bool>false</bool>
</property>
<property name="suffix">
<string>ms</string>
</property>
<property name="minimum">
<number>10</number>
</property>
<property name="maximum">
<number>100</number>
</property>
<property name="singleStep">
<number>5</number>
</property>
<property name="value">
<number>15</number>
</property>
</widget>
</item>
</layout>
</item>
<item>
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</item>
</layout>
</item> </item>
</layout> </layout>
</widget> </widget>
@ -269,6 +979,13 @@
<container>1</container> <container>1</container>
</customwidget> </customwidget>
</customwidgets> </customwidgets>
<tabstops>
<tabstop>chooseFlightSimulator</tabstop>
<tabstop>hostAddress</tabstop>
<tabstop>inputPort</tabstop>
<tabstop>remoteAddress</tabstop>
<tabstop>outputPort</tabstop>
</tabstops>
<resources/> <resources/>
<connections/> <connections/>
</ui> </ui>

391
ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp
View File

@ -116,49 +116,53 @@ void Simulator::onDeleteSimulator(void)
void Simulator::onStart() void Simulator::onStart()
{ {
QMutexLocker locker(&lock); QMutexLocker locker(&lock);
QThread* mainThread = QThread::currentThread(); QThread* mainThread = QThread::currentThread();
qDebug() << "Simulator Thread: "<< mainThread;
// Get required UAVObjects qDebug() << "Simulator Thread: "<< mainThread;
ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager* objManager = pm->getObject<UAVObjectManager>();
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);
// Listen to autopilot connection events // Get required UAVObjects
TelemetryManager* telMngr = pm->getObject<TelemetryManager>(); ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance();
connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect())); UAVObjectManager* objManager = pm->getObject<UAVObjectManager>();
connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); actDesired = ActuatorDesired::GetInstance(objManager);
//connect(telStats, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(telStatsUpdated(UAVObject*))); 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 // Listen to autopilot connection events
GCSTelemetryStats::DataFields stats = telStats->getData(); TelemetryManager* telMngr = pm->getObject<TelemetryManager>();
if ( stats.Status == GCSTelemetryStats::STATUS_CONNECTED ) connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect()));
onAutopilotConnect(); connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect()));
//connect(telStats, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(telStatsUpdated(UAVObject*)));
inSocket = new QUdpSocket(); // If already connect setup autopilot
outSocket = new QUdpSocket(); GCSTelemetryStats::DataFields stats = telStats->getData();
setupUdpPorts(settings.hostAddress,settings.inPort,settings.outPort); 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" + \ emit processOutput("\nLocal interface: " + settings.hostAddress + "\n" + \
"Remote interface: " + settings.remoteHostAddress + "\n" + \ "Remote interface: " + settings.remoteAddress + "\n" + \
"inputPort: " + QString::number(settings.inPort) + "\n" + \ "inputPort: " + QString::number(settings.inPort) + "\n" + \
"outputPort: " + QString::number(settings.outPort) + "\n"); "outputPort: " + QString::number(settings.outPort) + "\n");
qxtLog->info("\nLocal interface: " + settings.hostAddress + "\n" + \ qxtLog->info("\nLocal interface: " + settings.hostAddress + "\n" + \
"Remote interface: " + settings.remoteHostAddress + "\n" + \ "Remote interface: " + settings.remoteAddress + "\n" + \
"inputPort: " + QString::number(settings.inPort) + "\n" + \ "inputPort: " + QString::number(settings.inPort) + "\n" + \
"outputPort: " + QString::number(settings.outPort) + "\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);
}
}

35
ground/openpilotgcs/src/plugins/hitlnew/simulator.h
View File

@ -107,13 +107,42 @@ typedef struct _CONNECTION
QString binPath; QString binPath;
QString dataPath; QString dataPath;
QString hostAddress; QString hostAddress;
QString remoteHostAddress; QString remoteAddress;
int outPort; int outPort;
int inPort; int inPort;
bool manual;
bool startSim; bool startSim;
QString latitude; QString latitude;
QString longitude; 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; } SimulatorSettings;
@ -140,6 +169,7 @@ struct Output2OP{
float rollRate; //[deg/s] float rollRate; //[deg/s]
float pitchRate; //[deg/s] float pitchRate; //[deg/s]
float yawRate; //[deg/s] float yawRate; //[deg/s]
float delT;
}; };
@ -214,6 +244,7 @@ protected:
BaroAltitude* baroAlt; BaroAltitude* baroAlt;
BaroAirspeed* baroAirspeed; BaroAirspeed* baroAirspeed;
AttitudeActual* attActual; AttitudeActual* attActual;
AttitudeSettings* attSettings;
VelocityActual* velActual; VelocityActual* velActual;
GPSPosition* gpsPos; GPSPosition* gpsPos;
GPSVelocity* gpsVel; GPSVelocity* gpsVel;

100
ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.cpp
View File

@ -117,59 +117,59 @@ void XplaneSimulator::transmitUpdate()
// 11th data settings (flight con: ail/elv/rud) // 11th data settings (flight con: ail/elv/rud)
buf.clear(); buf.clear();
code = 11; code = 11;
//quint8 header[] = "DATA"; //quint8 header[] = "DATA";
/* /*
stream << *((quint32*)header) << stream << *((quint32*)header) <<
(quint8)0x30 << (quint8)0x30 <<
code << code <<
*((quint32*)&elevator) << *((quint32*)&elevator) <<
*((quint32*)&ailerons) << *((quint32*)&ailerons) <<
*((quint32*)&rudder) << *((quint32*)&rudder) <<
none << none <<
*((quint32*)&ailerons) << *((quint32*)&ailerons) <<
none << none <<
none << none <<
none; none;
*/ */
buf.append("DATA0"); buf.append("DATA0");
buf.append(reinterpret_cast<const char*>(&code), sizeof(code)); buf.append(reinterpret_cast<const char*>(&code), sizeof(code));
buf.append(reinterpret_cast<const char*>(&elevator), sizeof(elevator)); buf.append(reinterpret_cast<const char*>(&elevator), sizeof(elevator));
buf.append(reinterpret_cast<const char*>(&ailerons), sizeof(ailerons)); buf.append(reinterpret_cast<const char*>(&ailerons), sizeof(ailerons));
buf.append(reinterpret_cast<const char*>(&rudder), sizeof(rudder)); buf.append(reinterpret_cast<const char*>(&rudder), sizeof(rudder));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none)); buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&rudder), sizeof(rudder)); buf.append(reinterpret_cast<const char*>(&rudder), sizeof(rudder));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none)); buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none)); buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none)); buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
TraceBuf(buf.data(),41); // TraceBuf(buf.data(),41);
if(outSocket->writeDatagram(buf, QHostAddress(settings.remoteHostAddress), settings.outPort) == -1) if(outSocket->writeDatagram(buf, QHostAddress(settings.remoteAddress), settings.outPort) == -1)
{ {
emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n"); emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n");
} }
//outSocket->write(buf); //outSocket->write(buf);
// 25th data settings (throttle command) // 25th data settings (throttle command)
buf.clear(); buf.clear();
code = 25; code = 25;
//stream << *((quint32*)header) << (quint8)0x30 << code << *((quint32*)&throttle) << none << none //stream << *((quint32*)header) << (quint8)0x30 << code << *((quint32*)&throttle) << none << none
// << none << none << none << none << none; // << none << none << none << none << none;
buf.append("DATA0"); buf.append("DATA0");
buf.append(reinterpret_cast<const char*>(&code), sizeof(code)); buf.append(reinterpret_cast<const char*>(&code), sizeof(code));
buf.append(reinterpret_cast<const char*>(&throttle), sizeof(throttle)); buf.append(reinterpret_cast<const char*>(&throttle), sizeof(throttle));
buf.append(reinterpret_cast<const char*>(&throttle), sizeof(throttle)); buf.append(reinterpret_cast<const char*>(&throttle), sizeof(throttle));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none)); buf.append(reinterpret_cast<const char*>(&throttle), sizeof(throttle));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none)); buf.append(reinterpret_cast<const char*>(&throttle), sizeof(throttle));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none)); buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none)); buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none)); buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none)); buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
if(outSocket->writeDatagram(buf, QHostAddress(settings.remoteHostAddress), settings.outPort) == -1) if(outSocket->writeDatagram(buf, QHostAddress(settings.remoteAddress), settings.outPort) == -1)
{ {
emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n"); emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n");
} }
//outSocket->write(buf); //outSocket->write(buf);

12
ground/openpilotgcs/src/plugins/plugins.pro
View File

@ -147,12 +147,12 @@ plugin_hitlnew.depends += plugin_uavobjects
plugin_hitlnew.depends += plugin_uavtalk plugin_hitlnew.depends += plugin_uavtalk
SUBDIRS += plugin_hitlnew SUBDIRS += plugin_hitlnew
#HITLNEW Simulation gadget v2 ##HITLNEW Simulation gadget v2
plugin_hitl_v2.subdir = hitlv2 #plugin_hitl_v2.subdir = hitlv2
plugin_hitl_v2.depends = plugin_coreplugin #plugin_hitl_v2.depends = plugin_coreplugin
plugin_hitl_v2.depends += plugin_uavobjects #plugin_hitl_v2.depends += plugin_uavobjects
plugin_hitl_v2.depends += plugin_uavtalk #plugin_hitl_v2.depends += plugin_uavtalk
SUBDIRS += plugin_hitl_v2 #SUBDIRS += plugin_hitl_v2
# Export and Import GCS Configuration # Export and Import GCS Configuration
plugin_importexport.subdir = importexport plugin_importexport.subdir = importexport