zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Added coordinate conversion for global / local conversion

QGC4.4
LM 14 years ago
parent
b3ed0acbba
commit
63900c28fa
3 changed files with 89 additions and 68 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 14
      src/QGCGeo.h
  2. 128
      src/uas/UASManager.cc
  3. 15
      src/uas/UASManager.h

14
src/QGCGeo.h
Unescape View File

@ -1,14 +1,22 @@ @@ -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

128
src/uas/UASManager.cc
Unescape View File

@ -1,24 +1,4 @@ @@ -1,24 +1,4 @@
/*=====================================================================
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() @@ -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) @@ -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) @@ -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 @@ -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() @@ -214,10 +212,10 @@ UASManager::~UASManager()
void UASManager::run()
{
// forever
// {
// QGC::SLEEP::msleep(5000);
// }
// forever
// {
// QGC::SLEEP::msleep(5000);
// }
exec();
}

15
src/uas/UASManager.h
Unescape View File

@ -35,6 +35,8 @@ This file is part of the QGROUNDCONTROL project @@ -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: @@ -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: @@ -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);

Write Preview
Loading…
Cancel
Save