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

OP-1353 Fix Position and Velocity in Flightgear HITL bridge

Browse Source
This commit is contained in:
Corvus Corax 2014-05-19 00:13:45 +02:00
parent 124a31f0b5
commit 718a35c376
3 changed files with 17 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp
View File

@ -44,7 +44,7 @@ CoordinateConversions::CoordinateConversions()
* @param[in] LLA Longitude latitude altitude for this location * @param[in] LLA Longitude latitude altitude for this location
* @param[out] Rne[3][3] Rotation matrix * @param[out] Rne[3][3] Rotation matrix
*/ */
void CoordinateConversions::RneFromLLA(double LLA[3], double Rne[3][3]) void CoordinateConversions::RneFromLLA(double LLA[3], float Rne[3][3])
{ {
float sinLat, sinLon, cosLat, cosLon; float sinLat, sinLon, cosLat, cosLon;
@ -134,7 +134,7 @@ int CoordinateConversions::NED2LLA_HomeECEF(double BaseECEFm[3], double NED[3],
// stored value is in cm, convert to m // stored value is in cm, convert to m
double BaseLLA[3]; double BaseLLA[3];
double ECEF[3]; double ECEF[3];
double Rne[3][3]; float Rne[3][3];
// Get LLA address to compute conversion matrix // Get LLA address to compute conversion matrix
ECEF2LLA(BaseECEFm, BaseLLA); ECEF2LLA(BaseECEFm, BaseLLA);

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

@ -41,7 +41,7 @@ public:
CoordinateConversions(); CoordinateConversions();
int NED2LLA_HomeECEF(double BaseECEFcm[3], double NED[3], double position[3]); int NED2LLA_HomeECEF(double BaseECEFcm[3], double NED[3], double position[3]);
int NED2LLA_HomeLLA(double LLA[3], double NED[3], double position[3]); int NED2LLA_HomeLLA(double LLA[3], double NED[3], double position[3]);
void RneFromLLA(double LLA[3], double Rne[3][3]); void RneFromLLA(double LLA[3], float Rne[3][3]);
void LLA2ECEF(double LLA[3], double ECEF[3]); void LLA2ECEF(double LLA[3], double ECEF[3]);
int ECEF2LLA(double ECEF[3], double LLA[3]); int ECEF2LLA(double ECEF[3], double LLA[3]);
void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3]); void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3]);

29
ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp
View File

@ -268,12 +268,12 @@ void FGSimulator::processUpdate(const QByteArray & inp)
float temperature = fields[19].toFloat(); float temperature = fields[19].toFloat();
// Get pressure (kpa) // Get pressure (kpa)
float pressure = fields[20].toFloat() * INHG2KPA; float pressure = fields[20].toFloat() * INHG2KPA;
// Get VelocityState Down (cm/s) // Get VelocityState Down (m/s)
float velocityStateDown = -fields[21].toFloat() * FPS2CMPS; float velocityStateDown = -fields[21].toFloat() * FPS2CMPS * 1e-2f;
// Get VelocityState East (cm/s) // Get VelocityState East (m/s)
float velocityStateEast = fields[22].toFloat() * FPS2CMPS; float velocityStateEast = fields[22].toFloat() * FPS2CMPS * 1e-2f;
// Get VelocityState Down (cm/s) // Get VelocityState Down (m/s)
float velocityStateNorth = fields[23].toFloat() * FPS2CMPS; float velocityStateNorth = fields[23].toFloat() * FPS2CMPS * 1e-2f;
// Get UDP packets received by FG // Get UDP packets received by FG
int n = fields[24].toInt(); int n = fields[24].toInt();
@ -286,16 +286,15 @@ void FGSimulator::processUpdate(const QByteArray & inp)
Output2Hardware out; Output2Hardware out;
memset(&out, 0, sizeof(Output2Hardware)); memset(&out, 0, sizeof(Output2Hardware));
float NED[3]; HomeLocation::DataFields homeData = posHome->getData();
// convert from cm back to meters double HomeLLA[3] = { (double)homeData.Latitude * 1e-7, (double)homeData.Longitude * 1e-7, homeData.Altitude };
double HomeECEF[3];
float HomeRNE[3][3];
double LLA[3] = { latitude, longitude, altitude_msl }; double LLA[3] = { latitude, longitude, altitude_msl };
double ECEF[3]; float NED[3];
double RNE[9]; Utils::CoordinateConversions().RneFromLLA(HomeLLA, HomeRNE);
Utils::CoordinateConversions().RneFromLLA(LLA, (double(*)[3])RNE); Utils::CoordinateConversions().LLA2ECEF(HomeLLA, HomeECEF);
Utils::CoordinateConversions().LLA2ECEF(LLA, ECEF); Utils::CoordinateConversions().LLA2Base(LLA, HomeECEF, HomeRNE, NED);
Utils::CoordinateConversions().LLA2Base(LLA, ECEF, (float(*)[3])RNE, NED);
// Update GPS Position objects // Update GPS Position objects
out.latitude = latitude * 1e7; out.latitude = latitude * 1e7;