kleiax/Bachelorarbeit-Rover
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 1 Wiki Activity

delete everthing for bachelore

Browse Source
This commit is contained in:
kleiax
2023-09-02 18:34:06 +02:00
parent fd946f2aa3
commit d7a9e9cfd8
14 changed files with 0 additions and 2318 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/Navigation/navigation.cpp
-238
View File
@@ -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;
}
lib/Navigation/navigation.h
-219
View File
@@ -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 <Arduino.h>
#include <iostream>
#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
lib/NtripClient/ntripClient.cpp
-290
View File
@@ -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;
}
lib/NtripClient/ntripClient.h
-130
View File
@@ -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 <WiFiClient.h>
#include <Arduino.h>
#include <base64.h>
#include <iostream>
#include <SparkFun_u-blox_GNSS_Arduino_Library.h>
#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
lib/Route/route.cpp
-181
View File
@@ -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<int16_t>(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;
}
lib/Route/route.h
-237
View File
@@ -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 <cstdint>
#include <list>
#include <cmath>
#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<Point> points;
std::list<Point>::iterator it;
};
#endif // ROUTE_H
src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
-254
View File
@@ -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<int>(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();
}
src/SpecialMenus/driveModi/Autopilot/menuAutopilot.h
-59
View File
@@ -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
src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp
-138
View File
@@ -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();
}
src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h
-53
View File
@@ -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
src/driveModi/Modi/Autopilot/autopilot.cpp
-250
View File
@@ -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;
}
src/driveModi/Modi/Autopilot/autopilot.h
-134
View File
@@ -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
src/driveModi/Modi/CaptureRoute/captureRoute.cpp
-50
View File
@@ -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;
}
src/driveModi/Modi/CaptureRoute/captureRoute.h
-85
View File
@@ -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 <iostream>
#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