From cd5fad2ae9e5bb90eb09b2c532425099a077bee9 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 15 Mar 2012 02:07:00 -0500 Subject: [PATCH] Update the map to set locations with the home location representation --- .../src/libs/utils/coordinateconversions.cpp | 6 +- .../src/libs/utils/homelocationutil.cpp | 7 +- .../src/libs/utils/homelocationutil.h | 14 ++-- .../src/plugins/opmap/opmapgadgetwidget.cpp | 25 ++---- .../uavobjectutil/uavobjectutilmanager.cpp | 79 ++++--------------- 5 files changed, 36 insertions(+), 95 deletions(-) diff --git a/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp b/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp index 497f47412..4f1835f4f 100644 --- a/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp +++ b/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp @@ -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]; diff --git a/ground/openpilotgcs/src/libs/utils/homelocationutil.cpp b/ground/openpilotgcs/src/libs/utils/homelocationutil.cpp index 18b1fd9af..70d52597d 100644 --- a/ground/openpilotgcs/src/libs/utils/homelocationutil.cpp +++ b/ground/openpilotgcs/src/libs/utils/homelocationutil.cpp @@ -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 } diff --git a/ground/openpilotgcs/src/libs/utils/homelocationutil.h b/ground/openpilotgcs/src/libs/utils/homelocationutil.h index 6448c3e52..2c7ddcfa1 100644 --- a/ground/openpilotgcs/src/libs/utils/homelocationutil.h +++ b/ground/openpilotgcs/src/libs/utils/homelocationutil.h @@ -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: - }; +}; } diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp index 42ada44d3..c0a07a000 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp @@ -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::max()) longitude = 0; // +infinite - // else - // if (longitude < -std::numeric_limits::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 diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp index ac64c61d8..e0d74a6a7 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp @@ -29,7 +29,7 @@ #include "uavobjectutilmanager.h" #include "utils/homelocationutil.h" - +#include "homelocation.h" #include #include #include @@ -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(); if (!om) return -3; - UAVDataObject *obj = dynamic_cast(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])