87 lines
2.5 KiB
C++
87 lines
2.5 KiB
C++
/**
|
|
* @file captureRoute.cpp
|
|
* @author Alexander Klein (alex@kleiax.de)
|
|
* @brief Contains the implementation of the class CaptureRoute
|
|
* @version 0.1
|
|
* @date 2022-02-15
|
|
*
|
|
* @copyright Copyright (c) 2022
|
|
*
|
|
*/
|
|
|
|
#include "driveModi/Modi/CaptureRoute/captureRoute.h"
|
|
|
|
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 < this->delay) {
|
|
return;
|
|
}
|
|
this->runCaptureRoute();
|
|
this->lastMillis = millis();
|
|
}
|
|
|
|
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;
|
|
return true;
|
|
}
|
|
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++;
|
|
}
|
|
}
|