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;