diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h index 76d70c4..245f820 100644 --- a/lib/Navigation/navigation.h +++ b/lib/Navigation/navigation.h @@ -159,6 +159,7 @@ class Navigation { */ RouteInfo getRouteInfo() const { return this->route->getRouteInfo(); } Route* getRoute() const { return this->route; } + Point getCurrentPosition() const { return this->currentPosition; } /** * @brief Get azimuth diff --git a/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp b/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp index f4bfd27..ee97a38 100644 --- a/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp +++ b/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp @@ -12,23 +12,40 @@ void MenuCaptureRoute::printPage() const { RouteInfo routeInfo = this->captureRoute->getRouteInfo(); - uint8_t currentPage = this->getCurrentPage(); String lineOne = ""; String lineTwo = ""; - switch (currentPage) + switch (this->getCurrentPage()) { case 0: - lineOne = "Points: "; - lineOne.concat(routeInfo.totalPoints); - lineTwo = "You can drive."; + lineOne = "Capture Route"; + lineTwo = "You can drive"; break; case 1: - lineOne = "Average distance"; - lineTwo = "xxx meters"; + lineOne = "Saved waypoints"; + lineTwo.concat(routeInfo.totalPoints); break; + + case 2: + lineOne = "Distance to last"; + lineTwo = "point: "; + lineTwo.concat(this->captureRoute->getDistanceToLastPoint()); + break; + + case 3: { + NTRIPClientStates status = this->driveManager->getNavigation()->getNTRIPClient()->getClientState(); + lineOne = "NTRIP Client is"; + + if (status == NTRIPClientStates::pushData) + lineTwo = "enabled"; + else if (status == NTRIPClientStates::notAvailable) + lineTwo = "not available"; + else + lineTwo = "disabled"; + break; + } default: this->printDefault(); diff --git a/src/driveModi/Modi/CaptureRoute/captureRoute.cpp b/src/driveModi/Modi/CaptureRoute/captureRoute.cpp index a3ec413..3bf8b94 100644 --- a/src/driveModi/Modi/CaptureRoute/captureRoute.cpp +++ b/src/driveModi/Modi/CaptureRoute/captureRoute.cpp @@ -14,13 +14,18 @@ CaptureRoute::CaptureRoute(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation) : ManualControl(moveControl, input) { this->navigation = navigation; + this->navigation->getRoute()->clear(); this->routeInfo = navigation->getRouteInfo(); } +CaptureRoute::~CaptureRoute() { + this->navigation->getNTRIPClient()->setActivated(false); +} + void CaptureRoute::loop() { ManualControl::loop(); - if (millis() - this->lastMillis < delay) { + if (millis() - this->lastMillis < this->delay) { return; } this->runCaptureRoute(); @@ -28,14 +33,26 @@ void CaptureRoute::loop() { } void CaptureRoute::runCaptureRoute() { + this->checkNtrip(); + + if (this->state != NtripState::Enabled) + return; + if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)) { if (this->navigation->addCurrentPosToRoute()) { + this->lastSavedPoint = this->navigation->getCurrentPosition(); this->routeInfo = navigation->getRouteInfo(); this->updateDisplay = true; } } } +double CaptureRoute::getDistanceToLastPoint() { + if (!this->lastSavedPoint.isInit()) + return 0; + return this->lastSavedPoint.distanceTo(this->navigation->getCurrentPosition()); +} + bool CaptureRoute::shouldUpdate() { if (this->updateDisplay) { this->updateDisplay = false; @@ -43,3 +60,27 @@ bool CaptureRoute::shouldUpdate() { } return false; } + +void CaptureRoute::checkNtrip() { + NTRIPClient* client = this->navigation->getNTRIPClient(); + NTRIPClientStates state = client->getClientState(); + if (state == NTRIPClientStates::notAvailable) { + this->state = NtripState::NoNtrip; + } else if (state == NTRIPClientStates::pushData) { + this->ntripReconnectAttemps = 0; + this->state = NtripState::Enabled; + } else if (state ==NTRIPClientStates::wait) { + this->state = NtripState::Waiting; + if (millis() - this->reconnectNtripLastMillis < this->reconnectNtripDelayMillis) + return; + + if (this->ntripReconnectAttemps > this->ntripReconnectMaxAttemps) { + this->state = NtripState::NoNtrip; + return; + } + + this->reconnectNtripLastMillis = millis(); + client->setActivated(true); + this->ntripReconnectAttemps++; + } +} diff --git a/src/driveModi/Modi/CaptureRoute/captureRoute.h b/src/driveModi/Modi/CaptureRoute/captureRoute.h index e6fea7d..e622729 100644 --- a/src/driveModi/Modi/CaptureRoute/captureRoute.h +++ b/src/driveModi/Modi/CaptureRoute/captureRoute.h @@ -27,6 +27,12 @@ */ class CaptureRoute : public ManualControl { public: + enum NtripState { + NoNtrip, + Waiting, + Enabled + }; + /** * @brief Construct a new Capture Route object * @@ -35,6 +41,13 @@ class CaptureRoute : public ManualControl { */ CaptureRoute(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation); + /** + * @brief Destroy the Capture Route object + * + * Disconnect the NTRIP-Client + */ + ~CaptureRoute(); + /** * @brief Calls runCaptureRoute and ManualControl::loop * @@ -64,6 +77,13 @@ class CaptureRoute : public ManualControl { */ RouteInfo getRouteInfo() const { return this->routeInfo; } + /** + * @brief Get the distance to the last saved oint + * + * @return double in meters + */ + double getDistanceToLastPoint(); + /** * @brief Tells if there are new informations to display * @@ -72,11 +92,19 @@ class CaptureRoute : public ManualControl { */ bool shouldUpdate(); private: + void checkNtrip(); + Navigation* navigation; RouteInfo routeInfo; + Point lastSavedPoint; + NtripState state = NtripState::Waiting; - uint32_t lastMillis = 0; + uint8_t ntripReconnectAttemps = 0; + uint8_t ntripReconnectMaxAttemps = 10; + uint16_t reconnectNtripDelayMillis = 1000; uint16_t delay = 200; + uint32_t lastMillis = 0; + uint32_t reconnectNtripLastMillis = 0; bool updateDisplay = false; };