Bachelorarbeit-Rover/lib/Navigation/navigation.cpp

/**
 * @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->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;
    if (this->ntripClient)
        delete this->ntripClient;
}

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;
    this->addChildComponent(this->ntripClient);
}

void Navigation::run() {
    this->updateMagneticDeclination();
}


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 (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);
}

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;
}

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;
}