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

- remove NtripReconnect from Autopilot and CaptureRoute

Browse Source 
- shiftet moveControl States in the class namespace
- added auto reconeect to NtripClient
- a lot of refactor
This commit is contained in:
kleiax
2023-05-30 23:40:05 +02:00
parent 98b65c4aec
commit 22eb6788c3
18 changed files with 369 additions and 280 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/Navigation/navigation.cpp
+40 -38
View File
@@ -124,69 +124,54 @@ bool Navigation::startNavigation() {
return this->navigationStarted;
}
bool Navigation::getCourseCorrection(CourseCorrection& correction) {
//TODO: Check accuracy befor make something
correction.correction = 0;
correction.distance = 0;
Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction) {
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))
return Status::Unchanged;
int16_t targetCourse = this->currentPosition.courseTo(this->targetPoint);
double distance = this->currentPosition.distanceTo(this->targetPoint);
if (distance < 0 || this->navigationFinished)
return false;
// Check if I need a new Point
if (distance < this->minDistanceToReachPoint && this->preventNextPoint == false) {
if (!this->nextPoint()) {
this->navigationFinished = true;
this->navigationStarted = false;
return false; // End of navigation
return Status::Complete; // End of navigation
}
distance = this->currentPosition.distanceTo(this->targetPoint);
}
// correction = targetCourse - currentCourse
int16_t signedAzimuth = this->azimuth > 180 ? this->azimuth - 360 : this->azimuth;
int16_t correctionCourse = targetCourse - signedAzimuth;
if (correctionCourse > 180)
correctionCourse -= 360;
else if (correctionCourse < -180)
correctionCourse += 360;
//Rotate result by 180° to corrigate Azimuth
correctionCourse += 180;
correctionCourse = correctionCourse > 180 ? correctionCourse -360 : correctionCourse;
correction.correction = correctionCourse;
correction.correction = this->calculateCourseCorrection();
correction.distance = distance;
return true;
this->lastPointCalcCorrection = this->currentPosition;
return Status::Updated;
}
bool Navigation::addCurrentPosToRoute() {
if (!this->currentPosition.isValid()) { return false; }
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->lastPoint = this->currentPosition;
return true;
this->lastPointRouteInsert = this->currentPosition;
return Status::Updated;
}
// Every Point after the first
if (MIN_DISTANCE_BETWEEN_POINTS <= this->currentPosition.distanceTo(this->lastPoint)) {
if (MIN_DISTANCE_BETWEEN_POINTS <= this->currentPosition.distanceTo(this->lastPointRouteInsert)) {
this->route->addPointToRoute(this->currentPosition);
this->lastPoint = this->currentPosition;
return true;
this->lastPointRouteInsert = this->currentPosition;
return Status::Updated;
}
return false;
return Status::Unchanged;
}
// bool Navigation::setPointBeforeTurn() {
// if (millis() - this->currentPosition.getCreationTime() > 1000)
// return false;
// this->beforeTurnPoint = this->currentPosition;
// return true;
// }
void Navigation::updateCurrentLocation() {
if (Navigation::ubxUpdateTimeStatic == this->ubxUpdateTime)
return;
@@ -201,6 +186,23 @@ void Navigation::updateCurrentLocation() {
this->currentPosition = Point(coords, this->ubxData->hAcc);
}
int16_t Navigation::calculateCourseCorrection() {
int16_t targetCourse = this->currentPosition.courseTo(this->targetPoint);
// correction = targetCourse - currentCourse
int16_t signedAzimuth = this->azimuth > 180 ? this->azimuth - 360 : this->azimuth;
int16_t correctionCourse = targetCourse - signedAzimuth;
if (correctionCourse > 180)
correctionCourse -= 360;
else if (correctionCourse < -180)
correctionCourse += 360;
//Rotate result by 180° to corrigate Azimuth
correctionCourse += 180;
correctionCourse = correctionCourse > 180 ? correctionCourse -360 : correctionCourse;
return correctionCourse;
}
bool Navigation::nextPoint() {
if (!this->navigationStarted)
return false;
lib/Navigation/navigation.h
+15 -5
View File
@@ -29,7 +29,7 @@
* around the half accuracy of the positioning system
*
*/
#define MIN_DISTANCE_BETWEEN_POINTS 0.1
#define MIN_DISTANCE_BETWEEN_POINTS 0.3
#define MIN_DISTANCE_TO_REACH_POINT 0.5
#define AZIMUTH_UPDATE_DELAY 20
@@ -55,6 +55,14 @@ struct CourseCorrection {
*/
class Navigation {
public:
enum Status {
InsufficientAccuracy,
Unchanged,
Updated,
Complete
};
/**
* @brief Construct a new Navigation object and using I2C
*
@@ -123,7 +131,7 @@ class Navigation {
* @return true if new correction data provided
* @return false if route is finished
*/
bool getCourseCorrection(CourseCorrection& correction);
Status getCourseCorrection(CourseCorrection& correction);
/**
* @brief Tries to add the current Position to the route
@@ -133,7 +141,7 @@ class Navigation {
* @return true successful added point
* @return false no point added to route
*/
bool addCurrentPosToRoute();
Status addCurrentPosToRoute();
// TODO: can be deleted?
// bool setPointBeforeTurn();
@@ -189,6 +197,7 @@ class Navigation {
private:
void updateCurrentLocation();
int16_t calculateCourseCorrection();
/**
* @brief Set the next point as target
@@ -215,10 +224,11 @@ class Navigation {
NTRIPClient* ntripClient = nullptr;
QMC5883LCompass* compass;
Point lastPoint;
Point lastPointRouteInsert;
Point lastPointCalcCorrection;
Point targetPoint;
Point currentPosition;
Point beforeTurnPoint;
Point::Accuracy minAccuracy = Point::Accuracy::twoDigOfCM;
bool navigationStarted = false;
lib/Navigation/route.cpp
+6 -6
View File
@@ -90,17 +90,17 @@ void Point::init(uint32_t horizontalAccuracy) {
this->creationTime = millis();
if (horizontalAccuracy == UINT32_MAX)
this->accuracy = PointAccuracy::imported;
this->accuracy = Accuracy::imported;
else if (horizontalAccuracy > 9999)
this->accuracy = PointAccuracy::fourDigOfCM;
this->accuracy = Accuracy::fourDigOfCM;
else if (horizontalAccuracy > 999)
this->accuracy = PointAccuracy::threeDigOfCM;
this->accuracy = Accuracy::threeDigOfCM;
else if (horizontalAccuracy > 99)
this->accuracy = PointAccuracy::twoDigOfCM;
this->accuracy = Accuracy::twoDigOfCM;
else if (horizontalAccuracy > 1)
this->accuracy = PointAccuracy::oneDigOfCM;
this->accuracy = Accuracy::oneDigOfCM;
else
this->accuracy = PointAccuracy::none;
this->accuracy = Accuracy::none;
}
lib/Navigation/route.h
+5 -5
View File
@@ -47,7 +47,7 @@ class Point{
* @brief The Accuracy is set by the constructor
*
*/
enum PointAccuracy {
enum Accuracy {
none,
fourDigOfCM,
threeDigOfCM,
@@ -126,16 +126,16 @@ class Point{
* @brief Get the Accuracy object
*
* The higher the value, the greater the accuracy.
* You can check it by PointAccuracy.
* You can check it by Accuracy.
*
* @return PointAccuracy
* @return Accuracy
*/
PointAccuracy getAccuracy() { return this->accuracy; }
Accuracy getAccuracy() { return this->accuracy; }
private:
void init(uint32_t horizontalAccuracy);
PointAccuracy accuracy = PointAccuracy::none;
Accuracy accuracy = Accuracy::none;
Coordinates coordinates;
uint32_t creationTime = 0;
lib/NtripClient/ntripClient.cpp
+55 -31
View File
@@ -29,27 +29,14 @@ NTRIPClient::~NTRIPClient() {
}
void NTRIPClient::loop() {
this->pushGPGGA();
if (millis() - this->lastLoopTime < this->delayTime)
return;
this->lastLoopTime = millis();
if (millis() - this->lastGPGGAPushTime > this->pushGPGGATime && this->activated) {
this->lastGPGGAPushTime = millis();
// std::cout << "Try to push GPGGA data." << std::endl;
NMEA_GGA_data_t *data = new NMEA_GGA_data_t;
DebugTimes requestGpsData;
uint8_t res = this->gps->getLatestNMEAGPGGA(data);
requestGpsData.stopConsol("RequestGpsData", 5);
if (res == 2)
this->pushGPGGA(data);
delete data;
}
switch (this->state) {
case NTRIPClientStates::openConnection:
if (millis() - this->lastNtripConnectTime < this->tryReconnectTime)
break;
if (!this->activated) {
this->state = NTRIPClientStates::closeConnection;
break;
@@ -60,10 +47,9 @@ void NTRIPClient::loop() {
std::cout << "Connected to the NTRIP caster!" << std::endl;
this->state = NTRIPClientStates::pushData;
} else {
uint8_t seconds = this->tryReconnectTime / 1000;
std::cout << "Could not connect to the caster. Trying again in "
<< (int) seconds << " seconds." << std::endl;
this->lastNtripConnectTime = millis();
std::cout << "Failed!" << std::endl;
this->state = NTRIPClientStates::wait;
this->activated = false;
}
break;
@@ -81,9 +67,8 @@ void NTRIPClient::loop() {
case NTRIPClientStates::wait:
if (this->activated)
this->state = NTRIPClientStates::openConnection;
if (this->activated) {
// std::cout << "state is openConnection" << std::endl;
}
else
this->checkAutoReconnect();
break;
case NTRIPClientStates::notAvailable:
@@ -105,16 +90,27 @@ void NTRIPClient::gpsConfiguration() {
this->gps->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI, 10);
}
bool NTRIPClient::setActivated(bool b) {
bool NTRIPClient::setActivated(bool state) {
// std::cout << "NTRIPClient::setActivated: b - " << b << std::endl;
if (b && this->state != NTRIPClientStates::notAvailable)
if (state && this->state != NTRIPClientStates::notAvailable)
this->activated = true;
else if (b)
else if (state)
return false;
else
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() {
@@ -252,11 +248,39 @@ bool NTRIPClient::processConnection() {
return true;
}
void NTRIPClient::pushGPGGA(NMEA_GGA_data_t *nmeaData) {
if (this->ntripClient->connected() && this->transmitLocation)
this->ntripClient->print((const char *)nmeaData->nmea);
else
std::cout << "Failed to pushing GGA to server: " << (const char *)nmeaData->nmea << std::endl;
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() {
lib/NtripClient/ntripClient.h
+11 -5
View File
@@ -81,11 +81,12 @@ class NTRIPClient {
/**
* @brief Activate or deactivate the connection to the server.
*
* @param b
* @param state
* @return true success
* @return false failure
*/
bool setActivated(bool b);
bool setActivated(bool state);
void setAutoReconnect(bool state);
bool isConnected();
@@ -99,10 +100,11 @@ class NTRIPClient {
NTRIPClientStates getClientState() { return this->state; }
private:
void pushGPGGA(NMEA_GGA_data_t *nmeaData);
void pushGPGGA();
bool beginClient();
void closeConnection();
bool processConnection();
void checkAutoReconnect();
SFE_UBLOX_GNSS* gps;
WiFiClient* ntripClient;
@@ -110,20 +112,24 @@ class NTRIPClient {
bool transmitLocation = true;
bool activated = true;
bool autoReconnect = false;
uint8_t reconnectAttemps = 0;
uint16_t port;
uint32_t lastReceivedRtcmTime = 0;
uint32_t lastNtripConnectTime = 0;
// uint32_t lastNtripConnectTime = 0; // can deleted?
uint32_t lastGPGGAPushTime = 0;
uint32_t lastLoopTime = 0;
uint32_t lastReconnectTime = 0;
char host[128];
char mountPoint[128];
char user[128];
char password[128];
const uint8_t delayTime = 20;
const uint8_t maxReconnectAttemps = 10;
const uint16_t reconnectDelayTime = 1000;
const uint16_t timeOut = 5000;
const uint16_t tryReconnectTime = 5000;
const uint16_t bufferSize = 512;
const uint16_t pushGPGGATime = 10000;
};