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

revised getCourseCorrection

Browse Source
This commit is contained in:
kleiax
2022-12-28 10:24:09 +01:00
parent 63f8cb6470
commit 7f1ffc874f
1 changed files with 5 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/Navigation/navigation.cpp
+5 -8
View File
@@ -106,13 +106,13 @@ bool Navigation::startNavigation() {
} }
CourseCorrection Navigation::getCourseCorrection() { CourseCorrection Navigation::getCourseCorrection() {
//TODO: Check accuracy befor make something
CourseCorrection courseCorrection = {0, 0}; CourseCorrection courseCorrection = {0, 0};
this->updateCurrentLocation(); int16_t targetCourse = this->currentPosition.courseTo(this->targetPoint);
double targetCourse = this->currentPosition.courseTo(this->targetPoint);
double distance = this->currentPosition.distanceTo(this->targetPoint); double distance = this->currentPosition.distanceTo(this->targetPoint);
if (targetCourse < 0 || distance < 0 || this->navigationFinished) if (distance < 0 || this->navigationFinished)
return courseCorrection; return courseCorrection;
// Check if I need a new Point // Check if I need a new Point
@@ -125,11 +125,8 @@ CourseCorrection Navigation::getCourseCorrection() {
} }
} }
//FIXME: next line // correction = targetCourse - currentCourse
double currentCourse = 0; int16_t correction = targetCourse - this->ubxData->magDec;
//double currentCourse = this->gps->course.deg();
int16_t correction = currentCourse - targetCourse;
if (correction > 180) if (correction > 180)
correction -= 360; correction -= 360;
else if (correction < -180) else if (correction < -180)