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

gps now over spi

Browse Source
This commit is contained in:
kleiax
2022-12-09 14:10:57 +01:00
parent 599f176622
commit f14992c040
26 changed files with 974 additions and 225 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/Navigation/navigation.cpp
+96 -30
View File
@@ -11,6 +11,8 @@
#include "navigation.h"
bool Navigation::outputStatusPrintPVTdata = false;
Navigation::Navigation(Route* route) {
this->gps = new SFE_UBLOX_GNSS();
if (this->gps->begin() == false) {
@@ -20,9 +22,9 @@ Navigation::Navigation(Route* route) {
this->init(route);
}
Navigation::Navigation(SPIClass spiPort, uint8_t csPin, Route* route) {
Navigation::Navigation(SPIClass* spiPort, uint8_t csPin, Route* route) {
this->gps = new SFE_UBLOX_GNSS();
if (this->gps->begin(spiPort, csPin, 4000000) == false) {
if (this->gps->begin(*spiPort, csPin, 4000000) == false) {
std::cout << "u-blox GNSS not detected on SPI bus. Please check wiring. Freezing." << std::endl;
while (1);
}
@@ -34,10 +36,17 @@ void Navigation::init(Route* route) {
uint8_t versionLow = this->gps->getProtocolVersionLow();
std::cout << "u-blox protocol version: " << unsigned(versionHigh) << "." << unsigned(versionLow) << std::endl;
this->gps->setI2COutput(COM_TYPE_UBX);
this->gps->setUSBOutput(COM_TYPE_UBX | COM_TYPE_NMEA);
// std::cout << "Set GNSS Module to factory settings... ";
// this->gps->factoryReset();
// delay(5000);
// std::cout << "Complete" << std::endl;
this->gps->setSPIOutput(COM_TYPE_UBX);
this->gps->setUSBOutput(COM_TYPE_UBX | COM_TYPE_NMEA);
this->gps->setAutoPVTcallbackPtr(&(Navigation::printPVTdata));
// Navigation::setOutputStatusPrintPVTdata(true);
this->gps->setNavigationFrequency(1);
this->gps->setAutoPVT(true);
if (route)
this->route = route;
@@ -64,13 +73,32 @@ void Navigation::initNtrip(String host, uint16_t port, String mountPoint, String
}
void Navigation::loop() {
//FIXME: Hier sollte funktion zum aktualisieren des GPS Moduls stehen
static uint32_t lastTime = 0;
if (millis() - this->lastMillis < this->timeToWait)
return;
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) {
std::cout << "Keine Daten mehr!" << 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() {
@@ -90,9 +118,9 @@ bool Navigation::startNavigation() {
CourseCorrection Navigation::getCourseCorrection() {
CourseCorrection courseCorrection = {0, 0};
Point currentPos = this->currentLocation();
double targetCourse = Navigation::courseTo(currentPos, this->targetPoint);
double distance = Navigation::distanceBetween(currentPos, this->targetPoint);
this->updateCurrentLocation();
double targetCourse = this->currentPosition.courseTo(this->targetPoint);
double distance = this->currentPosition.distanceTo(this->targetPoint);
if (targetCourse < 0 || distance < 0 || this->navigationFinished)
return courseCorrection;
@@ -122,33 +150,32 @@ CourseCorrection Navigation::getCourseCorrection() {
}
bool Navigation::addCurrentPosToRoute() {
Point p = this->currentLocation();
if (!p.isValid()) { return false; }
// FIXME: Should I use updateCurrentLocation here? think overall
this->updateCurrentLocation();
if (!this->currentPosition.isValid()) { return false; }
// First Point
if (this->route->getRouteInfo().totalPoints == 0) {
this->route->addPointToRoute(p);
this->lastPoint = p;
this->route->addPointToRoute(this->currentPosition);
this->lastPoint = this->currentPosition;
return true;
}
// Ever Point after the first
if (MIN_DISTANCE_BETWEEN_POINTS <= this->distanceBetween(p, this->lastPoint)) {
this->route->addPointToRoute(p);
this->lastPoint = p;
if (MIN_DISTANCE_BETWEEN_POINTS <= this->currentPosition.distanceTo(this->lastPoint)) {
this->route->addPointToRoute(this->currentPosition);
this->lastPoint = this->currentPosition;
return true;
}
return false;
}
Point Navigation::currentLocation() {
Point p;
void Navigation::updateCurrentLocation() {
//TODO: check if the position is possible the true location
if (this->gps) {
p.lat = this->gps->getLatitude();
p.lon = this->gps->getLongitude();
this->currentPosition.lat = this->gps->getLatitude();
this->currentPosition.lon = this->gps->getLongitude();
}
return p;
}
bool Navigation::nextPoint() {
@@ -166,14 +193,53 @@ bool Navigation::setTargetPoint(Point target) {
return false;
}
double Navigation::distanceBetween(Point p1, Point p2) {
if (p1.isValid() && p2.isValid())
return NavigationUtils::distanceBetween(p1, p2);
return -1;
void Navigation::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
if (!Navigation::outputStatusPrintPVTdata)
return;
double latitude = (double) ubxDataStruct->lat / 10000000.0;
double longitude = (double) ubxDataStruct->lon / 10000000.0;
double altitude = (double) ubxDataStruct->hMSL / 1000.0;
uint8_t fixType = ubxDataStruct->fixType;
char fixTypeString[32];
if (fixType == 0)
strcpy(fixTypeString, "None");
else if (fixType == 1)
strcpy(fixTypeString, "Dead Reckoning");
else if (fixType == 2)
strcpy(fixTypeString, "2D");
else if (fixType == 3)
strcpy(fixTypeString, "3D");
else if (fixType == 3)
strcpy(fixTypeString, "GNSS + Dead Reckoning");
else if (fixType == 5)
strcpy(fixTypeString, "Time Only");
else
strcpy(fixTypeString, "UNKNOWN");
uint8_t carrSoln = ubxDataStruct->flags.bits.carrSoln;
char carrSolnString[16];
if (carrSoln == 0)
strcpy(carrSolnString, "None");
else if (carrSoln == 1)
strcpy(carrSolnString, "Floating");
else if (carrSoln == 2)
strcpy(carrSolnString, "Fixed");
else
strcpy(carrSolnString, "UNKNOWN");
uint32_t hAcc = ubxDataStruct->hAcc;
std::cout << "Lat: " << latitude
<< " Lng: " << longitude
<< " Alt: " << altitude << std::endl;
std::cout << "Fix: " << fixTypeString
<< " Carrier Solution: " << carrSolnString
<< " Horizontal Accuracy Estimate: " << hAcc << " mm" << std::endl;
}
double Navigation::courseTo(Point p1, Point p2) {
if (p1.isValid() && p2.isValid())
return NavigationUtils::courseTo(p1, p2);
return -1;
void Navigation::setOutputStatusPrintPVTdata(bool status) {
Navigation::outputStatusPrintPVTdata = status;
}
lib/Navigation/navigation.h
+8 -27
View File
@@ -18,7 +18,6 @@
#include <SPI.h>
#include "route.h"
#include "navigationUtils.h"
#include "NTRIPClient.h"
/**
@@ -64,7 +63,7 @@ class Navigation {
*
* @param route with which to navigate
*/
Navigation(SPIClass spiPort, uint8_t csPin, Route* route = nullptr);
Navigation(SPIClass* spiPort, uint8_t csPin, Route* route = nullptr);
/**
* @brief Destroy the Navigation object
@@ -132,6 +131,8 @@ class Navigation {
* @return SFE_UBLOX_GNSS*
*/
SFE_UBLOX_GNSS* getGPS() { return this->gps; }
static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
static void setOutputStatusPrintPVTdata(bool status);
/**
* @brief Returns the NTRIPClient object
@@ -151,12 +152,7 @@ class Navigation {
RouteInfo getRouteInfo() const { return this->route->getRouteInfo(); }
private:
/**
* @brief Returns the current Location
*
* @return Point
*/
Point currentLocation();
void updateCurrentLocation();
/**
* @brief Set the next point as target
@@ -175,24 +171,6 @@ class Navigation {
*/
bool setTargetPoint(Point target);
/**
* @brief Calculate the distance between to points
*
* @param p1 point 1
* @param p2 point 2
* @return double distance in meter
*/
static double distanceBetween(Point p1, Point p2);
/**
* @brief Calculates the course from point1 to point2
*
* @param p1 point 1
* @param p2 point 2
* @return double degree north = 0° west = 270°
*/
static double courseTo(Point p1, Point p2);
void init(Route* route);
SFE_UBLOX_GNSS* gps;
@@ -201,6 +179,7 @@ class Navigation {
Point lastPoint;
Point targetPoint;
Point currentPosition;
bool navigationStarted = false;
bool navigationFinished = false;
@@ -211,9 +190,11 @@ class Navigation {
char* user;
char* password;
uint8_t timeToWait = 5;
uint8_t timeToWait = 200;
uint16_t port;
uint32_t lastMillis = 0;
static bool outputStatusPrintPVTdata;
};
#endif // NAVIGATION_H
lib/Navigation/navigationUtils.cpp
-40
View File
@@ -1,40 +0,0 @@
/**
* @file navigationUtils.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @version 0.1
* @date 2022-03-24
*
* @copyright Copyright (c) 2022
*
*/
#include "navigationUtils.h"
double NavigationUtils::getCurrentCourse(Point p1) {
//TODO: write it!!!
return 0;
}
void NavigationUtils::setLastPoint(Point p1) {
this->lastPoint = p1;
}
double NavigationUtils::courseTo(Point p1, Point p2) {
//TODO: write it!!!
return 0;
}
double NavigationUtils::courseTo(double lat1, double lon1, double lat2, double lon2) {
return NavigationUtils::courseTo(Point(lat1, lon1), Point(lat2, lon2));
}
double NavigationUtils::distanceBetween(Point p1, Point p2) {
//TODO: write it!!!
return 0;
}
double NavigationUtils::distanceBetween(double lat1, double lon1, double lat2, double lon2) {
return NavigationUtils::distanceBetween(Point(lat1, lon1), Point(lat2, lon2));
}
lib/Navigation/navigationUtils.h
-30
View File
@@ -1,30 +0,0 @@
/**
* @file navigationUtils.h
* @author Alexander Klein (alex@kleiax.de)
* @version 0.1
* @date 2022-03-24
*
* @copyright Copyright (c) 2022
*
*/
#ifndef NAVIGATION_UTILS_H
#define NAVIGATION_UTILS_H
#include "route.h"
class NavigationUtils {
public:
double getCurrentCourse(Point p1);
void setLastPoint(Point p1);
static double courseTo(Point p1, Point p2);
static double courseTo(double lat1, double lon1, double lat2, double lon2);
static double distanceBetween(Point p1, Point p2);
static double distanceBetween(double lat1, double lon1, double lat2, double lon2);
private:
Point lastPoint;
};
#endif // NAVIGATION_UTILS_H
lib/Navigation/route.h
+4
View File
@@ -54,6 +54,10 @@ class Point{
* @return false
*/
bool isValid() const { return this->lat + this->lon; }
// FIXME: Add real implementations!
double distanceTo(const Point& point) const { return 0; }
double courseTo(const Point& point) const { return 0; }
};
/**