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

Fix map to work with the new HomeLocation (no ECEF)

Browse Source
This commit is contained in:
James Cotton 2012-03-03 12:37:01 -06:00
parent a83fb019f6
commit 56af74d17c
2 changed files with 39 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -129,24 +129,16 @@ int CoordinateConversions::ECEF2LLA(double ECEF[3], double LLA[3])
* @arg 0 success
* @arg -1 for failure
*/
int CoordinateConversions::GetLLA(double BaseECEFcm[3], double NED[3], double position[3])
int CoordinateConversions::GetLLA(double homeLLA[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];
float T[3];
T[0] = homeLLA[2]+6.378137E6f;
T[1] = cosf(homeLLA[0])*(homeLLA[2]+6.378137E6f);
T[2] = -1.0f;
// Get LLA address to compute conversion matrix
ECEF2LLA(BaseECEFm, BaseLLA);
RneFromLLA(BaseLLA, Rne);
/* P = ECEF + Rne' * NED */
for(i = 0; i < 3; i++)
ECEF[i] = BaseECEFm[i] + Rne[0][i]*NED[0] + Rne[1][i]*NED[1] + Rne[2][i]*NED[2];
ECEF2LLA(ECEF,position);
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;
}

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

@ -38,6 +38,9 @@
#include <QFile>
#include <QDateTime>
#include "homelocation.h"
#include "positionactual.h"
#include <math.h>
#include "utils/stylehelper.h"
@ -2317,54 +2320,53 @@ 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];
double homeLLA[3];
UAVObject *obj;
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 / 10e6;
homeLLA[1] = homeLocationData.Longitude / 10e6;
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 / 100;
NED[1] = positionActualData.East / 100;
NED[2] = positionActualData.Down / 100;
Utils::CoordinateConversions().GetLLA(BaseECEF, NED, LLA);
Utils::CoordinateConversions().GetLLA(homeLLA, NED, LLA);
latitude = LLA[0];
longitude = LLA[1];
altitude = LLA[2];
if (latitude != latitude) latitude = 0; // nan detection
// if (isNan(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;
// if (!isFinite(latitude)) latitude = 0;
// 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;
// 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;
if (altitude != altitude) altitude = 0; // nan detection