rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-07 18:46:06 +01:00
Code Issues Releases Activity
9a5d099a79
LibrePilot/ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.cpp
peabody124 0de938090b Fix GCS problems with renaming the accels_filtered to accels. You'll have to 
update your .ini file

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2708 ebee16cc-31ac-478f-84a7-5cbb03baadba
2011-02-03 02:42:46 +00:00

411 lines
15 KiB
C++
Raw Blame History

/**
******************************************************************************
*
* @file xplanesimulator.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief
* @see The GNU Public License (GPL) Version 3
* @defgroup hitlplugin
* @{
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/**
* Description of X-Plane Protocol:
*
* To see what data can be sended/recieved to/from X-Plane, launch X-Plane -> goto main menu
* (cursor at top of main X-Plane window) -> Settings -> Data Input and Output -> Data Set.
* Data Set shown all X-Plane params,
* each row has four checkbox: 1st check - out to UDP; 4 check - show on screen
* All the UDP messages for X-Plane have the same format, which is:
* 5-character MESSAGE PROLOUGE (to indicate the type of message)
* and then a DATA INPUT STRUCTURE (containing the message data that you want to send or receive)
*
* DATA INPUT/OUTPUT STRUCTURE is the following stuct:
*
* struct data_struct
* {
* int index; // data index, the index into the list of variables
// you can output from the Data Output screen in X-Plane.
* float data[8]; // the up to 8 numbers you see in the data output screen associated with that selection..
// many outputs do not use all 8, though.
* };
*
* For Example, update of aileron/elevon/rudder in X-Plane (11 row in Data Set)
* bytes value description
* [0-3] DATA message type
* [4] none no matter
* [5-8] 11 code of setting param(row in Data Set)
* [9-41] data message data (8 float values)
* total size: 41 byte
*
*/
#include "xplanesimulator.h"
#include "extensionsystem/pluginmanager.h"
#include <coreplugin/icore.h>
#include <coreplugin/threadmanager.h>
#include <math.h>
#include <qxtlogger.h>
void TraceBuf(const char* buf,int len);
XplaneSimulator::XplaneSimulator(const SimulatorSettings& params) :
Simulator(params)
{
once = false;
}
XplaneSimulator::~XplaneSimulator()
{
}
void XplaneSimulator::setupUdpPorts(const QString& host, int inPort, int outPort)
{
inSocket->bind(QHostAddress(host), inPort);
//outSocket->bind(QHostAddress(host), outPort);
once = false;
}
bool XplaneSimulator::setupProcess()
{
emit processOutput(QString("Please start X-Plane manually, and make sure it is setup to output its ") +
"data to host " + settings.hostAddress + " UDP port " + QString::number(settings.inPort));
return true;
}
/**
* update data in X-Plane simulator
*/
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*2-1.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);
#endif
// 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);
if(outSocket->writeDatagram(buf, QHostAddress(settings.remoteHostAddress), 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));
if(outSocket->writeDatagram(buf, QHostAddress(settings.remoteHostAddress), 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);
*/
}
/**
* process data string from X-Plane simulator
*/
void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
{
float altitude = 0;
float latitude = 0;
float longitude = 0;
float airspeed = 0;
float speed = 0;
float pitch = 0;
float roll = 0;
float heading = 0;
float pressure = 0;
float temperature = 0;
float velX = 0;
float velY = 0;
float velZ = 0;
float dstX = 0;
float dstY = 0;
float dstZ = 0;
float rollRate=0;
float pitchRate=0;
float yawRate=0;
QString str;
QByteArray& buf = const_cast<QByteArray&>(dataBuf);
QString data(buf);
if(data.left(4) == "DATA") // check type of packet
{
buf.remove(0,5);
if(dataBuf.size() % 36)
{
qxtLog->info("incorrect length of UDP packet: ",buf);
return; // incorrect length of struct
}
// check correctness of data length, length must be multiple of (id_size+8*float_size)=4+8*4=36
int channelCounter = dataBuf.size() / 36;
do
{
switch(buf[0]) // switch by id
{
case XplaneSimulator::LatitudeLongitude:
latitude = *((float*)(buf.data()+4*1));
longitude = *((float*)(buf.data()+4*2));
altitude = *((float*)(buf.data()+4*3))* FT2M;
break;
case XplaneSimulator::Speed:
airspeed = *((float*)(buf.data()+4*7));
speed = *((float*)(buf.data()+4*8));
break;
case XplaneSimulator::PitchRollHeading:
pitch = *((float*)(buf.data()+4*1));
roll = *((float*)(buf.data()+4*2));
heading = *((float*)(buf.data()+4*3));
break;
/*
case XplaneSimulator::SystemPressures:
pressure = *((float*)(buf.data()+4*1));
break;
*/
case XplaneSimulator::AtmosphereWeather:
pressure = *((float*)(buf.data()+4*1)) * INHG2KPA;
temperature = *((float*)(buf.data()+4*2));
break;
case XplaneSimulator::LocVelDistTraveled:
dstX = *((float*)(buf.data()+4*1));
dstY = *((float*)(buf.data()+4*2));
dstZ = *((float*)(buf.data()+4*3));
velX = *((float*)(buf.data()+4*4));
velY = *((float*)(buf.data()+4*5));
velZ = *((float*)(buf.data()+4*6));
break;
case XplaneSimulator::AngularVelocities:
pitchRate = *((float*)(buf.data()+4*1));
rollRate = *((float*)(buf.data()+4*2));
yawRate = *((float*)(buf.data()+4*3));
break;
default:
break;
}
channelCounter--;
buf.remove(0,36);
} while (channelCounter);
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 = latitude * 1e7;
homeData.Longitude = longitude * 1e7;
homeData.Altitude = altitude;
double LLA[3];
LLA[0]=latitude;
LLA[1]=longitude;
LLA[2]=altitude;
double ECEF[3];
double RNE[9];
Utils::CoordinateConversions().RneFromLLA(LLA,(double (*)[3])RNE);
for (int t=0;t<9;t++) {
homeData.RNE[t]=RNE[t];
}
Utils::CoordinateConversions().LLA2ECEF(LLA,ECEF);
homeData.ECEF[0]=ECEF[0]*100;
homeData.ECEF[1]=ECEF[1]*100;
homeData.ECEF[2]=ECEF[2]*100;
homeData.Be[0]=0;
homeData.Be[1]=0;
homeData.Be[2]=0;
posHome->setData(homeData);
posHome->updated();
// Initialize the initial distance
initX = dstX;
initY = dstY;
initZ = dstZ;
once=1;
}
// Update AltitudeActual object
BaroAltitude::DataFields altActualData;
memset(&altActualData, 0, sizeof(BaroAltitude::DataFields));
altActualData.Altitude = altitude;
altActualData.Temperature = temperature;
altActualData.Pressure = pressure;
altActual->setData(altActualData);
// Update attActual object
AttitudeActual::DataFields attActualData;
memset(&attActualData, 0, sizeof(AttitudeActual::DataFields));
attActualData.Roll = roll; //roll;
attActualData.Pitch = pitch; // pitch
attActualData.Yaw = heading; // Yaw
// attActualData.q1 = 0;
// attActualData.q2 = 0;
// attActualData.q3 = 0;
// attActualData.q4 = 0;
attActual->setData(attActualData);
// Update gps objects
GPSPosition::DataFields gpsData;
memset(&gpsData, 0, sizeof(GPSPosition::DataFields));
gpsData.Altitude = altitude;
gpsData.Heading = heading;
gpsData.Groundspeed = speed;
gpsData.Latitude = latitude*1e7;
gpsData.Longitude = longitude*1e7;
gpsData.Satellites = 10;
gpsData.Status = GPSPosition::STATUS_FIX3D;
gpsPos->setData(gpsData);
// Update VelocityActual.{Nort,East,Down}
VelocityActual::DataFields velocityActualData;
memset(&velocityActualData, 0, sizeof(VelocityActual::DataFields));
velocityActualData.North = velX*100;
velocityActualData.East = velY*100;
velocityActualData.Down = velZ*100;
velActual->setData(velocityActualData);
// Update PositionActual.{Nort,East,Down}
PositionActual::DataFields positionActualData;
memset(&positionActualData, 0, sizeof(PositionActual::DataFields));
positionActualData.North = (dstX-initX)*100;
positionActualData.East = (dstY-initY)*100;
positionActualData.Down = (dstZ-initZ)*100;
posActual->setData(positionActualData);
// Update AttitudeRaw object (filtered gyros only for now)
AttitudeRaw::DataFields rawData;
memset(&rawData, 0, sizeof(AttitudeRaw::DataFields));
rawData = attRaw->getData();
rawData.gyros[0] = rollRate;
//rawData.gyros_filtered[1] = cos(DEG2RAD * roll) * pitchRate + sin(DEG2RAD * roll) * yawRate;
//rawData.gyros_filtered[2] = cos(DEG2RAD * roll) * yawRate - sin(DEG2RAD * roll) * pitchRate;
rawData.gyros[1] = pitchRate;
rawData.gyros[2] = yawRate;
attRaw->setData(rawData);
}
// issue manual update
//attActual->updated();
//altActual->updated();
//posActual->updated();
}
void TraceBuf(const char* buf,int len)
{
QString str;
bool reminder=true;
for(int i=0; i < len; i++)
{
if(!(i%16))
{
if(i>0)
{
qDebug() << str;
str.clear();
reminder=false;
}
reminder=true;
}
str+=QString(" 0x%1").arg((quint8)buf[i],2,16,QLatin1Char('0'));
}
if(reminder)
qDebug() << str;
}
Reference in New Issue View Git Blame Copy Permalink