kleiax/Bachelorarbeit-Rover
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 1 Wiki Activity

spelling

Browse Source 
circle mode for autopilot
menu entry for circle mode
This commit is contained in:
kleiax
2023-08-14 08:25:24 +02:00
parent 9adbdece7f
commit 010544a35a
5 changed files with 97 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
+70 -56
View File
@@ -42,7 +42,7 @@ void MenuAutopilot::printPage() const {
lineOne = "Status: ";
lineTwo = "";
switch (this->autopilot->getState()) {
case Autopilot::State::InsufficientAccuarcy :
case Autopilot::State::InsufficientAccuracy :
lineTwo = "Err: LowAccuracy";
break;
@@ -75,15 +75,20 @@ void MenuAutopilot::printPage() const {
break;
default:
lineTwo = "UNKOWN";
lineTwo = "UNKOWN - ";
lineTwo.concat(static_cast<int>(this->autopilot->getState()));
break;
}
break;
}
case 1:
if (!navigationStarted)
if (!navigationStarted) {
lineOne = "Navigation is";
lineTwo = "not started";
break;
}
lineOne = "Distance: ";
lineOne.concat(distanceString);
lineTwo = "Turn: ";
@@ -100,39 +105,75 @@ void MenuAutopilot::printPage() const {
case 3: {
NTRIPClientStates status = this->driveManager->getNavigation()->getNTRIPClient()->getClientState();
lineOne = "NTRIP Client is";
uint8_t carrSoln = gpsData->flags.bits.carrSoln;
lineOne = "GNSS: ";
if (status == NTRIPClientStates::pushData)
lineTwo = "enabled";
if (carrSoln == 0)
lineOne.concat("None");
else if (carrSoln == 1)
lineOne.concat("Floating");
else if (carrSoln == 2)
lineOne.concat("Fixed");
else
lineOne.concat("UNKNOWN");
else if (status == NTRIPClientStates::notAvailable)
lineTwo = "not available";
lineOne.concat("No WiFi");
else
lineTwo = "disabled";
lineOne.concat("Offline");
lineTwo = "hAcc: ";
if (gpsData->fixType)
lineTwo.concat(gpsData->hAcc);
else
lineTwo.concat("0");
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";
case 4:
if (this->driveManager->getNavigation()->getCalcAzimuthState() == Navigation::CalcAzimuthState::Good
||this->driveManager->getNavigation()->getCalcAzimuthState() == Navigation::CalcAzimuthState::Super)
{
lineOne = "Calc Azi: ";
lineTwo = "State: ";
lineOne.concat(this->driveManager->getNavigation()->getCalcAzimuth());
switch (this->driveManager->getNavigation()->getCalcAzimuthState()) {
case Navigation::CalcAzimuthState::Bad :
lineTwo.concat("Bad");
break;
case Navigation::CalcAzimuthState::Good :
lineTwo.concat("Good");
break;
case Navigation::CalcAzimuthState::Invalid :
lineTwo.concat("Invalid");
break;
case Navigation::CalcAzimuthState::Ok :
lineTwo.concat("Ok");
break;
case Navigation::CalcAzimuthState::Super :
lineTwo.concat("Super");
break;
default:
lineTwo.concat("Unkown");
break;
}
} else {
lineOne = "Real Azi: ";
lineOne.concat(this->driveManager->getNavigation()->getAzimuth());
}
break;
}
case 5:
lineOne = "hAccuracy: ";
lineTwo = "Azimuth: ";
lineTwo.concat(this->driveManager->getNavigation()->getAzimuth());
if (gpsData->fixType)
lineOne.concat(gpsData->hAcc);
lineOne = "Loop mode is";
if (this->autopilot->getLoopMode())
lineTwo = "enabled";
else
lineOne.concat("0");
lineTwo = "disabled";
break;
case 6:
@@ -170,37 +211,6 @@ void MenuAutopilot::printPage() const {
lineTwo = "accuracy is low";
break;
case 11:
lineOne = "CalcAzi: ";
lineTwo = "State: ";
lineOne.concat(this->driveManager->getNavigation()->getCalcAzimuth());
switch (this->driveManager->getNavigation()->getCalcAzimuthState()) {
case Navigation::CalcAzimuthState::Bad :
lineTwo.concat("Bad");
break;
case Navigation::CalcAzimuthState::Good :
lineTwo.concat("Good");
break;
case Navigation::CalcAzimuthState::Invalid :
lineTwo.concat("Invalid");
break;
case Navigation::CalcAzimuthState::Ok :
lineTwo.concat("Ok");
break;
case Navigation::CalcAzimuthState::Super :
lineTwo.concat("Super");
break;
default:
lineTwo.concat("Unkown");
break;
}
break;
case 12:
lineOne = "Test rotate";
lineTwo = "180 degree";
@@ -231,6 +241,10 @@ void MenuAutopilot::printPage() const {
void MenuAutopilot::runCommand() const {
switch (this->getCurrentPage()) {
case 5:
this->autopilot->switchLoopMode();
break;
case 6:
this->autopilot->restart();
break;
src/driveModi/Modi/Autopilot/autopilot.cpp
+18 -8
View File
@@ -18,7 +18,7 @@ DirectionChangeSignal::DirectionChangeSignal(Navigation* navigation) {
}
DirectionChangeSignal::~DirectionChangeSignal() {
navigation->dissableCalcAzimuth();
navigation->disableCalcAzimuth();
}
void DirectionChangeSignal::action() {
@@ -59,7 +59,7 @@ void Autopilot::loop() {
void Autopilot::runAutopilot() {
switch (this->state) {
case State::InsufficientAccuarcy:
case State::InsufficientAccuracy:
this->askNavigationForOrder();
return;
@@ -97,6 +97,8 @@ void Autopilot::runAutopilot() {
break;
case State::TargetReached:
if (this->loopMode)
this->restartLoop();
break;
default:
@@ -163,12 +165,12 @@ void Autopilot::beginRotate() {
}
void Autopilot::rotate() {
if (abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < 5) {
if (abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct) {
this->endRotate();
return;
}
if ((abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < 15)
if ((abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct * 3)
&&
((this->courseCorrection.correction > 0
&& this->rotationAimAzimuth < this->navigation->getAzimuth())
@@ -217,18 +219,18 @@ void Autopilot::askNavigationForOrder() {
case Navigation::Status::InsufficientAccuracy:
this->lastState = this->state;
this->state = State::InsufficientAccuarcy;
this->state = State::InsufficientAccuracy;
this->moveControl->setSpeed(0);
this->moveControl->setRotationSpeed(0);
break;
case Navigation::Status::Unchanged:
if (this->state == State::InsufficientAccuarcy)
if (this->state == State::InsufficientAccuracy)
this->state = this->lastState;
break;
case Navigation::Status::Updated:
if (this->state == State::InsufficientAccuarcy)
if (this->state == State::InsufficientAccuracy)
this->state = this->lastState;
break;
@@ -241,8 +243,16 @@ void Autopilot::selfDriving() {
if (this->state != State::SelfDriving)
return;
if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBerforeAct)
if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBeforeAct)
this->beginRotate();
else
this->drive();
}
void Autopilot::restartLoop() {
this->navigation->startNavigation();
this->state = State::SelfDriving;
this->routeInfo = this->navigation->getRouteInfo();
this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection, true);
this->updateDisplay = true;
}
src/driveModi/Modi/Autopilot/autopilot.h
+6 -2
View File
@@ -39,7 +39,7 @@ class DirectionChangeSignal : public DirectionChangeWrapper {
class Autopilot : public ManualControl {
public:
enum State {
InsufficientAccuarcy = -2,
InsufficientAccuracy = -2,
NoRoute = -1,
None = 0,
NavigationStarted,
@@ -109,6 +109,8 @@ class Autopilot : public ManualControl {
bool shouldUpdate();
void testRotate(int16_t degree);
void endRotate();
void switchLoopMode() { this->loopMode = !this->loopMode; }
bool getLoopMode() const { return this->loopMode; }
private:
void init();
@@ -119,6 +121,7 @@ class Autopilot : public ManualControl {
void checkButtonInput();
void askNavigationForOrder();
void selfDriving();
void restartLoop();
Navigation* navigation;
CourseCorrection courseCorrection;
@@ -129,9 +132,10 @@ class Autopilot : public ManualControl {
DirectionChangeSignal* directionChangeSignal;
bool updateDisplay = false;
bool loopMode = false;
uint8_t loopDelayMillis = 40;
uint8_t maxCourseDeviationBerforeAct = 5;
uint8_t maxCourseDeviationBeforeAct = 5;
uint16_t displayUpdateDelayMillis = 1000;
uint16_t autopilotChangeDelayMillis = 500;
uint32_t displayUpdateLastMillis = 0;