Skip to content

Q
qgroundcontrol
Commit bff9eea2 authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

123

parent 7612232d
Hide whitespace changes
Inline Side-by-side
Showing with 158 additions and 205 deletions
src/Wima/CircularSurveyComplexItem.cc
View file @ bff9eea2
......@@ -299,7 +299,8 @@ void CircularSurveyComplexItem::_rebuildTransectsFast()
_transects.clear();
QPolygonF surveyPolygon = toQPolygonF(toCartesian2D(_surveyAreaPolygon.coordinateList(), _referencePoint));
QPolygonF surveyPolygon;
toCartesianList(_surveyAreaPolygon.coordinateList(), _referencePoint, surveyPolygon);
if (!_rebuildTransectsInputCheck(surveyPolygon))
return;
......@@ -386,7 +387,8 @@ void CircularSurveyComplexItem::_rebuildTransectsSlow()
_transects.clear();
QPolygonF surveyPolygon = toQPolygonF(toCartesian2D(_surveyAreaPolygon.coordinateList(), _referencePoint));
QPolygonF surveyPolygon;
toCartesianList(_surveyAreaPolygon.coordinateList(), _referencePoint, surveyPolygon);
if (!_rebuildTransectsInputCheck(surveyPolygon))
return;
......@@ -490,15 +492,16 @@ bool CircularSurveyComplexItem::_shortestPath(const QGeoCoordinate &start, const
{
using namespace GeoUtilities;
using namespace PolygonCalculus;
QPolygonF polygon2D;
toCartesianList(this->surveyAreaPolygon()->coordinateList(), /*origin*/ start, polygon2D);
QPointF start2D(0,0);
QPointF end2D;
toCartesian(destination, start, end2D);
QVector<QPointF> path2D;
bool retVal = PolygonCalculus::shortestPath(
toQPolygonF(toCartesian2D(this->surveyAreaPolygon()->coordinateList(), /*origin*/ start)),
/*start point*/ QPointF(0,0),
/*destination*/ toCartesian2D(destination, start),
/*shortest path*/ path2D);
bool retVal = PolygonCalculus::shortestPath(polygon2D, start2D, end2D, path2D);
if (retVal)
shortestPath.append(toGeo(path2D, /*origin*/ start));
toGeoList(path2D, /*origin*/ start, shortestPath);
return retVal;
}
......@@ -680,7 +683,8 @@ void CircularSurveyComplexItem::_rebuildTransectsToGeo(const QVector<QPointF> &p
{
using namespace GeoUtilities;
QVector<QGeoCoordinate> geoPath = toGeo(path, reference);
QVector<QGeoCoordinate> geoPath;
toGeoList(path, reference, geoPath);
QList<CoordInfo_t> transectList;
transectList.reserve(path.size());
for ( const QGeoCoordinate &coordinate : geoPath) {
......
src/Wima/Geometry/GeoUtilities.cc
View file @ bff9eea2
#include "GeoUtilities.h"
namespace GeoUtilities {
QGeoCoordinate toGeo(const QVector3D &point, const QGeoCoordinate &origin)
{
using namespace PolygonCalculus;
double z = point.z();
double h = origin.altitude();
QGeoCoordinate coordinate(toGeo(point.toPointF(), origin));
coordinate.setAltitude(z + h);
return coordinate;
}
QVector3D toCartesian(const QGeoCoordinate &coordinate, const QGeoCoordinate &origin)
{
double h = coordinate.altitude();
double hO = origin.altitude();
QVector3D point(toCartesian2D(coordinate, origin));
point.setZ(h - hO);
return point;
}
QGeoVector toGeo(const QVector3DList &points, const QGeoCoordinate &origin)
{
QGeoVector coordinates;
coordinates.reserve(points.size());
for (auto point : points)
coordinates.append(toGeo(point, origin));
return coordinates;
}
QVector3DList toCartesian(const QGeoList &coordinates, const QGeoCoordinate &origin)
{
QVector3DList points;
for (auto coordinate : coordinates )
points.append(toCartesian(coordinate, origin));
return points;
}
QPointF toCartesian2D(const QGeoCoordinate &point, const QGeoCoordinate &origin)
{
double lat = point.latitude()/180*M_PI;
double lon = point.longitude()/180*M_PI;
double latO = origin.latitude()/180*M_PI;
double lonO = origin.longitude()/180*M_PI;
double dlon = lon-lonO;
double dlat = lat-latO;
if (!qFuzzyCompare(dlon, M_PI_2) && !qFuzzyCompare(dlat, M_PI_2))
return QPointF(/* x */ qTan(dlon)*earthRadius*qCos(latO),
/* y */ qTan(dlat)*earthRadius);
else
return QPointF(); // singularity occurred (tan(pi/2) = inf)
}
QPointFList toCartesian2D(const QGeoList &coordinates, const QGeoCoordinate &origin)
{
QPointFList listF;
listF.reserve(coordinates.size());
for ( auto coordinate : coordinates)
listF.append(toCartesian2D(coordinate, origin));
return listF;
}
QGeoCoordinate toGeo(const QPointF &point, const QGeoCoordinate &origin)
{
double x = point.x();
double y = point.y();
double lat = origin.latitude();
double lon = origin.longitude();
//qWarning("%lf %lf %lf %lf", x, y, lat, lon);
if (!qFuzzyCompare(lat, M_PI_2)) // this could (unlikely) be a problem, replace with different coordinate transformation
return QGeoCoordinate(/* lat */ qAtan(y/earthRadius)*180/M_PI + lat,
/* lon */ qAtan(x/cos(lat/180*M_PI)/earthRadius)*180/M_PI + lon,
/* alt */ origin.altitude());
else
return QGeoCoordinate(); // singularity occurred (1/cos(pi/2) = inf)
}
QGeoVector toGeo(const QPointFList &points, const QGeoCoordinate &origin)
{
QGeoVector coordinates;
coordinates.reserve(points.size());
for ( auto point : points)
coordinates.append(toGeo(point, origin));
return coordinates;
}
QGeoVector toGeo(const QPointFVector &points, const QGeoCoordinate &origin)
{
QGeoVector coordinates;
coordinates.reserve(points.size());
for ( auto point : points)
coordinates.append(toGeo(point, origin));
return coordinates;
}
}
src/Wima/Geometry/GeoUtilities.h
View file @ bff9eea2
......@@ -17,18 +17,73 @@ namespace GeoUtilities {
typedef QList<QGeoCoordinate> QGeoList;
typedef QVector<QGeoCoordinate> QGeoVector;
const double earthRadius = 6378137; // meter
QGeoCoordinate toGeo (const QVector3D &point, const QGeoCoordinate &origin);
QGeoVector toGeo (const QVector3DList &points, const QGeoCoordinate &origin);
QGeoCoordinate toGeo (const QPointF &point, const QGeoCoordinate &origin);
QGeoVector toGeo (const QPointFList &points, const QGeoCoordinate &origin);
QGeoVector toGeo (const QPointFVector &points, const QGeoCoordinate &origin);
QVector3D toCartesian (const QGeoCoordinate &coordinate, const QGeoCoordinate &origin);
QVector3DList toCartesian (const QGeoList &coordinates, const QGeoCoordinate &origin);
QVector3DList toCartesian (const QGeoVector &coordinates, const QGeoCoordinate &origin);
QPointF toCartesian2D (const QGeoCoordinate &point, const QGeoCoordinate &origin);
QPointFList toCartesian2D (const QGeoList &coordinates, const QGeoCoordinate &origin);
QPointFList toCartesian2D (const QGeoVector &coordinates, const QGeoCoordinate &origin);
const double earthRadius = 6378137; // meter
template <class GeoPoint1, class GeoPoint2, class Point2D>
void toGeo(const Point2D &in, const GeoPoint1 &origin, GeoPoint2 &out){
double x = in.x();
double y = in.y();
double lat = origin.latitude();
double lon = origin.longitude();
//qWarning("%lf %lf %lf %lf", x, y, lat, lon);
// this could (unlikely) be a problem, replace with different coordinate transformation
if (!qFuzzyCompare(lat, M_PI_2)) {
out.setLatitude(qAtan(y/earthRadius)*180/M_PI + lat);
out.setLongitude(qAtan(x/cos(lat/180*M_PI)/earthRadius)*180/M_PI + lon);
out.setAltitude(origin.altitude());
}
return;
}
template <class GeoPointList, class GeoPoint, class Point2DList>
void toGeoList(const Point2DList &in, const GeoPoint &origin, GeoPointList &out){
out.reserve(in.size());
typedef typename GeoPointList::value_type GeoPoint2;
GeoPoint2 p;
for ( auto c : in) {
toGeo(c, origin, p);
out.append(p);
}
}
template <class GeoPoint1, class GeoPoint2, class Point2D>
void toCartesian(const GeoPoint1 &in, const GeoPoint2 &origin, Point2D &out)
{
double lat = in.latitude()/180*M_PI;
double lon = in.longitude()/180*M_PI;
double latO = origin.latitude()/180*M_PI;
double lonO = origin.longitude()/180*M_PI;
double dlon = lon-lonO;
double dlat = lat-latO;
if (!qFuzzyCompare(dlon, M_PI_2) && !qFuzzyCompare(dlat, M_PI_2)) {
out.setX(qTan(dlon)*earthRadius*qCos(latO));
out.setY(qTan(dlat)*earthRadius);
}
return;
}
template <class GeoPointList, class GeoPoint, class Point2DList>
void toCartesianList(const GeoPointList &in, const GeoPoint &origin, Point2DList &out){
out.reserve(in.size());
typedef typename Point2DList::value_type Point2D;
Point2D p;
for ( auto c : in) {
toCartesian(c, origin, p);
out.append(p);
}
return;
}
}
src/Wima/Geometry/WimaArea.cc
View file @ bff9eea2
......@@ -248,8 +248,10 @@ bool WimaArea::join(const WimaArea &area1, const WimaArea &area2, WimaArea &join
QPolygonF polygon1 = toQPolygonF(toCartesian2D(GeoPolygon1, origin));
QPolygonF polygon2 = toQPolygonF(toCartesian2D(GeoPolygon2, origin));
QPolygonF polygon1;
toCartesianList(GeoPolygon1, origin, polygon1);
QPolygonF polygon2;
toCartesianList(GeoPolygon2, origin, polygon2);
// qWarning("after 1 transform");
// qWarning() << polygon1;
......@@ -269,7 +271,8 @@ bool WimaArea::join(const WimaArea &area1, const WimaArea &area2, WimaArea &join
} else if (retValue == JoinPolygonError::PathSizeLow) {
qWarning("Polygon vertex count is low.");
} else {
QVector<QGeoCoordinate> path = toGeo(toQPointFList(joinedPolygon), origin);
QVector<QGeoCoordinate> path;
toGeoList(joinedPolygon, origin, path);
// qWarning("after transform");
// qWarning() << path;
joinedArea.setPath(path);
......@@ -385,7 +388,8 @@ bool WimaArea::isSimplePolygon() const
using namespace GeoUtilities;
if (this->count() > 2) {
QPolygonF polygon = toQPolygonF(toCartesian2D(this->coordinateList(), this->vertexCoordinate(0)));
QPolygonF polygon;
toCartesianList(this->coordinateList(), this->vertexCoordinate(0), polygon);
return PolygonCalculus::isSimplePolygon(polygon);
} else
return false;
......@@ -399,7 +403,8 @@ bool WimaArea::containsCoordinate(const QGeoCoordinate &coordinate) const
using namespace GeoUtilities;
if (this->count() > 2) {
QPolygonF polygon = toQPolygonF(toCartesian2D(this->coordinateList(), coordinate));
QPolygonF polygon;
toCartesianList(this->coordinateList(), coordinate, polygon);
return PlanimetryCalculus::contains(polygon, QPointF(0,0));
} else
return false;
......
src/Wima/Geometry/WimaAreaData.cc
View file @ bff9eea2
......@@ -2,7 +2,6 @@
WimaAreaData::WimaAreaData(QObject *parent)
: QObject(parent)
, _listValid(false)
{
_maxAltitude = 0;
}
......@@ -34,13 +33,7 @@ QGeoCoordinate WimaAreaData::center() const
const QList<QGeoCoordinate> &WimaAreaData::coordinateList() const
{
static QList<QGeoCoordinate> list;
if ( !_listValid ) {
for ( auto coorinate : _path)
list.append(coorinate.value<QGeoCoordinate>());
}
return list;
return _list;
}
bool WimaAreaData::containsCoordinate(const QGeoCoordinate &coordinate) const
......@@ -50,19 +43,20 @@ bool WimaAreaData::containsCoordinate(const QGeoCoordinate &coordinate) const
using namespace GeoUtilities;
if (_path.size() > 2) {
QPolygonF polygon = toQPolygonF(toCartesian2D(this->coordinateList(), coordinate));
QPolygonF polygon;
toCartesianList(this->coordinateList(), coordinate, polygon);
return PlanimetryCalculus::contains(polygon, QPointF(0,0));
} else
return false;
}
void WimaAreaData::append(const QGeoCoordinate &c) {
_listValid = false;
_list.append(c);
_path.push_back(QVariant::fromValue(c));
}
void WimaAreaData::clear() {
_listValid = false;
_list.clear();
_path.clear();
}
......@@ -84,9 +78,11 @@ void WimaAreaData::setMaxAltitude(double maxAltitude)
void WimaAreaData::setPath(const QVariantList &coordinateList)
{
_listValid = false;
_path.clear();
_path.append(coordinateList);
_path = coordinateList;
_list.clear();
for (auto variant : coordinateList) {
_list.push_back(variant.value<QGeoCoordinate>());
}
}
void WimaAreaData::setCenter(const QGeoCoordinate &center)
......@@ -126,9 +122,9 @@ void WimaAreaData::assign(const WimaArea &other)
*/
void WimaAreaData::setPath(const QList<QGeoCoordinate> &coordinateList)
{
_listValid = false;
_path.clear();
_list = coordinateList;
_path.clear();
// copy all coordinates to _path
for(int i = 0; i < coordinateList.size(); i++) {
_path.append(QVariant::fromValue(coordinateList.value(i)));
......
src/Wima/Geometry/WimaAreaData.h
View file @ bff9eea2
......@@ -50,7 +50,7 @@ private:
// see WimaArea.h for explanation
double _maxAltitude;
QVariantList _path;
QList<QGeoCoordinate> _list;
QGeoCoordinate _center;
bool _listValid;
};
src/Wima/WaypointManager/DefaultManager.cpp
View file @ bff9eea2
......@@ -91,13 +91,15 @@ bool WaypointManager::DefaultManager::_calcShortestPath(
{
using namespace GeoUtilities;
using namespace PolygonCalculus;
QVector<QPointF> path2D;
bool retVal = PolygonCalculus::shortestPath(
toQPolygonF(toCartesian2D(_areaInterface->joinedArea()->coordinateList(), /*origin*/ start)),
/*start point*/ QPointF(0,0),
/*destination*/ toCartesian2D(destination, start),
/*shortest path*/ path2D);
path.append(toGeo(path2D, /*origin*/ start));
QPolygonF joinedArea2D;
toCartesianList(_areaInterface->joinedArea()->coordinateList(), /*origin*/ start, joinedArea2D);
QPointF start2D(0,0);
QPointF end2D;
toCartesian(destination, start, end2D);
QVector<QPointF> path2DOut;
bool retVal = PolygonCalculus::shortestPath(joinedArea2D, start2D, end2D, path2DOut);
toGeoList(path2DOut, /*origin*/ start, path);
return retVal;
}
......@@ -131,10 +133,11 @@ bool WaypointManager::DefaultManager::_worker()
// calculate path from home to first waypoint
QVector<QGeoCoordinate> arrivalPath;
if (!_settings->homePosition().isValid()){
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): home position invalid.");
Q_ASSERT(false);
return false;
}
if ( !_calcShortestPath(_settings->homePosition(), _currentWaypoints.first(), arrivalPath) ) {
qWarning("WaypointManager::DefaultManager::next(): Not able to calc path from home position to first waypoint.");
return false;
......@@ -142,13 +145,8 @@ bool WaypointManager::DefaultManager::_worker()
arrivalPath.removeFirst();
// calculate path from last waypoint to home
qDebug() << "_currentWaypoints.size(): " << _currentWaypoints.size();
qDebug() << "_currentWaypoints.first(): " << _currentWaypoints.first();
qDebug() << "_currentWaypoints.last(): " << _currentWaypoints.last();
qDebug() << "_settings->homePosition(): " << _settings->homePosition();
QVector<QGeoCoordinate> returnPath;
if ( !_calcShortestPath(_currentWaypoints.last(), _settings->homePosition(), returnPath) ) {
//if ( !_calcShortestPath(_settings->homePosition(), _currentWaypoints.first(), returnPath) ) {
qWarning("WaypointManager::DefaultManager::next(): Not able to calc path from home position to last waypoint.");
return false;
}
......@@ -269,7 +267,7 @@ bool WaypointManager::DefaultManager::_worker()
// Set altitude.
for (int i = 0; i < _currentMissionItems.count(); ++i) {
for (int i = 1; i < _currentMissionItems.count(); ++i) {
SimpleMissionItem *item = _currentMissionItems.value<SimpleMissionItem *>(i);
if (item == nullptr) {
Q_ASSERT(false);
......
src/Wima/WimaController.cc
View file @ bff9eea2
......@@ -47,6 +47,11 @@ WimaController::WimaController(QObject *parent)
, _serviceArea ()
, _corridor ()
, _localPlanDataValid (false)
, _areaInterface (&_measurementArea, &_serviceArea, &_corridor, &_joinedArea)
, _managerSettings ()
, _defaultManager (_managerSettings, _areaInterface)
, _snakeManager (_managerSettings, _areaInterface)
, _currentManager (_defaultManager)
, _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/WimaController.SettingsGroup.json"), this))
, _enableWimaController (settingsGroup, _metaDataMap[enableWimaControllerName])
, _overlapWaypoints (settingsGroup, _metaDataMap[overlapWaypointsName])
......@@ -57,19 +62,14 @@ WimaController::WimaController(QObject *parent)
, _flightSpeed (settingsGroup, _metaDataMap[flightSpeedName])
, _arrivalReturnSpeed (settingsGroup, _metaDataMap[arrivalReturnSpeedName])
, _altitude (settingsGroup, _metaDataMap[altitudeName])
, _areaInterface(&_measurementArea, &_serviceArea, &_corridor, &_joinedArea)
, _managerSettings()
, _defaultManager(_managerSettings, _areaInterface)
, _snakeManager(_managerSettings, _areaInterface)
, _currentManager(_defaultManager)
, _uploadOverrideRequired (false)
, _measurementPathLength (-1)
, _arrivalPathLength (-1)
, _returnPathLength (-1)
, _phaseDistance (-1)
, _phaseDuration (-1)
, _phaseDistanceBuffer (-1)
, _phaseDurationBuffer (-1)
// , _arrivalPathLength (-1)
// , _returnPathLength (-1)
// , _phaseDistance (-1)
// , _phaseDuration (-1)
// , _phaseDistanceBuffer (-1)
// , _phaseDurationBuffer (-1)
, _vehicleHasLowBattery (false)
, _lowBatteryHandlingTriggered (false)
, _executingSmartRTL (false)
......@@ -382,13 +382,15 @@ bool WimaController::calcShortestPath(const QGeoCoordinate &start, const QGeoCoo
{
using namespace GeoUtilities;
using namespace PolygonCalculus;
QPolygonF polygon2D;
toCartesianList(_joinedArea.coordinateList(), /*origin*/ start, polygon2D);
QPointF start2D(0,0);
QPointF end2D;
toCartesian(destination, start, end2D);
QVector<QPointF> path2D;
bool retVal = PolygonCalculus::shortestPath(
toQPolygonF(toCartesian2D(_joinedArea.coordinateList(), /*origin*/ start)),
/*start point*/ QPointF(0,0),
/*destination*/ toCartesian2D(destination, start),
/*shortest path*/ path2D);
path.append(toGeo(path2D, /*origin*/ start));
bool retVal = PolygonCalculus::shortestPath(polygon2D, start2D, end2D, path2D);
toGeoList(path2D, /*origin*/ start, path);
return retVal;
}
......@@ -580,10 +582,17 @@ bool WimaController::_fetchContainerData()
bool WimaController::_calcNextPhase()
{
qDebug() << "WimaController::_calcNextPhase: "
<< "overlap=" << _currentManager.overlap()
<< "N=" << _currentManager.N()
<< "startIndex=" << _currentManager.startIndex();
bool value;
_currentManager.setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt(&value));
Q_ASSERT(value);
(void)value;
qDebug() << "overlap=" << _currentManager.overlap()
<< "N=" << _currentManager.N()
<< "startIndex=" << _currentManager.startIndex();
if ( !_currentManager.next() ) {
assert(false);
......
src/Wima/WimaController.h
View file @ bff9eea2
......@@ -409,12 +409,12 @@ private:
bool _uploadOverrideRequired; // Is set to true if uploadToVehicle() did not suceed because the vehicle is not inside the service area.
// The user can override the upload lock with a slider, this will reset this variable to false.
double _measurementPathLength; // the lenght of the phase in meters
double _arrivalPathLength; // the length of the arrival and return path in meters
double _returnPathLength; // the length of the arrival and return path in meters
double _phaseDistance; // the lenth in meters of the current phase
double _phaseDuration; // the phase duration in seconds
double _phaseDistanceBuffer; // buffer for storing _phaseDistance when doing smart RTL
double _phaseDurationBuffer; // buffer for storing _phaseDuration when doing smart RTL
// double _arrivalPathLength; // the length of the arrival and return path in meters
// double _returnPathLength; // the length of the arrival and return path in meters
// double _phaseDistance; // the lenth in meters of the current phase
// double _phaseDuration; // the phase duration in seconds
// double _phaseDistanceBuffer; // buffer for storing _phaseDistance when doing smart RTL
// double _phaseDurationBuffer; // buffer for storing _phaseDuration when doing smart RTL
QTimer _eventTimer;
QTimer _smartRTLAttemptTimer;
......
src/Wima/WimaPlaner.cc
View file @ bff9eea2
......@@ -656,17 +656,20 @@ void WimaPlaner::pushToContainer()
}
}
bool WimaPlaner::calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector<QGeoCoordinate> &path)
bool WimaPlaner::calcShortestPath(const QGeoCoordinate &start,
const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path)
{
using namespace GeoUtilities;
using namespace PolygonCalculus;
QPolygonF polygon2D;
toCartesianList(_joinedArea.coordinateList(), /*origin*/ start, polygon2D);
QPointF start2D(0,0);
QPointF end2D;
QVector<QPointF> path2D;
bool retVal = PolygonCalculus::shortestPath(
toQPolygonF(toCartesian2D(_joinedArea.coordinateList(), /*origin*/ start)),
/*start point*/ QPointF(0,0),
/*destination*/ toCartesian2D(destination, start),
/*shortest path*/ path2D);
path.append(toGeo(path2D, /*origin*/ start));
toCartesian(destination, start, end2D);
bool retVal = PolygonCalculus::shortestPath( polygon2D, start2D, end2D, path2D);
toGeoList(path2D, /*origin*/ start, path);
return retVal;
}
......
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