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

OP-169 hitl/FlightGear fixes

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1980 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
sambas 2010-10-17 17:37:51 +00:00 committed by sambas
parent dc943baeaa
commit 8c46d22aa7
3 changed files with 28 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
ground/src/libs/utils/coordinateconversions.cpp
View File

@ -151,4 +151,20 @@ int CoordinateConversions::GetLLA(double BaseECEFcm[3], double NED[3], double po
return 0;
}
void CoordinateConversions::LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3])
{
double ECEF[3];
float diff[3];
LLA2ECEF(LLA, ECEF);
diff[0] = (float)(ECEF[0] - BaseECEF[0]);
diff[1] = (float)(ECEF[1] - BaseECEF[1]);
diff[2] = (float)(ECEF[2] - BaseECEF[2]);
NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2];
NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2];
NED[2] = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2];
}
}

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

@ -45,6 +45,7 @@ public:
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]);
void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3]);
};
}

18
ground/src/plugins/hitlnew/fgsimulator.cpp
View File

@ -213,7 +213,7 @@ void FGSimulator::processUpdate(const QByteArray& inp)
float velocityActualNorth = fields[23].toFloat() * FPS2CMPS;
//run once
HomeLocation::DataFields homeData;
HomeLocation::DataFields homeData = posHome->getData();
if(!once)
{
memset(&homeData, 0, sizeof(HomeLocation::DataFields));
@ -226,7 +226,11 @@ void FGSimulator::processUpdate(const QByteArray& inp)
LLA[1]=longitude;
LLA[2]=0;
double ECEF[3];
Utils::CoordinateConversions().RneFromLLA(LLA,(double(*)[3])homeData.RNE);
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;
@ -286,15 +290,15 @@ void FGSimulator::processUpdate(const QByteArray& inp)
gpsData.Status = GPSPosition::STATUS_FIX3D;
gpsPos->setData(gpsData);
/*float NED[3];
float NED[3];
double LLA[3] = {(double) gpsData.Latitude / 1e7, (double) gpsData.Longitude / 1e7, (double) (gpsData.GeoidSeparation + gpsData.Altitude)};
// convert from cm back to meters
double ECEF[3] = {(double) (homeData.ECEF[0] / 100), (double) (homeData.ECEF[1] / 100), (double) (homeData.ECEF[2] / 100)};
Utils::CoordinateConversions().LLA2Base(LLA, ECEF, (float (*)[3]) homeData.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 100 because positionActual expects input in Centimeters.
posActual->setData(positionActualData);*/
positionActualData.North = NED[0]*100; //Currently hardcoded as there is no way of setting up a reference point to calculate distance
positionActualData.East = NED[1]*100; //Currently hardcoded as there is no way of setting up a reference point to calculate distance
positionActualData.Down = NED[2]*100; //Multiply by 100 because positionActual expects input in Centimeters.
posActual->setData(positionActualData);
}