Skip to content

Q
qgroundcontrol
Commit 63900c28 authored by LM's avatar LM
Browse files
Options

Added coordinate conversion for global / local conversion

parent b3ed0acb
Hide whitespace changes
Inline Side-by-side
Showing with 89 additions and 68 deletions
src/QGCGeo.h
View file @ 63900c28
#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
src/uas/UASManager.cc
View file @ 63900c28
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
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 <http://www.gnu.org/licenses/>.
/*==================================================================
======================================================================*/
/**
......@@ -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();
}
......
src/uas/UASManager.h
View file @ 63900c28
......@@ -35,6 +35,8 @@ This file is part of the QGROUNDCONTROL project
#include <QList>
#include <QMutex>
#include <UASInterface.h>
#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);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment