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

use callback to save data from gnss module

Browse Source
This commit is contained in:
kleiax
2022-12-18 10:04:05 +01:00
parent 49145d2b0f
commit 377b0ea03b
5 changed files with 73 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/Navigation/navigation.cpp
+29 -32
View File
@@ -12,6 +12,8 @@
#include "navigation.h"
bool Navigation::outputStatusPrintPVTdata = false;
uint32_t Navigation::ubxUpdateTimeStatic = 0;
UBX_NAV_PVT_data_t* Navigation::ubxDataStatic = nullptr;
Navigation::Navigation(Route* route) {
this->gps = new SFE_UBLOX_GNSS();
@@ -42,8 +44,9 @@ void Navigation::init(Route* route) {
// std::cout << "Complete" << std::endl;
this->gps->setSPIOutput(COM_TYPE_UBX);
this->gps->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI, 10);
this->gps->setUSBOutput(COM_TYPE_UBX | COM_TYPE_NMEA);
this->gps->setAutoPVTcallbackPtr(&(Navigation::printPVTdata));
this->gps->setAutoPVTcallbackPtr(&(Navigation::savePVTdata));
// Navigation::setOutputStatusPrintPVTdata(true);
this->gps->setNavigationFrequency(1);
this->gps->setAutoPVT(true);
@@ -75,32 +78,11 @@ void Navigation::initNtrip(String host, uint16_t port, String mountPoint, String
}
void Navigation::loop() {
static uint32_t lastTime = 0;
this->gps->checkUblox();
this->gps->checkCallbacks();
if (this->gps->getPVT() && this->gps->getGnssFixOk()) {
this->updateCurrentLocation();
std::cout << "duration between: " << millis() - lastTime << std::endl;
lastTime = millis();
} else if (millis() - lastTime > 20000 && !this->gps->getGnssFixOk()) {
std::cout << "No Data from ZED arrived since a long time. Navigation::loop" << std::endl;
this->gps->flushPVT();
this->gps->softwareResetGNSSOnly();
lastTime = millis();
//std::cout
// << "checkUblox: " << this->gps->checkUblox() << " -|- "
// << "getGnssFixOk: " << this->gps->getGnssFixOk() << " -|- "
// << "getHWstatus: " << this->gps->getHWstatus() << " -|- "
// << "isConnected: " << this->gps->isConnected() << " -|- "
// << std::endl;
}
if (this->isNtripInit)
this->ntripClient->loop();
if (millis() - this->lastMillis < this->timeToWait)
return;
}
void Navigation::newRoute() {
@@ -139,6 +121,7 @@ CourseCorrection Navigation::getCourseCorrection() {
//FIXME: next line
double currentCourse = 0;
//double currentCourse = this->gps->course.deg();
int16_t correction = currentCourse - targetCourse;
if (correction > 180)
@@ -152,8 +135,6 @@ CourseCorrection Navigation::getCourseCorrection() {
}
bool Navigation::addCurrentPosToRoute() {
// FIXME: Should I use updateCurrentLocation here? think overall
this->updateCurrentLocation();
if (!this->currentPosition.isValid()) { return false; }
// First Point
@@ -173,11 +154,19 @@ bool Navigation::addCurrentPosToRoute() {
}
void Navigation::updateCurrentLocation() {
//TODO: check if the position is possible the true location
if (this->gps) {
this->currentPosition.lat = this->gps->getLatitude();
this->currentPosition.lon = this->gps->getLongitude();
}
if (Navigation::ubxUpdateTimeStatic == this->ubxUpdateTime)
return;
this->ubxData = Navigation::ubxDataStatic;
this->ubxUpdateTime = Navigation::ubxUpdateTimeStatic;
Point p;
p.lon = this->ubxData->lon;
p.lat = this->ubxData->lat;
p.fixType = this->ubxData->fixType;
p.carrierSolution = this->ubxData->flags.all;
this->currentPosition = p;
}
bool Navigation::nextPoint() {
@@ -195,6 +184,10 @@ bool Navigation::setTargetPoint(Point target) {
return false;
}
void Navigation::setOutputStatusPrintPVTdata(bool status) {
Navigation::outputStatusPrintPVTdata = status;
}
void Navigation::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
if (!Navigation::outputStatusPrintPVTdata)
return;
@@ -242,6 +235,10 @@ void Navigation::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
<< " Horizontal Accuracy Estimate: " << hAcc << " mm" << std::endl;
}
void Navigation::setOutputStatusPrintPVTdata(bool status) {
Navigation::outputStatusPrintPVTdata = status;
void Navigation::savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
Navigation::printPVTdata(ubxDataStruct);
Navigation::ubxDataStatic = ubxDataStruct;
Navigation::ubxUpdateTimeStatic = millis();
}