mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
Implemented gcsReceiver.
This commit is contained in:
parent
494b3d156e
commit
f888eaed41
@ -916,7 +916,7 @@
|
||||
<baroAltitudeEnabled>false</baroAltitudeEnabled>
|
||||
<binPath>\home\lafargue\X-Plane 9\X-Plane-i686</binPath>
|
||||
<dataPath>\usr\share\games\FlightGear</dataPath>
|
||||
<gcsReceiver>true</gcsReceiver>
|
||||
<gcsReceiverEnabled>true</gcsReceiverEnabled>
|
||||
<gpsPosRate>100</gpsPosRate>
|
||||
<gpsPositionEnabled>false</gpsPositionEnabled>
|
||||
<groundTruthEnabled>true</groundTruthEnabled>
|
||||
@ -926,7 +926,7 @@
|
||||
<inputCommand>true</inputCommand>
|
||||
<latitude></latitude>
|
||||
<longitude></longitude>
|
||||
<manualControl>false</manualControl>
|
||||
<manualControlEnabled>false</manualControlEnabled>
|
||||
<manualOutput>false</manualOutput>
|
||||
<minOutputPeriod>100</minOutputPeriod>
|
||||
<outPort>49000</outPort>
|
||||
@ -952,7 +952,7 @@
|
||||
<baroAltitudeEnabled>false</baroAltitudeEnabled>
|
||||
<binPath>\usr\games\fgfs</binPath>
|
||||
<dataPath>\usr\share\games\FlightGear</dataPath>
|
||||
<gcsReceiver>true</gcsReceiver>
|
||||
<gcsReceiverEnabled>false</gcsReceiverEnabled>
|
||||
<gpsPosRate>100</gpsPosRate>
|
||||
<gpsPositionEnabled>false</gpsPositionEnabled>
|
||||
<groundTruthEnabled>true</groundTruthEnabled>
|
||||
@ -962,7 +962,7 @@
|
||||
<inputCommand>true</inputCommand>
|
||||
<latitude></latitude>
|
||||
<longitude></longitude>
|
||||
<manualControl>false</manualControl>
|
||||
<manualControlEnabled>true</manualControlEnabled>
|
||||
<manualOutput>false</manualOutput>
|
||||
<minOutputPeriod>100</minOutputPeriod>
|
||||
<outPort>9010</outPort>
|
||||
@ -988,7 +988,7 @@
|
||||
<baroAltitudeEnabled>false</baroAltitudeEnabled>
|
||||
<binPath>\home\lafargue\X-Plane 9\X-Plane-i686</binPath>
|
||||
<dataPath>\usr\share\games\FlightGear</dataPath>
|
||||
<gcsReceiver>true</gcsReceiver>
|
||||
<gcsReceiverEnabled>false</gcsReceiverEnabled>
|
||||
<gpsPosRate>100</gpsPosRate>
|
||||
<gpsPositionEnabled>false</gpsPositionEnabled>
|
||||
<groundTruthEnabled>true</groundTruthEnabled>
|
||||
@ -998,7 +998,7 @@
|
||||
<inputCommand>true</inputCommand>
|
||||
<latitude></latitude>
|
||||
<longitude></longitude>
|
||||
<manualControl>false</manualControl>
|
||||
<manualControlEnabled>true</manualControlEnabled>
|
||||
<manualOutput>false</manualOutput>
|
||||
<minOutputPeriod>100</minOutputPeriod>
|
||||
<outPort>49000</outPort>
|
||||
|
@ -125,6 +125,7 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data)
|
||||
return;
|
||||
}
|
||||
|
||||
#define AEROSIM_RCCHANNEL_NUMELEM 8
|
||||
float delT,
|
||||
homeX, homeY, homeZ,
|
||||
WpHX, WpHY, WpLat, WpLon,
|
||||
@ -136,7 +137,7 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data)
|
||||
yaw, pitch, roll, // model
|
||||
volt, curr,
|
||||
rx, ry, rz, fx, fy, fz, ux, uy, uz, // matrix
|
||||
ch[8];
|
||||
ch[AEROSIM_RCCHANNEL_NUMELEM];
|
||||
|
||||
stream >> delT;
|
||||
stream >> homeX >> homeY >> homeZ;
|
||||
@ -152,11 +153,17 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data)
|
||||
stream >> ch[0] >> ch[1] >> ch[2] >> ch[3] >> ch[4] >> ch[5] >> ch[6] >> ch[7];
|
||||
stream >> udpCounterASrecv;
|
||||
|
||||
Output2OP out;
|
||||
memset(&out, 0, sizeof(Output2OP));
|
||||
Output2Hardware out;
|
||||
memset(&out, 0, sizeof(Output2Hardware));
|
||||
|
||||
|
||||
out.delT=delT;
|
||||
|
||||
/*************************************************************************************/
|
||||
for (int i=0; i< AEROSIM_RCCHANNEL_NUMELEM; i++){
|
||||
out.rc_channel[i]=ch[i]; //Elements in rc_channel are between -1 and 1
|
||||
}
|
||||
|
||||
/**********************************************************************************************/
|
||||
QMatrix4x4 mat;
|
||||
mat = QMatrix4x4( fy, fx, -fz, 0.0, // model matrix
|
||||
@ -168,8 +175,8 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data)
|
||||
QQuaternion quat; // model quat
|
||||
asMatrix2Quat(mat, quat);
|
||||
|
||||
// rotate gravity
|
||||
/*************************************************************************************/
|
||||
// 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();
|
||||
|
@ -60,6 +60,8 @@ FGSimulator::~FGSimulator()
|
||||
|
||||
void FGSimulator::setupUdpPorts(const QString& host, int inPort, int outPort)
|
||||
{
|
||||
Q_UNUSED(outPort);
|
||||
|
||||
if(inSocket->bind(QHostAddress(host), inPort))
|
||||
emit processOutput("Successfully bound to address " + host + " on port " + QString::number(inPort) + "\n");
|
||||
else
|
||||
@ -115,7 +117,7 @@ bool FGSimulator::setupProcess()
|
||||
"--vc=100 " +
|
||||
"--log-level=alert " +
|
||||
"--generic=socket,out,20," + settings.hostAddress + "," + QString::number(settings.inPort) + ",udp,opfgprotocol");
|
||||
if(!settings.manualControl)
|
||||
if(settings.manualControlEnabled) // <--[BCH] What does this do? Why does it depend on ManualControl?
|
||||
{
|
||||
args.append(" --generic=socket,in,400," + settings.remoteAddress + "," + QString::number(settings.outPort) + ",udp,opfgprotocol");
|
||||
}
|
||||
@ -216,7 +218,7 @@ void FGSimulator::transmitUpdate()
|
||||
// V2.0 does not currently work with --generic-protocol
|
||||
}
|
||||
|
||||
if(!settings.manualControl)
|
||||
if(settings.manualControlEnabled)
|
||||
{
|
||||
actData.Roll = ailerons;
|
||||
actData.Pitch = -elevator;
|
||||
@ -291,8 +293,8 @@ void FGSimulator::processUpdate(const QByteArray& inp)
|
||||
///////
|
||||
// Output formatting
|
||||
///////
|
||||
Output2OP out;
|
||||
memset(&out, 0, sizeof(Output2OP));
|
||||
Output2Hardware out;
|
||||
memset(&out, 0, sizeof(Output2Hardware));
|
||||
|
||||
float NED[3];
|
||||
// convert from cm back to meters
|
||||
|
@ -35,7 +35,7 @@ HITLConfiguration::HITLConfiguration(QString classId, QSettings* qSettings, QObj
|
||||
settings.simulatorId = "";
|
||||
settings.binPath = "";
|
||||
settings.dataPath = "";
|
||||
settings.manualControl = false;
|
||||
settings.manualControlEnabled = true;
|
||||
settings.startSim = false;
|
||||
settings.addNoise = false;
|
||||
settings.hostAddress = "127.0.0.1";
|
||||
@ -45,65 +45,63 @@ HITLConfiguration::HITLConfiguration(QString classId, QSettings* qSettings, QObj
|
||||
settings.latitude = "";
|
||||
settings.longitude = "";
|
||||
|
||||
settings.attRawEnabled = false;
|
||||
settings.attRawRate = 20;
|
||||
settings.attRawEnabled = false;
|
||||
settings.attRawRate = 20;
|
||||
|
||||
settings.attActualEnabled = true;
|
||||
settings.attActHW = false;
|
||||
settings.attActSim = true;
|
||||
settings.attActCalc = false;
|
||||
settings.attActualEnabled = true;
|
||||
settings.attActHW = false;
|
||||
settings.attActSim = true;
|
||||
settings.attActCalc = false;
|
||||
|
||||
settings.gpsPositionEnabled = false;
|
||||
settings.gpsPosRate = 100;
|
||||
settings.gpsPositionEnabled = false;
|
||||
settings.gpsPosRate = 100;
|
||||
|
||||
settings.groundTruthEnabled = false;
|
||||
settings.groundTruthRate = 100;
|
||||
settings.groundTruthEnabled = false;
|
||||
settings.groundTruthRate = 100;
|
||||
|
||||
settings.inputCommand = false;
|
||||
settings.gcsReceiver = false;
|
||||
settings.manualControl = false;
|
||||
settings.manualOutput = false;
|
||||
settings.minOutputPeriod = 100;
|
||||
settings.inputCommand = false;
|
||||
settings.gcsReceiverEnabled = false;
|
||||
settings.manualControlEnabled= false;
|
||||
settings.minOutputPeriod = 100;
|
||||
|
||||
|
||||
// if a saved configuration exists load it, and overwrite defaults
|
||||
if (qSettings != 0) {
|
||||
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.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.binPath = qSettings->value("binPath").toString();
|
||||
settings.dataPath = qSettings->value("dataPath").toString();
|
||||
settings.binPath = qSettings->value("binPath").toString();
|
||||
settings.dataPath = qSettings->value("dataPath").toString();
|
||||
|
||||
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.latitude = qSettings->value("latitude").toString();
|
||||
settings.longitude = qSettings->value("longitude").toString();
|
||||
settings.startSim = qSettings->value("startSim").toBool();
|
||||
settings.addNoise = qSettings->value("noiseCheckBox").toBool();
|
||||
|
||||
settings.attRawEnabled = qSettings->value("attRawEnabled").toBool();
|
||||
settings.attRawRate = qSettings->value("attRawRate").toInt();
|
||||
settings.attRawEnabled = qSettings->value("attRawEnabled").toBool();
|
||||
settings.attRawRate = qSettings->value("attRawRate").toInt();
|
||||
|
||||
settings.attActualEnabled = qSettings->value("attActualEnabled").toBool();
|
||||
settings.attActHW = qSettings->value("attActHW").toBool();
|
||||
settings.attActSim = qSettings->value("attActSim").toBool();
|
||||
settings.attActCalc = qSettings->value("attActCalc").toBool();
|
||||
settings.attActualEnabled = qSettings->value("attActualEnabled").toBool();
|
||||
settings.attActHW = qSettings->value("attActHW").toBool();
|
||||
settings.attActSim = qSettings->value("attActSim").toBool();
|
||||
settings.attActCalc = qSettings->value("attActCalc").toBool();
|
||||
|
||||
settings.baroAltitudeEnabled= qSettings->value("baroAltitudeEnabled").toBool();
|
||||
settings.baroAltRate = qSettings->value("baroAltRate").toInt();
|
||||
settings.baroAltitudeEnabled = qSettings->value("baroAltitudeEnabled").toBool();
|
||||
settings.baroAltRate = qSettings->value("baroAltRate").toInt();
|
||||
|
||||
settings.gpsPositionEnabled = qSettings->value("gpsPositionEnabled").toBool();
|
||||
settings.gpsPosRate = qSettings->value("gpsPosRate").toInt();
|
||||
settings.gpsPositionEnabled = qSettings->value("gpsPositionEnabled").toBool();
|
||||
settings.gpsPosRate = qSettings->value("gpsPosRate").toInt();
|
||||
|
||||
settings.groundTruthEnabled = qSettings->value("groundTruthEnabled").toBool();
|
||||
settings.groundTruthRate = qSettings->value("groundTruthRate").toInt();
|
||||
settings.groundTruthEnabled = qSettings->value("groundTruthEnabled").toBool();
|
||||
settings.groundTruthRate = qSettings->value("groundTruthRate").toInt();
|
||||
|
||||
settings.inputCommand = qSettings->value("inputCommand").toBool();
|
||||
settings.gcsReceiver = qSettings->value("gcsReceiver").toBool();
|
||||
settings.manualControl = qSettings->value("manualControl").toBool();
|
||||
settings.manualOutput = qSettings->value("manualOutput").toBool();
|
||||
settings.minOutputPeriod = qSettings->value("minOutputPeriod").toInt();
|
||||
settings.inputCommand = qSettings->value("inputCommand").toBool();
|
||||
settings.gcsReceiverEnabled = qSettings->value("gcsReceiverEnabled").toBool();
|
||||
settings.manualControlEnabled= qSettings->value("manualControlEnabled").toBool();
|
||||
settings.minOutputPeriod = qSettings->value("minOutputPeriod").toInt();
|
||||
}
|
||||
}
|
||||
|
||||
@ -133,7 +131,9 @@ void HITLConfiguration::saveConfig(QSettings* qSettings) const {
|
||||
qSettings->setValue("longitude", settings.longitude);
|
||||
qSettings->setValue("addNoise", settings.addNoise);
|
||||
qSettings->setValue("startSim", settings.startSim);
|
||||
qSettings->setValue("manualControl", settings.manualControl);
|
||||
|
||||
qSettings->setValue("gcsReceiverEnabled", settings.gcsReceiverEnabled);
|
||||
qSettings->setValue("manualControlEnabled", settings.manualControlEnabled);
|
||||
|
||||
qSettings->setValue("attRawEnabled", settings.attRawEnabled);
|
||||
qSettings->setValue("attRawRate", settings.attRawRate);
|
||||
@ -148,9 +148,6 @@ void HITLConfiguration::saveConfig(QSettings* qSettings) const {
|
||||
qSettings->setValue("groundTruthEnabled", settings.groundTruthEnabled);
|
||||
qSettings->setValue("groundTruthRate", settings.groundTruthRate);
|
||||
qSettings->setValue("inputCommand", settings.inputCommand);
|
||||
qSettings->setValue("gcsReceiver", settings.gcsReceiver);
|
||||
qSettings->setValue("manualControl", settings.manualControl);
|
||||
qSettings->setValue("manualOutput", settings.manualOutput);
|
||||
qSettings->setValue("minOutputPeriod", settings.minOutputPeriod);
|
||||
|
||||
|
||||
|
@ -79,8 +79,8 @@ QWidget *HITLOptionsPage::createPage(QWidget *parent)
|
||||
m_optionsPage->executablePath->setPath(config->Settings().binPath);
|
||||
m_optionsPage->dataPath->setPath(config->Settings().dataPath);
|
||||
|
||||
m_optionsPage->manualControlRadioButton->setChecked(config->Settings().manualControl);
|
||||
m_optionsPage->gcsReceiverRadioButton->setChecked(config->Settings().gcsReceiver);
|
||||
m_optionsPage->manualControlRadioButton->setChecked(config->Settings().manualControlEnabled);
|
||||
m_optionsPage->gcsReceiverRadioButton->setChecked(config->Settings().gcsReceiverEnabled);
|
||||
|
||||
m_optionsPage->startSim->setChecked(config->Settings().startSim);
|
||||
m_optionsPage->noiseCheckBox->setChecked(config->Settings().addNoise);
|
||||
@ -153,8 +153,9 @@ void HITLOptionsPage::apply()
|
||||
|
||||
settings.minOutputPeriod = m_optionsPage->minOutputPeriodSpinbox->value();
|
||||
|
||||
settings.manualControl = m_optionsPage->manualControlRadioButton->isChecked();
|
||||
settings.gcsReceiver = m_optionsPage->gcsReceiverRadioButton->isChecked();
|
||||
settings.manualControlEnabled = m_optionsPage->manualControlRadioButton->isChecked();
|
||||
settings.gcsReceiverEnabled = m_optionsPage->gcsReceiverRadioButton->isChecked();
|
||||
|
||||
settings.attActHW = m_optionsPage->attActHW->isChecked();
|
||||
settings.attActSim = m_optionsPage->attActSim->isChecked();
|
||||
settings.attActCalc = m_optionsPage->attActCalc->isChecked();
|
||||
|
@ -588,13 +588,6 @@
|
||||
<property name="bottomMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
<widget class="QRadioButton" name="attActHW">
|
||||
<property name="text">
|
||||
<string>send simulated inertial data to board</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QRadioButton" name="attActSim">
|
||||
<property name="font">
|
||||
@ -614,6 +607,13 @@
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QRadioButton" name="attActHW">
|
||||
<property name="text">
|
||||
<string>send simulated inertial data to board</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QRadioButton" name="attActCalc">
|
||||
<property name="toolTip">
|
||||
@ -927,7 +927,7 @@
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="title">
|
||||
<string>Map command...</string>
|
||||
<string>Map transmitter commands...</string>
|
||||
</property>
|
||||
<property name="flat">
|
||||
<bool>true</bool>
|
||||
@ -952,15 +952,19 @@
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
<widget class="QRadioButton" name="gcsReceiverRadioButton">
|
||||
<widget class="QRadioButton" name="manualControlRadioButton">
|
||||
<property name="enabled">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>14</pointsize>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>from simulator to hardware (via GCSReceiver)</string>
|
||||
<string>from hardware to simulator (via ManualCtrl)</string>
|
||||
</property>
|
||||
<property name="checked">
|
||||
<bool>true</bool>
|
||||
@ -968,12 +972,12 @@
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QRadioButton" name="manualControlRadioButton">
|
||||
<property name="enabled">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<widget class="QRadioButton" name="gcsReceiverRadioButton">
|
||||
<property name="text">
|
||||
<string>from hardware to simulator (via ManualCtrl)</string>
|
||||
<string>from simulator to hardware (via GCSReceiver)</string>
|
||||
</property>
|
||||
<property name="checked">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
@ -986,10 +990,7 @@
|
||||
<number>6</number>
|
||||
</property>
|
||||
<item>
|
||||
<widget class="QCheckBox" name="manualOutput">
|
||||
<property name="enabled">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<widget class="QLabel" name="label_3">
|
||||
<property name="text">
|
||||
<string>Maximum GCS to hardware output rate:</string>
|
||||
</property>
|
||||
|
@ -241,8 +241,8 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
|
||||
///////
|
||||
// Output formatting
|
||||
///////
|
||||
Output2OP out;
|
||||
memset(&out, 0, sizeof(Output2OP));
|
||||
Output2Hardware out;
|
||||
memset(&out, 0, sizeof(Output2Hardware));
|
||||
|
||||
// Compute rotation matrix, for later calculations
|
||||
float Rbe[3][3];
|
||||
@ -254,24 +254,19 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
|
||||
Utils::CoordinateConversions().RPY2Quaternion(rpy,quat);
|
||||
Utils::CoordinateConversions().Quaternion2R(quat,Rbe);
|
||||
|
||||
//Calculate ECEF
|
||||
double RNE[9];
|
||||
double ECEF[3];
|
||||
double LLA[3];
|
||||
LLA[0]=settings.latitude.toFloat();
|
||||
LLA[1]=settings.longitude.toFloat();
|
||||
LLA[2]=0;
|
||||
Utils::CoordinateConversions().RneFromLLA(LLA,(double (*)[3])RNE);
|
||||
Utils::CoordinateConversions().LLA2ECEF(LLA,ECEF);
|
||||
|
||||
// Update GPS Position objects
|
||||
double HomeLLA[3];
|
||||
double LLA[3];
|
||||
double NED[3];
|
||||
HomeLLA[0]=settings.latitude.toFloat();
|
||||
HomeLLA[1]=settings.longitude.toFloat();
|
||||
HomeLLA[2]=0;
|
||||
NED[0] = current.Y;
|
||||
NED[1] = current.X;
|
||||
NED[2] = -current.Z;
|
||||
Utils::CoordinateConversions().NED2LLA_HomeECEF(ECEF,NED,LLA);
|
||||
out.latitude = settings.latitude.toFloat() * 1e7;
|
||||
out.longitude = settings.longitude.toFloat() * 1e7;
|
||||
Utils::CoordinateConversions().NED2LLA_HomeLLA(HomeLLA,NED,LLA);
|
||||
out.latitude = LLA[0] * 1e7;
|
||||
out.longitude = LLA[1] * 1e7;
|
||||
out.groundspeed = current.groundspeed;
|
||||
|
||||
out.calibratedAirspeed = current.ias;
|
||||
|
@ -66,6 +66,9 @@ Simulator::Simulator(const SimulatorSettings& params) :
|
||||
QTime currentTime=QTime::currentTime();
|
||||
gpsPosTime = currentTime;
|
||||
groundTruthTime = currentTime;
|
||||
gcsRcvrTime = currentTime;
|
||||
attRawTime = currentTime;
|
||||
baroAltTime = currentTime;
|
||||
|
||||
}
|
||||
|
||||
@ -134,6 +137,7 @@ void Simulator::onStart()
|
||||
actDesired = ActuatorDesired::GetInstance(objManager);
|
||||
actCommand = ActuatorCommand::GetInstance(objManager);
|
||||
manCtrlCommand = ManualControlCommand::GetInstance(objManager);
|
||||
gcsReceiver= GCSReceiver::GetInstance(objManager);
|
||||
flightStatus = FlightStatus::GetInstance(objManager);
|
||||
posHome = HomeLocation::GetInstance(objManager);
|
||||
velActual = VelocityActual::GetInstance(objManager);
|
||||
@ -230,19 +234,14 @@ void Simulator::receiveUpdate()
|
||||
|
||||
void Simulator::setupObjects()
|
||||
{
|
||||
setupInputObject(actDesired, settings.minOutputPeriod);
|
||||
|
||||
/* if (settings.gcsReciever) {
|
||||
setupInputObject(actCommand, settings.outputRate);
|
||||
setupOutputObject(gcsReceiver);
|
||||
} else if (settings.manualControl) {
|
||||
// setupInputObject(actDesired);
|
||||
// setupInputObject(camDesired);
|
||||
// setupInputObject(acsDesired);
|
||||
// setupOutputObject(manCtrlCommand);
|
||||
qDebug() << "ManualControlCommand not implemented yet";
|
||||
if (settings.gcsReceiverEnabled) {
|
||||
setupInputObject(actCommand, settings.minOutputPeriod); //Input to the simulator
|
||||
setupOutputObject(gcsReceiver, settings.minOutputPeriod);
|
||||
} else if (settings.manualControlEnabled) {
|
||||
setupInputObject(actDesired, settings.minOutputPeriod); //Input to the simulator
|
||||
}
|
||||
*/
|
||||
|
||||
setupOutputObject(posHome, 10000);
|
||||
setupOutputObject(baroAlt, 250);
|
||||
|
||||
@ -285,13 +284,8 @@ void Simulator::setupInputObject(UAVObject* obj, quint32 updatePeriod)
|
||||
UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READWRITE);
|
||||
UAVObject::SetFlightTelemetryAcked(mdata, false);
|
||||
|
||||
if (settings.manualOutput) {
|
||||
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
|
||||
mdata.flightTelemetryUpdatePeriod = updatePeriod;
|
||||
} else {
|
||||
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE);
|
||||
mdata.flightTelemetryUpdatePeriod = 0;
|
||||
}
|
||||
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
|
||||
mdata.flightTelemetryUpdatePeriod = updatePeriod;
|
||||
|
||||
obj->setMetadata(mdata);
|
||||
}
|
||||
@ -320,12 +314,15 @@ void Simulator::setupOutputObject(UAVObject* obj, quint32 updatePeriod)
|
||||
{
|
||||
UAVObject::Metadata mdata;
|
||||
mdata = obj->getDefaultMetadata();
|
||||
UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READONLY);
|
||||
UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READWRITE);
|
||||
UAVObject::SetFlightTelemetryUpdateMode(mdata,UAVObject::UPDATEMODE_MANUAL);
|
||||
UAVObject::SetGcsTelemetryAcked(mdata, false);
|
||||
|
||||
UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READWRITE);
|
||||
UAVObject::SetGcsTelemetryAcked(mdata, false);
|
||||
UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
|
||||
mdata.gcsTelemetryUpdatePeriod = updatePeriod;
|
||||
mdata.gcsTelemetryUpdatePeriod = updatePeriod;
|
||||
|
||||
UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READONLY);
|
||||
UAVObject::SetFlightTelemetryUpdateMode(mdata,UAVObject::UPDATEMODE_MANUAL);
|
||||
|
||||
obj->setMetadata(mdata);
|
||||
}
|
||||
|
||||
@ -373,7 +370,7 @@ void Simulator::resetInitialHomePosition(){
|
||||
}
|
||||
|
||||
|
||||
void Simulator::updateUAVOs(Output2OP out){
|
||||
void Simulator::updateUAVOs(Output2Hardware out){
|
||||
|
||||
QTime currentTime = QTime::currentTime();
|
||||
|
||||
@ -569,24 +566,23 @@ void Simulator::updateUAVOs(Output2OP out){
|
||||
/*****************************************/
|
||||
}
|
||||
|
||||
if (settings.gcsReceiverEnabled) {
|
||||
if (gcsRcvrTime.msecsTo(currentTime) >= settings.minOutputPeriod) {
|
||||
GCSReceiver::DataFields gcsRcvrData;
|
||||
memset(&gcsRcvrData, 0, sizeof(GCSReceiver::DataFields));
|
||||
|
||||
if (settings.gcsReceiver) {
|
||||
// static QTime gcsRcvrTime = currentTime;
|
||||
// if (!settings.manualOutput || gcsRcvrTime.msecsTo(currentTime) >= settings.outputRate) {
|
||||
// GCSReceiver::DataFields gcsRcvrData;
|
||||
// gcsRcvrData = gcsReceiver->getData();
|
||||
for (quint16 i = 0; i < GCSReceiver::CHANNEL_NUMELEM; i++){
|
||||
gcsRcvrData.Channel[i] = 1500 + (out.rc_channel[i]*500); //Elements in rc_channel are between -1 and 1
|
||||
}
|
||||
|
||||
// for (int i = 0; i < 8; ++i)
|
||||
// gcsRcvrData.Channel[i] = 1500 + (ch[i] * 500);
|
||||
gcsReceiver->setData(gcsRcvrData);
|
||||
|
||||
// gcsReceiver->setData(gcsRcvrData);
|
||||
// if (settings.manualOutput)
|
||||
// gcsRcvrTime = currentTime;
|
||||
// }
|
||||
} else if (settings.manualControl) {
|
||||
// not implemented yet
|
||||
gcsRcvrTime.addMSecs(settings.minOutputPeriod);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (settings.gpsPositionEnabled) {
|
||||
if (gpsPosTime.msecsTo(currentTime) >= settings.gpsPosRate) {
|
||||
// Update GPS Position objects
|
||||
|
@ -136,23 +136,22 @@ typedef struct _CONNECTION
|
||||
quint16 gpsPosRate;
|
||||
|
||||
bool inputCommand;
|
||||
bool gcsReceiver;
|
||||
bool manualControl;
|
||||
bool manualOutput;
|
||||
bool gcsReceiverEnabled;
|
||||
bool manualControlEnabled;
|
||||
quint16 minOutputPeriod;
|
||||
|
||||
} SimulatorSettings;
|
||||
|
||||
|
||||
struct Output2OP{
|
||||
struct Output2Hardware{
|
||||
float latitude;
|
||||
float longitude;
|
||||
float altitude;
|
||||
float heading;
|
||||
float groundspeed; //[m/s]
|
||||
float calibratedAirspeed; //[m/s]
|
||||
float pitch;
|
||||
float roll;
|
||||
float pitch;
|
||||
float pressure;
|
||||
float temperature;
|
||||
float velNorth; //[m/s]
|
||||
@ -168,8 +167,27 @@ struct Output2OP{
|
||||
float pitchRate; //[deg/s]
|
||||
float yawRate; //[deg/s]
|
||||
float delT;
|
||||
|
||||
float rc_channel[GCSReceiver::CHANNEL_NUMELEM]; //Elements in rc_channel are between -1 and 1
|
||||
|
||||
|
||||
float rollDesired;
|
||||
float pitchDesired;
|
||||
float yawDesired;
|
||||
float throttleDesired;
|
||||
};
|
||||
|
||||
//struct Output2Simulator{
|
||||
// float roll;
|
||||
// float pitch;
|
||||
// float yaw;
|
||||
// float throttle;
|
||||
|
||||
// float ailerons;
|
||||
// float rudder;
|
||||
// float elevator;
|
||||
// float motor;
|
||||
//};
|
||||
|
||||
class Simulator : public QObject
|
||||
{
|
||||
@ -198,7 +216,7 @@ public:
|
||||
virtual void setupUdpPorts(const QString& host, int inPort, int outPort) { Q_UNUSED(host) Q_UNUSED(inPort) Q_UNUSED(outPort)}
|
||||
|
||||
void resetInitialHomePosition();
|
||||
void updateUAVOs(Output2OP out);
|
||||
void updateUAVOs(Output2Hardware out);
|
||||
|
||||
signals:
|
||||
void autopilotConnected();
|
||||
@ -253,6 +271,7 @@ protected:
|
||||
Accels* accels;
|
||||
Gyros* gyros;
|
||||
GCSTelemetryStats* telStats;
|
||||
GCSReceiver* gcsReceiver;
|
||||
|
||||
SimulatorSettings settings;
|
||||
|
||||
@ -277,6 +296,7 @@ private:
|
||||
QTime gpsPosTime;
|
||||
QTime groundTruthTime;
|
||||
QTime baroAltTime;
|
||||
QTime gcsRcvrTime;
|
||||
|
||||
QString name;
|
||||
QString simulatorId;
|
||||
|
@ -99,29 +99,30 @@ bool XplaneSimulator::setupProcess()
|
||||
*/
|
||||
void XplaneSimulator::transmitUpdate()
|
||||
{
|
||||
//Read ActuatorDesired from autopilot
|
||||
ActuatorDesired::DataFields actData = actDesired->getData();
|
||||
float ailerons = actData.Roll;
|
||||
float elevator = actData.Pitch;
|
||||
float rudder = actData.Yaw;
|
||||
float throttle = actData.Throttle > 0? actData.Throttle : 0;
|
||||
float none = -999;
|
||||
//quint32 none = *((quint32*)&tmp); // get float as 4 bytes
|
||||
|
||||
quint32 code;
|
||||
QByteArray buf;
|
||||
QDataStream stream(&buf,QIODevice::ReadWrite);
|
||||
|
||||
// !!! LAN byte order - Big Endian
|
||||
if (settings.manualControlEnabled) {
|
||||
//Read ActuatorDesired from autopilot
|
||||
ActuatorDesired::DataFields actData = actDesired->getData();
|
||||
float ailerons = actData.Roll;
|
||||
float elevator = actData.Pitch;
|
||||
float rudder = actData.Yaw;
|
||||
float throttle = actData.Throttle > 0? actData.Throttle : 0;
|
||||
float none = -999;
|
||||
//quint32 none = *((quint32*)&tmp); // get float as 4 bytes
|
||||
|
||||
quint32 code;
|
||||
QByteArray buf;
|
||||
QDataStream stream(&buf,QIODevice::ReadWrite);
|
||||
|
||||
// !!! LAN byte order - Big Endian
|
||||
#if Q_BYTE_ORDER == Q_LITTLE_ENDIAN
|
||||
stream.setByteOrder(QDataStream::LittleEndian);
|
||||
stream.setByteOrder(QDataStream::LittleEndian);
|
||||
#endif
|
||||
|
||||
// 11th data settings (flight con: ail/elv/rud)
|
||||
buf.clear();
|
||||
code = 11;
|
||||
//quint8 header[] = "DATA";
|
||||
/*
|
||||
|
||||
// 11th data settings (flight con: ail/elv/rud)
|
||||
buf.clear();
|
||||
code = 11;
|
||||
//quint8 header[] = "DATA";
|
||||
/*
|
||||
stream << *((quint32*)header) <<
|
||||
(quint8)0x30 <<
|
||||
code <<
|
||||
@ -134,57 +135,58 @@ void XplaneSimulator::transmitUpdate()
|
||||
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.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*>(&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.remoteAddress), settings.outPort) == -1)
|
||||
{
|
||||
emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n");
|
||||
}
|
||||
|
||||
//outSocket->write(buf);
|
||||
|
||||
|
||||
|
||||
/** !!! this settings was given from ardupilot X-Plane.pl, I comment them
|
||||
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.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*>(&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.remoteAddress), settings.outPort) == -1)
|
||||
{
|
||||
emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n");
|
||||
}
|
||||
|
||||
//outSocket->write(buf);
|
||||
|
||||
|
||||
|
||||
/** !!! this settings was given from ardupilot X-Plane.pl, I comment them
|
||||
but if it needed comment should be removed !!!
|
||||
|
||||
|
||||
// 8th data settings (joystick 1 ail/elv/rud)
|
||||
stream << "DATA0" << quint32(11) << elevator << ailerons << rudder
|
||||
<< float(-999) << float(-999) << float(-999) << float(-999) << float(-999);
|
||||
outSocket->write(buf);
|
||||
*/
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -294,8 +296,8 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
|
||||
///////
|
||||
// Output formatting
|
||||
///////
|
||||
Output2OP out;
|
||||
memset(&out, 0, sizeof(Output2OP));
|
||||
Output2Hardware out;
|
||||
memset(&out, 0, sizeof(Output2Hardware));
|
||||
|
||||
// Update GPS Position objects
|
||||
out.latitude = latitude * 1e7;
|
||||
@ -354,7 +356,7 @@ void TraceBuf(const char* buf,int len)
|
||||
{
|
||||
if(i>0)
|
||||
{
|
||||
// qDebug() << str;
|
||||
qDebug() << str;
|
||||
str.clear();
|
||||
reminder=false;
|
||||
}
|
||||
@ -364,6 +366,6 @@ void TraceBuf(const char* buf,int len)
|
||||
}
|
||||
|
||||
if(reminder){
|
||||
// qDebug() << str;
|
||||
qDebug() << str;
|
||||
}
|
||||
}
|
||||
|
Loading…
x
Reference in New Issue
Block a user