delete everthing for bachelore
This commit is contained in:
@@ -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;
|
||||
}
|
||||
@@ -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
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user