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

added correction data for gnss

Browse Source
This commit is contained in:
kleiax
2022-04-07 22:46:46 +02:00
parent ece89eba60
commit 9790a4a66f
11 changed files with 289 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/Navigation/navigation.cpp
+23 -11
View File
@@ -11,9 +11,21 @@
#include "navigation.h"
Navigation::Navigation(uint8_t rx, uint8_t tx, Route* route) {
this->gps = new TinyGPSPlus;
Serial1.begin(9600, SERIAL_8N1, rx, tx);
Navigation::Navigation(Route* route) {
this->gps = new SFE_UBLOX_GNSS();
if (this->gps->begin() == false) {
std::cout << "u-blox GNSS not detected at default I2C address. Please check wiring. Freezing." << std::endl;
while (1);
}
uint8_t versionHigh = this->gps->getProtocolVersionHigh();
uint8_t versionLow = this->gps->getProtocolVersionLow();
std::cout << "Neo-M8Q protocol version: " << unsigned(versionHigh) << "." << unsigned(versionLow) << std::endl;
this->gps->setI2COutput(COM_TYPE_UBX);
this->gps->setUSBOutput(COM_TYPE_UBX | COM_TYPE_NMEA);
if (route)
this->route = route;
else
@@ -27,8 +39,7 @@ Navigation::~Navigation() {
}
void Navigation::loop() {
while (Serial1.available())
gps->encode(Serial1.read());
this->gps->checkUbloxI2C();
}
void Navigation::newRoute() {
@@ -65,7 +76,7 @@ CourseCorrection Navigation::getCourseCorrection() {
}
}
double currentCourse = gps->course.deg();
double currentCourse = this->gps->course.deg();
int16_t correction = currentCourse - targetCourse;
if (correction > 180)
correction -= 360;
@@ -99,9 +110,10 @@ bool Navigation::addCurrentPosToRoute() {
Point Navigation::currentLocation() {
Point p;
if (this->gps->location.isValid()) {
p.lat = this->gps->location.lat();
p.lon = this->gps->location.lng();
//TODO: check if the position is possible the true location
if (this->gps) {
p.lat = this->gps->getLatitude();
p.lon = this->gps->getLongitude();
}
return p;
}
@@ -123,12 +135,12 @@ bool Navigation::setTargetPoint(Point target) {
double Navigation::distanceBetween(Point p1, Point p2) {
if (p1.isValid() && p2.isValid())
return TinyGPSPlus::distanceBetween(p1.lat, p1.lon, p2.lat, p2.lon);
return NavigationUtils::distanceBetween(p1, p2);
return -1;
}
double Navigation::courseTo(Point p1, Point p2) {
if (p1.isValid() && p2.isValid())
return TinyGPSPlus::courseTo(p1.lat, p1.lon, p2.lat, p2.lon);
return NavigationUtils::courseTo(p1, p2);
return -1;
}