Skip to content

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

SnakeWorker issues solved, about to add tile visualization.

parent b4c66b1b
Hide whitespace changes
Inline Side-by-side
Showing with 231 additions and 193 deletions
src/Snake/snake.cpp
View file @ 99b27028
......@@ -4,6 +4,10 @@
#include "clipper/clipper.hpp"
#define CLIPPER_SCALE 10000
#ifndef NDEBUG
//#define SHOW_TIME
#endif
using namespace snake_geometry;
using namespace std;
......@@ -71,6 +75,7 @@ bool Scenario::_areas2enu()
}
bg::correct(_serviceAreaENU);
polygonCenter(_serviceAreaENU, _homePositionENU);
fromENU(_geoOrigin, Point3D{_homePositionENU.get<0>(), _homePositionENU.get<1>(), 0}, _homePosition);
_corridorENU.clear();
if (_corridor.geoPolygon.size() > 0){
......@@ -146,6 +151,8 @@ bool Scenario::_calculateTiles(double tileWidth, double tileHeight, double minTi
{
_tilesENU.clear();
_tileCenterPointsENU.clear();
_tiles.clear();
_tileCenterPoints.clear();
if (tileWidth <= 0 || tileHeight <= 0 || minTileArea <= 0) {
errorString = "Parameters tileWidth, tileHeight, minTileArea must be positive.";
......@@ -223,6 +230,23 @@ bool Scenario::_calculateTiles(double tileWidth, double tileHeight, double minTi
return false;
}
for ( auto tile : _tilesENU){
GeoPoint3DList geoTile;
for ( int i=0; i < int(tile.outer().size())-1; ++i) {
BoostPoint vertex(tile.outer()[i]);
GeoPoint3D geoVertex;
fromENU(_geoOrigin, Point3D{vertex.get<0>(), vertex.get<1>(), 0}, geoVertex);
geoTile.push_back(geoVertex);
}
_tiles.push_back(geoTile);
}
for ( auto vertex : _tileCenterPointsENU){
GeoPoint3D geoVertex;
fromENU(_geoOrigin, Point3D{vertex.get<0>(), vertex.get<1>(), 0}, geoVertex);
_tileCenterPoints.push_back(geoVertex);
}
return true;
}
......@@ -240,8 +264,9 @@ bool Scenario::_calculateJoinedArea()
// Corridor overlaping with measurement area?
if ( bg::intersects(_corridorENU, _measurementAreaENU) ) {
// Corridor overlaping with service area?
if ( bg::intersects(_corridorENU, _serviceAreaENU) )
if ( bg::intersects(_corridorENU, _serviceAreaENU) ) {
corridor_is_connection = true;
}
}
}
......@@ -291,12 +316,16 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength)
#ifndef NDEBUG
_PathVertices.clear();
#endif
#ifdef SHOW_TIME
auto start = std::chrono::high_resolution_clock::now();
#endif
if (!_generateTransects(lineDistance, minTransectLength))
return false;
#ifdef SHOW_TIME
auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
cout << endl;
cout << "Execution time _generateTransects(): " << delta.count() << " ms" << endl;
#endif
//=======================================
// Route Transects using Google or-tools.
......@@ -330,10 +359,14 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength)
RoutingDataModel_t dataModel;
Matrix<double> connectionGraph(n1, n1);
#ifdef SHOW_TIME
start = std::chrono::high_resolution_clock::now();
#endif
_generateRoutingModel(vertices, jAreaOffset, n0, dataModel, connectionGraph);
#ifdef SHOW_TIME
delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
cout << "Execution time _generateRoutingModel(): " << delta.count() << " ms" << endl;
#endif
// Create Routing Index Manager.
RoutingIndexManager manager(dataModel.distanceMatrix.getN(), dataModel.numVehicles,
......@@ -350,6 +383,9 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength)
return dataModel.distanceMatrix.get(from_node, to_node);
});
// Define cost of each arc.
routing.SetArcCostEvaluatorOfAllVehicles(transit_callback_index);
// Define Constraints.
size_t n = _transects.size()*2;
......@@ -373,9 +409,6 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength)
solver->AddConstraint(c);
}
// Define cost of each arc.
routing.SetArcCostEvaluatorOfAllVehicles(transit_callback_index);
// Setting first solution heuristic.
RoutingSearchParameters searchParameters = DefaultRoutingSearchParameters();
......@@ -386,10 +419,14 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength)
searchParameters.set_allocated_time_limit(tMax);
// Solve the problem.
#ifdef SHOW_TIME
start = std::chrono::high_resolution_clock::now();
#endif
const Assignment* solution = routing.SolveWithParameters(searchParameters);
#ifdef SHOW_TIME
delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
cout << "Execution time routing.SolveWithParameters(): " << delta.count() << " ms" << endl;
#endif
if (!solution || solution->Size() <= 1){
errorString = "Not able to solve the routing problem.";
......@@ -413,14 +450,25 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength)
{
_waypointsENU.push_back(vertices[route[0]]);
vector<size_t> pathIdx;
long arrivalPathLength = 0;
for (long i=0; i<long(route.size())-1; ++i){
size_t idx0 = route[i];
size_t idx1 = route[i+1];
pathIdx.clear();
shortestPathFromGraph(connectionGraph, idx0, idx1, pathIdx);
if ( i==0 )
arrivalPathLength = pathIdx.size();
for (size_t j=1; j<pathIdx.size(); ++j)
_waypointsENU.push_back(vertices[pathIdx[j]]);
}
long returnPathLength = pathIdx.size();
for (long i=returnPathLength; i > 0; --i)
_returnPathIdx.push_back(_waypointsENU.size()-i);
for (long i=0; i < arrivalPathLength; ++i)
_arrivalPathIdx.push_back(i);
}
// Back transform waypoints.
......@@ -431,18 +479,6 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength)
_waypoints.push_back(GeoPoint2D{geoVertex[0], geoVertex[1]});
}
// Reconstruct arrival path;
int i = 0;
for (; route[i] >= n_t; ++i)
_arrivalPathIdx.push_back(i);
_arrivalPathIdx.push_back(i); // vertex of first transect
// Reconstruct return path;
i = route.size()-1;
for (; route[i] >= n_t; --i)
_returnPathIdx.push_back(i);
_returnPathIdx.push_back(i); // vertex of last transect
return true;
}
......@@ -450,7 +486,13 @@ bool FlightPlan::_generateTransects(double lineDistance, double minTransectLengt
{
_transects.clear();
if (_scenario.getTilesENU().size() != _progress.size()){
errorString = "Number of tiles is not equal to progress array length";
ostringstream strstream;
strstream << "Number of tiles ("
<< _scenario.getTilesENU().size()
<< ") is not equal to progress array length ("
<< _progress.size()
<< ")";
errorString = strstream.str();
return false;
}
......@@ -573,10 +615,14 @@ void FlightPlan::_generateRoutingModel(const BoostLineString &vertices,
FlightPlan::RoutingDataModel_t &dataModel,
Matrix<double> &graph)
{
#ifdef SHOW_TIME
auto start = std::chrono::high_resolution_clock::now();
#endif
graphFromPolygon(polygonOffset, vertices, graph);
#ifdef SHOW_TIME
auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now()-start);
cout << "Execution time graphFromPolygon(): " << delta.count() << " ms" << endl;
#endif
// cout << endl;
// for (size_t i=0; i<graph.size(); ++i){
// vector<double> &row = graph[i];
......@@ -587,10 +633,14 @@ void FlightPlan::_generateRoutingModel(const BoostLineString &vertices,
// }
// cout << endl;
Matrix<double> distanceMatrix(graph);
#ifdef SHOW_TIME
start = std::chrono::high_resolution_clock::now();
#endif
toDistanceMatrix(distanceMatrix);
#ifdef SHOW_TIME
delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now()-start);
cout << "Execution time toDistanceMatrix(): " << delta.count() << " ms" << endl;
#endif
dataModel.distanceMatrix.setDimension(n0, n0);
for (size_t i=0; i<n0; ++i){
......@@ -605,4 +655,23 @@ void FlightPlan::_generateRoutingModel(const BoostLineString &vertices,
dataModel.depot = n0-1;
}
Area::Area() : Area(GeoPoint2DList(), 0, 1, AreaType::MeasurementArea)
{
}
Area::Area(const GeoPoint2DList &gP, AreaType tp) : Area(gP, 0, 1, tp)
{
}
Area::Area(const GeoPoint2DList &gP, double alt, size_t l, AreaType tp) :
geoPolygon(gP)
, altitude(alt)
, layers(l)
, type(tp)
{
}
}
src/Snake/snake.h
View file @ 99b27028
......@@ -20,7 +20,12 @@ namespace snake {
enum AreaType {MeasurementArea, ServiceArea, Corridor};
struct Area {
class Area {
public:
Area();
Area(const GeoPoint2DList &gP, AreaType tp);
Area(const GeoPoint2DList &gP, double alt, size_t l, AreaType tp);
GeoPoint2DList geoPolygon;
double altitude;
size_t layers;
......@@ -51,6 +56,10 @@ namespace snake {
const min_bbox_rt &getMeasurementAreaBBoxENU() {return _mAreaBoundingBox;}
const BoostPoint &getHomePositonENU() {return _homePositionENU;}
const vector<GeoPoint3DList> &getTiles() {return _tiles;}
const vector<GeoPoint3D> &getTileCenterPoints() {return _tileCenterPoints;}
const GeoPoint3D &getHomePositon() {return _homePosition;}
bool defined(double tileWidth, double tileHeight, double minTileArea);
string errorString;
......@@ -78,8 +87,13 @@ namespace snake {
vector<BoostPolygon> _tilesENU;
BoostLineString _tileCenterPointsENU;
vector<GeoPoint3DList> _tiles;
vector<GeoPoint3D> _tileCenterPoints;
GeoPoint3D _geoOrigin;
BoostPoint _homePositionENU;
GeoPoint3D _homePosition;
};
//========================================================================================
......
src/Wima/WimaAreaData.h
View file @ 99b27028
......@@ -15,8 +15,6 @@ public:
Q_PROPERTY(QString type READ type CONSTANT)
WimaAreaData(QObject *parent = nullptr);
//WimaAreaData(const WimaAreaData &other, QObject *parent = nullptr);
//WimaAreaData(const WimaArea &other, QObject *parent = nullptr);
WimaAreaData& operator=(const WimaAreaData& otherData) = delete; // avoid slicing
double maxAltitude() const;
......@@ -25,8 +23,10 @@ public:
QList<QGeoCoordinate> coordinateList() const;
bool containsCoordinate(const QGeoCoordinate &coordinate) const;
void append(const QGeoCoordinate &c) {_path.push_back(QVariant::fromValue(c));}
void clear() {_path.clear();}
virtual QString type() const = 0;
virtual QString type() const { return "Default";}
signals:
void maxAltitudeChanged (double maxAltitude);
......
src/Wima/WimaController.cc
View file @ 99b27028
......@@ -91,11 +91,6 @@ WimaController::WimaController(QObject *parent)
connect(&_snakeWorker, &SnakeWorker::resultReady, this, &WimaController::_snakeStoreWorkerResults);
}
QmlObjectListModel* WimaController::visualItems()
{
return &_visualItems;
}
QStringList WimaController::loadNameFilters() const
{
QStringList filters;
......@@ -113,81 +108,6 @@ QStringList WimaController::saveNameFilters() const
return filters;
}
WimaDataContainer *WimaController::dataContainer()
{
return _container;
}
QmlObjectListModel *WimaController::missionItems()
{
return &_missionItems;
}
QmlObjectListModel *WimaController::currentMissionItems()
{
return &_currentMissionItems;
}
QVariantList WimaController::waypointPath()
{
return _waypointPath;
}
QVariantList WimaController::currentWaypointPath()
{
return _currentWaypointPath;
}
Fact *WimaController::enableWimaController()
{
return &_enableWimaController;
}
Fact *WimaController::overlapWaypoints()
{
return &_overlapWaypoints;
}
Fact *WimaController::maxWaypointsPerPhase()
{
return &_maxWaypointsPerPhase;
}
Fact *WimaController::showAllMissionItems()
{
return &_showAllMissionItems;
}
Fact *WimaController::showCurrentMissionItems()
{
return &_showCurrentMissionItems;
}
Fact *WimaController::flightSpeed()
{
return &_flightSpeed;
}
Fact *WimaController::arrivalReturnSpeed()
{
return &_arrivalReturnSpeed;
}
Fact *WimaController::altitude()
{
return &_altitude;
}
Fact *WimaController::reverse()
{
return &_reverse;
}
Fact *WimaController::enableSnake()
{
return &_enableSnake;
}
bool WimaController::uploadOverrideRequired() const
{
return _uploadOverrideRequired;
......@@ -218,11 +138,6 @@ bool WimaController::snakeCalcInProgress() const
return _snakeCalcInProgress;
}
Fact *WimaController::startWaypointIndex()
{
return &_nextPhaseStartWaypointIndex;
}
void WimaController::setMasterController(PlanMasterController *masterC)
{
_masterController = masterC;
......@@ -515,7 +430,7 @@ bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVa
*/
bool WimaController::_fetchContainerData()
{
// fetch only if valid, return true on sucess
// fetch only if valid, return true on success
// reset visual items
_visualItems.clear();
......@@ -523,11 +438,13 @@ bool WimaController::_fetchContainerData()
_currentMissionItems.clearAndDeleteContents();
_waypointPath.clear();
_currentWaypointPath.clear();
_snakeTiles.clearAndDeleteContents();
emit visualItemsChanged();
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit snakeTilesChanged();
_localPlanDataValid = false;
......@@ -643,11 +560,31 @@ bool WimaController::_fetchContainerData()
corridor.geoPolygon.push_back(GeoPoint2D{c.latitude(), c.longitude()});
}
corridor.type = AreaType::Corridor;
_scenario.addArea(mArea);
_scenario.addArea(sArea);
_scenario.addArea(corridor);
_verifyScenarioDefinedWithErrorMessage();
// Check if scenario is defined.
if ( !_verifyScenarioDefinedWithErrorMessage() )
return false;
// Get tiles.
const auto &tiles = _scenario.getTiles();
const auto &cps = _scenario.getTileCenterPoints();
for ( int i=0; i < int(tiles.size()); ++i ) {
const auto &tile = tiles[i];
WimaAreaData *QTile = new WimaAreaData(this);
for ( const auto &vertex : tile) {
QGeoCoordinate QVertex(vertex[0], vertex[1], vertex[2]);
QTile->append(QVertex);
}
const auto &centerPoint = cps[i];
QGeoCoordinate QCenterPoint(centerPoint[0], centerPoint[1], centerPoint[2]);
QTile->setCenter(QCenterPoint);
_snakeTiles.append(QTile);
}
emit visualItemsChanged();
emit missionItemsChanged();
......@@ -759,7 +696,7 @@ bool WimaController::_calcNextPhase()
// calculate path from last waypoint to home
QVector<QGeoCoordinate> returnPath;
if ( !calcShortestPath(CSWpList.last(), _takeoffLandPostion, returnPath) ) {
qWarning("WimaController::calcNextPhase(): Not able to calc path from home to first waypoint.");
qWarning("WimaController::calcNextPhase(): Not able to calc path from home to last waypoint.");
return false;
}
......@@ -1039,8 +976,8 @@ void WimaController::_eventTimerHandler()
_snakeWorker.setProgress(progress);
_snakeWorker.setLineDistance(_snakeLineDistance.rawValue().toDouble());
_snakeWorker.setMinTransectLength(_snakeMinTransectLength.rawValue().toDouble());
_snakeWorker.start();
_setSnakeCalcInProgress(true);
_snakeWorker.start();
}
}
}
......@@ -1316,8 +1253,10 @@ bool WimaController::_verifyScenarioDefinedWithErrorMessage()
return value;
}
void WimaController::_snakeStoreWorkerResults(const WorkerResult_t &r)
{
void WimaController::_snakeStoreWorkerResults()
{
_missionItems.clearAndDeleteContents();
const WorkerResult_t &r = _snakeWorker.getResult();
_setSnakeCalcInProgress(false);
if (!r.success) {
qgcApp()->showMessage(r.errorMessage);
......@@ -1330,40 +1269,37 @@ void WimaController::_snakeStoreWorkerResults(const WorkerResult_t &r)
QVector<MissionItem> missionItems;
missionItems.reserve(int(n));
unsigned long startIdx = r.returnPathIdx.last();
// Remove all items from mission controller.
_missionController->removeAll();
QmlObjectListModel* missionControllerVisualItems = _missionController->visualItems();
// Create QVector<QGeoCoordinate> containing all waypoints;
unsigned long startIdx = r.arrivalPathIdx.last();
unsigned long endIdx = r.returnPathIdx.first();
QObject parent;
int seqNum = 0;
QVector<QGeoCoordinate> waypoints;
for (unsigned long i = startIdx; i <= endIdx; ++i) {
QGeoCoordinate wp{r.waypoints[int(i)].value<QGeoCoordinate>()};
MissionItem item = new MissionItem(seqNum++,
MAV_CMD_NAV_WAYPOINT,
MAV_FRAME_GLOBAL_RELATIVE_ALT,
0, // Hold time (delay for hover and capture to settle vehicle before image is taken)
0.0, // No acceptance radius specified
0.0, // Pass through waypoint
std::numeric_limits<double>::quiet_NaN(), // Yaw unchanged
wp.latitude(),
wp.longitude(),
wp.altitude(),
true, // autoContinue
false, // isCurrentItem
&parent);
waypoints.append(wp);
}
// create mission items
_missionController->removeAll();
QmlObjectListModel* missionControllerVisualItems = _missionController->visualItems();
// create SimpleMissionItem by using _missionController
for ( int i = 0; i < missionItems.size(); i++) {
_missionController->insertSimpleMissionItem(missionItems[i], missionControllerVisualItems->count());
long insertIdx = missionControllerVisualItems->count();
for (auto wp : waypoints)
_missionController->insertSimpleMissionItem(wp, insertIdx++);
SimpleMissionItem *takeOffItem = missionControllerVisualItems->value<SimpleMissionItem*>(1);
if (takeOffItem == nullptr) {
qWarning("WimaController::_snakeStoreWorkerResults(): Nullptr at SimpleMissionItem!");
return;
}
takeOffItem->setCommand(MAV_CMD_NAV_WAYPOINT);
takeOffItem->setCoordinate(waypoints[0]);
// copy mission items from _missionController to _missionItems
cout.precision(10);
for ( int i = 1; i < missionControllerVisualItems->count(); i++) {
SimpleMissionItem *visualItem = qobject_cast<SimpleMissionItem *>((*missionControllerVisualItems)[i]);
if (visualItem == nullptr) {
qWarning("WimaController::fetchContainerData(): Nullptr at SimpleMissionItem!");
qWarning("WimaController::_snakeStoreWorkerResults(): Nullptr at SimpleMissionItem!");
return;
}
SimpleMissionItem *visualItemCopy = new SimpleMissionItem(*visualItem, true, this);
......@@ -1378,8 +1314,8 @@ void WimaController::_snakeStoreWorkerResults(const WorkerResult_t &r)
_nextPhaseStartWaypointIndex.setRawValue(reverse? _missionItems.count() : int(1));
connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
if(!_calcNextPhase())
return;
_calcNextPhase();
emit missionItemsChanged();
}
void WimaController::_loadCurrentMissionItemsFromBuffer()
......
src/Wima/WimaController.h
View file @ 99b27028
......@@ -31,7 +31,7 @@
#define SMART_RTL_MAX_ATTEMPTS 3 // times
#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms
#define EVENT_TIMER_INTERVAL 50 // ms
#define SNAKE_EVENT_LOOP_INTERVAL 50 // ms
#define SNAKE_EVENT_LOOP_INTERVAL 5000 // ms
using namespace snake;
......@@ -86,39 +86,43 @@ public:
Q_PROPERTY(Fact* snakeMinTileArea READ snakeMinTileArea CONSTANT)
Q_PROPERTY(Fact* snakeLineDistance READ snakeLineDistance CONSTANT)
Q_PROPERTY(Fact* snakeMinTransectLength READ snakeMinTransectLength CONSTANT)
Q_PROPERTY(QmlObjectListModel* snakeTiles READ snakeTiles NOTIFY snakeTilesChanged)
// Property accessors
PlanMasterController* masterController (void) { return _masterController; }
MissionController* missionController (void) { return _missionController; }
QmlObjectListModel* visualItems (void);
QString currentFile (void) const { return _currentFile; }
PlanMasterController* masterController (void) { return _masterController; }
MissionController* missionController (void) { return _missionController; }
QmlObjectListModel* visualItems (void) { return &_visualItems; }
QString currentFile (void) const { return _currentFile; }
QStringList loadNameFilters (void) const;
QStringList saveNameFilters (void) const;
QString fileExtension (void) const { return wimaFileExtension; }
QString fileExtension (void) const { return wimaFileExtension; }
QGCMapPolygon joinedArea (void) const;
WimaDataContainer* dataContainer (void);
QmlObjectListModel* missionItems (void);
QmlObjectListModel* currentMissionItems (void);
QVariantList waypointPath (void);
QVariantList currentWaypointPath (void);
Fact* enableWimaController (void);
Fact* overlapWaypoints (void);
Fact* maxWaypointsPerPhase (void);
Fact* startWaypointIndex (void);
Fact* showAllMissionItems (void);
Fact* showCurrentMissionItems(void);
Fact* flightSpeed (void);
Fact* arrivalReturnSpeed (void);
Fact* altitude (void);
Fact* reverse (void);
Fact* enableSnake (void);
Fact* snakeTileWidth (void) {return &_snakeTileWidth;}
Fact* snakeTileHeight (void) {return &_snakeTileHeight;}
Fact* snakeMinTileArea (void) {return &_snakeMinTileArea;}
Fact* snakeLineDistance (void) {return &_snakeLineDistance;}
Fact* snakeMinTransectLength (void) {return &_snakeMinTransectLength;}
WimaDataContainer* dataContainer (void) { return _container; }
QmlObjectListModel* missionItems (void) { return &_missionItems; }
QmlObjectListModel* currentMissionItems (void) { return &_currentMissionItems; }
QVariantList waypointPath (void) { return _waypointPath; }
QVariantList currentWaypointPath (void) { return _currentWaypointPath; }
Fact* enableWimaController (void) { return &_enableWimaController; }
Fact* overlapWaypoints (void) { return &_overlapWaypoints; }
Fact* maxWaypointsPerPhase (void) { return &_maxWaypointsPerPhase; }
Fact* startWaypointIndex (void) { return &_nextPhaseStartWaypointIndex; }
Fact* showAllMissionItems (void) { return &_showAllMissionItems; }
Fact* showCurrentMissionItems(void) { return &_showCurrentMissionItems; }
Fact* flightSpeed (void) { return &_flightSpeed; }
Fact* arrivalReturnSpeed (void) { return &_arrivalReturnSpeed; };
Fact* altitude (void) { return &_altitude; }
Fact* reverse (void) { return &_reverse; }
Fact* enableSnake (void) { return &_enableSnake; }
Fact* snakeTileWidth (void) { return &_snakeTileWidth;}
Fact* snakeTileHeight (void) { return &_snakeTileHeight;}
Fact* snakeMinTileArea (void) { return &_snakeMinTileArea;}
Fact* snakeLineDistance (void) { return &_snakeLineDistance;}
Fact* snakeMinTransectLength (void) { return &_snakeMinTransectLength;}
QmlObjectListModel* snakeTiles (void) { return &_snakeTiles;}
bool uploadOverrideRequired (void) const;
double phaseDistance (void) const;
double phaseDuration (void) const;
......@@ -208,6 +212,7 @@ signals:
void returnUserRequestConfirmRequired (void);
void snakeConnectionStatusChanged (void);
void snakeCalcInProgressChanged (void);
void snakeTilesChanged (void);
private:
enum SRTL_Reason {BatteryLow, UserRequest};
private slots:
......@@ -232,7 +237,7 @@ private slots:
void _setSnakeCalcInProgress (bool inProgress);
bool _verifyScenarioDefined (void);
bool _verifyScenarioDefinedWithErrorMessage (void);
void _snakeStoreWorkerResults (const WorkerResult_t &r);
void _snakeStoreWorkerResults ();
private:
......
src/Wima/WimaControllerDetail.cc
View file @ 99b27028
......@@ -16,9 +16,9 @@ void SnakeWorker::setScenario(const Scenario &scenario)
void SnakeWorker::setProgress(const QList<qint8> &progress)
{
_progress.clear();
for (auto p : progress) {
assert(p >= -1);
assert(p <= 100);
assert(p >= -1 && p <= 100);
_progress.push_back(p);
}
}
......@@ -35,15 +35,20 @@ void SnakeWorker::setMinTransectLength(double minTransectLength)
_minTransectLength = minTransectLength;
}
const WorkerResult_t &SnakeWorker::getResult() const
{
return _result;
}
void SnakeWorker::run()
{
WorkerResult_t result{QVariantList{},
QVector<unsigned long>{},
QVector<unsigned long>{},
false,
QString{}};
// Reset _result struct.
_result.success = false;
_result.errorMessage = "";
_result.waypoints.clear();
_result.returnPathIdx.clear();
_result.arrivalPathIdx.clear();
// partial success
FlightPlan flightplan;
flightplan.setScenario(_scenario);
flightplan.setProgress(_progress);
......@@ -52,28 +57,29 @@ void SnakeWorker::run()
if ( !flightplan.generate(_lineDistance, _minTransectLength) ){
// error
for (auto c : flightplan.errorString){
result.errorMessage.push_back(QChar(c));
_result.errorMessage.push_back(QChar(c));
}
} else {
//success!!!
_result.success = true;
// Fill result struct.
auto waypoints = flightplan.getWaypoints();
for (auto vertex : waypoints){
result.waypoints.append(QVariant::fromValue(QGeoCoordinate(vertex[0], vertex[1], 0)));
_result.waypoints.append(QVariant::fromValue(QGeoCoordinate(vertex[0], vertex[1], 0)));
}
auto arrivalPathIdx = flightplan.getArrivalPathIdx();
for (auto idx : arrivalPathIdx){
result.arrivalPathIdx.append(idx);
_result.arrivalPathIdx.append(idx);
}
auto returnPathIdx = flightplan.getReturnPathIdx();
for (auto idx : returnPathIdx){
result.returnPathIdx.append(idx);
_result.returnPathIdx.append(idx);
}
}
emit resultReady(result);
emit resultReady();
}
src/Wima/WimaControllerDetail.h
View file @ 99b27028
......@@ -34,8 +34,10 @@ public:
void setLineDistance (double lineDistance);
void setMinTransectLength (double minTransectLength);
const WorkerResult_t &getResult() const;
signals:
void resultReady(const WorkerResult_t &result);
void resultReady();
protected:
void run() override;
......@@ -45,5 +47,6 @@ private:
vector<int8_t> _progress;
double _lineDistance;
double _minTransectLength;
WorkerResult_t _result;
};
src/Wima/WimaPlaner.cc
View file @ 99b27028
......@@ -33,7 +33,7 @@ WimaPlaner::WimaPlaner(QObject *parent)
connect(&_updateTimer, &QTimer::timeout, this, &WimaPlaner::updateTimerSlot);
connect(this, &WimaPlaner::joinedAreaValidChanged, this, &WimaPlaner::updateMission);
_updateTimer.setInterval(500); // 250 ms means: max update time 2*250 ms
_updateTimer.setInterval(300); // 300 ms means: max update time 2*300 ms
_updateTimer.start();
// for debugging and testing purpose, remove if not needed anymore
......@@ -445,8 +445,10 @@ bool WimaPlaner::loadFromFile(const QString &filename)
}
//if (_circularSurvey == nullptr)
recalcJoinedArea();
updateMission();
if ( !recalcJoinedArea() )
return false;
if ( !updateMission() )
return false;
// remove temporary file
if ( !temporaryFile.remove() ){
......@@ -770,8 +772,9 @@ void WimaPlaner::updateTimerSlot()
// measurementArea
if (_measurementAreaChanging) {
if (_measurementArea.path() == _lastMeasurementAreaPath) { // is it still changing?
recalcJoinedArea();
calcArrivalAndReturnPath();
setReadyForSync(false);
if ( recalcJoinedArea() && calcArrivalAndReturnPath() )
setReadyForSync(true);
_measurementAreaChanging = false;
}
} else {
......@@ -782,8 +785,9 @@ void WimaPlaner::updateTimerSlot()
// corridor
if (_corridorChanging) {
if (_corridor.path() == _lastCorridorPath) { // is it still changing?
recalcJoinedArea();
calcArrivalAndReturnPath();
setReadyForSync(false);
if ( recalcJoinedArea() && calcArrivalAndReturnPath() )
setReadyForSync(true);
_corridorChanging = false;
}
} else {
......@@ -794,8 +798,9 @@ void WimaPlaner::updateTimerSlot()
// service area
if (_serviceAreaChanging) {
if (_serviceArea.path() == _lastServiceAreaPath) { // is it still changing?
recalcJoinedArea();
calcArrivalAndReturnPath();
setReadyForSync(false);
if ( recalcJoinedArea() && calcArrivalAndReturnPath() )
setReadyForSync(true);
_serviceAreaChanging = false;
}
} else {
......
src/comm/utilities.h
View file @ 99b27028
......@@ -48,13 +48,13 @@ public:
//! \return True if the event is ready to be executed, false either.
//!
//! This function must be called by a user provided timer interrupt handler. A internal counter increases with every call.
//! If timerInterval <= tickInerval*counter the counter gets reset and the function returns true. If the condition is not
//! If _tickInerval <= _timerInterval*_tickCount the counter gets reset and the function returns true. If the condition is not
//! met the function returns false.
//!
bool ready(void) {
assert(_isInitialized == true);
_tickCount++;
bool ready = _timerInterval <= _tickInerval*_tickCount;
bool ready = _tickInerval <= _timerInterval*_tickCount;
if (ready)
_tickCount = 0;
return ready;
......
src/main.cc
View file @ 99b27028
......@@ -148,7 +148,7 @@ int main(int argc, char *argv[])
// install the message handler
AppMessages::installHandler();
QList<QPair<QByteArray,QByteArray>
#ifdef Q_OS_MAC
#ifndef __ios__
// Prevent Apple's app nap from screwing us over
......@@ -253,8 +253,8 @@ QList<QPair<QByteArray,QByteArray>
// on in the code.
qRegisterMetaType<QList<QPair<QByteArray,QByteArray> > >();
// Snake
qRegisterMetaType<WorkerResult>("WorkerResult");
// Snake (Is this the right place for the following statement?)
//qRegisterMetaType<WorkerResult_t>("WorkerResult");
app->_initCommon();
//-- Initialize Cache System
......
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