92 lines
2.6 KiB
C++
92 lines
2.6 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::printMenu() {
|
|
if (this->firstPrint)
|
|
this->init();
|
|
|
|
char buf1[17];
|
|
char buf2[17];
|
|
char unit[3];
|
|
|
|
bool navigationStarted = this->autopilot->getNavigationStarted();
|
|
bool navigationEnded = this->autopilot->getNavigationEnded();
|
|
bool selfDriving = this->autopilot->getSelfDriving();
|
|
bool selfDrivingAvailable = this->autopilot->getSelfDrivingAvailable();
|
|
|
|
// 999m -360°
|
|
CourseCorrection correction = this->autopilot->getCourseCorrection();
|
|
uint16_t distance = 0;
|
|
|
|
if (correction.distance < 1) {
|
|
distance = (uint16_t) (correction.distance * 100);
|
|
sprintf(unit, "cm");
|
|
} else if (correction.distance < 1000) {
|
|
distance = (uint16_t) correction.distance;
|
|
sprintf(unit, "m");
|
|
} else {
|
|
distance = (uint16_t) (correction.distance / 1000);
|
|
sprintf(unit, "km");
|
|
}
|
|
|
|
if (navigationStarted) {
|
|
sprintf(buf1 ,"Points %u/%u",this->routeInfo.currentPoint ,this->routeInfo.totalPoints);
|
|
if (selfDriving) {
|
|
sprintf(buf2, "Auto: %u%s %d", distance, unit, correction.correction);
|
|
} else {
|
|
if (selfDrivingAvailable) {
|
|
sprintf(buf2, "Self -> Auto");
|
|
} else {
|
|
sprintf(buf2, "Self: %u%s %d", distance, unit, correction.correction);
|
|
}
|
|
}
|
|
} else {
|
|
if (navigationEnded) {
|
|
sprintf(buf1, "Am Ziel");
|
|
sprintf(buf2, "angekommen.");
|
|
} else {
|
|
sprintf(buf1, "Keine Route");
|
|
sprintf(buf2, "vorhanden.");
|
|
}
|
|
}
|
|
|
|
if (this->lcd) {
|
|
this->lcd->clear();
|
|
this->lcd->setCursor(0, 0);
|
|
this->lcd->print(buf1);
|
|
this->lcd->setCursor(0, 1);
|
|
this->lcd->print(buf2);
|
|
|
|
}
|
|
std::cout << buf1 << " " << buf2 << std::endl;
|
|
}
|
|
|
|
void MenuAutopilot::update() {
|
|
if (millis() - this->lastMillis < this->minDelay)
|
|
return;
|
|
this->lastMillis = millis();
|
|
|
|
if (!this->autopilot->shouldUpdate())
|
|
return;
|
|
|
|
this->routeInfo = this->autopilot->getRouteInfo();
|
|
this->printMenu();
|
|
}
|
|
|
|
void MenuAutopilot::init() {
|
|
this->firstPrint = false;
|
|
this->driveManager->changeModus(Modi::Autopilot);
|
|
this->autopilot = (Autopilot*) this->driveManager->getDriveModiPtr();
|
|
this->routeInfo = this->autopilot->getRouteInfo();
|
|
}
|