From 63900c28fa6e306f4b3aa0f2199b60a10fb70b28 Mon Sep 17 00:00:00 2001 From: LM Date: Wed, 20 Jul 2011 17:24:04 +0200 Subject: [PATCH] Added coordinate conversion for global / local conversion --- src/QGCGeo.h | 14 ++++- src/uas/UASManager.cc | 128 +++++++++++++++++++++--------------------- src/uas/UASManager.h | 15 +++++ 3 files changed, 89 insertions(+), 68 deletions(-) diff --git a/src/QGCGeo.h b/src/QGCGeo.h index 3aa6afd68..897910ce7 100644 --- a/src/QGCGeo.h +++ b/src/QGCGeo.h @@ -1,14 +1,22 @@ #ifndef QGCGEO_H #define QGCGEO_H +#define DEG2RAD (M_PI/180.0) + +/* Safeguard for systems lacking sincos (e.g. Mac OS X Leopard) */ +#ifndef sincos +#define sincos(th,x,y) { (*(x))=sin(th); (*(y))=cos(th); } +#endif + + /** * Converting from latitude / longitude to tangent on earth surface * @link http://psas.pdx.edu/CoordinateSystem/Latitude_to_LocalTangent.pdf * @link http://dspace.dsto.defence.gov.au/dspace/bitstream/1947/3538/1/DSTO-TN-0432.pdf */ -void LatLonToENU(double lat, double lon, double alt, double originLat, double originLon, double originAlt, double* x, double* y, double* z) -{ +//void LatLonToENU(double lat, double lon, double alt, double originLat, double originLon, double originAlt, double* x, double* y, double* z) +//{ -} +//} #endif // QGCGEO_H diff --git a/src/uas/UASManager.cc b/src/uas/UASManager.cc index 3e26a05f5..a105b8675 100644 --- a/src/uas/UASManager.cc +++ b/src/uas/UASManager.cc @@ -1,24 +1,4 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -This file is part of the QGROUNDCONTROL project - - QGROUNDCONTROL is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - QGROUNDCONTROL is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with QGROUNDCONTROL. If not, see . - +/*================================================================== ======================================================================*/ /** @@ -68,8 +48,8 @@ void UASManager::loadSettings() settings.sync(); settings.beginGroup("QGC_UASMANAGER"); bool changed = setHomePosition(settings.value("HOMELAT", homeLat).toDouble(), - settings.value("HOMELON", homeLon).toDouble(), - settings.value("HOMEALT", homeAlt).toDouble()); + settings.value("HOMELON", homeLon).toDouble(), + settings.value("HOMEALT", homeAlt).toDouble()); // Make sure to fire the change - this will // make sure widgets get the signal once @@ -95,11 +75,15 @@ bool UASManager::setHomePosition(double lat, double lon, double alt) if (homeLon != lon) changed = true; if (homeAlt != alt) changed = true; + // Initialize conversion reference in any case + initReference(lat, lon, alt); + if (changed) { homeLat = lat; homeLon = lon; homeAlt = alt; + emit homePositionChanged(homeLat, homeLon, homeAlt); // Update all UAVs @@ -113,53 +97,67 @@ bool UASManager::setHomePosition(double lat, double lon, double alt) } -//void UASManager::initReference(const double & latitude, const double & longitude, const double & altitude) -//{ -// Eigen::Matrix3d R; -// double s_long, s_lat, c_long, c_lat; -// sincos(latitude * DEG2RAD, &s_lat, &c_lat); -// sincos(longitude * DEG2RAD, &s_long, &c_long); +void UASManager::initReference(const double & latitude, const double & longitude, const double & altitude) +{ + Eigen::Matrix3d R; + double s_long, s_lat, c_long, c_lat; + sincos(latitude * DEG2RAD, &s_lat, &c_lat); + sincos(longitude * DEG2RAD, &s_long, &c_long); -// R(0, 0) = -s_long; -// R(0, 1) = c_long; -// R(0, 2) = 0; + R(0, 0) = -s_long; + R(0, 1) = c_long; + R(0, 2) = 0; -// R(1, 0) = -s_lat * c_long; -// R(1, 1) = -s_lat * s_long; -// R(1, 2) = c_lat; + R(1, 0) = -s_lat * c_long; + R(1, 1) = -s_lat * s_long; + R(1, 2) = c_lat; -// R(2, 0) = c_lat * c_long; -// R(2, 1) = c_lat * s_long; -// R(2, 2) = s_lat; + R(2, 0) = c_lat * c_long; + R(2, 1) = c_lat * s_long; + R(2, 2) = s_lat; -// ecef_ref_orientation_ = Eigen::Quaterniond(R); + ecef_ref_orientation_ = Eigen::Quaterniond(R); -// ecef_ref_point_ = wgs84ToEcef(latitude, longitude, altitude); -//} + ecef_ref_point_ = wgs84ToEcef(latitude, longitude, altitude); +} -//Eigen::Vector3d UASManager::wgs84ToEcef(const double & latitude, const double & longitude, const double & altitude) -//{ -// const double a = 6378137.0; // semi-major axis -// const double e_sq = 6.69437999014e-3; // first eccentricity squared +Eigen::Vector3d UASManager::wgs84ToEcef(const double & latitude, const double & longitude, const double & altitude) +{ + const double a = 6378137.0; // semi-major axis + const double e_sq = 6.69437999014e-3; // first eccentricity squared -// double s_long, s_lat, c_long, c_lat; -// sincos(latitude * DEG2RAD, &s_lat, &c_lat); -// sincos(longitude * DEG2RAD, &s_long, &c_long); + double s_long, s_lat, c_long, c_lat; + sincos(latitude * DEG2RAD, &s_lat, &c_lat); + sincos(longitude * DEG2RAD, &s_long, &c_long); -// const double N = a / sqrt(1 - e_sq * s_lat * s_lat); + const double N = a / sqrt(1 - e_sq * s_lat * s_lat); -// Eigen::Vector3d ecef; + Eigen::Vector3d ecef; -// ecef[0] = (N + altitude) * c_lat * c_long; -// ecef[1] = (N + altitude) * c_lat * s_long; -// ecef[2] = (N * (1 - e_sq) + altitude) * s_lat; + ecef[0] = (N + altitude) * c_lat * c_long; + ecef[1] = (N + altitude) * c_lat * s_long; + ecef[2] = (N * (1 - e_sq) + altitude) * s_lat; -// return ecef; -//} + return ecef; +} + +Eigen::Vector3d UASManager::ecefToEnu(const Eigen::Vector3d & ecef) +{ + return ecef_ref_orientation_ * (ecef - ecef_ref_point_); +} + +void UASManager::wgs84ToEnu(const double& lat, const double& lon, const double& alt, double* east, double* north, double* up) +{ + Eigen::Vector3d ecef = wgs84ToEcef(lat, lon, alt); + Eigen::Vector3d enu = ecefToEnu(ecef); + *east = enu.x(); + *north = enu.y(); + *up = enu.z(); +} -//Eigen::Vector3d UASManager::ecefToEnu(const Eigen::Vector3d & ecef) +//void UASManager::wgs84ToNed(const double& lat, const double& lon, const double& alt, double* north, double* east, double* down) //{ -// return ecef_ref_orientation_ * (ecef - ecef_ref_point_); + //} @@ -193,10 +191,10 @@ void UASManager::uavChangedHomePosition(int uav, double lat, double lon, double * This class implements the singleton design pattern and has therefore only a private constructor. **/ UASManager::UASManager() : - activeUAS(NULL), - homeLat(47.3769), - homeLon(8.549444), - homeAlt(470.0) + activeUAS(NULL), + homeLat(47.3769), + homeLon(8.549444), + homeAlt(470.0) { start(QThread::LowPriority); loadSettings(); @@ -214,10 +212,10 @@ UASManager::~UASManager() void UASManager::run() { -// forever -// { -// QGC::SLEEP::msleep(5000); -// } + // forever + // { + // QGC::SLEEP::msleep(5000); + // } exec(); } diff --git a/src/uas/UASManager.h b/src/uas/UASManager.h index 0d42b8afb..85fc4217a 100644 --- a/src/uas/UASManager.h +++ b/src/uas/UASManager.h @@ -35,6 +35,8 @@ This file is part of the QGROUNDCONTROL project #include #include #include +#include "Eigen/Eigen" +#include "QGCGeo.h" /** * @brief Central manager for all connected aerial vehicles @@ -83,6 +85,15 @@ public: return homeAlt; } + /** @brief Convert WGS84 coordinates to earth centric frame */ + Eigen::Vector3d wgs84ToEcef(const double & latitude, const double & longitude, const double & altitude); + /** @brief Convert earth centric frame to EAST-NORTH-UP frame (x-y-z directions */ + Eigen::Vector3d ecefToEnu(const Eigen::Vector3d & ecef); + /** @brief Convert WGS84 lat/lon coordinates to carthesian coordinates with home position as origin */ + void wgs84ToEnu(const double& lat, const double& lon, const double& alt, double* east, double* north, double* up); + +// void wgs84ToNed(const double& lat, const double& lon, const double& alt, double* north, double* east, double* down); + public slots: @@ -194,6 +205,10 @@ protected: double homeLat; double homeLon; double homeAlt; + Eigen::Quaterniond ecef_ref_orientation_; + Eigen::Vector3d ecef_ref_point_; + + void initReference(const double & latitude, const double & longitude, const double & altitude); signals: void UASCreated(UASInterface* UAS); -- 2.22.0