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();
}
lib/Navigation/navigation.h
+24 -5
View File
@@ -125,14 +125,21 @@ class Navigation {
*/
bool addCurrentPosToRoute();
// /**
// * @brief Returns the GPS object
// *
// * @return SFE_UBLOX_GNSS*
// */
// SFE_UBLOX_GNSS* getGPS() { return this->gps; }
/**
* @brief Returns the GPS object
* @brief Get the Ubx Data object
*
* @return SFE_UBLOX_GNSS*
* This struct includes the most Data from the GNSS-Module.
*
* @return UBX_NAV_PVT_data_t*
*/
SFE_UBLOX_GNSS* getGPS() { return this->gps; }
static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
static void setOutputStatusPrintPVTdata(bool status);
UBX_NAV_PVT_data_t* getUbxData() { return this->ubxData; }
/**
* @brief Returns the NTRIPClient object
@@ -151,6 +158,8 @@ class Navigation {
*/
RouteInfo getRouteInfo() const { return this->route->getRouteInfo(); }
static void setOutputStatusPrintPVTdata(bool status);
private:
void updateCurrentLocation();
@@ -174,6 +183,7 @@ class Navigation {
void init(Route* route);
SFE_UBLOX_GNSS* gps;
UBX_NAV_PVT_data_t* ubxData = nullptr;
Route* route = nullptr;
NTRIPClient* ntripClient = nullptr;
@@ -181,6 +191,7 @@ class Navigation {
Point targetPoint;
Point currentPosition;
bool navigationStarted = false;
bool navigationFinished = false;
bool isNtripInit = false;
@@ -193,6 +204,14 @@ class Navigation {
uint8_t timeToWait = 200;
uint16_t port;
uint32_t lastMillis = 0;
uint32_t ubxUpdateTime = 0;
static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
static void savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
static UBX_NAV_PVT_data_t* ubxDataStatic;
static uint32_t ubxUpdateTimeStatic;
static bool outputStatusPrintPVTdata;
};
lib/Navigation/route.h
+6 -2
View File
@@ -39,6 +39,9 @@ class Point{
double lat = 0;
double lon = 0;
uint8_t fixType = -1;
uint8_t carrierSolution = -1;
uint32_t horizontalAccuracy = -1;
/**
* @brief checks if to points are equal
@@ -52,12 +55,13 @@ class Point{
}
/**
* @brief Checks if the Point is valid
* @brief Checks if the Point is initalized.
*
* @return true
* @return false
*/
bool isValid() const { return this->lat + this->lon; }
bool isInit() const { return this->lat + this->lon; }
bool isValid() const { return (this->horizontalAccuracy < 5000) ? true : false; }
double distanceTo(const Point& point) const;
double courseTo(const Point& point) const;