From ef83d7a8ec34d4c95986b78a31f5fcad9fa1de1e Mon Sep 17 00:00:00 2001 From: sambas Date: Sun, 17 Oct 2010 14:05:01 +0000 Subject: [PATCH] FlightGear homelocation set git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1971 ebee16cc-31ac-478f-84a7-5cbb03baadba --- ground/src/plugins/hitlnew/fgsimulator.cpp | 50 +++++++++++++++++++--- 1 file changed, 44 insertions(+), 6 deletions(-) diff --git a/ground/src/plugins/hitlnew/fgsimulator.cpp b/ground/src/plugins/hitlnew/fgsimulator.cpp index 249a1cd0b..c861af394 100644 --- a/ground/src/plugins/hitlnew/fgsimulator.cpp +++ b/ground/src/plugins/hitlnew/fgsimulator.cpp @@ -159,6 +159,7 @@ void FGSimulator::transmitUpdate() void FGSimulator::processUpdate(const QByteArray& inp) { + static char once=0; // Split QString data(inp); QStringList fields = data.split(","); @@ -205,11 +206,37 @@ void FGSimulator::processUpdate(const QByteArray& inp) // Get pressure (kpa) float pressure = fields[20].toFloat() * INHG2KPA; // Get VelocityActual Down (cm/s) - float velocityActualDown = fields[21].toFloat() * FPS2CMPS; + float velocityActualDown = fields[21].toFloat() * FPS2CMPS; // Get VelocityActual East (cm/s) float velocityActualEast = fields[22].toFloat() * FPS2CMPS; // Get VelocityActual Down (cm/s) float velocityActualNorth = fields[23].toFloat() * FPS2CMPS; + + //run once + HomeLocation::DataFields homeData; + 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]; + 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); + once=1; + } // Update VelocityActual.{Nort,East,Down} VelocityActual::DataFields velocityActualData; @@ -222,10 +249,10 @@ void FGSimulator::processUpdate(const QByteArray& inp) // 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.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 * 100); //Multiply by 100 because positionActual expects input in Centimeters. - posActual->setData(positionActualData); + posActual->setData(positionActualData); // Update AltitudeActual object BaroAltitude::DataFields altActualData; @@ -251,12 +278,23 @@ void FGSimulator::processUpdate(const QByteArray& inp) GPSPosition::DataFields gpsData; memset(&gpsData, 0, sizeof(GPSPosition::DataFields)); gpsData.Altitude = altitude; - gpsData.Heading = heading; + gpsData.Heading = heading; gpsData.Groundspeed = groundspeed; - gpsData.Latitude = latitude; - gpsData.Longitude = longitude; + gpsData.Latitude = latitude*1e7; + gpsData.Longitude = longitude*1e7; gpsData.Satellites = 10; gpsData.Status = GPSPosition::STATUS_FIX3D; gpsPos->setData(gpsData); + + /*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);*/ }