diff --git a/.vscode/settings.json b/.vscode/settings.json index ba00c90..3370206 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -125,6 +125,7 @@ "ardui", "blox", "bluedroid", + "carr", "CIPO", "COPI", "gast", @@ -144,6 +145,7 @@ "RHEDE", "Rtcm", "Schalke", + "Soln", "TPMOBIL", "UBLOX", "ZLPJ" diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h index d8cc34e..43c552d 100644 --- a/lib/Navigation/navigation.h +++ b/lib/Navigation/navigation.h @@ -127,7 +127,7 @@ class Navigation { bool startNavigation(); void freezeTargetPoint(bool val = true) { this->preventNextPoint = val; }; void drivingDirectionChange(); - void dissableCalcAzimuth() { this->directionChangeMode = false; } + void disableCalcAzimuth() { this->directionChangeMode = false; } double increaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint += 0.1; } double decreaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint -= 0.1; } diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp index c7f4f1f..3fce3a2 100644 --- a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp +++ b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp @@ -42,7 +42,7 @@ void MenuAutopilot::printPage() const { lineOne = "Status: "; lineTwo = ""; switch (this->autopilot->getState()) { - case Autopilot::State::InsufficientAccuarcy : + case Autopilot::State::InsufficientAccuracy : lineTwo = "Err: LowAccuracy"; break; @@ -75,15 +75,20 @@ void MenuAutopilot::printPage() const { break; default: - lineTwo = "UNKOWN"; + lineTwo = "UNKOWN - "; + lineTwo.concat(static_cast(this->autopilot->getState())); + break; } break; } case 1: - if (!navigationStarted) + if (!navigationStarted) { + lineOne = "Navigation is"; + lineTwo = "not started"; break; + } lineOne = "Distance: "; lineOne.concat(distanceString); lineTwo = "Turn: "; @@ -100,39 +105,75 @@ void MenuAutopilot::printPage() const { case 3: { NTRIPClientStates status = this->driveManager->getNavigation()->getNTRIPClient()->getClientState(); - lineOne = "NTRIP Client is"; + uint8_t carrSoln = gpsData->flags.bits.carrSoln; + lineOne = "GNSS: "; if (status == NTRIPClientStates::pushData) - lineTwo = "enabled"; + if (carrSoln == 0) + lineOne.concat("None"); + else if (carrSoln == 1) + lineOne.concat("Floating"); + else if (carrSoln == 2) + lineOne.concat("Fixed"); + else + lineOne.concat("UNKNOWN"); else if (status == NTRIPClientStates::notAvailable) - lineTwo = "not available"; + lineOne.concat("No WiFi"); else - lineTwo = "disabled"; + lineOne.concat("Offline"); + + lineTwo = "hAcc: "; + if (gpsData->fixType) + lineTwo.concat(gpsData->hAcc); + else + lineTwo.concat("0"); break; } - case 4: { - lineOne = "Carrier Solution"; - uint8_t carrSoln = gpsData->flags.bits.carrSoln; - if (carrSoln == 0) - lineTwo = "None"; - else if (carrSoln == 1) - lineTwo = "Floating"; - else if (carrSoln == 2) - lineTwo = "Fixed"; - else - lineTwo = "UNKNOWN"; + case 4: + if (this->driveManager->getNavigation()->getCalcAzimuthState() == Navigation::CalcAzimuthState::Good + ||this->driveManager->getNavigation()->getCalcAzimuthState() == Navigation::CalcAzimuthState::Super) + { + lineOne = "Calc Azi: "; + lineTwo = "State: "; + lineOne.concat(this->driveManager->getNavigation()->getCalcAzimuth()); + switch (this->driveManager->getNavigation()->getCalcAzimuthState()) { + case Navigation::CalcAzimuthState::Bad : + lineTwo.concat("Bad"); + break; + + case Navigation::CalcAzimuthState::Good : + lineTwo.concat("Good"); + break; + + case Navigation::CalcAzimuthState::Invalid : + lineTwo.concat("Invalid"); + break; + + case Navigation::CalcAzimuthState::Ok : + lineTwo.concat("Ok"); + break; + + case Navigation::CalcAzimuthState::Super : + lineTwo.concat("Super"); + break; + + default: + lineTwo.concat("Unkown"); + break; + } + } else { + lineOne = "Real Azi: "; + lineOne.concat(this->driveManager->getNavigation()->getAzimuth()); + } break; - } case 5: - lineOne = "hAccuracy: "; - lineTwo = "Azimuth: "; - lineTwo.concat(this->driveManager->getNavigation()->getAzimuth()); - if (gpsData->fixType) - lineOne.concat(gpsData->hAcc); + lineOne = "Loop mode is"; + if (this->autopilot->getLoopMode()) + lineTwo = "enabled"; else - lineOne.concat("0"); + lineTwo = "disabled"; break; case 6: @@ -170,37 +211,6 @@ void MenuAutopilot::printPage() const { lineTwo = "accuracy is low"; break; - case 11: - lineOne = "CalcAzi: "; - lineTwo = "State: "; - lineOne.concat(this->driveManager->getNavigation()->getCalcAzimuth()); - switch (this->driveManager->getNavigation()->getCalcAzimuthState()) { - case Navigation::CalcAzimuthState::Bad : - lineTwo.concat("Bad"); - break; - - case Navigation::CalcAzimuthState::Good : - lineTwo.concat("Good"); - break; - - case Navigation::CalcAzimuthState::Invalid : - lineTwo.concat("Invalid"); - break; - - case Navigation::CalcAzimuthState::Ok : - lineTwo.concat("Ok"); - break; - - case Navigation::CalcAzimuthState::Super : - lineTwo.concat("Super"); - break; - - default: - lineTwo.concat("Unkown"); - break; - } - break; - case 12: lineOne = "Test rotate"; lineTwo = "180 degree"; @@ -231,6 +241,10 @@ void MenuAutopilot::printPage() const { void MenuAutopilot::runCommand() const { switch (this->getCurrentPage()) { + case 5: + this->autopilot->switchLoopMode(); + break; + case 6: this->autopilot->restart(); break; diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp index 004138f..5e64608 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.cpp +++ b/src/driveModi/Modi/Autopilot/autopilot.cpp @@ -18,7 +18,7 @@ DirectionChangeSignal::DirectionChangeSignal(Navigation* navigation) { } DirectionChangeSignal::~DirectionChangeSignal() { - navigation->dissableCalcAzimuth(); + navigation->disableCalcAzimuth(); } void DirectionChangeSignal::action() { @@ -59,7 +59,7 @@ void Autopilot::loop() { void Autopilot::runAutopilot() { switch (this->state) { - case State::InsufficientAccuarcy: + case State::InsufficientAccuracy: this->askNavigationForOrder(); return; @@ -97,6 +97,8 @@ void Autopilot::runAutopilot() { break; case State::TargetReached: + if (this->loopMode) + this->restartLoop(); break; default: @@ -163,12 +165,12 @@ void Autopilot::beginRotate() { } void Autopilot::rotate() { - if (abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < 5) { + if (abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct) { this->endRotate(); return; } - if ((abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < 15) + if ((abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct * 3) && ((this->courseCorrection.correction > 0 && this->rotationAimAzimuth < this->navigation->getAzimuth()) @@ -217,18 +219,18 @@ void Autopilot::askNavigationForOrder() { case Navigation::Status::InsufficientAccuracy: this->lastState = this->state; - this->state = State::InsufficientAccuarcy; + this->state = State::InsufficientAccuracy; this->moveControl->setSpeed(0); this->moveControl->setRotationSpeed(0); break; case Navigation::Status::Unchanged: - if (this->state == State::InsufficientAccuarcy) + if (this->state == State::InsufficientAccuracy) this->state = this->lastState; break; case Navigation::Status::Updated: - if (this->state == State::InsufficientAccuarcy) + if (this->state == State::InsufficientAccuracy) this->state = this->lastState; break; @@ -241,8 +243,16 @@ void Autopilot::selfDriving() { if (this->state != State::SelfDriving) return; - if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBerforeAct) + if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBeforeAct) this->beginRotate(); else this->drive(); } + +void Autopilot::restartLoop() { + this->navigation->startNavigation(); + this->state = State::SelfDriving; + this->routeInfo = this->navigation->getRouteInfo(); + this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection, true); + this->updateDisplay = true; +} diff --git a/src/driveModi/Modi/Autopilot/autopilot.h b/src/driveModi/Modi/Autopilot/autopilot.h index 2fd53c5..b50bf45 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.h +++ b/src/driveModi/Modi/Autopilot/autopilot.h @@ -39,7 +39,7 @@ class DirectionChangeSignal : public DirectionChangeWrapper { class Autopilot : public ManualControl { public: enum State { - InsufficientAccuarcy = -2, + InsufficientAccuracy = -2, NoRoute = -1, None = 0, NavigationStarted, @@ -109,6 +109,8 @@ class Autopilot : public ManualControl { bool shouldUpdate(); void testRotate(int16_t degree); void endRotate(); + void switchLoopMode() { this->loopMode = !this->loopMode; } + bool getLoopMode() const { return this->loopMode; } private: void init(); @@ -119,6 +121,7 @@ class Autopilot : public ManualControl { void checkButtonInput(); void askNavigationForOrder(); void selfDriving(); + void restartLoop(); Navigation* navigation; CourseCorrection courseCorrection; @@ -129,9 +132,10 @@ class Autopilot : public ManualControl { DirectionChangeSignal* directionChangeSignal; bool updateDisplay = false; + bool loopMode = false; uint8_t loopDelayMillis = 40; - uint8_t maxCourseDeviationBerforeAct = 5; + uint8_t maxCourseDeviationBeforeAct = 5; uint16_t displayUpdateDelayMillis = 1000; uint16_t autopilotChangeDelayMillis = 500; uint32_t displayUpdateLastMillis = 0;