rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-15 07:29:15 +01:00
Code Issues Releases Activity

Implemented gcsReceiver.

Browse Source
This commit is contained in:
Kenz Dale 2012-08-29 14:24:58 +02:00
parent 494b3d156e
commit f888eaed41
10 changed files with 234 additions and 213 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ground/openpilotgcs/src/plugins/coreplugin/OpenPilotGCS.xml
View File

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

15
ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp
View File

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

10
ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp
View File

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

93
ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp
View File

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

9
ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp
View File

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

39
ground/openpilotgcs/src/plugins/hitl/hitloptionspage.ui
View File

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

25
ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp
View File

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

70
ground/openpilotgcs/src/plugins/hitl/simulator.cpp
View File

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

32
ground/openpilotgcs/src/plugins/hitl/simulator.h
View File

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

142
ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp
View File

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