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

rotate correctionCourse in getCourseCorrection by

Browse Source 
180°
This commit is contained in:
kleiax
2023-05-25 19:58:57 +02:00
parent 466ee39acc
commit 1faab632d5
1 changed files with 13 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/Navigation/navigation.cpp
+13 -7
View File
@@ -58,16 +58,16 @@ void Navigation::init(Route* route) {
this->route = new Route(); this->route = new Route();
this->compass = new QMC5883LCompass(); this->compass = new QMC5883LCompass();
// Init Compass // Init Compass
Wire.beginTransmission(0x0d); Wire.beginTransmission(0x0d);
Wire.write(0x0b); Wire.write(0x0b);
Wire.write(0x01); Wire.write(0x01);
Wire.endTransmission(); Wire.endTransmission();
this->compass->setMode(0x01,0x0C,0x10,0X00); this->compass->setMode(0x01,0x0C,0x10,0X00);
CalibrateCompass caliCompass(this->compass); // CalibrateCompass caliCompass(this->compass);
caliCompass.loadData(); // caliCompass.loadData();
caliCompass.useData(); // caliCompass.useData();
// std::cout << "Navigation::init compass correction data: " << caliCompass << std::endl;
} }
Navigation::~Navigation() { Navigation::~Navigation() {
@@ -98,8 +98,14 @@ void Navigation::loop() {
this->ntripClient->loop(); this->ntripClient->loop();
if (millis() - this->lastMillis > AZIMUTH_UPDATE_DELAY) { if (millis() - this->lastMillis > AZIMUTH_UPDATE_DELAY) {
// if (millis() - this->lastMillis > 500) {
this->compass->read(); this->compass->read();
this->azimuth = this->compass->getAzimuth(); this->azimuth = this->compass->getAzimuth();
// std::cout << "Compass x: " << compass->getX()
// << " y: " << compass->getY()
// << " z: " << compass->getZ()
// << " Azi: " << compass->getAzimuth()
// << std::endl;
this->lastMillis = millis(); this->lastMillis = millis();
} }
} }
@@ -132,7 +138,6 @@ bool Navigation::getCourseCorrection(CourseCorrection& correction) {
// Check if I need a new Point // Check if I need a new Point
if (distance < this->minDistanceToReachPoint && this->preventNextPoint == false) { if (distance < this->minDistanceToReachPoint && this->preventNextPoint == false) {
if (!this->nextPoint()) { if (!this->nextPoint()) {
std::cout << "Navigation::getCourseCorrection finished" << std::endl;
this->navigationFinished = true; this->navigationFinished = true;
this->navigationStarted = false; this->navigationStarted = false;
return false; // End of navigation return false; // End of navigation
@@ -147,8 +152,9 @@ bool Navigation::getCourseCorrection(CourseCorrection& correction) {
else if (correctionCourse < -180) else if (correctionCourse < -180)
correctionCourse += 360; correctionCourse += 360;
// //Rotate result by 180° to corrigate Azimuth //Rotate result by 180° to corrigate Azimuth
// correctionCourse = correctionCourse > 180 ? correctionCourse -360 : correctionCourse; correctionCourse += 180;
correctionCourse = correctionCourse > 180 ? correctionCourse -360 : correctionCourse;
correction.correction = correctionCourse; correction.correction = correctionCourse;
correction.distance = distance; correction.distance = distance;