229 lines
7.0 KiB
C++
229 lines
7.0 KiB
C++
/**
|
|
* @file menuAutopilot.cpp
|
|
* @author Alexander Klein (alex@kleiax.de)
|
|
* @brief Contains an implementation of the class MenuAutopilot
|
|
* @version 0.1
|
|
* @date 2022-02-03
|
|
*
|
|
* @copyright Copyright (c) 2022
|
|
*
|
|
*/
|
|
|
|
#include "menuAutopilot.h"
|
|
|
|
void MenuAutopilot::printPage() const {
|
|
CourseCorrection correction = this->autopilot->getCourseCorrection();
|
|
UBX_NAV_PVT_data_t* gpsData = this->driveManager->getNavigation()->getUbxData();
|
|
bool navigationStarted = this->autopilot->getState() >= Autopilot::State::NavigationStarted;
|
|
|
|
String distanceString = "";
|
|
if (navigationStarted) {
|
|
uint16_t distance = 0;
|
|
if (correction.distance < 1) {
|
|
distance = (uint16_t) (correction.distance * 100);
|
|
distanceString.concat(distance);
|
|
distanceString.concat("cm");
|
|
} else if (correction.distance < 1000) {
|
|
distance = (uint16_t) correction.distance;
|
|
distanceString.concat(distance);
|
|
distanceString.concat("m");
|
|
} else {
|
|
distance = (uint16_t) (correction.distance / 1000);
|
|
distanceString.concat(distance);
|
|
distanceString.concat("km");
|
|
}
|
|
}
|
|
|
|
String lineOne = "No information";
|
|
String lineTwo = "available";
|
|
|
|
switch (this->getCurrentPage()) {
|
|
case 0: {
|
|
lineOne = "Status: ";
|
|
lineTwo = "";
|
|
switch (this->autopilot->getState()) {
|
|
case Autopilot::State::InsufficientAccuarcy :
|
|
lineTwo = "Err: LowAccuracy";
|
|
break;
|
|
|
|
case Autopilot::State::NoRoute :
|
|
lineTwo = "Err: No route";
|
|
break;
|
|
|
|
case Autopilot::State::NavigationStarted :
|
|
lineTwo = "Nav started";
|
|
break;
|
|
|
|
case Autopilot::State::GetToStartPoint :
|
|
lineTwo = "Drive to start";
|
|
break;
|
|
|
|
case Autopilot::State::SelfDrivingAvailable :
|
|
lineTwo = "Autopilot ready";
|
|
break;
|
|
|
|
case Autopilot::State::SelfDriving :
|
|
lineTwo = "Autopilot active";
|
|
break;
|
|
|
|
case Autopilot::State::TargetReached :
|
|
lineTwo = "Target reached";
|
|
break;
|
|
|
|
default:
|
|
lineTwo = "UNKOWN";
|
|
break;
|
|
}
|
|
break;
|
|
}
|
|
|
|
case 1:
|
|
if (!navigationStarted)
|
|
break;
|
|
lineOne = "Distance: ";
|
|
lineOne.concat(distanceString);
|
|
lineTwo = "Turn: ";
|
|
lineTwo.concat(correction.correction);
|
|
break;
|
|
|
|
case 2:
|
|
lineOne = "Target waypoint";
|
|
lineTwo = "";
|
|
lineTwo.concat(this->autopilot->getRouteInfo().currentPoint);
|
|
lineTwo.concat(" from ");
|
|
lineTwo.concat(this->autopilot->getRouteInfo().totalPoints);
|
|
break;
|
|
|
|
case 3: {
|
|
NTRIPClientStates status = this->driveManager->getNavigation()->getNTRIPClient()->getClientState();
|
|
lineOne = "NTRIP Client is";
|
|
|
|
if (status == NTRIPClientStates::pushData)
|
|
lineTwo = "enabled";
|
|
else if (status == NTRIPClientStates::notAvailable)
|
|
lineTwo = "not available";
|
|
else
|
|
lineTwo = "disabled";
|
|
break;
|
|
}
|
|
|
|
case 4: {
|
|
lineOne = "Carrier Solution";
|
|
uint8_t carrSoln = gpsData->flags.bits.carrSoln;
|
|
if (carrSoln == 0)
|
|
lineTwo = "None";
|
|
else if (carrSoln == 1)
|
|
lineTwo = "Floating";
|
|
else if (carrSoln == 2)
|
|
lineTwo = "Fixed";
|
|
else
|
|
lineTwo = "UNKNOWN";
|
|
break;
|
|
}
|
|
|
|
case 5:
|
|
lineOne = "hAccuracy: ";
|
|
lineTwo = "Azimuth: ";
|
|
lineTwo.concat(this->driveManager->getNavigation()->getAzimuth());
|
|
if (gpsData->fixType)
|
|
lineOne.concat(gpsData->hAcc);
|
|
else
|
|
lineOne.concat("0");
|
|
break;
|
|
|
|
case 6:
|
|
lineOne = "Restart the";
|
|
lineTwo = "Autopilot";
|
|
break;
|
|
|
|
case 7:
|
|
lineOne = "Freeze target is";
|
|
if (this->targetFreezed)
|
|
lineTwo = "activated";
|
|
else
|
|
lineTwo = "deactivated";
|
|
break;
|
|
|
|
case 8:
|
|
lineOne = "MinDisToPoint:";
|
|
lineTwo = "";
|
|
lineTwo.concat(this->minDistance);
|
|
lineTwo.concat(" - Increase");
|
|
break;
|
|
|
|
case 9:
|
|
lineOne = "MinDisToPoint:";
|
|
lineTwo = "";
|
|
lineTwo.concat(this->minDistance);
|
|
lineTwo.concat(" - Decrease");
|
|
break;
|
|
|
|
case 10:
|
|
lineOne = "Current minimal";
|
|
if (this->driveManager->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM)
|
|
lineTwo = "accuracy is high";
|
|
else
|
|
lineTwo = "accuracy is low";
|
|
break;
|
|
|
|
default:
|
|
this->printDefault();
|
|
return;
|
|
}
|
|
|
|
this->print(lineOne, lineTwo);
|
|
}
|
|
|
|
void MenuAutopilot::runCommand() const {
|
|
switch (this->getCurrentPage()) {
|
|
case 6:
|
|
this->autopilot->restart();
|
|
break;
|
|
|
|
case 7:
|
|
if (this->targetFreezed) {
|
|
this->targetFreezed = false;
|
|
this->driveManager->getNavigation()->freezeTargetPoint(false);
|
|
} else {
|
|
this->targetFreezed = true;
|
|
this->driveManager->getNavigation()->freezeTargetPoint();
|
|
}
|
|
break;
|
|
|
|
case 8:
|
|
this->minDistance = this->driveManager->getNavigation()->increaseMinDistanceToReachPoint();
|
|
break;
|
|
|
|
case 9:
|
|
this->minDistance = this->driveManager->getNavigation()->decreaseMinDistanceToReachPoint();
|
|
break;
|
|
|
|
case 10:
|
|
if (this->driveManager->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM)
|
|
this->driveManager->getNavigation()->setMinAccuracy(Point::Accuracy::none);
|
|
else
|
|
this->driveManager->getNavigation()->setMinAccuracy(Point::Accuracy::twoDigOfCM);
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
void MenuAutopilot::update() {
|
|
if (!this->autopilot->shouldUpdate())
|
|
return;
|
|
|
|
this->routeInfo = this->autopilot->getRouteInfo();
|
|
this->printMenu();
|
|
}
|
|
|
|
void MenuAutopilot::init() {
|
|
this->setCountPages(11);
|
|
this->driveManager->changeModus(Modi::Autopilot);
|
|
this->autopilot = (Autopilot*) this->driveManager->getDriveModiPtr();
|
|
this->routeInfo = this->autopilot->getRouteInfo();
|
|
this->driveManager->getNavigation()->increaseMinDistanceToReachPoint();
|
|
this->minDistance = this->driveManager->getNavigation()->decreaseMinDistanceToReachPoint();
|
|
}
|