156 lines
4.7 KiB
C++
156 lines
4.7 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(const SensorData* sensorData, Route* route) {
|
|
this->sensorData = sensorData;
|
|
this->init(route);
|
|
}
|
|
|
|
void Navigation::init(Route* route) {
|
|
if (route)
|
|
this->route = route;
|
|
else
|
|
this->route = new Route();
|
|
|
|
Component::loopDelay = Navigation::loopDelay;
|
|
}
|
|
|
|
Navigation::~Navigation() {
|
|
delete this->route;
|
|
}
|
|
|
|
void Navigation::run() {}
|
|
|
|
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;
|
|
}
|
|
|
|
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 (Navigation::minDisBetweenPoints <= distance
|
|
&& Navigation::maxDisBetweenPoints >= 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);
|
|
// }
|
|
|
|
int16_t Navigation::calculateCourseCorrection(Point& point) {
|
|
int16_t targetCourse = point.courseTo(this->targetPoint);
|
|
int16_t correctionCourse;
|
|
|
|
if (this->sensorData->getCalcAzimuthState() == CalcAzimuth::State::Good
|
|
|| this->sensorData->getCalcAzimuthState() == CalcAzimuth::State::Super)
|
|
{
|
|
correctionCourse = targetCourse - this->sensorData->getCalcAzimuth();
|
|
this->lastUsedCalcAzimuth = true;
|
|
} else {
|
|
correctionCourse = targetCourse - this->sensorData->getRealAzimuth();
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|