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

Update the map to set locations with the home location representation

Browse Source
This commit is contained in:
James Cotton 2012-03-15 02:07:00 -05:00
parent 62117430cf
commit cd5fad2ae9
5 changed files with 36 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -131,9 +131,9 @@ int CoordinateConversions::ECEF2LLA(double ECEF[3], double LLA[3])
*/
int CoordinateConversions::GetLLA(double homeLLA[3], double NED[3], double position[3])
{
float T[3];
T[0] = homeLLA[2]+6.378137E6f;
T[1] = cosf(homeLLA[0])*(homeLLA[2]+6.378137E6f);
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];

7
ground/openpilotgcs/src/libs/utils/homelocationutil.cpp
View File

@ -46,7 +46,7 @@ namespace Utils {
// input params: LLA
//
// output params: ECEF, RNE and Be
int HomeLocationUtil::getDetails(double LLA[3], double ECEF[3], double RNE[9], double Be[3])
int HomeLocationUtil::getDetails(double LLA[3], double Be[3])
{
// *************
// check input parms
@ -66,10 +66,7 @@ namespace Utils {
QDateTime dt = QDateTime::currentDateTime().toUTC();
CoordinateConversions().LLA2ECEF(LLA, ECEF);
CoordinateConversions().RneFromLLA(LLA, (double (*)[3])RNE);
if (WorldMagModel().GetMagVector(LLA, dt.date().month(), dt.date().day(), dt.date().year(), Be) < 0)
return -6;
Q_ASSERT(WorldMagModel().GetMagVector(LLA, dt.date().month(), dt.date().day(), dt.date().year(), Be) >= 0);
return 0; // OK
}

14
ground/openpilotgcs/src/libs/utils/homelocationutil.h
View File

@ -35,16 +35,16 @@
namespace Utils {
class QTCREATOR_UTILS_EXPORT HomeLocationUtil
{
public:
HomeLocationUtil();
class QTCREATOR_UTILS_EXPORT HomeLocationUtil
{
public:
HomeLocationUtil();
int getDetails(double LLA[3], double ECEF[3], double RNE[9], double Be[3]);
int getDetails(double LLA[3], double Be[3]);
private:
private:
};
};
}

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

@ -2339,9 +2339,9 @@ bool OPMapGadgetWidget::getUAVPosition(double &latitude, double &longitude, doub
Q_ASSERT(positionActual != NULL);
PositionActual::DataFields positionActualData = positionActual->getData();
NED[0] = positionActualData.North / 100;
NED[1] = positionActualData.East / 100;
NED[2] = positionActualData.Down / 100;
NED[0] = positionActualData.North;
NED[1] = positionActualData.East;
NED[2] = positionActualData.Down;
Utils::CoordinateConversions().GetLLA(homeLLA, NED, LLA);
@ -2350,23 +2350,12 @@ bool OPMapGadgetWidget::getUAVPosition(double &latitude, double &longitude, doub
altitude = LLA[2];
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

79
ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp
View File

@ -29,7 +29,7 @@
#include "uavobjectutilmanager.h"
#include "utils/homelocationutil.h"
#include "homelocation.h"
#include <QMutexLocker>
#include <QDebug>
#include <QEventLoop>
@ -339,17 +339,13 @@ QByteArray UAVObjectUtilManager::getBoardDescription()
int UAVObjectUtilManager::setHomeLocation(double LLA[3], bool save_to_sdcard)
{
double ECEF[3];
double RNE[9];
double Be[3];
UAVObjectField *field;
QMutexLocker locker(mutex);
if (Utils::HomeLocationUtil().getDetails(LLA, ECEF, RNE, Be) < 0)
return -1; // error
Q_ASSERT (Utils::HomeLocationUtil().getDetails(LLA, Be) >= 0);
// ******************
// ******************
// save the new settings
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
@ -358,68 +354,27 @@ int UAVObjectUtilManager::setHomeLocation(double LLA[3], bool save_to_sdcard)
UAVObjectManager *om = pm->getObject<UAVObjectManager>();
if (!om) return -3;
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(om->getObject(QString("HomeLocation")));
if (!obj) return -4;
HomeLocation *homeLocation = HomeLocation::GetInstance(om);
Q_ASSERT(homeLocation != NULL);
UAVObjectField *ECEF_field = obj->getField(QString("ECEF"));
if (!ECEF_field) return -5;
HomeLocation::DataFields homeLocationData = homeLocation->getData();
homeLocationData.Latitude = LLA[0] * 10e6;
homeLocationData.Longitude = LLA[1] * 10e6;
homeLocationData.Altitude = LLA[2] * 10e6;
UAVObjectField *RNE_field = obj->getField(QString("RNE"));
if (!RNE_field) return -6;
homeLocationData.Be[0] = Be[0];
homeLocationData.Be[1] = Be[1];
homeLocationData.Be[2] = Be[2];
UAVObjectField *Be_field = obj->getField(QString("Be"));
if (!Be_field) return -7;
homeLocationData.Set = HomeLocation::SET_TRUE;
field = obj->getField("Latitude");
if (!field) return -8;
field->setDouble(LLA[0] * 10e6);
field = obj->getField("Longitude");
if (!field) return -9;
field->setDouble(LLA[1] * 10e6);
field = obj->getField("Altitude");
if (!field) return -10;
field->setDouble(LLA[2]);
for (int i = 0; i < 3; i++)
ECEF_field->setDouble(ECEF[i] * 100, i);
for (int i = 0; i < 9; i++)
RNE_field->setDouble(RNE[i], i);
for (int i = 0; i < 3; i++)
Be_field->setDouble(Be[i], i);
field = obj->getField("Set");
if (!field) return -11;
field->setValue("TRUE");
obj->updated();
// ******************
// save the new setting to SD card
homeLocation->setData(homeLocationData);
homeLocation->updated();
if (save_to_sdcard)
saveObjectToSD(obj);
saveObjectToSD(homeLocation);
// ******************
// debug
/*
qDebug() << "setting HomeLocation UAV Object .. " << endl;
QString s;
s = " LAT:" + QString::number(LLA[0], 'f', 7) + " LON:" + QString::number(LLA[1], 'f', 7) + " ALT:" + QString::number(LLA[2], 'f', 1);
qDebug() << s << endl;
s = " ECEF "; for (int i = 0; i < 3; i++) s += " " + QString::number((int)(ECEF[i] * 100));
qDebug() << s << endl;
s = " RNE "; for (int i = 0; i < 9; i++) s += " " + QString::number(RNE[i], 'f', 7);
qDebug() << s << endl;
s = " Be "; for (int i = 0; i < 3; i++) s += " " + QString::number(Be[i], 'f', 2);
qDebug() << s << endl;
*/
// ******************
return 0; // OK
return 0;
}
int UAVObjectUtilManager::getHomeLocation(bool &set, double LLA[3])