355 lines
11 KiB
C++
355 lines
11 KiB
C++
/**
|
|
* @file navigation.cpp
|
|
* @author Alexander Klein (alex@kleiax.de)
|
|
* @brief Contains the implementation of the class Navigation
|
|
* @version 0.1
|
|
* @date 2022-01-31
|
|
*
|
|
* @copyright Copyright (c) 2022
|
|
*
|
|
*/
|
|
|
|
#include "navigation.h"
|
|
|
|
bool Navigation::outputStatusPrintPVTdata = false;
|
|
bool Navigation::newData = false;
|
|
uint32_t Navigation::ubxUpdateTimeStatic = 0;
|
|
UBX_NAV_PVT_data_t* Navigation::ubxDataStatic = nullptr;
|
|
|
|
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);
|
|
}
|
|
this->init(route);
|
|
}
|
|
|
|
Navigation::Navigation(SPIClass* spiPort, uint8_t csPin, Route* route) {
|
|
this->gps = new SFE_UBLOX_GNSS();
|
|
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);
|
|
}
|
|
this->init(route);
|
|
}
|
|
|
|
void Navigation::init(Route* route) {
|
|
uint8_t versionHigh = this->gps->getProtocolVersionHigh();
|
|
uint8_t versionLow = this->gps->getProtocolVersionLow();
|
|
std::cout << "u-blox protocol version: " << unsigned(versionHigh) << "." << unsigned(versionLow) << std::endl;
|
|
|
|
// 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->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI, 10);
|
|
this->gps->setUSBOutput(COM_TYPE_UBX | COM_TYPE_NMEA);
|
|
this->gps->setAutoPVTcallbackPtr(&(Navigation::savePVTdata));
|
|
// Navigation::setOutputStatusPrintPVTdata(true);
|
|
this->gps->setNavigationFrequency(1);
|
|
this->gps->setAutoPVT(true);
|
|
|
|
if (route)
|
|
this->route = route;
|
|
else
|
|
this->route = new Route();
|
|
|
|
this->compass = new QMC5883LCompass();
|
|
// Init Compass
|
|
Wire.beginTransmission(0x0d);
|
|
Wire.write(0x0b);
|
|
Wire.write(0x01);
|
|
Wire.endTransmission();
|
|
this->compass->setMode(0x01,0x0C,0x10,0X00);
|
|
CalibrateCompass caliCompass(this->compass);
|
|
caliCompass.loadData();
|
|
caliCompass.useData();
|
|
// std::cout << "Navigation::init compass correction data: " << caliCompass << std::endl;
|
|
}
|
|
|
|
Navigation::~Navigation() {
|
|
delete this->gps;
|
|
delete this->compass;
|
|
delete this->ntripClient;
|
|
delete this->route;
|
|
}
|
|
|
|
void Navigation::initNtrip(String host, uint16_t port, String mountPoint, String user, String password) {
|
|
this->ntripClient = new NTRIPClient(this->gps, host.c_str(), port, mountPoint.c_str(), user.c_str(), password.c_str());
|
|
this->ntripClient->gpsConfiguration();
|
|
this->ntripClient->loop();
|
|
this->ntripClient->setActivated(false);
|
|
this->isNtripInit = true;
|
|
}
|
|
|
|
void Navigation::loop() {
|
|
this->gps->checkUblox();
|
|
this->gps->checkCallbacks();
|
|
|
|
if (Navigation::newData) {
|
|
this->updateCurrentLocation();
|
|
Navigation::newData = false;
|
|
}
|
|
|
|
if (this->ntripClient)
|
|
this->ntripClient->loop();
|
|
|
|
if (millis() - this->lastMillis > AZIMUTH_UPDATE_DELAY) {
|
|
this->compass->read();
|
|
this->realAzimuth = this->compass->getAzimuth();
|
|
|
|
this->updateMagneticDeclination();
|
|
|
|
this->lastMillis = millis();
|
|
}
|
|
}
|
|
|
|
void Navigation::newRoute() {
|
|
if (this->route)
|
|
delete this->route;
|
|
this->route = new Route();
|
|
}
|
|
|
|
bool Navigation::startNavigation() {
|
|
Point newTargetPoint = this->route->startRoute();
|
|
this->navigationStarted = this->setTargetPoint(newTargetPoint);
|
|
if (this->navigationStarted)
|
|
this->navigationFinished = false;
|
|
return this->navigationStarted;
|
|
}
|
|
|
|
void Navigation::drivingDirectionChange() {
|
|
Point tmp = this->currentPosition;
|
|
if (tmp.isInit() && tmp.isValid()) {
|
|
this->directionChangeMode = true;
|
|
this->lastPointDrivingDirectionChange = tmp;
|
|
this->calcAzimuthState = CalcAzimuthState::Invalid;
|
|
}
|
|
}
|
|
|
|
Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction, bool forceUpdate) {
|
|
if (this->navigationFinished)
|
|
return Status::Complete;
|
|
|
|
if (this->currentPosition.getAccuracy() <= this->minAccuracy)
|
|
return Status::InsufficientAccuracy;
|
|
|
|
if (this->currentPosition.distanceTo(this->lastPointCalcCorrection) < (this->minDistanceToReachPoint / 2.0)
|
|
&& !forceUpdate) {
|
|
correction.correction = this->calculateCourseCorrection(this->lastPointCalcCorrection);
|
|
correction.distance = this->lastPointCalcCorrection.distanceTo(this->targetPoint);
|
|
return Status::Unchanged;
|
|
}
|
|
|
|
double distance = this->currentPosition.distanceTo(this->targetPoint);
|
|
|
|
// Check if I need a new Point
|
|
if (distance < this->minDistanceToReachPoint && this->preventNextPoint == false) {
|
|
if (!this->nextPoint()) {
|
|
this->navigationFinished = true;
|
|
this->navigationStarted = false;
|
|
return Status::Complete; // End of navigation
|
|
}
|
|
distance = this->currentPosition.distanceTo(this->targetPoint);
|
|
}
|
|
|
|
correction.correction = this->calculateCourseCorrection(this->currentPosition);
|
|
correction.distance = distance;
|
|
this->lastPointCalcCorrection = this->currentPosition;
|
|
return Status::Updated;
|
|
}
|
|
|
|
Navigation::Status Navigation::addCurrentPosToRoute() {
|
|
if (this->currentPosition.getAccuracy() <= this->minAccuracy)
|
|
return Status::InsufficientAccuracy;
|
|
|
|
// First Point
|
|
if (this->route->getRouteInfo().totalPoints == 0) {
|
|
this->route->addPointToRoute(this->currentPosition);
|
|
this->lastPointRouteInsert = this->currentPosition;
|
|
return Status::Updated;
|
|
}
|
|
|
|
// Every Point after the first
|
|
double distance = this->currentPosition.distanceTo(this->lastPointRouteInsert);
|
|
if (MIN_DISTANCE_BETWEEN_POINTS <= distance
|
|
&& MAX_DISTANCE_BETWEEN_POINTS >= distance){
|
|
this->route->addPointToRoute(this->currentPosition);
|
|
this->lastPointRouteInsert = this->currentPosition;
|
|
return Status::Updated;
|
|
}
|
|
return Status::Unchanged;
|
|
}
|
|
|
|
void Navigation::updateCurrentLocation() {
|
|
if (Navigation::ubxUpdateTimeStatic == this->ubxUpdateTime)
|
|
return;
|
|
|
|
this->ubxData = Navigation::ubxDataStatic;
|
|
this->ubxUpdateTime = Navigation::ubxUpdateTimeStatic;
|
|
|
|
Point::Coordinates coords;
|
|
coords.lat = this->ubxData->lat / 10000000.0;
|
|
coords.lon = this->ubxData->lon / 10000000.0;
|
|
|
|
this->currentPosition = Point(coords, this->ubxData->hAcc);
|
|
}
|
|
|
|
void Navigation::updateMagneticDeclination() {
|
|
if (!this->directionChangeMode
|
|
|| this->lastPointDrivingDirectionChange.distanceTo(this->currentPosition) < 1.0)
|
|
{
|
|
this->calcAzimuthState = CalcAzimuthState::Invalid;
|
|
this->calcAzimuth = 999;
|
|
return;
|
|
}
|
|
|
|
this->calcAzimuth = this->lastPointDrivingDirectionChange.courseTo(this->currentPosition);
|
|
|
|
// Map point accuracy to CalcAzimuthState
|
|
if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::oneDigOfCM
|
|
|| this->currentPosition.getAccuracy() == Point::Accuracy::oneDigOfCM)
|
|
{
|
|
this->calcAzimuthState = CalcAzimuthState::Good;
|
|
}
|
|
else if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::twoDigOfCM
|
|
|| this->currentPosition.getAccuracy() == Point::Accuracy::twoDigOfCM)
|
|
{
|
|
this->calcAzimuthState = CalcAzimuthState::Ok;
|
|
}
|
|
else if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::threeDigOfCM
|
|
|| this->currentPosition.getAccuracy() == Point::Accuracy::threeDigOfCM)
|
|
{
|
|
this->calcAzimuthState = CalcAzimuthState::Bad;
|
|
}
|
|
else
|
|
{
|
|
this->calcAzimuthState = CalcAzimuthState::Invalid;
|
|
}
|
|
|
|
// Upgrade quality if the range grows up
|
|
if (this->lastPointDrivingDirectionChange.distanceTo(this->currentPosition) > 2.0) {
|
|
switch (this->calcAzimuthState) {
|
|
case CalcAzimuthState::Bad :
|
|
this->calcAzimuthState = CalcAzimuthState::Ok;
|
|
break;
|
|
|
|
case CalcAzimuthState::Ok :
|
|
this->calcAzimuthState = CalcAzimuthState::Good;
|
|
break;
|
|
|
|
case CalcAzimuthState::Good :
|
|
this->calcAzimuthState = CalcAzimuthState::Super;
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
int16_t Navigation::calculateCourseCorrection(Point& point) {
|
|
int16_t targetCourse = point.courseTo(this->targetPoint);
|
|
int16_t correctionCourse;
|
|
|
|
if (this->calcAzimuthState == CalcAzimuthState::Good
|
|
|| this->calcAzimuthState == CalcAzimuthState::Super)
|
|
{
|
|
correctionCourse = targetCourse - this->calcAzimuth;
|
|
this->lastUsedCalcAzimuth = true;
|
|
} else {
|
|
correctionCourse = targetCourse - this->realAzimuth;
|
|
this->lastUsedCalcAzimuth = false;
|
|
}
|
|
|
|
return Navigation::fixDegree(correctionCourse);
|
|
}
|
|
|
|
bool Navigation::nextPoint() {
|
|
if (!this->navigationStarted)
|
|
return false;
|
|
return this->setTargetPoint(this->route->getNextPoint());
|
|
}
|
|
|
|
bool Navigation::setTargetPoint(Point target) {
|
|
if (target.isInit()) {
|
|
this->targetPoint = target;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void Navigation::setOutputStatusPrintPVTdata(bool status) {
|
|
Navigation::outputStatusPrintPVTdata = status;
|
|
}
|
|
|
|
int16_t Navigation::fixDegree(int16_t degree) {
|
|
while (degree < -180 || degree > 180) {
|
|
if (degree > 180)
|
|
degree -= 360;
|
|
else if (degree < -180)
|
|
degree += 360;
|
|
}
|
|
return degree;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
void Navigation::savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
|
|
Navigation::printPVTdata(ubxDataStruct);
|
|
|
|
Navigation::newData = true;
|
|
Navigation::ubxDataStatic = ubxDataStruct;
|
|
Navigation::ubxUpdateTimeStatic = millis();
|
|
}
|
|
|