From 6874aed6ed4bfee15b3f8cfb4455168064af026e Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Thu, 11 May 2023 19:37:57 +0200 Subject: [PATCH] refactor all autopilot stuff --- lib/Navigation/navigation.cpp | 30 ++-- lib/Navigation/navigation.h | 6 +- .../driveModi/Autopilot/menuAutopilot.cpp | 163 +++++++++++++----- src/driveModi/Modi/Autopilot/autopilot.cpp | 73 +++++--- src/driveModi/Modi/Autopilot/autopilot.h | 61 +++---- 5 files changed, 217 insertions(+), 116 deletions(-) diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp index eb8b0c5..caf9def 100644 --- a/lib/Navigation/navigation.cpp +++ b/lib/Navigation/navigation.cpp @@ -116,40 +116,40 @@ bool Navigation::startNavigation() { return this->navigationStarted; } -CourseCorrection Navigation::getCourseCorrection() { +bool Navigation::getCourseCorrection(CourseCorrection& correction) { //TODO: Check accuracy befor make something - CourseCorrection courseCorrection = {0, 0}; + correction.correction = 0; + correction.distance = 0; int16_t targetCourse = this->currentPosition.courseTo(this->targetPoint); double distance = this->currentPosition.distanceTo(this->targetPoint); if (distance < 0 || this->navigationFinished) - return courseCorrection; + return false; // Check if I need a new Point if (distance < MIN_DISTANCE_TO_REACH_POINT) { - bool goOn = this->nextPoint(); - if (!goOn) { + if (!this->nextPoint()) { this->navigationFinished = true; this->navigationStarted = false; - return courseCorrection; // End of navigation + return false; // End of navigation } } // correction = targetCourse - currentCourse int16_t signedAzimuth = this->azimuth > 180 ? this->azimuth - 360 : this->azimuth; - int16_t correction = targetCourse - signedAzimuth; - if (correction > 180) - correction -= 360; - else if (correction < -180) - correction += 360; + int16_t correctionCourse = targetCourse - signedAzimuth; + if (correctionCourse > 180) + correctionCourse -= 360; + else if (correctionCourse < -180) + correctionCourse += 360; // //Rotate result by 180° to corrigate Azimuth - // correction = correction > 180 ? correction -360 : correction; + // correctionCourse = correctionCourse > 180 ? correctionCourse -360 : correctionCourse; - courseCorrection.correction = correction; - courseCorrection.distance = distance; - return courseCorrection; + correction.correction = correctionCourse; + correction.distance = distance; + return true; } bool Navigation::addCurrentPosToRoute() { diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h index 4609573..76d70c4 100644 --- a/lib/Navigation/navigation.h +++ b/lib/Navigation/navigation.h @@ -114,9 +114,11 @@ class Navigation { * * This should be called by the driver to get new instructions. * - * @return CourseCorrection + * @param correction passed as refernce to get the data + * @return true if new correction data provided + * @return false if route is finished */ - CourseCorrection getCourseCorrection(); + bool getCourseCorrection(CourseCorrection& correction); /** * @brief Tries to add the current Position to the route diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp index 3cad3e6..7a9b22a 100644 --- a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp +++ b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp @@ -12,52 +12,131 @@ #include "menuAutopilot.h" void MenuAutopilot::printPage() const { - char buf1[17]; - char buf2[17]; - char unit[3]; - - bool navigationStarted = this->autopilot->getNavigationStarted(); - bool navigationEnded = this->autopilot->getNavigationEnded(); - bool selfDriving = this->autopilot->getSelfDriving(); - bool selfDrivingAvailable = this->autopilot->getSelfDrivingAvailable(); - - // 999m -360° CourseCorrection correction = this->autopilot->getCourseCorrection(); - uint16_t distance = 0; + UBX_NAV_PVT_data_t* gpsData = this->driveManager->getNavigation()->getUbxData(); + bool navigationStarted = this->autopilot->getState() >= Autopilot::State::NavigationStarted; - if (correction.distance < 1) { - distance = (uint16_t) (correction.distance * 100); - sprintf(unit, "cm"); - } else if (correction.distance < 1000) { - distance = (uint16_t) correction.distance; - sprintf(unit, "m"); - } else { - distance = (uint16_t) (correction.distance / 1000); - sprintf(unit, "km"); + String distanceString = ""; + if (navigationStarted) { + uint16_t distance = 0; + if (correction.distance < 1) { + distance = (uint16_t) (correction.distance * 100); + distanceString.concat(distance); + distanceString.concat("cm"); + } else if (correction.distance < 1000) { + distance = (uint16_t) correction.distance; + distanceString.concat(distance); + distanceString.concat("m"); + } else { + distance = (uint16_t) (correction.distance / 1000); + distanceString.concat(distance); + distanceString.concat("km"); + } } - if (navigationStarted) { - sprintf(buf1 ,"Points %u/%u",this->routeInfo.currentPoint ,this->routeInfo.totalPoints); - if (selfDriving) { - sprintf(buf2, "Auto: %u%s %d", distance, unit, correction.correction); - } else { - if (selfDrivingAvailable) { - sprintf(buf2, "Self -> Auto"); - } else { - sprintf(buf2, "Self: %u%s %d", distance, unit, correction.correction); - } - } - } else { - if (navigationEnded) { - sprintf(buf1, "Am Ziel"); - sprintf(buf2, "angekommen."); - } else { - sprintf(buf1, "Keine Route"); - sprintf(buf2, "vorhanden."); - } - } + String lineOne = "No information"; + String lineTwo = "available"; - this->print(buf1, buf2); + switch (this->getCurrentPage()) { + case 0: { + lineOne = "Status: "; + lineTwo = ""; + switch (this->autopilot->getState()) { + case Autopilot::State::NoNtrip : + lineTwo = "Err: No NTRIP"; + break; + + case Autopilot::State::NoRoute : + lineTwo = "Err: No route"; + break; + + case Autopilot::State::NavigationStarted : + lineTwo = "Nav started"; + break; + + case Autopilot::State::GetToStartPoint : + lineTwo = "Drive to start"; + break; + + case Autopilot::State::SelfDrivingAvailable : + lineTwo = "Autopilot ready"; + break; + + case Autopilot::State::SelfDriving : + lineTwo = "Autopilot active"; + break; + + case Autopilot::State::TargetReached : + lineTwo = "Target reached"; + break; + + default: + lineTwo = "UNKOWN"; + break; + } + break; + } + + case 1: + if (!navigationStarted) + break; + lineOne = "Distance: "; + lineOne.concat(distanceString); + lineTwo = "Turn: "; + lineTwo.concat(correction.correction); + break; + + case 2: + lineOne = "Target waypoint"; + lineTwo = ""; + lineTwo.concat(this->autopilot->getRouteInfo().currentPoint); + lineTwo.concat(" from "); + lineTwo.concat(this->autopilot->getRouteInfo().totalPoints); + 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; + } + + 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"; + break; + } + + case 5: + lineOne = "hAccuracy: "; + lineTwo = "Azimuth: "; + lineTwo.concat(this->driveManager->getNavigation()->getAzimuth()); + if (gpsData->fixType) + lineOne.concat(gpsData->hAcc); + else + lineOne.concat("0"); + break; + + default: + this->printDefault(); + return; + } + + this->print(lineOne, lineTwo); } void MenuAutopilot::update() { @@ -69,7 +148,7 @@ void MenuAutopilot::update() { } void MenuAutopilot::init() { - this->setCountPages(4); + this->setCountPages(6); 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 0fdb4d7..37d50c0 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.cpp +++ b/src/driveModi/Modi/Autopilot/autopilot.cpp @@ -14,15 +14,25 @@ Autopilot::Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation) : ManualControl(moveControl, input) { this->navigation = navigation; - this->navigationStarted = this->navigation->startNavigation(); + if (this->navigation->startNavigation()) + this->state = State::NavigationStarted; + else + this->state = State::NoRoute; this->routeInfo = this->navigation->getRouteInfo(); + + this->lastState = this->navigation->getNTRIPClient()->getClientState(); + this->courseCorrection.correction = 0; this->courseCorrection.distance = 0; this->updateDisplay = true; } +Autopilot::~Autopilot() { + this->navigation->getNTRIPClient()->setActivated(false); +} + void Autopilot::loop() { - if (!this->selfDriving && this->navigationStarted) + if (this->state < State::SelfDriving) ManualControl::loop(); if (millis() - this->loopLastMillis < loopDelayMillis) @@ -33,10 +43,16 @@ void Autopilot::loop() { } void Autopilot::runAutopilot() { - if (!this->navigationStarted || this->navigationEnded) + if (this->state < State::NavigationStarted) return; - this->courseCorrection = this->navigation->getCourseCorrection(); + if (!this->navigation->getCourseCorrection(this->courseCorrection)) { + this->state = State::TargetReached; + this->moveControl->setSpeed(0); + this->moveControl->setRotationSpeed(0); + this->updateDisplay = true; + } + this->routeInfo = this->navigation->getRouteInfo(); if (millis() - this->displayUpdateLastMillis > this->displayUpdateDelayMillis) { @@ -44,23 +60,19 @@ void Autopilot::runAutopilot() { this->displayUpdateLastMillis = millis(); } + this->checkNtrip(); + // Check if start Point is near to current Location - if (this->routeInfo.currentPoint >= 2) - this->selfDrivingAvailable = true; + if (this->routeInfo.currentPoint >= 2 && this->state == State::GetToStartPoint) + this->state = State::SelfDrivingAvailable; - if (!this->selfDriving - && this->selfDrivingAvailable - && ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)) - this->setSelfDriving(true); - - if (this->routeInfo.currentPoint == this->routeInfo.totalPoints) { - this->setSelfDriving(false); - this->navigationEnded = true; - this->navigationStarted = false; - this->selfDrivingAvailable = false; + if (this->state == State::SelfDrivingAvailable + && ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)) { + this->state = State::SelfDriving; + this->updateDisplay = true; } - if (selfDriving) { + if (this->state == State::SelfDriving) { this->setSpeedInRelToDistance(this->courseCorrection.distance); this->setRotInRelToDistance(this->courseCorrection.correction); } @@ -74,15 +86,28 @@ bool Autopilot::shouldUpdate() { return false; } -void Autopilot::setSelfDriving(bool val) { - if (!this->navigationStarted) - return; +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; - this->updateDisplay = true; + if (this->ntripReconnectAttemps > this->ntripReconnectMaxAttemps) { + this->state = State::NoNtrip; + return; + } - this->selfDriving = val; - this->moveControl->setSpeed(0); - this->moveControl->setRotationSpeed(0); + this->reconnectNtripLastMillis = millis(); + client->setActivated(true); + this->ntripReconnectAttemps++; + } } void Autopilot::setSpeedInRelToDistance(double distance) { diff --git a/src/driveModi/Modi/Autopilot/autopilot.h b/src/driveModi/Modi/Autopilot/autopilot.h index b9d8998..43b23e6 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.h +++ b/src/driveModi/Modi/Autopilot/autopilot.h @@ -29,6 +29,17 @@ */ class Autopilot : public ManualControl { public: + enum State { + NoNtrip = -2, + NoRoute = -1, + None = 0, + NavigationStarted, + GetToStartPoint, + SelfDrivingAvailable, + SelfDriving, + TargetReached + }; + /** * @brief Construct a new Autopilot object * @@ -37,6 +48,13 @@ class Autopilot : public ManualControl { */ Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation); + /** + * @brief Destroy the Autopilot object + * + * Disconnect the NTRIP-Client + */ + ~Autopilot(); + /** * @brief Calls runAutopilot or ManualControl loop * @@ -73,36 +91,11 @@ class Autopilot : public ManualControl { CourseCorrection getCourseCorrection() const { return this->courseCorrection; } /** - * @brief Get the Navigation Started status + * @brief Get the State object * - * @return true - * @return false + * @return State */ - bool getNavigationStarted() const { return this->navigationStarted; } - - /** - * @brief Get the Navigation Ended status - * - * @return true - * @return false - */ - bool getNavigationEnded() const { return this->navigationEnded; } - - /** - * @brief Get the Self Driving status - * - * @return true - * @return false - */ - bool getSelfDriving() const { return this->selfDriving; } - - /** - * @brief Get the Self Driving Available status - * - * @return true - * @return false - */ - bool getSelfDrivingAvailable() const { return this->selfDrivingAvailable; } + State getState() const { return this->state; } /** * @brief Tells if there are new informations to display @@ -113,24 +106,26 @@ class Autopilot : public ManualControl { bool shouldUpdate(); private: - void setSelfDriving(bool val); + void checkNtrip(); void setSpeedInRelToDistance(double distance); void setRotInRelToDistance(int16_t course); Navigation* navigation; CourseCorrection courseCorrection; RouteInfo routeInfo; + State state = State::None; + NTRIPClientStates lastState; - bool navigationStarted = false; - bool navigationEnded = false; - bool selfDriving = false; - bool selfDrivingAvailable = false; bool updateDisplay = false; uint8_t loopDelayMillis = 40; + uint8_t ntripReconnectAttemps = 0; + uint8_t ntripReconnectMaxAttemps = 10; uint16_t displayUpdateDelayMillis = 1000; + uint16_t reconnectNtripDelayMillis = 1000; uint32_t displayUpdateLastMillis = 0; uint32_t loopLastMillis = 0; + uint32_t reconnectNtripLastMillis = 0; }; #endif // AUTOPILOT_H