mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-02 10:24:11 +01:00
Extended HiTL API to the rest of the simulators.
This commit is contained in:
Kenz Dale
parent
cb377e0f1d
commit
d9139c6529
@ -34,9 +34,6 @@
|
||||
#define M_PI 3.14159265358979323846
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
//FGSimulator::FGSimulator(QString hostAddr, int outPort, int inPort, bool manual, QString binPath, QString dataPath) :
|
||||
// Simulator(hostAddr, outPort, inPort, manual, binPath, dataPath),
|
||||
// fgProcess(NULL)
|
||||
@ -63,8 +60,6 @@ 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
|
||||
@ -237,7 +232,6 @@ void FGSimulator::transmitUpdate()
|
||||
void FGSimulator::processUpdate(const QByteArray& inp)
|
||||
{
|
||||
//TODO: this does not use the FLIGHT_PARAM structure, it should!
|
||||
static char once=0;
|
||||
// Split
|
||||
QString data(inp);
|
||||
QStringList fields = data.split(",");
|
||||
@ -248,11 +242,11 @@ void FGSimulator::processUpdate(const QByteArray& inp)
|
||||
// Get zRate (deg/s)
|
||||
// float zRate = fields[2].toFloat() * 180.0/M_PI;
|
||||
// Get xAccel (m/s^2)
|
||||
// float xAccel = fields[3].toFloat() * FT2M;
|
||||
float xAccel = fields[3].toFloat() * FT2M;
|
||||
// Get yAccel (m/s^2)
|
||||
// float yAccel = fields[4].toFloat() * FT2M;
|
||||
float yAccel = fields[4].toFloat() * FT2M;
|
||||
// Get xAccel (m/s^2)
|
||||
// float zAccel = fields[5].toFloat() * FT2M;
|
||||
float zAccel = fields[5].toFloat() * FT2M;
|
||||
// Get pitch (deg)
|
||||
float pitch = fields[6].toFloat();
|
||||
// Get pitchRate (deg/s)
|
||||
@ -278,7 +272,7 @@ void FGSimulator::processUpdate(const QByteArray& inp)
|
||||
// Get groundspeed (m/s)
|
||||
float groundspeed = fields[17].toFloat() * KT2MPS;
|
||||
// Get airspeed (m/s)
|
||||
// float airspeed = fields[18].toFloat() * KT2MPS;
|
||||
float airspeed = fields[18].toFloat() * KT2MPS;
|
||||
// Get temperature (degC)
|
||||
float temperature = fields[19].toFloat();
|
||||
// Get pressure (kpa)
|
||||
@ -294,112 +288,60 @@ void FGSimulator::processUpdate(const QByteArray& inp)
|
||||
int n = fields[24].toInt();
|
||||
udpCounterFGrecv = n;
|
||||
|
||||
//run once
|
||||
HomeLocation::DataFields homeData = posHome->getData();
|
||||
if(!once)
|
||||
{
|
||||
memset(&homeData, 0, sizeof(HomeLocation::DataFields));
|
||||
// Update homelocation
|
||||
homeData.Latitude = latitude * 10e6;
|
||||
homeData.Longitude = longitude * 10e6;
|
||||
homeData.Altitude = 0;
|
||||
double LLA[3];
|
||||
LLA[0]=latitude;
|
||||
LLA[1]=longitude;
|
||||
LLA[2]=0;
|
||||
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);
|
||||
once=1;
|
||||
}
|
||||
|
||||
// Update VelocityActual.{Nort,East,Down}
|
||||
VelocityActual::DataFields velocityActualData;
|
||||
memset(&velocityActualData, 0, sizeof(VelocityActual::DataFields));
|
||||
velocityActualData.North = velocityActualNorth;
|
||||
velocityActualData.East = velocityActualEast;
|
||||
velocityActualData.Down = velocityActualDown;
|
||||
velActual->setData(velocityActualData);
|
||||
|
||||
// Update PositionActual.{Nort,East,Down}
|
||||
PositionActual::DataFields positionActualData;
|
||||
memset(&positionActualData, 0, sizeof(PositionActual::DataFields));
|
||||
positionActualData.North = 0; //Currently hardcoded as there is no way of setting up a reference point to calculate distance
|
||||
positionActualData.East = 0; //Currently hardcoded as there is no way of setting up a reference point to calculate distance
|
||||
positionActualData.Down = altitude ; //Multiply by 1 because positionActual expects input in meters.
|
||||
posActual->setData(positionActualData);
|
||||
|
||||
// Update AltitudeActual object
|
||||
BaroAltitude::DataFields baroAltData;
|
||||
memset(&baroAltData, 0, sizeof(BaroAltitude::DataFields));
|
||||
baroAltData.Altitude = altitudeAGL;
|
||||
baroAltData.Temperature = temperature;
|
||||
baroAltData.Pressure = pressure;
|
||||
baroAlt->setData(baroAltData);
|
||||
|
||||
// Update attActual object
|
||||
AttitudeActual::DataFields attActualData;
|
||||
memset(&attActualData, 0, sizeof(AttitudeActual::DataFields));
|
||||
attActualData.Roll = roll;
|
||||
attActualData.Pitch = pitch;
|
||||
attActualData.Yaw = 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 = groundspeed;
|
||||
gpsData.Latitude = latitude*1e7;
|
||||
gpsData.Longitude = longitude*1e7;
|
||||
gpsData.Satellites = 10;
|
||||
gpsData.Status = GPSPosition::STATUS_FIX3D;
|
||||
gpsPos->setData(gpsData);
|
||||
///////
|
||||
// Output formatting
|
||||
///////
|
||||
Output2OP out;
|
||||
memset(&out, 0, sizeof(Output2OP));
|
||||
|
||||
float NED[3];
|
||||
// convert from cm back to meters
|
||||
|
||||
double hLLA[3] = {(double) homeData.Latitude / 1e7, (double) homeData.Longitude / 1e7, (double) (homeData.Altitude)};
|
||||
double LLA[3] = {latitude, longitude, altitude};
|
||||
double ECEF[3];
|
||||
double RNE[9];
|
||||
Utils::CoordinateConversions().RneFromLLA(hLLA,(double (*)[3])RNE);
|
||||
Utils::CoordinateConversions().LLA2ECEF(hLLA,ECEF);
|
||||
Utils::CoordinateConversions().LLA2Base(hLLA, ECEF, (float (*)[3]) RNE, NED);
|
||||
Utils::CoordinateConversions().RneFromLLA(LLA,(double (*)[3])RNE);
|
||||
Utils::CoordinateConversions().LLA2ECEF(LLA,ECEF);
|
||||
Utils::CoordinateConversions().LLA2Base(LLA, ECEF, (float (*)[3]) RNE, NED);
|
||||
|
||||
positionActualData.North = NED[0]; //Currently hardcoded as there is no way of setting up a reference point to calculate distance
|
||||
positionActualData.East = NED[1]; //Currently hardcoded as there is no way of setting up a reference point to calculate distance
|
||||
positionActualData.Down = NED[2]; //Multiply by 1 because positionActual expects input in meters.
|
||||
posActual->setData(positionActualData);
|
||||
|
||||
// Update AttitudeRaw object (filtered gyros only for now)
|
||||
//AttitudeRaw::DataFields rawData;
|
||||
//AttitudeRaw::DataFields rawData;
|
||||
Gyros::DataFields gyroData;
|
||||
Accels::DataFields accelData;
|
||||
memset(&gyroData, 0, sizeof(Gyros::DataFields));
|
||||
memset(&accelData, 0, sizeof(Accels::DataFields));
|
||||
gyroData = gyros->getData();
|
||||
accelData = accels->getData();
|
||||
//rawData.gyros[0] = rollRate;
|
||||
//rawData.gyros[1] = cos(DEG2RAD * roll) * pitchRate + sin(DEG2RAD * roll) * yawRate;
|
||||
//rawData.gyros[2] = cos(DEG2RAD * roll) * yawRate - sin(DEG2RAD * roll) * pitchRate;
|
||||
//rawData.gyros[1] = pitchRate;
|
||||
//rawData.gyros[2] = yawRate;
|
||||
gyroData.x = rollRate;
|
||||
gyroData.y = pitchRate;
|
||||
gyroData.z = yawRate;
|
||||
// TODO: Accels are still missing!!!!
|
||||
gyros->setData(gyroData);
|
||||
// attRaw->updated();
|
||||
// Update GPS Position objects
|
||||
out.latitude = latitude * 1e7;
|
||||
out.longitude = longitude * 1e7;
|
||||
out.altitude = altitude;
|
||||
out.groundspeed = groundspeed;
|
||||
|
||||
out.calibratedAirspeed = airspeed;
|
||||
|
||||
|
||||
// Update BaroAltitude object
|
||||
out.temperature = temperature;
|
||||
out.pressure = pressure;
|
||||
|
||||
// Update attActual object
|
||||
out.roll = roll; //roll;
|
||||
out.pitch = pitch; // pitch
|
||||
out.heading = yaw; // yaw
|
||||
|
||||
out.dstN= NED[0];
|
||||
out.dstE= NED[1];
|
||||
out.dstD= NED[2];
|
||||
|
||||
// Update VelocityActual.{North,East,Down}
|
||||
out.velNorth = velocityActualNorth;
|
||||
out.velEast = velocityActualEast;
|
||||
out.velDown = velocityActualDown;
|
||||
|
||||
//Update gyroscope sensor data
|
||||
out.rollRate = rollRate;
|
||||
out.pitchRate = pitchRate;
|
||||
out.yawRate = yawRate;
|
||||
|
||||
//Update accelerometer sensor data
|
||||
out.accX = xAccel;
|
||||
out.accY = yAccel;
|
||||
out.accZ = -zAccel;
|
||||
|
||||
updateUAVOs(out);
|
||||
}
|
||||
|
||||
|
||||
@ -237,127 +237,84 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
|
||||
current.dAzimuth = (angleDifference(current.azimuth,old.azimuth)/current.dT + TURN_FILTER * (old.dAzimuth)) / (TURN_FILTER+1);
|
||||
current.dPitch = (angleDifference(current.pitch,old.pitch)/current.dT + TURN_FILTER * (old.dPitch)) / (TURN_FILTER+1);
|
||||
current.dRoll = (angleDifference(current.roll,old.roll)/current.dT + TURN_FILTER * (old.dRoll)) / (TURN_FILTER+1);
|
||||
// Update AltitudeActual object
|
||||
BaroAltitude::DataFields baroAltData;
|
||||
memset(&baroAltData, 0, sizeof(BaroAltitude::DataFields));
|
||||
baroAltData.Altitude = current.Z;
|
||||
baroAltData.Temperature = TEMP_GROUND + (current.Z * TEMP_LAPSE_RATE) - 273.0;
|
||||
baroAltData.Pressure = PRESSURE(current.Z)/1000.0; // kpa
|
||||
|
||||
// Update attActual object
|
||||
AttitudeActual::DataFields attActualData;
|
||||
memset(&attActualData, 0, sizeof(AttitudeActual::DataFields));
|
||||
attActualData.Roll = current.roll;
|
||||
attActualData.Pitch = current.pitch;
|
||||
attActualData.Yaw = current.azimuth;
|
||||
///////
|
||||
// Output formatting
|
||||
///////
|
||||
Output2OP out;
|
||||
memset(&out, 0, sizeof(Output2OP));
|
||||
|
||||
// Compute rotation matrix, for later calculations
|
||||
float Rbe[3][3];
|
||||
float rpy[3];
|
||||
float quat[4];
|
||||
rpy[0]=current.roll;
|
||||
rpy[1]=current.pitch;
|
||||
rpy[2]=current.azimuth;
|
||||
Utils::CoordinateConversions().RPY2Quaternion(rpy,quat);
|
||||
attActualData.q1 = quat[0];
|
||||
attActualData.q2 = quat[1];
|
||||
attActualData.q3 = quat[2];
|
||||
attActualData.q4 = quat[3];
|
||||
|
||||
// Update positionActual objects
|
||||
PositionActual::DataFields posData;
|
||||
memset(&posData, 0, sizeof(PositionActual::DataFields));
|
||||
posData.North = current.Y;
|
||||
posData.East = current.X;
|
||||
posData.Down = current.Z*-1.;
|
||||
|
||||
// Update velocityActual objects
|
||||
VelocityActual::DataFields velData;
|
||||
memset(&velData, 0, sizeof(VelocityActual::DataFields));
|
||||
velData.North = current.dY;
|
||||
velData.East = current.dX;
|
||||
velData.Down = current.dZ*-1.;
|
||||
|
||||
// Update AttitudeRaw object (filtered gyros and accels only for now)
|
||||
//AttitudeRaw::DataFields rawData;
|
||||
//memset(&rawData, 0, sizeof(AttitudeRaw::DataFields));
|
||||
//rawData = attRaw->getData();
|
||||
Gyros::DataFields gyroData;
|
||||
Accels::DataFields accelData;
|
||||
memset(&gyroData, 0, sizeof(Gyros::DataFields));
|
||||
memset(&accelData, 0, sizeof(Accels::DataFields));
|
||||
gyroData = gyros->getData();
|
||||
accelData = accels->getData();
|
||||
|
||||
// rotate turn rates and accelerations into body frame
|
||||
// (note: rotation deltas are NOT in NED frame but in RPY - manual conversion!)
|
||||
gyroData.x = current.dRoll;
|
||||
gyroData.y = cos(DEG2RAD * current.roll) * current.dPitch + sin(DEG2RAD * current.roll) * current.dAzimuth;
|
||||
gyroData.z = cos(DEG2RAD * current.roll) * current.dAzimuth - sin(DEG2RAD * current.roll) * current.dPitch;
|
||||
// accels are in NED and can be converted using standard ned->local rotation matrix
|
||||
float Rbe[3][3];
|
||||
Utils::CoordinateConversions().Quaternion2R(quat,Rbe);
|
||||
accelData.x = current.ddX*Rbe[0][0]
|
||||
+current.ddY*Rbe[0][1]
|
||||
+(current.ddZ+GEE)*Rbe[0][2];
|
||||
accelData.y = current.ddX*Rbe[1][0]
|
||||
+current.ddY*Rbe[1][1]
|
||||
+(current.ddZ+GEE)*Rbe[1][2];
|
||||
accelData.z = - (current.ddX*Rbe[2][0]
|
||||
+current.ddY*Rbe[2][1]
|
||||
+(current.ddZ+GEE)*Rbe[2][2]);
|
||||
|
||||
// Update homelocation
|
||||
HomeLocation::DataFields homeData;
|
||||
memset(&homeData, 0, sizeof(HomeLocation::DataFields));
|
||||
homeData = posHome->getData();
|
||||
homeData.Latitude = settings.latitude.toFloat() * 10e6;
|
||||
homeData.Longitude = settings.longitude.toFloat() * 10e6;
|
||||
homeData.Altitude = 0;
|
||||
//Calculate ECEF
|
||||
double RNE[9];
|
||||
double ECEF[3];
|
||||
double LLA[3];
|
||||
LLA[0]=settings.latitude.toFloat();
|
||||
LLA[1]=settings.longitude.toFloat();
|
||||
LLA[2]=0;
|
||||
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;
|
||||
homeData.Set=1;
|
||||
|
||||
// Update gps objects
|
||||
GPSPosition::DataFields gpsData;
|
||||
memset(&gpsData, 0, sizeof(GPSPosition::DataFields));
|
||||
gpsData.Altitude = current.Z;
|
||||
gpsData.Heading = current.azimuth;
|
||||
gpsData.Groundspeed = current.groundspeed;
|
||||
// Update GPS Position objects
|
||||
double NED[3];
|
||||
NED[0] = current.Y;
|
||||
NED[1] = current.X;
|
||||
NED[2] = -current.Z;
|
||||
Utils::CoordinateConversions().NED2LLA_HomeECEF(ECEF,NED,LLA);
|
||||
gpsData.Latitude = LLA[0] * 10e6;
|
||||
gpsData.Longitude = LLA[1] * 10e6;
|
||||
gpsData.Satellites = 7;
|
||||
gpsData.Status = GPSPosition::STATUS_FIX3D;
|
||||
out.latitude = settings.latitude.toFloat() * 1e7;
|
||||
out.longitude = settings.longitude.toFloat() * 1e7;
|
||||
out.groundspeed = current.groundspeed;
|
||||
|
||||
// issue manual update
|
||||
// update every time (50ms)
|
||||
attActual->setData(attActualData);
|
||||
//attActual->updated();
|
||||
//attRaw->setData(rawData);
|
||||
gyros->setData(gyroData);
|
||||
accels->setData(accelData);
|
||||
//attRaw->updated();
|
||||
velActual->setData(velData);
|
||||
//velActual->updated();
|
||||
posActual->setData(posData);
|
||||
//posActual->updated();
|
||||
baroAlt->setData(baroAltData);
|
||||
//baroAlt->updated();
|
||||
gpsPos->setData(gpsData);
|
||||
//gpsPos->updated();
|
||||
posHome->setData(homeData);
|
||||
//posHome->updated();
|
||||
out.calibratedAirspeed = current.ias;
|
||||
|
||||
out.dstN=current.Y;
|
||||
out.dstE=current.X;
|
||||
out.dstD=-current.Z;
|
||||
|
||||
// Update BaroAltitude object
|
||||
out.altitude = current.Z;
|
||||
out.temperature = TEMP_GROUND + (current.Z * TEMP_LAPSE_RATE) - 273.0;
|
||||
out.pressure = PRESSURE(current.Z)/1000.0; // kpa
|
||||
|
||||
|
||||
// Update attActual object
|
||||
out.roll = current.roll; //roll;
|
||||
out.pitch = current.pitch; // pitch
|
||||
out.heading = current.azimuth; // yaw
|
||||
|
||||
|
||||
// Update VelocityActual.{North,East,Down}
|
||||
out.velNorth = current.dY;
|
||||
out.velEast = current.dX;
|
||||
out.velDown = -current.dZ;
|
||||
|
||||
// rotate turn rates and accelerations into body frame
|
||||
// (note: rotation deltas are NOT in NED frame but in RPY - manual conversion!)
|
||||
out.rollRate = current.dRoll;
|
||||
out.pitchRate = cos(DEG2RAD * current.roll) * current.dPitch + sin(DEG2RAD * current.roll) * current.dAzimuth;
|
||||
out.yawRate = cos(DEG2RAD * current.roll) * current.dAzimuth - sin(DEG2RAD * current.roll) * current.dPitch;
|
||||
|
||||
//Update accelerometer sensor data
|
||||
out.accX = current.ddX*Rbe[0][0]
|
||||
+current.ddY*Rbe[0][1]
|
||||
+(current.ddZ+GEE)*Rbe[0][2];
|
||||
out.accY = current.ddX*Rbe[1][0]
|
||||
+current.ddY*Rbe[1][1]
|
||||
+(current.ddZ+GEE)*Rbe[1][2];
|
||||
out.accZ = - (current.ddX*Rbe[2][0]
|
||||
+current.ddY*Rbe[2][1]
|
||||
+(current.ddZ+GEE)*Rbe[2][2]);
|
||||
|
||||
updateUAVOs(out);
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@ -368,6 +368,11 @@ void Simulator::telStatsUpdated(UAVObject* obj)
|
||||
}
|
||||
|
||||
|
||||
void Simulator::resetInitialHomePosition(){
|
||||
once=false;
|
||||
}
|
||||
|
||||
|
||||
void Simulator::updateUAVOs(Output2OP out){
|
||||
|
||||
QTime currentTime = QTime::currentTime();
|
||||
|
||||
@ -68,7 +68,7 @@ void TraceBuf(const char* buf,int len);
|
||||
XplaneSimulator::XplaneSimulator(const SimulatorSettings& params) :
|
||||
Simulator(params)
|
||||
{
|
||||
once = false;
|
||||
resetInitialHomePosition();
|
||||
}
|
||||
|
||||
|
||||
@ -82,7 +82,7 @@ void XplaneSimulator::setupUdpPorts(const QString& host, int inPort, int outPort
|
||||
|
||||
inSocket->bind(QHostAddress(host), inPort);
|
||||
//outSocket->bind(QHostAddress(host), outPort);
|
||||
once = false;
|
||||
resetInitialHomePosition();
|
||||
|
||||
}
|
||||
|
||||
@ -104,7 +104,7 @@ void XplaneSimulator::transmitUpdate()
|
||||
float ailerons = actData.Roll;
|
||||
float elevator = actData.Pitch;
|
||||
float rudder = actData.Yaw;
|
||||
float throttle = actData.Throttle*2-1.0;
|
||||
float throttle = actData.Throttle > 0? actData.Throttle : 0;
|
||||
float none = -999;
|
||||
//quint32 none = *((quint32*)&tmp); // get float as 4 bytes
|
||||
|
||||
@ -196,8 +196,8 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
|
||||
float altitude = 0;
|
||||
float latitude = 0;
|
||||
float longitude = 0;
|
||||
float airspeed = 0;
|
||||
float speed = 0;
|
||||
float airspeed_keas = 0;
|
||||
float groundspeed_ktgs = 0;
|
||||
float pitch = 0;
|
||||
float roll = 0;
|
||||
float heading = 0;
|
||||
@ -216,7 +216,7 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
|
||||
float pitchRate_rad=0;
|
||||
float yawRate_rad=0;
|
||||
|
||||
QString str;
|
||||
// QString str;
|
||||
QByteArray& buf = const_cast<QByteArray&>(dataBuf);
|
||||
QString data(buf);
|
||||
|
||||
@ -234,15 +234,15 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
|
||||
{
|
||||
switch(buf[0]) // switch by id
|
||||
{
|
||||
case XplaneSimulator::LatitudeLongitude:
|
||||
case XplaneSimulator::LatitudeLongitudeAltitude:
|
||||
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));
|
||||
airspeed_keas = *((float*)(buf.data()+4*2));
|
||||
groundspeed_ktgs = *((float*)(buf.data()+4*4));
|
||||
break;
|
||||
|
||||
case XplaneSimulator::PitchRollHeading:
|
||||
@ -291,125 +291,56 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
|
||||
} 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);
|
||||
Utils::CoordinateConversions().LLA2ECEF(LLA,ECEF);
|
||||
homeData.Be[0]=0;
|
||||
homeData.Be[1]=0;
|
||||
homeData.Be[2]=0;
|
||||
posHome->setData(homeData);
|
||||
posHome->updated();
|
||||
///////
|
||||
// Output formatting
|
||||
///////
|
||||
Output2OP out;
|
||||
memset(&out, 0, sizeof(Output2OP));
|
||||
|
||||
// Initialize the initial distance
|
||||
initX = dstX;
|
||||
initY = dstY;
|
||||
initZ = dstZ;
|
||||
once=1;
|
||||
}
|
||||
// Update GPS Position objects
|
||||
out.latitude = latitude * 1e7;
|
||||
out.longitude = longitude * 1e7;
|
||||
out.altitude = altitude;
|
||||
out.groundspeed = groundspeed_ktgs*1.15*1.6089/3.6; //Convert from [kts] to [m/s]
|
||||
|
||||
out.calibratedAirspeed = airspeed_keas*1.15*1.6089/3.6; //Convert from [kts] to [m/s]
|
||||
|
||||
// Update AltitudeActual object
|
||||
BaroAltitude::DataFields baroAltData;
|
||||
memset(&baroAltData, 0, sizeof(BaroAltitude::DataFields));
|
||||
baroAltData.Altitude = altitude;
|
||||
baroAltData.Temperature = temperature;
|
||||
baroAltData.Pressure = pressure;
|
||||
baroAlt->setData(baroAltData);
|
||||
// Update BaroAltitude object
|
||||
out.temperature = temperature;
|
||||
out.pressure = pressure;
|
||||
|
||||
// Update attActual object
|
||||
AttitudeActual::DataFields attActualData;
|
||||
memset(&attActualData, 0, sizeof(AttitudeActual::DataFields));
|
||||
attActualData.Roll = roll; //roll;
|
||||
attActualData.Pitch = pitch; // pitch
|
||||
attActualData.Yaw = heading; // Yaw
|
||||
float rpy[3];
|
||||
float quat[4];
|
||||
rpy[0] = roll;
|
||||
rpy[1] = pitch;
|
||||
rpy[2] = heading;
|
||||
Utils::CoordinateConversions().RPY2Quaternion(rpy,quat);
|
||||
attActualData.q1 = quat[0];
|
||||
attActualData.q2 = quat[1];
|
||||
attActualData.q3 = quat[2];
|
||||
attActualData.q4 = quat[3];
|
||||
attActual->setData(attActualData);
|
||||
out.roll = roll; //roll;
|
||||
out.pitch = pitch; // pitch
|
||||
out.heading = heading; // yaw
|
||||
|
||||
// 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 = velY;
|
||||
velocityActualData.East = velX;
|
||||
velocityActualData.Down = -velZ;
|
||||
velActual->setData(velocityActualData);
|
||||
out.dstN=dstY;
|
||||
out.dstE=dstX;
|
||||
out.dstD=-dstZ;
|
||||
|
||||
// Update PositionActual.{Nort,East,Down}
|
||||
PositionActual::DataFields positionActualData;
|
||||
memset(&positionActualData, 0, sizeof(PositionActual::DataFields));
|
||||
positionActualData.North = (dstY-initY);
|
||||
positionActualData.East = (dstX-initX);
|
||||
positionActualData.Down = -(dstZ-initZ);
|
||||
posActual->setData(positionActualData);
|
||||
// Update VelocityActual.{North,East,Down}
|
||||
out.velNorth = velY;
|
||||
out.velEast = velX;
|
||||
out.velDown = -velZ;
|
||||
|
||||
// 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_rad + sin(DEG2RAD * roll) * yawRate_rad;
|
||||
//rawData.gyros_filtered[2] = cos(DEG2RAD * roll) * yawRate_rad - sin(DEG2RAD * roll) * pitchRate_rad;
|
||||
//rawData.gyros[1] = pitchRate;
|
||||
//rawData.gyros[2] = yawRate;
|
||||
//rawData.accels[0] = accX;
|
||||
//rawData.accels[1] = accY;
|
||||
//rawData.accels[2] = -accZ;
|
||||
//attRaw->setData(rawData);
|
||||
Gyros::DataFields gyroData;
|
||||
memset(&gyroData, 0, sizeof(Gyros::DataFields));
|
||||
#define Pi 3.141529654
|
||||
gyroData.x = rollRate_rad*180/Pi;
|
||||
gyroData.y = pitchRate_rad*180/Pi;
|
||||
gyroData.z = yawRate_rad*180/Pi;
|
||||
gyros->setData(gyroData);
|
||||
//Update gyroscope sensor data
|
||||
out.rollRate = rollRate_rad;
|
||||
out.pitchRate = pitchRate_rad;
|
||||
out.yawRate = yawRate_rad;
|
||||
|
||||
Accels::DataFields accelData;
|
||||
memset(&accelData, 0, sizeof(Accels::DataFields));
|
||||
accelData.x = accX;
|
||||
accelData.y = accY;
|
||||
accelData.z = -accZ;
|
||||
accels->setData(accelData);
|
||||
//Update accelerometer sensor data
|
||||
out.accX = accX;
|
||||
out.accY = accY;
|
||||
out.accZ = -accZ;
|
||||
|
||||
updateUAVOs(out);
|
||||
}
|
||||
// issue manual update
|
||||
//attActual->updated();
|
||||
//baroAlt->updated();
|
||||
//altActual->updated();
|
||||
//posActual->updated();
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -45,12 +45,8 @@ private slots:
|
||||
void transmitUpdate();
|
||||
|
||||
private:
|
||||
bool once;
|
||||
float initX;
|
||||
float initY;
|
||||
float initZ;
|
||||
enum XplaneOutputData
|
||||
{
|
||||
enum XplaneOutputData //***WARNING***: Elements in this enum are in a precise order, do
|
||||
{ // not change. Cf. http://www.nuclearprojects.com/xplane/info.shtml
|
||||
FramRate,
|
||||
Times,
|
||||
SimStats,
|
||||
@ -71,7 +67,7 @@ private:
|
||||
AngularVelocities,
|
||||
PitchRollHeading,
|
||||
AoA,
|
||||
LatitudeLongitude,
|
||||
LatitudeLongitudeAltitude,
|
||||
LocVelDistTraveled
|
||||
};
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user