From 2e0b7f7f8d3cb4fc308ca964afe51b3cb56a6675 Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Wed, 9 Aug 2023 22:41:48 +0200 Subject: [PATCH] no time for comment --- autoVersionIncrement | 1 + include/Version.h | 4 +- lib/Navigation/navigation.cpp | 13 +++++- lib/Navigation/navigation.h | 2 + .../driveModi/Autopilot/menuAutopilot.cpp | 46 +++++++++++++++++-- src/driveModi/Modi/Autopilot/autopilot.cpp | 35 +++++++++++--- src/driveModi/Modi/Autopilot/autopilot.h | 4 ++ version | 2 +- 8 files changed, 91 insertions(+), 16 deletions(-) create mode 160000 autoVersionIncrement diff --git a/autoVersionIncrement b/autoVersionIncrement new file mode 160000 index 0000000..fd51b62 --- /dev/null +++ b/autoVersionIncrement @@ -0,0 +1 @@ +Subproject commit fd51b62f000d23d5649da611c2560af8e2327415 diff --git a/include/Version.h b/include/Version.h index 534eccd..5ed91a8 100644 --- a/include/Version.h +++ b/include/Version.h @@ -1,9 +1,9 @@ // AUTO GENERATED FILE, DO NOT EDIT #ifndef VERSION - #define VERSION "0.8.27" + #define VERSION "0.8.28" #endif #ifndef BUILD_TIMESTAMP - #define BUILD_TIMESTAMP "2023-08-09 10:33:06.900861" + #define BUILD_TIMESTAMP "2023-08-09 19:28:27.390516" #endif \ No newline at end of file diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp index 4a5ec70..7c962fc 100644 --- a/lib/Navigation/navigation.cpp +++ b/lib/Navigation/navigation.cpp @@ -252,7 +252,18 @@ void Navigation::updateMagneticDeclination() { int16_t Navigation::calculateCourseCorrection(Point& point) { int16_t targetCourse = point.courseTo(this->targetPoint); - int16_t correctionCourse = targetCourse - this->realAzimuth; + int16_t correctionCourse; + + if (this->calcAzimuthState == CalcAzimuthState::Good + || this->calcAzimuthState == CalcAzimuthState::Super) + { + correctionCourse = targetCourse - this->calcAzimuth; + this->lastUsedCalcAzimuth = true; + } else { + correctionCourse = targetCourse - this->realAzimuth; + this->lastUsedCalcAzimuth = false; + } + if (correctionCourse > 180) correctionCourse -= 360; else if (correctionCourse < -180) diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h index b44abae..b302bf7 100644 --- a/lib/Navigation/navigation.h +++ b/lib/Navigation/navigation.h @@ -196,6 +196,7 @@ class Navigation { QMC5883LCompass* getCompass() const { return this->compass; } int16_t getCalcAzimuth() const { return this->calcAzimuth; } CalcAzimuthState getCalcAzimuthState() const { return this->calcAzimuthState; } + bool getLastUsedCalcAzimuth() const { return this->lastUsedCalcAzimuth; } Point::Accuracy getMinAccuracy() const { return this->minAccuracy; } void setMinAccuracy(Point::Accuracy accuracy) { this->minAccuracy = accuracy; } @@ -239,6 +240,7 @@ class Navigation { bool isNtripInit = false; bool preventNextPoint = false; bool directionChangeMode = false; + bool lastUsedCalcAzimuth = false; char* host; char* mountPoint; diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp index b5dcd8b..269b195 100644 --- a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp +++ b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp @@ -197,10 +197,30 @@ void MenuAutopilot::printPage() const { } break; - default: - this->printDefault(); - return; - } + case 12: + lineOne = "Test rotate"; + lineTwo = "180 degree"; + break; + + case 13: + lineOne = "Test rotate"; + lineTwo = "45 degree"; + break; + + case 14: + lineOne = "Test rotate"; + lineTwo = "15 degree"; + break; + + case 15: + lineOne = "Test rotate"; + lineTwo = "5 degree"; + break; + + default: + this->printDefault(); + return; + } this->print(lineOne, lineTwo); } @@ -235,6 +255,22 @@ void MenuAutopilot::runCommand() const { else this->driveManager->getNavigation()->setMinAccuracy(Point::Accuracy::twoDigOfCM); break; + + case 12: + this->autopilot->rotate(180); + break; + + case 13: + this->autopilot->rotate(45); + break; + + case 14: + this->autopilot->rotate(15); + break; + + case 15: + this->autopilot->rotate(5); + break; default: break; @@ -250,7 +286,7 @@ void MenuAutopilot::update() { } void MenuAutopilot::init() { - this->setCountPages(12); + this->setCountPages(16); this->driveManager->changeModus(Modi::Autopilot); this->autopilot = (Autopilot*) this->driveManager->getDriveModiPtr(); this->routeInfo = this->autopilot->getRouteInfo(); diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp index e6a9e94..c36f362 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.cpp +++ b/src/driveModi/Modi/Autopilot/autopilot.cpp @@ -91,6 +91,11 @@ void Autopilot::runAutopilot() { this->selfDriving(); break; + case SelfDrivingRotate: + this->checkButtonInput(); + this->rotate(); + break; + case State::TargetReached: break; @@ -111,6 +116,11 @@ bool Autopilot::shouldUpdate() { return false; } +void Autopilot::rotate(int16_t degree) { + this->courseCorrection.correction = degree; + this->rotate(); +} + void Autopilot::init() { if (this->navigation->startNavigation()) this->state = State::NavigationStarted; @@ -135,12 +145,23 @@ void Autopilot::drive() { } void Autopilot::rotate() { - this->moveControl->setSpeed(0); - if (this->courseCorrection.correction > 0) - this->moveControl->setRotationSpeed(-this->rotationSpeed); - else - this->moveControl->setRotationSpeed(this->rotationSpeed); - this->navigation->drivingDirectionChange(); + if (this->state != State::SelfDrivingRotate) { + this->lastState = this->state; + this->state = State::SelfDrivingRotate; + this->rotationAimAzimuth = this->navigation->getAzimuth() + this->courseCorrection.correction; + + this->moveControl->setSpeed(0); + if (this->courseCorrection.correction > 0) + this->moveControl->setRotationSpeed(-this->rotationSpeed); + else + this->moveControl->setRotationSpeed(this->rotationSpeed); + } else { + if (abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < 3) { + this->state = this->lastState; + this->navigation->drivingDirectionChange(); + this->moveControl->setRotationSpeed(0); + } + } } void Autopilot::checkButtonInput() { @@ -149,7 +170,7 @@ void Autopilot::checkButtonInput() { if (this->state == State::SelfDrivingAvailable) this->state = State::SelfDriving; - else if (this->state == State::SelfDriving) + else if (this->state == State::SelfDriving || this->state == State::SelfDrivingRotate) this->state = State::SelfDrivingAvailable; else if (this->state == State::NavigationStarted) this->state = State::GetToStartPoint; diff --git a/src/driveModi/Modi/Autopilot/autopilot.h b/src/driveModi/Modi/Autopilot/autopilot.h index dfbf21c..d7bc259 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.h +++ b/src/driveModi/Modi/Autopilot/autopilot.h @@ -46,6 +46,7 @@ class Autopilot : public ManualControl { GetToStartPoint, SelfDrivingAvailable, SelfDriving, + SelfDrivingRotate, TargetReached }; @@ -106,6 +107,7 @@ class Autopilot : public ManualControl { * @return false */ bool shouldUpdate(); + void rotate(int16_t degree); private: void init(); @@ -134,6 +136,8 @@ class Autopilot : public ManualControl { uint32_t loopLastMillis = 0; uint32_t lastAutopilotChangeMillis = 0; + int16_t rotationAimAzimuth; + double drivingSpeed = 1; double rotationSpeed = 4.5; double minRemainingDistance = 0.25; diff --git a/version b/version index bdf3118..09318a8 100644 --- a/version +++ b/version @@ -1 +1 @@ -0.8.27 \ No newline at end of file +0.8.28 \ No newline at end of file