diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp index d7a650b..dfa2eac 100644 --- a/lib/Navigation/navigation.cpp +++ b/lib/Navigation/navigation.cpp @@ -128,7 +128,7 @@ bool Navigation::getCourseCorrection(CourseCorrection& correction) { return false; // Check if I need a new Point - if (distance < MIN_DISTANCE_TO_REACH_POINT) { + if (distance < MIN_DISTANCE_TO_REACH_POINT && !this->preventNextPoint) { if (!this->nextPoint()) { this->navigationFinished = true; this->navigationStarted = false; diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h index 245f820..92d401b 100644 --- a/lib/Navigation/navigation.h +++ b/lib/Navigation/navigation.h @@ -108,6 +108,7 @@ class Navigation { * @return false route can not be started */ bool startNavigation(); + void freezeTargetPoint(bool val = true) { this->preventNextPoint = val; }; /** * @brief Get the Course Correction object @@ -218,6 +219,7 @@ class Navigation { bool navigationStarted = false; bool navigationFinished = false; bool isNtripInit = false; + bool preventNextPoint = false; char* host; char* mountPoint; diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp index 9f20e18..9ca1220 100644 --- a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp +++ b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp @@ -145,6 +145,14 @@ void MenuAutopilot::printPage() const { this->autopilot->restart(); break; + case 7: + lineOne = "Freeze target is"; + if (this->targetFreezed) + lineTwo = "activated"; + else + lineTwo = "deactivated"; + break; + default: this->printDefault(); return; @@ -158,6 +166,13 @@ void MenuAutopilot::runCommand() const { case 6: this->autopilot->restart(); break; + + case 7: + if (this->targetFreezed) + this->driveManager->getNavigation()->freezeTargetPoint(false); + else + this->driveManager->getNavigation()->freezeTargetPoint(); + break; default: break; @@ -173,7 +188,7 @@ void MenuAutopilot::update() { } void MenuAutopilot::init() { - this->setCountPages(7); + this->setCountPages(8); this->driveManager->changeModus(Modi::Autopilot); this->autopilot = (Autopilot*) this->driveManager->getDriveModiPtr(); this->routeInfo = this->autopilot->getRouteInfo(); diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.h b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.h index 0a3fee9..71c8ce3 100644 --- a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.h +++ b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.h @@ -47,6 +47,8 @@ class MenuAutopilot : public MenuDriveMode { private: void init() override; + bool targetFreezed = false; + Autopilot* autopilot; RouteInfo routeInfo; };