diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp deleted file mode 100644 index 5c505a4..0000000 --- a/lib/Navigation/navigation.cpp +++ /dev/null @@ -1,238 +0,0 @@ -/** - * @file navigation.cpp - * @author Alexander Klein (alex@kleiax.de) - * @brief Contains the implementation of the class Navigation - * @version 0.1 - * @date 2022-01-31 - * - * @copyright Copyright (c) 2022 - * - */ - -#include "navigation.h" - -bool Navigation::outputStatusPrintPVTdata = false; -bool Navigation::newData = false; -uint32_t Navigation::ubxUpdateTimeStatic = 0; -UBX_NAV_PVT_data_t* Navigation::ubxDataStatic = nullptr; - -Navigation::Navigation(Route* route) { - this->init(route); -} - -void Navigation::init(Route* route) { - - - if (route) - this->route = route; - else - this->route = new Route(); - - Component::loopDelay = Navigation::loopDelay; -} - -Navigation::~Navigation() { - delete this->route; - if (this->ntripClient) - delete this->ntripClient; -} - -void Navigation::initNtrip(String host, uint16_t port, String mountPoint, String user, String password) { - this->ntripClient = new NTRIPClient(this->gps, host.c_str(), port, mountPoint.c_str(), user.c_str(), password.c_str()); - this->ntripClient->gpsConfiguration(); - this->ntripClient->loop(); - this->ntripClient->setActivated(false); - this->isNtripInit = true; - this->addChildComponent(this->ntripClient); -} - -void Navigation::run() { - this->updateMagneticDeclination(); -} - - -void Navigation::newRoute() { - if (this->route) - delete this->route; - this->route = new Route(); -} - -bool Navigation::startNavigation() { - Point newTargetPoint = this->route->startRoute(); - this->navigationStarted = this->setTargetPoint(newTargetPoint); - if (this->navigationStarted) - this->navigationFinished = false; - return this->navigationStarted; -} - -void Navigation::drivingDirectionChange() { - Point tmp = this->currentPosition; - if (tmp.isInit() && tmp.isValid()) { - this->directionChangeMode = true; - this->lastPointDrivingDirectionChange = tmp; - this->calcAzimuthState = CalcAzimuthState::Invalid; - } -} - -Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction, bool forceUpdate) { - if (this->navigationFinished) - return Status::Complete; - - if (this->currentPosition.getAccuracy() <= this->minAccuracy) - return Status::InsufficientAccuracy; - - if (this->currentPosition.distanceTo(this->lastPointCalcCorrection) < (this->minDistanceToReachPoint / 2.0) - && !forceUpdate) { - correction.correction = this->calculateCourseCorrection(this->lastPointCalcCorrection); - correction.distance = this->lastPointCalcCorrection.distanceTo(this->targetPoint); - return Status::Unchanged; - } - - double distance = this->currentPosition.distanceTo(this->targetPoint); - - // Check if I need a new Point - if (distance < this->minDistanceToReachPoint && this->preventNextPoint == false) { - if (!this->nextPoint()) { - this->navigationFinished = true; - this->navigationStarted = false; - return Status::Complete; // End of navigation - } - distance = this->currentPosition.distanceTo(this->targetPoint); - } - - correction.correction = this->calculateCourseCorrection(this->currentPosition); - correction.distance = distance; - this->lastPointCalcCorrection = this->currentPosition; - return Status::Updated; -} - -Navigation::Status Navigation::addCurrentPosToRoute() { - if (this->currentPosition.getAccuracy() <= this->minAccuracy) - return Status::InsufficientAccuracy; - - // First Point - if (this->route->getRouteInfo().totalPoints == 0) { - this->route->addPointToRoute(this->currentPosition); - this->lastPointRouteInsert = this->currentPosition; - return Status::Updated; - } - - // Every Point after the first - double distance = this->currentPosition.distanceTo(this->lastPointRouteInsert); - if (Navigation::minDisBetweenPoints <= distance - && Navigation::maxDisBetweenPoints >= distance){ - this->route->addPointToRoute(this->currentPosition); - this->lastPointRouteInsert = this->currentPosition; - return Status::Updated; - } - return Status::Unchanged; -} - -void Navigation::updateCurrentLocation() { - if (Navigation::ubxUpdateTimeStatic == this->ubxUpdateTime) - return; - - this->ubxData = Navigation::ubxDataStatic; - this->ubxUpdateTime = Navigation::ubxUpdateTimeStatic; - - Point::Coordinates coords; - coords.lat = this->ubxData->lat / 10000000.0; - coords.lon = this->ubxData->lon / 10000000.0; - - this->currentPosition = Point(coords, this->ubxData->hAcc); -} - -void Navigation::updateMagneticDeclination() { - if (!this->directionChangeMode - || this->lastPointDrivingDirectionChange.distanceTo(this->currentPosition) < 1.0) - { - this->calcAzimuthState = CalcAzimuthState::Invalid; - this->calcAzimuth = 999; - return; - } - - this->calcAzimuth = this->lastPointDrivingDirectionChange.courseTo(this->currentPosition); - - // Map point accuracy to CalcAzimuthState - if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::oneDigOfCM - || this->currentPosition.getAccuracy() == Point::Accuracy::oneDigOfCM) - { - this->calcAzimuthState = CalcAzimuthState::Good; - } - else if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::twoDigOfCM - || this->currentPosition.getAccuracy() == Point::Accuracy::twoDigOfCM) - { - this->calcAzimuthState = CalcAzimuthState::Ok; - } - else if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::threeDigOfCM - || this->currentPosition.getAccuracy() == Point::Accuracy::threeDigOfCM) - { - this->calcAzimuthState = CalcAzimuthState::Bad; - } - else - { - this->calcAzimuthState = CalcAzimuthState::Invalid; - } - - // Upgrade quality if the range grows up - if (this->lastPointDrivingDirectionChange.distanceTo(this->currentPosition) > 2.0) { - switch (this->calcAzimuthState) { - case CalcAzimuthState::Bad : - this->calcAzimuthState = CalcAzimuthState::Ok; - break; - - case CalcAzimuthState::Ok : - this->calcAzimuthState = CalcAzimuthState::Good; - break; - - case CalcAzimuthState::Good : - this->calcAzimuthState = CalcAzimuthState::Super; - break; - - default: - break; - } - } - -} - -int16_t Navigation::calculateCourseCorrection(Point& point) { - int16_t targetCourse = point.courseTo(this->targetPoint); - 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; - } - - return Navigation::fixDegree(correctionCourse); -} - -bool Navigation::nextPoint() { - if (!this->navigationStarted) - return false; - return this->setTargetPoint(this->route->getNextPoint()); -} - -bool Navigation::setTargetPoint(Point target) { - if (target.isInit()) { - this->targetPoint = target; - return true; - } - return false; -} - -int16_t Navigation::fixDegree(int16_t degree) { - while (degree < -180 || degree > 180) { - if (degree > 180) - degree -= 360; - else if (degree < -180) - degree += 360; - } - return degree; -} diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h deleted file mode 100644 index 006e7da..0000000 --- a/lib/Navigation/navigation.h +++ /dev/null @@ -1,219 +0,0 @@ -/** - * @file navigation.h - * @author Alexander Klein (alex@kleiax.de) - * @brief Contains a class which navigate an object by the given route - * @version 0.1 - * @date 2022-01-10 - * - * @copyright Copyright (c) 2022 - * - */ - -#ifndef NAVIGATION_H -#define NAVIGATION_H - -#include -#include - -#include "route.h" -#include "NTRIPClient.h" - -#include "component.h" - -/** - * @brief This struct inherits the result of the navigation - * - * The drive get objects of this struct and should - * correct the direction in dependency on this. - * - */ -struct CourseCorrection { - int16_t correction; - double distance; -}; - -/** - * @brief This class navigate an object - * - * The class use the given Route and the gps device - * to tell the driver in which direction he have to - * be drive and the distance to the next checkpoint. - * - */ -class Navigation : public Component { - public: - enum Status { - InsufficientAccuracy, - Unchanged, - Updated, - Complete - }; - - /** - * @brief Construct a new Navigation object and using I2C - * - * @param route with which to navigate - */ - Navigation(Route* route = nullptr); - - /** - * @brief Destroy the Navigation object - * - */ - ~Navigation(); - - /** - * @brief - * - * @param host - * @param port - * @param mountPoint - * @param user - * @param password - */ - void initNtrip(String host, uint16_t port, String mountPoint, String user, String password); - - /** - * @brief creates a new empty route - * - */ - void newRoute(); - - /** - * @brief Tries to start the route - * - * For example the route can not be started - * if there are no Points or wrong Points. - * - * @return true route is started - * @return false route can not be started - */ - bool startNavigation(); - void freezeTargetPoint(bool val = true) { this->preventNextPoint = val; }; - void drivingDirectionChange(); - void disableCalcAzimuth() { this->directionChangeMode = false; } - - double increaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint += 0.1; } - double decreaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint -= 0.1; } - - // TODO: Dokumentation korrigieren. - /** - * @brief Get the Course Correction object - * - * This should be called by the driver to get new instructions. - * - * @param correction passed as refernce to get the data - * @return true if new correction data provided - * @return false if route is finished - */ - Status getCourseCorrection(CourseCorrection& correction, bool forceUpdate = false); - - - // TODO: Dokumentation korrigieren. - /** - * @brief Tries to add the current Position to the route - * - * This can be go wrong if there is no valid GPS signal - * - * @return true successful added point - * @return false no point added to route - */ - Status addCurrentPosToRoute(); - - /** - * @brief Get the Ubx Data object - * - * This struct includes the most Data from the GNSS-Module. - * - * @return UBX_NAV_PVT_data_t* - */ - UBX_NAV_PVT_data_t* getUbxData() { return this->ubxData; } - - /** - * @brief Returns the NTRIPClient object - * - * @return NTRIPClient* - */ - NTRIPClient* getNTRIPClient() { return this->ntripClient; } - - /** - * @brief Get the Route Info object - * - * This object contains information about the route. - * For example the stored points. - * - * @return RouteInfo - */ - RouteInfo getRouteInfo() const { return this->route->getRouteInfo(); } - Route* getRoute() const { return this->route; } - Point getCurrentPosition() const { return this->currentPosition; } - - /** - * @brief Get realAzimuth - * - * This value represents the angle between north and - * the line of sight. Clockwise. - * - * @return uint16_t degree - */ - int16_t getAzimuth() const { return this->realAzimuth; } - 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; } - - // map input in range from -180 to 180 degree - static int16_t fixDegree(int16_t degree); - - private: - void run() override; - void updateMagneticDeclination(); - void init(Route* route); - bool nextPoint(); - bool setTargetPoint(Point target); - int16_t calculateCourseCorrection(Point& point); - - static constexpr uint8_t loopDelay = 20; - static constexpr uint8_t maxDisBetweenPoints = 10; - static constexpr float minDisBetweenPoints = 0.3; - - - UBX_NAV_PVT_data_t* ubxData = nullptr; - Route* route = nullptr; - NTRIPClient* ntripClient = nullptr; - - - Point lastPointRouteInsert; - Point lastPointCalcCorrection; - Point lastPointDrivingDirectionChange; - Point targetPoint; - Point currentPosition; - Point::Accuracy minAccuracy = Point::Accuracy::twoDigOfCM; - - - bool navigationStarted = false; - bool navigationFinished = false; - bool isNtripInit = false; - bool preventNextPoint = false; - bool directionChangeMode = false; - bool lastUsedCalcAzimuth = false; - - char* host; - char* mountPoint; - char* user; - char* password; - - int16_t calcAzimuth = INT16_MAX; - - uint8_t timeToWait = 200; - uint16_t port; - uint32_t lastMillis = 0; - uint32_t ubxUpdateTime = 0; - - double minDistanceToReachPoint = 0.5; -}; - -#endif // NAVIGATION_H diff --git a/lib/NtripClient/ntripClient.cpp b/lib/NtripClient/ntripClient.cpp deleted file mode 100644 index 847f877..0000000 --- a/lib/NtripClient/ntripClient.cpp +++ /dev/null @@ -1,290 +0,0 @@ -/** - * @file NTRIPClient.cpp - * @author Alexander Klein (alex@kleiax.de) - * @brief Contains the implementation of the class NTRIPClient. - * @version 0.1 - * @date 2022-09-18 - * - * @copyright Copyright (c) 2022 - * - */ - -#include "ntripClient.h" - - -NTRIPClient::NTRIPClient(SFE_UBLOX_GNSS* gps, const char* host, uint16_t port, const char* mountPoint, const char* user, const char* password) { - this->gps = gps; - strcpy(this->host, host); - this->port = port; - strcpy(this->mountPoint, mountPoint); - strcpy(this->user, user); - strcpy(this->password, password); - - this->ntripClient = new WiFiClient; - this->state = NTRIPClientStates::closeConnection; - - this->loopDelay = 20; -} - -NTRIPClient::~NTRIPClient() { - delete this->ntripClient; -} - -void NTRIPClient::run() { - switch (this->state) { - case NTRIPClientStates::openConnection: - if (!this->activated) { - this->state = NTRIPClientStates::closeConnection; - break; - } - - std::cout << "Connecting to the NTRIP caster..." << std::endl; - if (this->beginClient()) { - std::cout << "Connected to the NTRIP caster!" << std::endl; - this->state = NTRIPClientStates::pushData; - } else { - std::cout << "Failed!" << std::endl; - this->state = NTRIPClientStates::wait; - this->activated = false; - } - break; - - case NTRIPClientStates::pushData: - if (!processConnection() || !this->activated) - this->state = NTRIPClientStates::closeConnection; - break; - - case NTRIPClientStates::closeConnection: - std::cout << "Closing the connection to the NTRIP caster..." << std::endl; - this->closeConnection(); - state = NTRIPClientStates::wait; - break; - - case NTRIPClientStates::wait: - if (this->activated) - this->state = NTRIPClientStates::openConnection; - else - this->checkAutoReconnect(); - break; - - case NTRIPClientStates::notAvailable: - break; - - default: - std::cout << "Wrong state in NTRIPClient.cpp..." << std::endl; - this->state = NTRIPClientStates::closeConnection; - break; - } -} - -void NTRIPClient::runAsChild() { - this->pushGPGGA(); -} - -void NTRIPClient::gpsConfiguration() { - this->gps->setSPIOutput(COM_TYPE_UBX | COM_TYPE_NMEA); - this->gps->setPortInput(COM_PORT_SPI, COM_TYPE_UBX | COM_TYPE_NMEA | COM_TYPE_RTCM3); - // Set the differential mode - ambiguities are fixed whenever possible - this->gps->setDGNSSConfiguration(SFE_UBLOX_DGNSS_MODE_FIXED); - this->gps->setMainTalkerID(SFE_UBLOX_MAIN_TALKER_ID_GP); - this->gps->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI, 10); -} - -bool NTRIPClient::setActivated(bool state) { - // std::cout << "NTRIPClient::setActivated: b - " << b << std::endl; - if (state && this->state != NTRIPClientStates::notAvailable) - this->activated = true; - else if (state) - return false; - else { - this->activated = false; - this->autoReconnect = false; - } - return true; -} - -void NTRIPClient::setAutoReconnect(bool state) { - if (state) { - this->reconnectAttemps = 0; - this->autoReconnect = true; - return; - } - - this->autoReconnect = false; -} - -bool NTRIPClient::beginClient() { - std::cout << "Opening socket to " << this->host << std::endl; - - char serverRequest[this->bufferSize]; - char credentials[this->bufferSize]; - - if (!this->ntripClient->connect(this->host, this->port)) { - std::cout << "Connection to caster failed" << std::endl; - return false; - } else { - std::cout << "Connected to " << this->host << " : " << this->port << std::endl; - std::cout << "Requesting NTRIP Data from mount point " << this->mountPoint << std::endl; - - // Generate the server request (GET) - snprintf(serverRequest, - this->bufferSize, - "GET /%s HTTP/1.0\r\nUser-Agent: NTRIP SparkFun u-blox Client v1.0\r\n", - this->mountPoint); - - // Credentials - uint8_t userCredentialsLength = strlen(this->user) + strlen(this->password) + 2; - char* userCredentials = new char[userCredentialsLength]; - snprintf(userCredentials, userCredentialsLength, "%s:%s", this->user, this->password); - - std::cout << "Sending credentials: " << userCredentials << std::endl; - - //Encode - base64 b; - String strEncodedCredentials = b.encode(userCredentials); - delete userCredentials; - char encodedCredentials[strEncodedCredentials.length() + 1]; - strEncodedCredentials.toCharArray(encodedCredentials, sizeof(encodedCredentials)); - - snprintf(credentials, sizeof(credentials), "Authorization: Basic %s\r\n", encodedCredentials); - } - - // Add the encoded credentials to the server request - strncat(serverRequest, credentials, this->bufferSize); - strncat(serverRequest, "\r\n", this->bufferSize); - - std::cout << "serverRequest size: " - << strlen(serverRequest) - << " of " - << this->bufferSize - << " bytes available" - << std::endl; - - // Send the server request - std::cout << "Sending server request: " << serverRequest << std::endl; - this->ntripClient->write(serverRequest, strlen(serverRequest)); - - //Wait up to 5 seconds for response - uint32_t lastMillis = millis(); - while (!ntripClient->available()) { - if (millis() - lastMillis > this->timeOut) { - std::cout << "Caster timed out!" << std::endl; - this->ntripClient->stop(); - return false; - } - delay(10); - } - - //Check reply - uint16_t httpStatusCode = 0; - char response[this->bufferSize]; - uint16_t responseIndex = 0; - while (this->ntripClient->available()) { - if (responseIndex == sizeof(response)) - break; - - response[responseIndex++] = ntripClient->read(); - - if (httpStatusCode == 0) { - if (strstr(response, "200") != nullptr) - httpStatusCode = 200; - if (strstr(response, "401") != nullptr) - httpStatusCode = 401; - } - } - response[responseIndex] = '\0'; - - // std::cout << "Caster response: " << response << std::endl; - - if (httpStatusCode != 200) { - std::cout << "Failed to connect to " << this->host << " - HTTP Code: " << (int) httpStatusCode - << " Length of Response: " << responseIndex << std::endl; - - if (httpStatusCode == 0) - std::cout << "Response: " << response << std::endl; - else if (httpStatusCode == 401) - std::cout << "Statuscode 401 - Unauthorized" << std::endl; - return false; - } - - std::cout << "Connected to: " << this->host << std::endl; - this->lastReceivedRtcmTime = millis(); - return true; -} - -void NTRIPClient::closeConnection() { - if (this->ntripClient->connected()) - this->ntripClient->stop(); - this->activated = false; - std::cout << "NtripClient disconnected from: " << this->host << std::endl; -} - -bool NTRIPClient::processConnection() { - if (this->ntripClient->connected()) { - uint8_t rtcmData[this->bufferSize * 8]; - uint16_t rtcmCount = 0; - - while (this->ntripClient->available()) { - rtcmData[rtcmCount++] = ntripClient->read(); - if (rtcmCount == sizeof(rtcmData)) - break; - } - - if (rtcmCount > 0) { - this->lastReceivedRtcmTime = millis(); - this->gps->pushRawData(rtcmData, rtcmCount); - // std::cout << "Pushed " << rtcmCount << " RTCM bytes to ZED." << std::endl; - } - } else { - std::cout << "Connection to " << this->host << " dropped!" << std::endl; - return false; - } - - if (millis() - this->lastReceivedRtcmTime > this->timeOut) { - std::cout << "RTCM timeout!" << std::endl; - return false; - } - - return true; -} - -void NTRIPClient::checkAutoReconnect() { - if (!this->autoReconnect) - return; - - if (millis() - this->lastReconnectTime < this->reconnectDelayTime) - return; - this->lastReconnectTime = millis(); - - if (this->reconnectAttemps >= this->maxReconnectAttemps) { - this->autoReconnect = false; - return; - } - - this->activated = true; - this->reconnectAttemps++; -} - -void NTRIPClient::pushGPGGA() { - if (!this->transmitLocation && !this->activated) - return; - - if (millis() - this->lastGPGGAPushTime < this->pushGPGGATime) - return; - this->lastGPGGAPushTime = millis(); - - if (!this->ntripClient->connected()) - std::cout << "Failed to pushing GGA to server: " << std::endl; - - NMEA_GGA_data_t *data = new NMEA_GGA_data_t; - uint8_t res = this->gps->getLatestNMEAGPGGA(data); - if (res == 2) - this->ntripClient->print((const char *)data); - delete data; -} - -bool NTRIPClient::isConnected() { - if (this->state == NTRIPClientStates::pushData) - return true; - return false; -} diff --git a/lib/NtripClient/ntripClient.h b/lib/NtripClient/ntripClient.h deleted file mode 100644 index 577bbfa..0000000 --- a/lib/NtripClient/ntripClient.h +++ /dev/null @@ -1,130 +0,0 @@ -/** - * @file ntripClient.h - * @author Alexander Klein (alex@kleiax.de) - * @brief Contains the class NTRIPClient - * @version 0.1 - * @date 2023-02-13 - * - * @copyright Copyright (c) 2023 - * - */ - -#ifndef NTRIP_CLIENT -#define NTRIP_CLIENT - -#include -#include -#include -#include -#include - -#include "debugTimes.h" -#include "component.h" - -/** - * @brief States for the state machine. - * - */ -enum NTRIPClientStates { - openConnection, - pushData, - closeConnection, - wait, - notAvailable -}; - -/** - * @brief Ntrip Client - * - * This class can connect to a ntrip server to pull correction - * data and push it to a given gnss module. This module have to be compatible - * with the SparkFun u-blox GNSS Arduino Library. - */ -class NTRIPClient : public Component { - public: - /** - * @brief Construct a new NTRIPClient object - * - * @param gps The Gnss module - * @param host - * @param port - * @param mountPoint - * @param user - * @param password - */ - NTRIPClient(SFE_UBLOX_GNSS* gps, const char* host, uint16_t port, const char* mountPoint, const char* user, const char* password); - ~NTRIPClient(); - - /** - * @brief Configure the Gnss module to accept correction data - * - */ - void gpsConfiguration(); - - /** - * @brief Activate or deactivate the location transmission - * - * Some server need the position of the Gnss module to send the - * right correction data. - * - * @param b - */ - void setTransmitLocation(bool b) { this->transmitLocation = b; } - - /** - * @brief Activate or deactivate the connection to the server. - * - * @param state - * @return true success - * @return false failure - */ - bool setActivated(bool state); - void setAutoReconnect(bool state); - - bool isConnected(); - - /** - * @brief Get the Client State object - * - * Returns the state of the State machine - * - * @return NTRIPClientStates - */ - NTRIPClientStates getClientState() { return this->state; } - - private: - void run() override; - void runAsChild() override; - void pushGPGGA(); - bool beginClient(); - void closeConnection(); - bool processConnection(); - void checkAutoReconnect(); - - SFE_UBLOX_GNSS* gps; - WiFiClient* ntripClient; - NTRIPClientStates state = NTRIPClientStates::notAvailable; - - bool transmitLocation = false; - bool activated = true; - bool autoReconnect = false; - - uint8_t reconnectAttemps = 0; - uint16_t port; - uint32_t lastReceivedRtcmTime = 0; - // uint32_t lastNtripConnectTime = 0; // can deleted? - uint32_t lastGPGGAPushTime = 0; - uint32_t lastReconnectTime = 0; - - char host[128]; - char mountPoint[128]; - char user[128]; - char password[128]; - const uint8_t maxReconnectAttemps = 10; - const uint16_t reconnectDelayTime = 1000; - const uint16_t timeOut = 10000; - const uint16_t bufferSize = 512; - const uint16_t pushGPGGATime = 10000; -}; - -#endif diff --git a/lib/Route/route.cpp b/lib/Route/route.cpp deleted file mode 100644 index a9039fe..0000000 --- a/lib/Route/route.cpp +++ /dev/null @@ -1,181 +0,0 @@ -/** - * @file route.cpp - * @author Alexander Klein (alex@kleiax.de) - * @brief Implements the class Route and Point - * @version 0.1 - * @date 2022-01-31 - * - * @copyright Copyright (c) 2022 - * - */ - -#include "route.h" - -Point::Point(double lat, double lon, uint32_t horizontalAccuracy, uint32_t creationTime) { - this->coordinates.lat = lat; - this->coordinates.lon = lon; - this->init(horizontalAccuracy, creationTime); -} - -Point::Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy, uint32_t creationTime) { - this->coordinates.lat = lat / 10000000.0; - this->coordinates.lon = lon / 10000000.0; - this->init(horizontalAccuracy, creationTime); -} - -Point::Point(Coordinates coords, uint32_t horizontalAccuracy, uint32_t creationTime) { - this->coordinates = coords; - this->init(horizontalAccuracy, creationTime); -} - -Point::Point(Coordinates coords, bool imported) { - this->coordinates = coords; - if (imported) - this->init(UINT32_MAX, 0); - else - this->init(0, 0); -} - -Point::Point() { - this->coordinates.lat = 0; - this->coordinates.lon = 0; - this->init(0, 0); -} - -bool Point::operator==(const Point& rhs) const { - return this->coordinates == rhs.getCoordinates(); -} - - // distance = sqrt(dx * dx + dy * dy) - // mit distance: Entfernung in km - // dx = 111.3 * cos(lat) * (lon1 - lon2) - // lat = (lat1 + lat2) / 2 * 0.01745 - // dy = 111.3 * (lat1 - lat2) - // lat1, lat2, lon1, lon2: Breite, Länge in Grad -double Point::distanceTo(const Coordinates& point) const { - Coordinates begin = this->coordinates; - Coordinates end = point; - - double lat = (begin.lat + end.lat) / 2 * ROUTE_DEGREE_TO_RADIANT; - double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (begin.lat - end.lat); - double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (begin.lon - end.lon); - - return sqrt(dx * dx + dy * dy); -} - -double Point::distanceTo(const Point &point) const { - return this->distanceTo(point.getCoordinates()); -} - -int16_t Point::courseTo(const Coordinates& point) const { - Coordinates begin = this->coordinates; - Coordinates end = point; - - double phi = log( tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) ); - double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT); - - int16_t res = static_cast(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1; - - // if (res < 0) - // res += 360; - - return res; -} - -int16_t Point::courseTo(const Point &point) const { - return this->courseTo(point.getCoordinates()); -} - -void Point::init(uint32_t horizontalAccuracy, uint32_t creationTime) { - this->creationTime = creationTime; - - if (horizontalAccuracy == UINT32_MAX) - this->accuracy = Accuracy::imported; - else if (horizontalAccuracy > 9999) - this->accuracy = Accuracy::fourDigOfCM; - else if (horizontalAccuracy > 999) - this->accuracy = Accuracy::threeDigOfCM; - else if (horizontalAccuracy > 99) - this->accuracy = Accuracy::twoDigOfCM; - else if (horizontalAccuracy > 1) - this->accuracy = Accuracy::oneDigOfCM; - else - this->accuracy = Accuracy::none; - -} - -Route::Route() { - -} - -void Route::addPointToRoute(Point point) { - this->points.push_back(point); -} - -void Route::clear() { - this->points.clear(); - this->currentPoint = 0; - this->started = false; -} - -Point Route::startRoute() { - if (this->points.size() < 1) { - this->started = false; - return Point(); - } - - this->it = this->points.begin(); - this->currentPoint = 1; - this->started = true; - return *this->it; -} - -Point Route::endRoute() { - if (this->points.size() < 1) { - this->started = false; - return Point(); - } - - this->it = this->points.end(); - // TODO: Understand why i have to decrement the iterator first to get realy the last element. - this->it--; - this->currentPoint = this->points.size(); - this->started = true; - return *this->it; -} - -Point Route::getNextPoint() { - if (!this->started) - return Point(); - - if (this->it != --this->points.end()) { - this->it++; - this->currentPoint++; - return *this->it; - // } else if (this->it == this->points.end() && this->currentPoint != this->points.size()) { - // this->currentPoint++; - // return *this->it; - } - - return Point(); -} - -Point Route::getPreviousPoint() { - if (!this->started) - return Point(); - - if (this->it != this->points.begin()) { - this->it--; - this->currentPoint--; - return *this->it; - } else { - return Point(); - } -} - -RouteInfo Route::getRouteInfo() { - RouteInfo info; - info.totalPoints = this->points.size(); - info.currentPoint = this->currentPoint; - return info; -} diff --git a/lib/Route/route.h b/lib/Route/route.h deleted file mode 100644 index 3aceff3..0000000 --- a/lib/Route/route.h +++ /dev/null @@ -1,237 +0,0 @@ -/** - * @file route.h - * @author Alexander Klein (alex@kleiax.de) - * @brief Contains the class Point and Route - * @version 0.1 - * @date 2022-01-31 - * - * @copyright Copyright (c) 2022 - * - */ - -#ifndef ROUTE_H -#define ROUTE_H - -#include -#include -#include - -#define ROUTE_DEGREE_TO_RADIANT 0.01745 -#define ROUTE_DISTANCE_BETWEEN_LATITUDE 111300 - -/** - * @brief A to handle points on the earth - * - * The points inherits latidue and longitude as doubles - * - */ -class Point{ - public: - /** - * @brief Hold the data longitude and latitude - * - */ - struct Coordinates { - double lon; - double lat; - - bool operator==(const Coordinates rhs) const { - return ( this->lon == rhs.lon ) && ( this->lon == rhs.lon ); - } - }; - - /** - * @brief The Accuracy is set by the constructor - * - */ - enum Accuracy { - none, - fourDigOfCM, - threeDigOfCM, - twoDigOfCM, - oneDigOfCM, - imported - }; - - - /** - * @brief Construct a new Point object - * - * @param lat latitude - * @param lon longitude - * @param horizontalAccuracy mm - * @param coords Coordinates - * @param imported if true than highest accuracy - */ - Point(double lat, double lon, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0); - Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0); - Point(Coordinates coords, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0); - Point(Coordinates coords, bool imported); - Point(); - - /** - * @brief Checks if to points are equal. - * - * @param rhs - * @return true - * @return false - */ - bool operator==(const Point& rhs) const; - - /** - * @brief Checks if the point is initalized. - * - * @return true - * @return false - */ - bool isInit() const { return this->coordinates.lat + this->coordinates.lon; } - - /** - * @brief Checks if the point is valid. - * - * If the accuracy is higher than zero, true will be returned. - * - * @return true - * @return false - */ - bool isValid() const { return (this->accuracy > 0) ? true : false; } - - /** - * @brief Calculates the distance between to points. - * - * @param point - * @return double meter - */ - double distanceTo(const Coordinates& point) const; - double distanceTo(const Point& point) const; - - /** - * @brief Calculates the course to an other point. - * - * @param point - * @return int16_t degree - */ - int16_t courseTo(const Coordinates& point) const; - int16_t courseTo(const Point& point) const; - - uint32_t getCreationTime() const { return this->creationTime; } - double getLongitude() const { return this->coordinates.lon; } - double getLatitude() const { return this->coordinates.lat; } - Coordinates getCoordinates() const { return this->coordinates; } - - /** - * @brief Get the Accuracy object - * - * The higher the value, the greater the accuracy. - * You can check it by Accuracy. - * - * @return Accuracy - */ - Accuracy getAccuracy() const { return this->accuracy; } - - private: - void init(uint32_t horizontalAccuracy, uint32_t creationTime); - - Accuracy accuracy = Accuracy::none; - Coordinates coordinates; - - uint32_t creationTime = 0; -}; - -/** - * @brief Holds some route information - * - */ -struct RouteInfo{ - /** - * @brief Selected number of Points - */ - uint16_t currentPoint; - - /** - * @brief Total points stored in route - */ - uint16_t totalPoints; -}; - -/** - * @brief A class to manage multiple points - * - * The list of point presents a route which can be driven - */ -class Route { - public: - /** - * @brief Construct a new Route object. - */ - Route(); - - /** - * @brief Adds a point to the list. - * - * @param point - */ - void addPointToRoute(Point point); - - /** - * @brief Delete all points. - * - */ - void clear(); - - /** - * @brief Select the first point as target. - * - * @return Point - */ - Point startRoute(); - - /** - * @brief Select the last point as target. - * - * @return Point - */ - Point endRoute(); - - /** - * @brief Get the next point and set it as target. - * - * @return Point is zero if there are no more Points. - */ - Point getNextPoint(); - - /** - * @brief Get the previous point and set it as target. - * - * @return Point - */ - Point getPreviousPoint(); - - /** - * @brief Get the Route Info object - * - * @return RouteInfo - */ - RouteInfo getRouteInfo(); - - /** - * @brief Get the Started object - * - * The Route will be marked as started when startRoute or - * endRoute has been called. - * - * @return true - * @return false - */ - bool getStarted() const { return this->started; } - - private: - uint16_t currentPoint = 0; - bool started = false; - - std::list points; - std::list::iterator it; - -}; - -#endif // ROUTE_H diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp deleted file mode 100644 index 523e532..0000000 --- a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp +++ /dev/null @@ -1,254 +0,0 @@ -/** - * @file menuAutopilot.cpp - * @author Alexander Klein (alex@kleiax.de) - * @brief Contains an implementation of the class MenuAutopilot - * @version 0.1 - * @date 2022-02-03 - * - * @copyright Copyright (c) 2022 - * - */ - -#include "menuAutopilot.h" - -void MenuAutopilot::printPage() const { - CourseCorrection correction = this->autopilot->getCourseCorrection(); - UBX_NAV_PVT_data_t* gpsData = this->driveManager->getNavigation()->getUbxData(); - bool navigationStarted = this->autopilot->getState() >= Autopilot::State::NavigationStarted; - - 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"); - } - } - - String lineOne = "No information"; - String lineTwo = "available"; - - switch (this->getCurrentPage()) { - case 0: { - lineOne = "Status: "; - lineTwo = ""; - switch (this->autopilot->getState()) { - case Autopilot::State::InsufficientAccuracy : - lineTwo = "Err: LowAccuracy"; - 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::SelfDrivingRotate : - lineTwo = "Rotating"; - break; - - case Autopilot::State::TargetReached : - lineTwo = "Target reached"; - break; - - default: - lineTwo = "UNKOWN - "; - lineTwo.concat(static_cast(this->autopilot->getState())); - - break; - } - break; - } - - case 1: - if (!navigationStarted) { - lineOne = "Navigation is"; - lineTwo = "not started"; - 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(); - uint8_t carrSoln = gpsData->flags.bits.carrSoln; - lineOne = "GNSS: "; - - if (status == NTRIPClientStates::pushData) - 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) - lineOne.concat("No WiFi"); - else - lineOne.concat("Offline"); - - lineTwo = "hAcc: "; - if (gpsData->fixType) - lineTwo.concat(gpsData->hAcc); - else - lineTwo.concat("0"); - break; - } - - 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: "; - lineTwo = ""; - lineOne.concat(this->driveManager->getNavigation()->getAzimuth()); - } - break; - - case 5: - lineOne = "Loop mode is"; - if (this->autopilot->getLoopMode()) - lineTwo = "enabled"; - else - lineTwo = "disabled"; - break; - - case 6: - lineOne = "Freeze target is"; - if (this->targetFreezed) - lineTwo = "activated"; - else - lineTwo = "deactivated"; - break; - - case 7: - lineOne = "MinDisToPoint:"; - lineTwo = "<- "; - lineTwo.concat(this->minDistance); - lineTwo.concat(" ->"); - break; - - default: - this->printDefault(); - return; - } - - this->print(lineOne, lineTwo); -} - -void MenuAutopilot::runCommand() const { - switch (this->getCurrentPage()) { - case 5: - this->autopilot->switchLoopMode(); - break; - - case 6: - if (this->targetFreezed) { - this->targetFreezed = false; - this->driveManager->getNavigation()->freezeTargetPoint(false); - } else { - this->targetFreezed = true; - this->driveManager->getNavigation()->freezeTargetPoint(); - } - break; - - case 7: - this->minDistance = this->driveManager->getNavigation()->increaseMinDistanceToReachPoint(); - break; - - default: - break; - } -} - -void MenuAutopilot::runCommandNo() const { - switch (this->getCurrentPage()) { - case 7: - this->minDistance = this->driveManager->getNavigation()->decreaseMinDistanceToReachPoint(); - break; - - default: - break; - } -} - -void MenuAutopilot::update() { - if (!this->autopilot->shouldUpdate()) - return; - - this->routeInfo = this->autopilot->getRouteInfo(); - this->printMenu(); -} - -void MenuAutopilot::init() { - this->setCountPages(8); - this->driveManager->changeModus(Modi::Autopilot); - this->autopilot = (Autopilot*) this->driveManager->getDriveModiPtr(); - this->routeInfo = this->autopilot->getRouteInfo(); - this->driveManager->getNavigation()->increaseMinDistanceToReachPoint(); - this->minDistance = this->driveManager->getNavigation()->decreaseMinDistanceToReachPoint(); -} diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.h b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.h deleted file mode 100644 index f662459..0000000 --- a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.h +++ /dev/null @@ -1,59 +0,0 @@ -/** - * @file menuAutopilot.h - * @author Alexander Klein (alex@kleiax.de) - * @brief Contains a class to print informations about the Autopilot - * @version 0.1 - * @date 2022-02-03 - * - * @copyright Copyright (c) 2022 - * - */ - -#ifndef MENU_AUTOPILOT_DRIVE_H -#define MENU_AUTOPILOT_DRIVE_H - -#include "SpecialMenus/driveModi/menuDriveMode.h" - -/** - * @brief A class to print informations about the Autopilot - * - */ -class MenuAutopilot : public MenuDriveMode { - public: - /** - * @brief Construct a new Menu Autopilot object - * - * @param driveManager for MenuDriveMode - */ - MenuAutopilot(DriveManager* driveManager) : MenuDriveMode(driveManager) {} - - /** - * @brief Prints the Information to display and console - * - * The informations are only printed to the display if it - * set. - * - * Changes the DriveModi to Autopilot if it is the first time called - * and save the Autopilot object. - */ - void printPage() const override; - - - /** - * @brief can be called to update shown data - */ - void update() override; - - private: - void init() override; - void runCommand() const override; - void runCommandNo() const override; - - bool mutable targetFreezed = false; - double mutable minDistance; - - Autopilot* autopilot; - RouteInfo routeInfo; -}; - -#endif // MENU_AUTOPILOT_DRIVE_H diff --git a/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp b/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp deleted file mode 100644 index 94da0a9..0000000 --- a/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp +++ /dev/null @@ -1,138 +0,0 @@ -/** - * @file menuCaptureRoute.cpp - * @author Alexander Klein (alex@kleiax.de) - * @brief Contains an implementation of the class MenuCaptureRoute - * @version 0.1 - * @date 2022-01-31 - * - * @copyright Copyright (c) 2022 - * - */ -#include "menuCaptureRoute.h" - -void MenuCaptureRoute::printPage() const { - RouteInfo routeInfo = this->captureRoute->getRouteInfo(); - UBX_NAV_PVT_data_t* gpsData = this->driveManager->getNavigation()->getUbxData(); - - String lineOne = ""; - String lineTwo = ""; - - switch (this->getCurrentPage()) { - case 0: - lineOne = "Capture Route"; - lineTwo = "You can drive"; - break; - - case 1: - lineOne = "Saved waypoints"; - lineTwo.concat(routeInfo.totalPoints); - break; - - case 2: - lineOne = "Last status:"; - switch (this->captureRoute->getLastStatus()) { - case Navigation::Status::InsufficientAccuracy: - lineTwo = "Poor Accuracy"; - break; - - case Navigation::Status::Updated: - lineTwo = "Point added"; - break; - - case Navigation::Status::Unchanged: - lineTwo = "Point too close"; - break; - - default: - lineTwo = "---"; - break; - } - break; - - case 3: - lineOne = "Distance to last"; - lineTwo = "point: "; - lineTwo.concat(this->captureRoute->getDistanceToLastPoint()); - break; - - case 4: { - 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 5: { - 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 6: - lineOne = "hAccuracy: "; - lineTwo = "Azimuth: "; - lineTwo.concat(this->driveManager->getNavigation()->getAzimuth()); - if (gpsData->fixType) - lineOne.concat(gpsData->hAcc); - else - lineOne.concat("0"); - break; - - case 7: - lineOne = "Current minimal"; - if (this->driveManager->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM) - lineTwo = "accuracy is high"; - else - lineTwo = "accuracy is low"; - break; - - default: - this->printDefault(); - return; - } - - this->print(lineOne, lineTwo); -} - -void MenuCaptureRoute::update() { - if (!this->captureRoute->shouldUpdate()) - return; - this->printMenu(); -} - -void MenuCaptureRoute::runCommand() const { - switch (this->getCurrentPage()) - { - case 7: - if (this->driveManager->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM) - this->driveManager->getNavigation()->setMinAccuracy(Point::Accuracy::none); - else - this->driveManager->getNavigation()->setMinAccuracy(Point::Accuracy::twoDigOfCM); - break; - - default: - break; - } -} - -void MenuCaptureRoute::init() { - this->setCountPages(8); - this->updateDelay = 500; - this->driveManager->changeModus(Modi::CaptureRoute); - this->captureRoute = (CaptureRoute*) this->driveManager->getDriveModiPtr(); - -} diff --git a/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h b/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h deleted file mode 100644 index 3f321ea..0000000 --- a/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h +++ /dev/null @@ -1,53 +0,0 @@ -/** - * @file menuCaptureRoute.h - * @author Alexander Klein (alex@kleiax.de) - * @brief Contains a class to print informations about CaptureRoute - * @version 0.1 - * @date 2022-01-31 - * - * @copyright Copyright (c) 2022 - * - */ -#ifndef MENU_CAPTURE_ROUTE_H -#define MENU_MANUAL_DRIVE_H - -#include "SpecialMenus/driveModi/menuDriveMode.h" - -/** - * @brief A class to print informations about the Autopilot - * - */ -class MenuCaptureRoute : public MenuDriveMode { - public: - /** - * @brief Construct a new Menu Capture Route object - * - * @param driveManager - */ - MenuCaptureRoute(DriveManager* driveManager) : MenuDriveMode(driveManager) {} - - /** - * @brief Prints the Information to display and console - * - * The informations are only printed to the display if it - * set. - * - * Changes the DriveModi to CaptureRoute if it is the first time called - * and save the CaptureRoute object. - */ - void printPage() const override; - - /** - * @brief can be called to update shown data - */ - void update() override; - - void runCommand() const override; - - private: - void init() override; - - CaptureRoute* captureRoute; -}; - -#endif // MENU_MANUAL_DRIVE_H diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp deleted file mode 100644 index 2b40228..0000000 --- a/src/driveModi/Modi/Autopilot/autopilot.cpp +++ /dev/null @@ -1,250 +0,0 @@ -/** - * @file autopilot.cpp - * @author Alexander Klein (alex@kleiax.de) - * @brief Contains the implementation of the class Autopilot - * @version 0.1 - * @date 2022-02-02 - * - * @copyright Copyright (c) 2022 - * - */ - -#include "driveModi/Modi/Autopilot/autopilot.h" -#include "autopilot.h" - -DirectionChangeSignal::DirectionChangeSignal(Navigation* navigation) { - this->navigation = navigation; - this->action(); -} - -DirectionChangeSignal::~DirectionChangeSignal() { - navigation->disableCalcAzimuth(); -} - -void DirectionChangeSignal::action() { - this->navigation->drivingDirectionChange(); -} - - -Autopilot::Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation) - : ManualControl(moveControl, input) { - this->setInputMode(ManualControl::InputMode::Digital); - this->directionChangeSignal = new DirectionChangeSignal(navigation); - this->setDirectionChangeCallback(this->directionChangeSignal); - this->navigation = navigation; - this->init(); -} - -Autopilot::~Autopilot() { - delete this->directionChangeSignal; - // this->navigation->getNTRIPClient()->setActivated(false); -} - -void Autopilot::run() { - this->routeInfo = this->navigation->getRouteInfo(); - - switch (this->state) { - case State::InsufficientAccuracy: - this->askNavigationForOrder(); - return; - - case State::NoRoute: - return; - - case State::None: - return; - - case State::NavigationStarted: - this->askNavigationForOrder(); - this->checkButtonInput(); - break; - - case State::GetToStartPoint: - ManualControl::run(); - this->askNavigationForOrder(); - if (this->routeInfo.currentPoint >= 2) - this->state = State::SelfDrivingAvailable; - break; - - case State::SelfDrivingAvailable: - ManualControl::run(); - this->askNavigationForOrder(); - this->checkButtonInput(); - break; - - case State::SelfDriving: - this->askNavigationForOrder(); - this->checkButtonInput(); - this->selfDriving(); - break; - - case SelfDrivingRotate: - this->checkButtonInput(); - this->rotate(); - break; - - case State::TargetReached: - if (this->loopMode) - this->restartLoop(); - break; - - default: - break; - } -} - -void Autopilot::restart() { - this->init(); -} - -bool Autopilot::shouldUpdate() { - if (this->updateDisplay) { - this->updateDisplay = false; - return true; - } - return false; -} - -void Autopilot::testRotate(int16_t degree) { - if (!degree) - return; - - this->courseCorrection.correction = degree; - this->beginRotate(); -} - -void Autopilot::init() { - if (this->navigation->startNavigation()) - this->state = State::NavigationStarted; - else - this->state = State::NoRoute; - this->routeInfo = this->navigation->getRouteInfo(); - this->navigation->getNTRIPClient()->setAutoReconnect(true); - - this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection, true); - - this->courseCorrection.correction = 0; - this->courseCorrection.distance = 0; - this->updateDisplay = true; - - this->maxRotationSpeed = 7; - this->maxForwardSpeed = 1.5; -} - -void Autopilot::drive() { - this->moveControl->setRotationSpeed(0); - if (this->courseCorrection.distance >= this->minRemainingDistance) - this->moveControl->setSpeed(this->maxForwardSpeed); - else - this,moveControl->setSpeed(0); -} - -void Autopilot::beginRotate() { - if (this->state != State::SelfDrivingRotate) { - this->lastState = this->state; - this->state = State::SelfDrivingRotate; - this->rotationAimAzimuth = this->navigation->getAzimuth() + this->courseCorrection.correction; - this->rotationAimAzimuth = Navigation::fixDegree(this->rotationAimAzimuth); - - this->moveControl->setSpeed(0); - if (this->courseCorrection.correction > 0) - this->moveControl->setRotationSpeed(-this->maxRotationSpeed); - else - this->moveControl->setRotationSpeed(this->maxRotationSpeed); - } -} - -void Autopilot::rotate() { - if (abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct) { - this->endRotate(); - return; - } - - if ((abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct * 3) - && - ((this->courseCorrection.correction > 0 - && this->rotationAimAzimuth < this->navigation->getAzimuth()) - || (this->courseCorrection.correction < 0 - && this->rotationAimAzimuth > this->navigation->getAzimuth()))) - { - this->endRotate(); - std::cout << "Autopilot::rotate: Rover rotated too far" << std::endl; - } -} - -void Autopilot::endRotate() { - if (this->state != State::SelfDrivingRotate) - return; - - this->state = this->lastState; - this->navigation->drivingDirectionChange(); - this->moveControl->setRotationSpeed(0); - this->moveControl->emergencyStop(); - this->moveControl->setDrivingStatus(MoveControl::Status::Drive); -} - -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; - else if (this->state == State::SelfDriving || this->state == State::SelfDrivingRotate) - this->state = State::SelfDrivingAvailable; - else if (this->state == State::NavigationStarted) - this->state = State::GetToStartPoint; - 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: - if (this->state == State::InsufficientAccuracy) - break; - this->lastState = this->state; - this->state = State::InsufficientAccuracy; - this->moveControl->setSpeed(0); - this->moveControl->setRotationSpeed(0); - break; - - case Navigation::Status::Unchanged: - if (this->state == State::InsufficientAccuracy) - this->state = this->lastState; - break; - - case Navigation::Status::Updated: - if (this->state == State::InsufficientAccuracy) - this->state = this->lastState; - break; - - default: - break; - } -} - -void Autopilot::selfDriving() { - if (this->state != State::SelfDriving) - return; - - 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 deleted file mode 100644 index c3697f6..0000000 --- a/src/driveModi/Modi/Autopilot/autopilot.h +++ /dev/null @@ -1,134 +0,0 @@ -/** - * @file autopilot.h - * @author Alexander Klein (alex@kleiax.de) - * @brief Contains a class which use the navigate class to drive automaticaly - * @version 0.1 - * @date 2022-02-02 - * - * @copyright Copyright (c) 2022 - * - */ - -#ifndef AUTOPILOT_H -#define AUTOPILOT_H - - -#include "navigation.h" -#include "moveControl.h" -#include "driveModi/Modi/ManualControl/manualControl.h" - -class DirectionChangeSignal : public DirectionChangeWrapper { - public: - DirectionChangeSignal(Navigation* navigation); - ~DirectionChangeSignal(); - void action() override; - - private: - Navigation* navigation; -}; - -/** - * @brief This class use the navigate class to drive automaticaly - * - * This class get the information from the navigate class. When an - * object of this class is constructed the rover can be driven manually. - * When the Rover is near to the first position of the Route, you can - * switch to automatic drive. - * - */ -class Autopilot : public ManualControl { - public: - enum State { - InsufficientAccuracy = -2, - NoRoute = -1, - None = 0, - NavigationStarted, - GetToStartPoint, - SelfDrivingAvailable, - SelfDriving, - SelfDrivingRotate, - TargetReached - }; - - /** - * @brief Construct a new Autopilot object - * - * @param moveControl for ManualControl - * @param navigation for route instructions - */ - Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation); - - /** - * @brief Destroy the Autopilot object - * - * Disconnect the NTRIP-Client - */ - ~Autopilot(); - - void restart(); - - /** - * @brief Get the Route Info object - * - * @return RouteInfo - */ - RouteInfo getRouteInfo() const { return this->routeInfo; } - - /** - * @brief Get the Course Correction object - * - * @return CourseCorrection - */ - CourseCorrection getCourseCorrection() const { return this->courseCorrection; } - - /** - * @brief Get the State object - * - * @return State - */ - State getState() const { return this->state; } - - /** - * @brief Tells if there are new informations to display - * - * @return true - * @return false - */ - bool shouldUpdate(); - void testRotate(int16_t degree); - void endRotate(); - void switchLoopMode() { this->loopMode = !this->loopMode; } - bool getLoopMode() const { return this->loopMode; } - - private: - void init(); - void drive(); - void beginRotate(); - void rotate(); - void run() override; - void checkButtonInput(); - void askNavigationForOrder(); - void selfDriving(); - void restartLoop(); - - Navigation* navigation; - CourseCorrection courseCorrection; - RouteInfo routeInfo; - State state = State::None; - State lastState = State::None; - Navigation::Status lastOrderStatus; - DirectionChangeSignal* directionChangeSignal; - - bool updateDisplay = false; - bool loopMode = false; - - uint8_t maxCourseDeviationBeforeAct = 5; - uint16_t autopilotChangeDelayMillis = 500; - uint32_t lastAutopilotChangeMillis = 0; - - int16_t rotationAimAzimuth; - - double minRemainingDistance = 0.25; -}; - -#endif // AUTOPILOT_H diff --git a/src/driveModi/Modi/CaptureRoute/captureRoute.cpp b/src/driveModi/Modi/CaptureRoute/captureRoute.cpp deleted file mode 100644 index b334e19..0000000 --- a/src/driveModi/Modi/CaptureRoute/captureRoute.cpp +++ /dev/null @@ -1,50 +0,0 @@ -/** - * @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->navigation->getNTRIPClient()->setAutoReconnect(true); - this->routeInfo = navigation->getRouteInfo(); -} - -CaptureRoute::~CaptureRoute() { - // this->navigation->getNTRIPClient()->setActivated(false); -} - -void CaptureRoute::run() { - ManualControl::run(); - if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)) { - this->status = this->navigation->addCurrentPosToRoute(); - if (this->status == Navigation::Status::Updated) { - this->lastSavedPoint = this->navigation->getCurrentPosition(); - this->routeInfo = navigation->getRouteInfo(); - this->updateDisplay = true; - } - } -} - -double CaptureRoute::getDistanceToLastPoint() const { - 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; -} diff --git a/src/driveModi/Modi/CaptureRoute/captureRoute.h b/src/driveModi/Modi/CaptureRoute/captureRoute.h deleted file mode 100644 index c262a98..0000000 --- a/src/driveModi/Modi/CaptureRoute/captureRoute.h +++ /dev/null @@ -1,85 +0,0 @@ -/** - * @file captureRoute.h - * @author Alexander Klein (alex@kleiax.de) - * @brief Contains a class to capture a driven route - * @version 0.1 - * @date 2022-02-15 - * - * @copyright Copyright (c) 2022 - * - */ - -#ifndef CAPTURE_ROUTE_H -#define CAPTURE_ROUTE_H - -#include - -#include "driveModi/Modi/ManualControl/manualControl.h" -#include "navigation.h" - -/** - * @brief A class to capture a driven class - * - * This class inherits ManualControl so you can drive - * normally as in ManualControl. If GPS signal is valid - * you can add a Point everytime you want. - * - */ -class CaptureRoute : public ManualControl { - public: - /** - * @brief Construct a new Capture Route object - * - * @param moveControl for ManualControl - * @param navigation to add Points - */ - CaptureRoute(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation); - - /** - * @brief Destroy the Capture Route object - * - * Disconnect the NTRIP-Client - */ - ~CaptureRoute(); - - // /** - // * @brief Get the Navigation object - // * - // * @return Navigation* - // */ - // Navigation* getNavigation() const { return this->navigation; } - - /** - * @brief Get the Route Info object - * - * @return RouteInfo - */ - RouteInfo getRouteInfo() const { return this->routeInfo; } - Navigation::Status getLastStatus() const { return this->status; } - - /** - * @brief Get the distance to the last saved oint - * - * @return double in meters - */ - double getDistanceToLastPoint() const; - - /** - * @brief Tells if there are new informations to display - * - * @return true - * @return false - */ - bool shouldUpdate(); - private: - void run() override; - - Navigation* navigation; - RouteInfo routeInfo; - Point lastSavedPoint; - Navigation::Status status = Navigation::Status::Complete; - - bool updateDisplay = false; -}; - -#endif // CAPTURE_ROUTE_H