rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +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>
<x>0</x>
<y>0</y>
<width>587</width>
<width>738</width>
<height>379</height>
</rect>
</property>

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

@ -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) {

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

@ -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;

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

@ -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
}

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"));
// 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()

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

@ -6,258 +6,968 @@
<rect>
<x>0</x>
<y>0</y>
<width>590</width>
<height>367</height>
<width>885</width>
<height>741</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Expanding">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
<layout class="QGridLayout" name="gridLayout">
<property name="margin">
<layout class="QVBoxLayout" name="verticalLayout">
<property name="spacing">
<number>3</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<item row="1" column="1" colspan="2">
<widget class="QLabel" name="label_3">
<property name="text">
<string>Choose flight simulator:</string>
</property>
</widget>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</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 row="1" column="3" colspan="2">
<widget class="QComboBox" name="chooseFlightSimulator"/>
</item>
<item row="2" column="1" colspan="4">
<widget class="Line" name="line">
<property name="orientation">
<enum>Qt::Horizontal</enum>
<item>
<widget class="QGroupBox" name="groupBox_3">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
</widget>
</item>
<item row="11" column="2">
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<property name="minimumSize">
<size>
<width>20</width>
<height>182</height>
<width>0</width>
<height>100</height>
</size>
</property>
</spacer>
</item>
<item row="3" column="1" colspan="2">
<widget class="QLabel" name="label_6">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
<property name="maximumSize">
<size>
<width>350</width>
<height>16777215</height>
</size>
</property>
<property name="text">
<string>Local interface address (IP):</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
<property name="title">
<string>IP addresses</string>
</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>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_7">
<property name="text">
<string>Latitude in degrees:</string>
<item>
<widget class="QGroupBox" name="groupBox_4">
<property name="minimumSize">
<size>
<width>0</width>
<height>180</height>
</size>
</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>
</item>
<item row="6" column="3">
<widget class="QLabel" name="label_8">
<property name="text">
<string>Longitude in degrees:</string>
<item>
<layout class="QFormLayout" name="formLayout">
<property name="fieldGrowthPolicy">
<enum>QFormLayout::AllNonFixedFieldsGrow</enum>
</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 row="8" column="1">
<widget class="QLabel" name="label_2">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string>Data directory:</string>
</property>
</widget>
</item>
<item row="5" column="2">
<widget class="QLineEdit" name="inputPort">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="toolTip">
<string>For receiving data from sim</string>
</property>
</widget>
</item>
<item row="7" column="2" colspan="3">
<widget class="Utils::PathChooser" name="executablePath" native="true">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>1</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
</widget>
</item>
<item row="5" column="3">
<widget class="QLabel" name="label_4">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string>Output Port:</string>
</property>
</widget>
</item>
<item row="8" column="2" colspan="3">
<widget class="Utils::PathChooser" name="dataPath" native="true"/>
</item>
<item row="6" column="4">
<widget class="QLineEdit" name="longitude">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
</widget>
</item>
<item row="5" column="4">
<widget class="QLineEdit" name="outputPort">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="toolTip">
<string>For sending data to sim</string>
</property>
</widget>
</item>
<item row="9" column="1" colspan="4">
<widget class="QCheckBox" name="manualControl">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="toolTip">
<string>Manual aircraft control (can be used when hardware is not available)</string>
</property>
<property name="text">
<string>Manual aircraft control (can be used when hardware is not available)</string>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string>Path executable:</string>
</property>
</widget>
</item>
<item row="6" column="2">
<widget class="QLineEdit" name="latitude">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_5">
<property name="text">
<string>Input Port:</string>
</property>
</widget>
</item>
<item row="10" column="1" colspan="2">
<widget class="QCheckBox" name="startSim">
<property name="enabled">
<bool>true</bool>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</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>
</item>
<item row="4" column="1" colspan="2">
<widget class="QLabel" name="label_9">
<property name="text">
<string>Remote interface address (IP):</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item row="4" column="3">
<widget class="QLineEdit" name="remoteHostAddress">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</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>
</widget>
</item>
<item row="3" column="3">
<widget class="QLineEdit" name="hostAddress">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</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>
</widget>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_3">
<item>
<widget class="QGroupBox" name="groupBox">
<property name="title">
<string>Attitude data</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_2">
<property name="spacing">
<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="attRawCheckbox">
<property name="title">
<string>AttitudeRaw (gyro, accels)</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_4">
<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_8">
<property name="text">
<string>Refresh rate</string>
</property>
</widget>
</item>
<item>
<widget class="QSpinBox" name="attRawRate">
<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="value">
<number>20</number>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="attActualCheckbox">
<property name="title">
<string>AttitudeActual</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_4">
<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="attActHW">
<property name="text">
<string>send raw data to board</string>
</property>
</widget>
</item>
<item>
<widget class="QRadioButton" name="attActSim">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string/>
</property>
<property name="text">
<string>use values from simulator</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QRadioButton" name="attActCalc">
<property name="toolTip">
<string/>
</property>
<property name="text">
<string>calculate from (noised) simulated sensor data</string>
</property>
</widget>
</item>
<item>
<spacer name="verticalSpacer_2">
<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>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_2">
<property name="minimumSize">
<size>
<width>0</width>
<height>275</height>
</size>
</property>
<property name="title">
<string>Other data</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_3">
<property name="spacing">
<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>
</layout>
</widget>
@ -269,6 +979,13 @@
<container>1</container>
</customwidget>
</customwidgets>
<tabstops>
<tabstop>chooseFlightSimulator</tabstop>
<tabstop>hostAddress</tabstop>
<tabstop>inputPort</tabstop>
<tabstop>remoteAddress</tabstop>
<tabstop>outputPort</tabstop>
</tabstops>
<resources/>
<connections/>
</ui>

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

@ -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<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);
qDebug() << "Simulator Thread: "<< mainThread;
// Listen to autopilot connection events
TelemetryManager* telMngr = pm->getObject<TelemetryManager>();
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<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);
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<TelemetryManager>();
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);
}
}

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

@ -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;

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)
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<const char*>(&code), sizeof(code));
buf.append(reinterpret_cast<const char*>(&elevator), sizeof(elevator));
buf.append(reinterpret_cast<const char*>(&ailerons), sizeof(ailerons));
buf.append(reinterpret_cast<const char*>(&rudder), sizeof(rudder));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
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));
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<const char*>(&code), sizeof(code));
buf.append(reinterpret_cast<const char*>(&elevator), sizeof(elevator));
buf.append(reinterpret_cast<const char*>(&ailerons), sizeof(ailerons));
buf.append(reinterpret_cast<const char*>(&rudder), sizeof(rudder));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
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));
// 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<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*>(&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));
// 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<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*>(&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)
{
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);

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

@ -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