rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00
Code Issues Releases Activity

OP-169 - HITL - hitl core is set up to update PositionActual, VelocityActual and HomeLocation - il2simulator.cpp is updated to set those correctly

Browse Source 
(status: compilation OK, function yet untested)
xplane and flightgear yet need to be fixed
Note: I needed to change /libs/utils/coordinateconversions.h to have access to previously protected utility functions used to convert LLA to ECEF/RNE for HomeLocation
Code for this is "inspired by" flight/OpenPilot/Modules/GPS


git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1964 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
corvus 2010-10-16 14:30:36 +00:00 committed by corvus
parent 593c947f5e
commit 82c533e914
4 changed files with 51 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ground/src/libs/utils/coordinateconversions.h
View File

@ -42,8 +42,6 @@ class QTCREATOR_UTILS_EXPORT CoordinateConversions
public:
CoordinateConversions();
int GetLLA(double LLA[3], double NED[3], double position[3]);
protected:
void RneFromLLA(double LLA[3], double Rne[3][3]);
void LLA2ECEF(double LLA[3], double ECEF[3]);
int ECEF2LLA(double ECEF[3], double LLA[3]);

53
ground/src/plugins/hitlnew/il2simulator.cpp
View File

@ -229,21 +229,53 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
attActualData.q4 = 0;
attActual->setData(attActualData);
// Update positionActual objects
PositionActual::DataFields posData;
posData.North = current.Y*100;
posData.East = current.X*100;
posData.Down = current.Z*-100;
posActual->setData(posData);
// Update velocityActual objects
VelocityActual::DataFields velData;
velData.North = current.dY*100;
velData.East = current.dX*100;
velData.Down = current.dZ*100;
velActual->setData(velData);
// Update homelocation
HomeLocation::DataFields homeData;
homeData.Latitude = settings.latitude.toFloat() * 10e6;
homeData.Longitude = settings.longitude.toFloat() * 10e6;
homeData.Altitude = 0;
double LLA[3];
LLA[0]=settings.latitude.toFloat();
LLA[1]=settings.longitude.toFloat();
LLA[2]=0;
double ECEF[3];
Utils::CoordinateConversions().RneFromLLA(LLA,(double(*)[3])homeData.RNE);
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);
// Update gps objects
GPSPosition::DataFields gpsData;
memset(&gpsData, 0, sizeof(GPSPosition::DataFields));
gpsData.Altitude = current.Z;
gpsData.Heading = current.azimuth;
gpsData.Groundspeed = current.groundspeed;
gpsData.Latitude = settings.latitude.toFloat() + current.Y * DEG2M;
if (gpsData.Latitude<-89.0 or gpsData.Latitude>89.0) {
// oops - this is rare enough to just prevent overflow here...
// IL2 has no north pole map anyway
gpsData.Latitude=0.0;
}
gpsData.Longitude = settings.longitude.toFloat() + current.X * (1.0/cos(gpsData.Latitude*DEG2RAD)) * DEG2M;
while (gpsData.Longitude<-180.0) gpsData.Longitude+=360.0;
while (gpsData.Longitude>180.0) gpsData.Longitude-=360.0;
double NED[3];
NED[0] = current.Y;
NED[1] = current.X;
NED[2] = -current.Z;
Utils::CoordinateConversions().GetLLA(ECEF,NED,LLA);
gpsData.Latitude = LLA[0];
gpsData.Longitude = LLA[1];
gpsData.Satellites = 7;
gpsData.Status = GPSPosition::STATUS_FIX3D;
gpsPos->setData(gpsData);
@ -251,6 +283,9 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
// issue manual update
attActual->updated();
altActual->updated();
posActual->updated();
velActual->updated();
posHome->updated();
gpsPos->updated();
}

4
ground/src/plugins/hitlnew/simulator.cpp
View File

@ -116,6 +116,7 @@ void Simulator::onStart()
ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager* objManager = pm->getObject<UAVObjectManager>();
actDesired = ActuatorDesired::GetInstance(objManager);
posHome = HomeLocation::GetInstance(objManager);
velActual = VelocityActual::GetInstance(objManager);
posActual = PositionActual::GetInstance(objManager);
altActual = BaroAltitude::GetInstance(objManager);
@ -205,6 +206,9 @@ void Simulator::setupObjects()
setupOutputObject(altActual, 250);
setupOutputObject(attActual, 75);
setupOutputObject(gpsPos, 250);
setupOutputObject(posActual, 250);
setupOutputObject(velActual, 250);
setupOutputObject(posHome, 1000);
}
void Simulator::setupInputObject(UAVObject* obj, int updatePeriod)

3
ground/src/plugins/hitlnew/simulator.h
View File

@ -42,8 +42,10 @@
#include "uavobjects/baroaltitude.h"
#include "uavobjects/attitudeactual.h"
#include "uavobjects/gpsposition.h"
#include "uavobjects/homelocation.h"
#include "uavobjects/gcstelemetrystats.h"
#include "utils/coordinateconversions.h"
/**
* just imagine this was a class without methods and all public properties
@ -149,6 +151,7 @@ protected:
AttitudeActual* attActual;
VelocityActual* velActual;
PositionActual* posActual;
HomeLocation* posHome;
GPSPosition* gpsPos;
GCSTelemetryStats* telStats;