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;
src/SpecialMenus/GPS/menuGPS.cpp
+12 -11
View File
@@ -12,8 +12,8 @@
#include "menuGPS.h"
MenuGPS::MenuGPS(DriveManager* driveManager) {
this->gps = driveManager->getNavigation()->getGPS();
this->ntripClient = driveManager->getNavigation()->getNTRIPClient();
this->navigation = driveManager->getNavigation();
this->ntripClient = this->navigation->getNTRIPClient();
this->lastMillis = millis();
}
@@ -64,19 +64,20 @@ void MenuGPS::update() {
}
void MenuGPS::printPage() const {
uint8_t fixType = this->gps->getFixType();
UBX_NAV_PVT_data_t* gpsData = this->navigation->getUbxData();
uint8_t fixType = gpsData->fixType;
switch (this->currentPage) {
case 0: {
uint8_t siv = this->gps->getSIV();
uint16_t hdop = this->gps->getHorizontalDOP();
uint8_t siv = gpsData->numSV;
uint32_t pDOP = gpsData->pDOP;
if (this->lcd) {
lcd->clear();
lcd->setCursor(0, 0);
if (fixType) {
lcd->printf("Sats: %3.u", siv);
lcd->setCursor(0, 1);
lcd->printf("HDOP: %u", hdop);
lcd->printf("PDOP: %u", pDOP);
} else {
lcd->printf("Data isn't valid");
lcd->setCursor(0, 1);
@@ -85,15 +86,15 @@ void MenuGPS::printPage() const {
}
if (fixType)
std::cout << "Sats: " << (int) siv
<< " HDOP: " << (int) hdop << std::endl;
<< " PDOP: " << (int) pDOP << std::endl;
else
std::cout << "Data isn't valid or no GPS signal" << std::endl;
}
break;
case 1: {
int32_t lat = this->gps->getLatitude();
int32_t lng = this->gps->getLongitude();
int32_t lat = gpsData->lat;
int32_t lng = gpsData->lon;
if (this->lcd) {
lcd->clear();
lcd->setCursor(0, 0);
@@ -116,7 +117,7 @@ void MenuGPS::printPage() const {
break;
case 2: {
uint32_t unixEpoch = this->gps->getUnixEpoch();
uint32_t unixEpoch = gpsData->sec;
if (this->lcd) {
lcd->clear();
@@ -154,7 +155,7 @@ void MenuGPS::printPage() const {
break;
case 4: {
uint32_t accuracy = this->gps->getHorizontalAccuracy();
uint32_t accuracy = gpsData->hAcc;
if (this->lcd) {
lcd->clear();
src/SpecialMenus/GPS/menuGPS.h
+2 -1
View File
@@ -16,6 +16,7 @@
#include "menuControl.h"
#include "driveModi/driveManager.h"
#include "navigation.h"
#include "ntripClient.h"
/**
@@ -82,8 +83,8 @@ class MenuGPS : public MenuControl {
void printPage() const;
void runCommand() const;
SFE_UBLOX_GNSS* gps;
NTRIPClient* ntripClient;
Navigation* navigation;
const uint16_t delay = 5000;
uint32_t lastMillis = 0;