kleiax/Bachelorarbeit-Rover
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 1 Wiki Activity

Points are now saveable in captureRoute

Browse Source
This commit is contained in:
kleiax
2022-02-03 20:37:26 +01:00
parent bc37abcb66
commit b4a5814ef1
2 changed files with 18 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/Navigation/navigation.cpp
+14 -1
View File
@@ -78,9 +78,22 @@ CourseCorrection Navigation::getCourseCorrection() {
}
bool Navigation::addCurrentPosToRoute() {
Point p = currentLocation();
std::cout << "Navigation::addCurrentPosToRoute 1" << std::endl;
Point p = this->currentLocation();
std::cout << "Navigation::addCurrentPosToRoute " << p.lat << " " << p.lon << std::endl;
if (!p.isValid()) { return false; }
// First Point
if (this->route->getRouteInfo().totalPoints == 0) {
std::cout << "Navigation::addCurrentPosToRoute 2" << std::endl;
this->route->addPointToRoute(p);
this->lastPoint = p;
return true;
}
// Ever Point after the first
if (MIN_DISTANCE_BETWEEN_POINTS <= this->distanceBetwenn(p, this->lastPoint)) {
std::cout << "Navigation::addCurrentPosToRoute 3" << std::endl;
this->route->addPointToRoute(p);
this->lastPoint = p;
return true;