- remove NtripReconnect from Autopilot and CaptureRoute
- shiftet moveControl States in the class namespace - added auto reconeect to NtripClient - a lot of refactor
This commit is contained in:
@@ -10,6 +10,7 @@
|
||||
*/
|
||||
|
||||
#include "driveModi/Modi/Autopilot/autopilot.h"
|
||||
#include "autopilot.h"
|
||||
|
||||
Autopilot::Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation)
|
||||
: ManualControl(moveControl, input) {
|
||||
@@ -18,65 +19,65 @@ Autopilot::Autopilot(MoveControl* moveControl, const ControlPadInput *input, Nav
|
||||
}
|
||||
|
||||
Autopilot::~Autopilot() {
|
||||
this->navigation->getNTRIPClient()->setActivated(false);
|
||||
// this->navigation->getNTRIPClient()->setActivated(false);
|
||||
}
|
||||
|
||||
void Autopilot::loop() {
|
||||
if (this->state < State::SelfDriving)
|
||||
ManualControl::loop();
|
||||
|
||||
if (millis() - this->loopLastMillis < loopDelayMillis)
|
||||
return;
|
||||
|
||||
this->runAutopilot();
|
||||
this->loopLastMillis = millis();
|
||||
}
|
||||
|
||||
void Autopilot::runAutopilot() {
|
||||
if (this->state < State::NavigationStarted)
|
||||
return;
|
||||
|
||||
if (!this->navigation->getCourseCorrection(this->courseCorrection)) {
|
||||
this->state = State::TargetReached;
|
||||
this->moveControl->setSpeed(0);
|
||||
this->moveControl->setRotationSpeed(0);
|
||||
this->updateDisplay = true;
|
||||
return;
|
||||
}
|
||||
|
||||
this->routeInfo = this->navigation->getRouteInfo();
|
||||
|
||||
if (millis() - this->displayUpdateLastMillis > this->displayUpdateDelayMillis) {
|
||||
this->updateDisplay = true;
|
||||
this->displayUpdateLastMillis = millis();
|
||||
}
|
||||
|
||||
this->checkNtrip();
|
||||
if (millis() - this->loopLastMillis < loopDelayMillis)
|
||||
return;
|
||||
|
||||
// Check if start Point is near to current Location
|
||||
if (this->routeInfo.currentPoint >= 2 && this->state == State::GetToStartPoint)
|
||||
this->state = State::SelfDrivingAvailable;
|
||||
this->routeInfo = this->navigation->getRouteInfo();
|
||||
this->runAutopilot();
|
||||
this->loopLastMillis = millis();
|
||||
}
|
||||
|
||||
if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)
|
||||
&& millis() - this->lastAutopilotChangeMillis > this->autopilotChangeDelayMillis) {
|
||||
void Autopilot::runAutopilot() {
|
||||
switch (this->state) {
|
||||
case State::InsufficientAccuarcy:
|
||||
this->askNavigationForOrder();
|
||||
return;
|
||||
|
||||
case State::NoRoute:
|
||||
return;
|
||||
|
||||
case State::None:
|
||||
return;
|
||||
|
||||
case State::NavigationStarted:
|
||||
this->askNavigationForOrder();
|
||||
break;
|
||||
|
||||
case State::GetToStartPoint:
|
||||
this->askNavigationForOrder();
|
||||
if (this->routeInfo.currentPoint >= 2)
|
||||
this->state = State::SelfDrivingAvailable;
|
||||
break;
|
||||
|
||||
case State::SelfDrivingAvailable:
|
||||
this->askNavigationForOrder();
|
||||
this->checkButtonInput();
|
||||
break;
|
||||
|
||||
case State::SelfDriving:
|
||||
this->askNavigationForOrder();
|
||||
this->checkButtonInput();
|
||||
this->selfDriving();
|
||||
break;
|
||||
|
||||
case State::TargetReached:
|
||||
break;
|
||||
|
||||
if (this->state == State::SelfDrivingAvailable) {
|
||||
this->state = State::SelfDriving;
|
||||
this->updateDisplay = true;
|
||||
this->lastAutopilotChangeMillis = millis();
|
||||
} else if (this->state == State::SelfDriving) {
|
||||
this->state = State::SelfDrivingAvailable;
|
||||
this->updateDisplay = true;
|
||||
this->lastAutopilotChangeMillis = millis();
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
if (this->state == State::SelfDriving) {
|
||||
if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBerforeAct)
|
||||
this->rotate();
|
||||
else
|
||||
this->drive();
|
||||
}
|
||||
}
|
||||
|
||||
void Autopilot::restart() {
|
||||
@@ -97,38 +98,15 @@ void Autopilot::init() {
|
||||
else
|
||||
this->state = State::NoRoute;
|
||||
this->routeInfo = this->navigation->getRouteInfo();
|
||||
this->navigation->getNTRIPClient()->setAutoReconnect(true);
|
||||
|
||||
this->lastState = this->navigation->getNTRIPClient()->getClientState();
|
||||
this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection);
|
||||
|
||||
this->courseCorrection.correction = 0;
|
||||
this->courseCorrection.distance = 0;
|
||||
this->updateDisplay = true;
|
||||
}
|
||||
|
||||
void Autopilot::checkNtrip() {
|
||||
NTRIPClient* client = this->navigation->getNTRIPClient();
|
||||
NTRIPClientStates state = client->getClientState();
|
||||
if (state == NTRIPClientStates::notAvailable) {
|
||||
this->state = State::NoNtrip;
|
||||
} else if (state == NTRIPClientStates::pushData) {
|
||||
this->ntripReconnectAttemps = 0;
|
||||
if (this->state < State::GetToStartPoint)
|
||||
this->state = State::GetToStartPoint;
|
||||
} else if (state ==NTRIPClientStates::wait) {
|
||||
if (millis() - this->reconnectNtripLastMillis < this->reconnectNtripDelayMillis)
|
||||
return;
|
||||
|
||||
if (this->ntripReconnectAttemps > this->ntripReconnectMaxAttemps) {
|
||||
this->state = State::NoNtrip;
|
||||
return;
|
||||
}
|
||||
|
||||
this->reconnectNtripLastMillis = millis();
|
||||
client->setActivated(true);
|
||||
this->ntripReconnectAttemps++;
|
||||
}
|
||||
}
|
||||
|
||||
void Autopilot::drive() {
|
||||
this->moveControl->setRotationSpeed(0);
|
||||
if (this->courseCorrection.distance >= this->minRemainingDistance)
|
||||
@@ -142,5 +120,62 @@ void Autopilot::rotate() {
|
||||
if (this->courseCorrection.correction > 0)
|
||||
this->moveControl->setRotationSpeed(this->rotationSpeed);
|
||||
else
|
||||
this->moveControl->setRotationSpeed(-this->rotationSpeed);
|
||||
|
||||
this->moveControl->setRotationSpeed(-this->rotationSpeed);
|
||||
}
|
||||
|
||||
void Autopilot::checkButtonInput() {
|
||||
if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)
|
||||
&& millis() - this->lastAutopilotChangeMillis > this->autopilotChangeDelayMillis) {
|
||||
|
||||
if (this->state == State::SelfDrivingAvailable) {
|
||||
this->state = State::SelfDriving;
|
||||
this->updateDisplay = true;
|
||||
this->lastAutopilotChangeMillis = millis();
|
||||
} else if (this->state == State::SelfDriving) {
|
||||
this->state = State::SelfDrivingAvailable;
|
||||
this->updateDisplay = true;
|
||||
this->lastAutopilotChangeMillis = millis();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Autopilot::askNavigationForOrder() {
|
||||
this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection);
|
||||
switch (this->lastOrderStatus) {
|
||||
case Navigation::Status::Complete:
|
||||
this->state = State::TargetReached;
|
||||
this->moveControl->setSpeed(0);
|
||||
this->moveControl->setRotationSpeed(0);
|
||||
break;
|
||||
|
||||
case Navigation::Status::InsufficientAccuracy:
|
||||
this->lastState = this->state;
|
||||
this->state = State::InsufficientAccuarcy;
|
||||
this->moveControl->setSpeed(0);
|
||||
this->moveControl->setRotationSpeed(0);
|
||||
break;
|
||||
|
||||
case Navigation::Status::Unchanged:
|
||||
if (this->state == State::InsufficientAccuarcy)
|
||||
this->state = this->lastState;
|
||||
break;
|
||||
|
||||
case Navigation::Status::Updated:
|
||||
if (this->state == State::InsufficientAccuarcy)
|
||||
this->state = this->lastState;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Autopilot::selfDriving() {
|
||||
if (this->lastOrderStatus == Navigation::Status::Updated) {
|
||||
if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBerforeAct)
|
||||
this->rotate();
|
||||
else
|
||||
this->drive();
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user