134 lines
3.4 KiB
C++
134 lines
3.4 KiB
C++
/**
|
|
* @file navigation.cpp
|
|
* @author Alexander Klein (alex@kleiax.de)
|
|
* @brief
|
|
* @version 0.1
|
|
* @date 2022-01-31
|
|
*
|
|
* @copyright Copyright (c) 2022
|
|
*
|
|
*/
|
|
|
|
#include "navigation.h"
|
|
|
|
Navigation::Navigation(uint8_t rx, uint8_t tx, Route* route) {
|
|
this->gps = new TinyGPSPlus;
|
|
Serial1.begin(9600, SERIAL_8N1, rx, tx);
|
|
if (route)
|
|
this->route = route;
|
|
else
|
|
this->route = new Route();
|
|
}
|
|
|
|
Navigation::~Navigation() {
|
|
delete this->gps;
|
|
if (this->route)
|
|
delete this->route;
|
|
}
|
|
|
|
void Navigation::loop() {
|
|
while (Serial1.available())
|
|
gps->encode(Serial1.read());
|
|
}
|
|
|
|
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::distanceBetwenn(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 = 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->distanceBetwenn(p, this->lastPoint)) {
|
|
this->route->addPointToRoute(p);
|
|
this->lastPoint = p;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
Point Navigation::currentLocation() {
|
|
Point p;
|
|
if (this->gps->location.isValid()) {
|
|
p.lat = this->gps->location.lat();
|
|
p.lon = this->gps->location.lng();
|
|
}
|
|
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::distanceBetwenn(Point p1, Point p2) {
|
|
if (p1.isValid() && p2.isValid())
|
|
return TinyGPSPlus::distanceBetween(p1.lat, p1.lon, p2.lat, p2.lon);
|
|
return -1;
|
|
}
|
|
|
|
double Navigation::courseTo(Point p1, Point p2) {
|
|
if (p1.isValid() && p2.isValid())
|
|
return TinyGPSPlus::courseTo(p1.lat, p1.lon, p2.lat, p2.lon);
|
|
return -1;
|
|
} |