147 lines
3.9 KiB
C++
147 lines
3.9 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"
|
|
|
|
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);
|
|
}
|
|
|
|
uint8_t versionHigh = this->gps->getProtocolVersionHigh();
|
|
uint8_t versionLow = this->gps->getProtocolVersionLow();
|
|
std::cout << "Neo-M8Q protocol version: " << unsigned(versionHigh) << "." << unsigned(versionLow) << std::endl;
|
|
this->gps->setI2COutput(COM_TYPE_UBX);
|
|
this->gps->setUSBOutput(COM_TYPE_UBX | COM_TYPE_NMEA);
|
|
|
|
|
|
|
|
if (route)
|
|
this->route = route;
|
|
else
|
|
this->route = new Route();
|
|
}
|
|
|
|
Navigation::~Navigation() {
|
|
delete this->gps;
|
|
if (this->route)
|
|
delete this->route;
|
|
}
|
|
|
|
void Navigation::loop() {
|
|
this->gps->checkUbloxI2C();
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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);
|
|
|
|
if (targetCourse < 0 || distance < 0 || this->navigationFinished)
|
|
return courseCorrection;
|
|
|
|
// Check if I need a new Point
|
|
if (distance < MIN_DISTANCE_BETWEEN_POINTS) {
|
|
bool goOn = this->nextPoint();
|
|
if (!goOn) {
|
|
this->navigationFinished = true;
|
|
this->navigationStarted = false;
|
|
return courseCorrection; // End of navigation
|
|
}
|
|
}
|
|
|
|
double currentCourse = this->gps->course.deg();
|
|
int16_t correction = currentCourse - targetCourse;
|
|
if (correction > 180)
|
|
correction -= 360;
|
|
else if (correction < -180)
|
|
correction += 360;
|
|
|
|
courseCorrection.correction = correction;
|
|
courseCorrection.distance = distance;
|
|
return courseCorrection;
|
|
}
|
|
|
|
bool Navigation::addCurrentPosToRoute() {
|
|
Point p = this->currentLocation();
|
|
if (!p.isValid()) { return false; }
|
|
|
|
// First Point
|
|
if (this->route->getRouteInfo().totalPoints == 0) {
|
|
this->route->addPointToRoute(p);
|
|
this->lastPoint = p;
|
|
return true;
|
|
}
|
|
|
|
// Ever Point after the first
|
|
if (MIN_DISTANCE_BETWEEN_POINTS <= this->distanceBetween(p, this->lastPoint)) {
|
|
this->route->addPointToRoute(p);
|
|
this->lastPoint = p;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
Point Navigation::currentLocation() {
|
|
Point p;
|
|
//TODO: check if the position is possible the true location
|
|
if (this->gps) {
|
|
p.lat = this->gps->getLatitude();
|
|
p.lon = this->gps->getLongitude();
|
|
}
|
|
return p;
|
|
}
|
|
|
|
bool Navigation::nextPoint() {
|
|
if (!this->navigationStarted)
|
|
return false;
|
|
Point newTargetPoint = this->route->getNextPoint();
|
|
return this->setTargetPoint(newTargetPoint);
|
|
}
|
|
|
|
bool Navigation::setTargetPoint(Point target) {
|
|
if (target.isValid()) {
|
|
this->targetPoint = target;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
double Navigation::distanceBetween(Point p1, Point p2) {
|
|
if (p1.isValid() && p2.isValid())
|
|
return NavigationUtils::distanceBetween(p1, p2);
|
|
return -1;
|
|
}
|
|
|
|
double Navigation::courseTo(Point p1, Point p2) {
|
|
if (p1.isValid() && p2.isValid())
|
|
return NavigationUtils::courseTo(p1, p2);
|
|
return -1;
|
|
}
|