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

Fixed getUAVPosition to use position actual data correctly.

Browse Source 
Refactored coordinate conversion names in order to be clearer, since
two identically named functions-- one in revo, one here-- performed
very different maths.
This commit is contained in:
Laura Sebesta 2012-08-08 18:20:15 +02:00 committed by Kenz Dale
parent f3e1e768ad
commit fe6ea7e0e2
4 changed files with 51 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -121,19 +121,18 @@ int CoordinateConversions::ECEF2LLA(double ECEF[3], double LLA[3])
}
/**
* Get the current location in Longitude, Latitude Altitude (above WSG-48 ellipsoid)
* @param[in] BaseECEF the ECEF of the home location (in cm)
* Get the current location in Longitude, Latitude Altitude (above WSG-84 ellipsoid)
* @param[in] BaseECEF the ECEF of the home location (in m)
* @param[in] NED the offset from the home location (in m)
* @param[out] position three element double for position in degrees and meters
* @param[out] position three element double for position in decimal degrees and altitude in meters
* @returns
* @arg 0 success
* @arg -1 for failure
*/
int CoordinateConversions::GetLLA(double BaseECEFcm[3], double NED[3], double position[3])
int CoordinateConversions::NED2LLA_HomeECEF(double BaseECEFm[3], double NED[3], double position[3])
{
int i;
// stored value is in cm, convert to m
double BaseECEFm[3] = {BaseECEFcm[0], BaseECEFcm[1], BaseECEFcm[2]};
double BaseLLA[3];
double ECEF[3];
double Rne [3][3];
@ -151,6 +150,29 @@ int CoordinateConversions::GetLLA(double BaseECEFcm[3], double NED[3], double po
return 0;
}
/**
* Get the current location in Longitude, Latitude, Altitude (above WSG-84 ellipsoid)
* @param[in] homeLLA the latitude, longitude, and altitude of the home location (in [m])
* @param[in] NED the offset from the home location (in [m])
* @param[out] position three element double for position in decimal degrees and altitude in meters
* @returns
* @arg 0 success
* @arg -1 for failure
*/
int CoordinateConversions::NED2LLA_HomeLLA(double homeLLA[3], double NED[3], double position[3])
{
double T[3];
T[0] = homeLLA[2]+6.378137E6f * M_PI / 180.0;
T[1] = cosf(homeLLA[0] * M_PI / 180.0)*(homeLLA[2]+6.378137E6f) * M_PI / 180.0;
T[2] = -1.0f;
position[0] = homeLLA[0] + NED[0] / T[0];
position[1] = homeLLA[1] + NED[1] / T[1];
position[2] = homeLLA[2] + NED[2] / T[2];
return 0;
}
void CoordinateConversions::LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3])
{
double ECEF[3];

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

@ -41,7 +41,8 @@ class QTCREATOR_UTILS_EXPORT CoordinateConversions
{
public:
CoordinateConversions();
int GetLLA(double LLA[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]);
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]);

2
ground/openpilotgcs/src/plugins/hitlnew/il2simulator.cpp
View File

@ -333,7 +333,7 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
NED[0] = current.Y;
NED[1] = current.X;
NED[2] = -current.Z;
Utils::CoordinateConversions().GetLLA(ECEF,NED,LLA);
Utils::CoordinateConversions().NED2LLA_HomeECEF(ECEF,NED,LLA);
gpsData.Latitude = LLA[0] * 10e6;
gpsData.Longitude = LLA[1] * 10e6;
gpsData.Satellites = 7;

53
ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp
View File

@ -2017,54 +2017,43 @@ internals::PointLatLng OPMapGadgetWidget::destPoint(internals::PointLatLng sourc
bool OPMapGadgetWidget::getUAVPosition(double &latitude, double &longitude, double &altitude)
{
double BaseECEF[3];
double NED[3];
double LLA[3];
UAVObject *obj;
double homeLLA[3];
if (!obm)
return false;
Q_ASSERT(obm != NULL);
obj = dynamic_cast<UAVDataObject*>(obm->getObject(QString("HomeLocation")));
if (!obj) return false;
BaseECEF[0] = obj->getField(QString("ECEF"))->getDouble(0) / 100;
BaseECEF[1] = obj->getField(QString("ECEF"))->getDouble(1) / 100;
BaseECEF[2] = obj->getField(QString("ECEF"))->getDouble(2) / 100;
HomeLocation *homeLocation = HomeLocation::GetInstance(obm);
Q_ASSERT(homeLocation != NULL);
HomeLocation::DataFields homeLocationData = homeLocation->getData();
obj = dynamic_cast<UAVDataObject*>(obm->getObject(QString("PositionActual")));
if (!obj) return false;
NED[0] = obj->getField(QString("North"))->getDouble() / 100;
NED[1] = obj->getField(QString("East"))->getDouble() / 100;
NED[2] = obj->getField(QString("Down"))->getDouble() / 100;
homeLLA[0] = homeLocationData.Latitude / 1e7;
homeLLA[1] = homeLocationData.Longitude / 1e7;
homeLLA[2] = homeLocationData.Altitude;
// obj = dynamic_cast<UAVDataObject*>(om->getObject(QString("PositionDesired")));
PositionActual *positionActual = PositionActual::GetInstance(obm);
Q_ASSERT(positionActual != NULL);
PositionActual::DataFields positionActualData = positionActual->getData();
// obj = dynamic_cast<UAVDataObject*>(objManager->getObject("VelocityActual")); // air speed
NED[0] = positionActualData.North;
NED[1] = positionActualData.East;
NED[2] = positionActualData.Down;
Utils::CoordinateConversions().GetLLA(BaseECEF, NED, LLA);
Utils::CoordinateConversions().NED2LLA_HomeLLA(homeLLA, NED, LLA);
latitude = LLA[0];
longitude = LLA[1];
altitude = LLA[2];
qDebug()<< " " << latitude << " " << longitude << " " << altitude;
if (latitude != latitude) latitude = 0; // nan detection
// if (isNan(latitude)) latitude = 0; // nan detection
else
// if (!isFinite(latitude)) latitude = 0;
// else
if (latitude > 90) latitude = 90;
else
if (latitude < -90) latitude = -90;
else if (latitude > 90) latitude = 90;
else if (latitude < -90) latitude = -90;
if (longitude != longitude) longitude = 0; // nan detection
else
// if (longitude > std::numeric_limits<double>::max()) longitude = 0; // +infinite
// else
// if (longitude < -std::numeric_limits<double>::max()) longitude = 0; // -infinite
// else
if (longitude > 180) longitude = 180;
else
if (longitude < -180) longitude = -180;
else if (longitude > 180) longitude = 180;
else if (longitude < -180) longitude = -180;
if (altitude != altitude) altitude = 0; // nan detection