From 8796da97b552cc2d61c2571b613d8b900f332dda Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Fri, 2 Jun 2023 22:30:21 +0200 Subject: [PATCH] - added wifi channel on boot screen - added wifi to systeminformation - added accuracy settings to capturee route and autopilot - fix data loss in calibrate compass - fix autopilot dont switch to ready for selfdriving --- include/network.h | 2 + lib/MotorControl/motorControl.h | 2 +- lib/Navigation/navigation.h | 3 ++ .../menuSysteminformatio.cpp | 8 +++- .../Systeminformation/menuSysteminformation.h | 1 + .../driveModi/Autopilot/menuAutopilot.cpp | 17 ++++++- .../CalibrateCompass/menuCalibrateCompass.cpp | 18 ++++++-- .../CaptureRoute/menuCaptureRoute.cpp | 25 +++++++++- .../driveModi/CaptureRoute/menuCaptureRoute.h | 2 + src/driveModi/Modi/Autopilot/autopilot.cpp | 14 +++--- src/main.cpp | 46 ++++++------------- src/network.cpp | 13 ++++++ 12 files changed, 104 insertions(+), 47 deletions(-) diff --git a/include/network.h b/include/network.h index 2a85d24..cf2cd9b 100644 --- a/include/network.h +++ b/include/network.h @@ -16,6 +16,7 @@ #include #include #include +#include #include "networkConfig.h" @@ -44,6 +45,7 @@ class Network { */ static bool connectWifi(); static bool connectEspNow(recieveCallbackPtr reci, sendCallbackPtr send); + static uint8_t getCurrentChannel(); /** * @brief Set all general settings to connect to a broker. diff --git a/lib/MotorControl/motorControl.h b/lib/MotorControl/motorControl.h index 4880a42..9632e99 100644 --- a/lib/MotorControl/motorControl.h +++ b/lib/MotorControl/motorControl.h @@ -21,7 +21,7 @@ #define PWMRES 8 #define POWERSTEPS 2 // A total of 20 levels ( 100 / SPEED_STEPS ) * RUN_MOTOR_CONTROL_DELAY = 500ms #define PWMMIN 55 -#define PWMMAX 80 // Max 98% of 2^PWM_RES +#define PWMMAX 90 // Max 98% of 2^PWM_RES /** * @brief A class which use PWM to control the power of DC Motor diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h index 56f4366..f9665b3 100644 --- a/lib/Navigation/navigation.h +++ b/lib/Navigation/navigation.h @@ -185,6 +185,9 @@ class Navigation { uint16_t getAzimuth() const { return this->azimuth; } QMC5883LCompass* getCompass() const { return this->compass; } + Point::Accuracy getMinAccuracy() const { return this->minAccuracy; } + void setMinAccuracy(Point::Accuracy accuracy) { this->minAccuracy = accuracy; } + /** * @brief Set the output status for PVTdata. * diff --git a/src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp b/src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp index 44793a2..9773c38 100644 --- a/src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp +++ b/src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp @@ -12,7 +12,7 @@ #include "menuSysteminformation.h" MenuSysteminformation::MenuSysteminformation(Battery* mainBattery) - : MenuInformationSites(5) { + : MenuInformationSites(6) { this->mainBattery = mainBattery; } @@ -44,6 +44,12 @@ void MenuSysteminformation::printPage() const { break; case 4: + lineOne = "Current WiFi"; + lineTwo = "channel: "; + lineTwo.concat(Network::getCurrentChannel()); + break; + + case 5: lineOne = "Kleiax Rover by"; lineTwo = "Alexander Klein"; break; diff --git a/src/SpecialMenus/Systeminformation/menuSysteminformation.h b/src/SpecialMenus/Systeminformation/menuSysteminformation.h index 2a046ab..cc51d39 100644 --- a/src/SpecialMenus/Systeminformation/menuSysteminformation.h +++ b/src/SpecialMenus/Systeminformation/menuSysteminformation.h @@ -17,6 +17,7 @@ #include "controlPadInput.h" #include "menuInformationSites.h" #include "battery.h" +#include "network.h" /** * @brief Prints information about the current system status. diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp index 4f7ac06..1c6bb46 100644 --- a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp +++ b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp @@ -158,6 +158,14 @@ void MenuAutopilot::printPage() const { 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; @@ -189,6 +197,13 @@ void MenuAutopilot::runCommand() const { 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; @@ -204,7 +219,7 @@ void MenuAutopilot::update() { } void MenuAutopilot::init() { - this->setCountPages(10); + this->setCountPages(11); this->driveManager->changeModus(Modi::Autopilot); this->autopilot = (Autopilot*) this->driveManager->getDriveModiPtr(); this->routeInfo = this->autopilot->getRouteInfo(); diff --git a/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp b/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp index 82c18f1..013a5f5 100644 --- a/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp +++ b/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp @@ -77,20 +77,25 @@ void MenuCalibrateCompass::printPage() const { break; case 7: + lineOne = "Reset for new"; + lineTwo = "calibration run"; + break; + + case 8: lineOne = "X min: "; lineOne.concat(this->caliCompass->getCallibrationData().data[0][0]); lineTwo = "X max: "; lineTwo.concat(this->caliCompass->getCallibrationData().data[0][1]); break; - case 8: + case 9: lineOne = "Y min: "; lineOne.concat(this->caliCompass->getCallibrationData().data[1][0]); lineTwo = "Y max: "; lineTwo.concat(this->caliCompass->getCallibrationData().data[1][1]); break; - case 9: + case 10: lineOne = "Z min: "; lineOne.concat(this->caliCompass->getCallibrationData().data[2][0]); lineTwo = "Z max: "; @@ -109,7 +114,7 @@ void MenuCalibrateCompass::init() { this->firstPrint = false; this->driveManager->changeModus(Modi::ManualControl); this->manualControl = (ManualControl*) this->driveManager->getDriveModiPtr(); - this->setCountPages(10); + this->setCountPages(11); this->updateDelay = 500; } @@ -125,7 +130,6 @@ void MenuCalibrateCompass::runCommand() const { case CalibrateCompass::State::Finished: this->manualControl->setCalibrateCompass(); this->caliCompass->useData(); - this->caliCompass->reset(); break; default: @@ -147,7 +151,11 @@ void MenuCalibrateCompass::runCommand() const { case 6: this->caliCompass->useData(); - break; + break; + + case 7: + this->caliCompass->reset(); + break; default: break; diff --git a/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp b/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp index 8d6846f..94da0a9 100644 --- a/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp +++ b/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp @@ -91,6 +91,14 @@ void MenuCaptureRoute::printPage() const { else lineOne.concat("0"); break; + + case 7: + lineOne = "Current minimal"; + if (this->driveManager->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM) + lineTwo = "accuracy is high"; + else + lineTwo = "accuracy is low"; + break; default: this->printDefault(); @@ -106,8 +114,23 @@ void MenuCaptureRoute::update() { this->printMenu(); } +void MenuCaptureRoute::runCommand() const { + switch (this->getCurrentPage()) + { + case 7: + 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 MenuCaptureRoute::init() { - this->setCountPages(7); + this->setCountPages(8); this->updateDelay = 500; this->driveManager->changeModus(Modi::CaptureRoute); this->captureRoute = (CaptureRoute*) this->driveManager->getDriveModiPtr(); diff --git a/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h b/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h index c29dfe0..3f321ea 100644 --- a/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h +++ b/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h @@ -42,6 +42,8 @@ class MenuCaptureRoute : public MenuDriveMode { */ void update() override; + void runCommand() const override; + private: void init() override; diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp index 46efabe..8fbc417 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.cpp +++ b/src/driveModi/Modi/Autopilot/autopilot.cpp @@ -53,6 +53,7 @@ void Autopilot::runAutopilot() { case State::NavigationStarted: this->askNavigationForOrder(); + this->checkButtonInput(); break; case State::GetToStartPoint: @@ -128,15 +129,14 @@ void Autopilot::checkButtonInput() { if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action) && millis() - this->lastAutopilotChangeMillis > this->autopilotChangeDelayMillis) { - if (this->state == State::SelfDrivingAvailable) { + if (this->state == State::SelfDrivingAvailable) this->state = State::SelfDriving; - this->updateDisplay = true; - this->lastAutopilotChangeMillis = millis(); - } else if (this->state == State::SelfDriving) { + else if (this->state == State::SelfDriving) this->state = State::SelfDrivingAvailable; - this->updateDisplay = true; - this->lastAutopilotChangeMillis = millis(); - } + else if (this->state == State::NavigationStarted) + this->state = State::GetToStartPoint; + this->updateDisplay = true; + this->lastAutopilotChangeMillis = millis(); } } diff --git a/src/main.cpp b/src/main.cpp index a02a11b..163feed 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -118,7 +118,7 @@ void setup() { lcd->backlight(); lcd->printf("%c Kleiax-Rover %c", wifiIndicator, wifiIndicator); lcd->setCursor(0, 1); - lcd->print("Press PS-Button"); + lcd->printf("WiFi channel %u", Network::getCurrentChannel()); lcdWrapper = new LcdWrapper(lcd); lcdWrapper->setCallback(lcdWrapperCallback); @@ -231,41 +231,25 @@ void makeMenu() { sys_m->setUpdateDelay(1500); rout_m->setLcd(lcdWrapper); - - // Entry for the main menu - MenuAction* mode_e = new MenuAction("Mode", mode_m); - MenuAction* gps_e = new MenuAction("GPS", gps_m); - MenuAction* pid_e = new MenuAction("PID", pid_m); - MenuAction* restart_e = new MenuAction("Restart", restart); - MenuAction* sys_e = new MenuAction("Systeminfo", sys_m); - MenuAction* rout_e = new MenuAction("Route", rout_m); - main_m->addEntry(mode_e); - main_m->addEntry(gps_e); - main_m->addEntry(rout_e); - main_m->addEntry(pid_e); - main_m->addEntry(sys_e); - main_m->addEntry(restart_e); + main_m->addEntry(new MenuAction("Mode", mode_m)); + main_m->addEntry(new MenuAction("GPS", gps_m)); + main_m->addEntry(new MenuAction("Route", rout_m)); + main_m->addEntry(new MenuAction("PID", pid_m)); + main_m->addEntry(new MenuAction("Systeminfo", sys_m)); + main_m->addEntry(new MenuAction("Restart", restart)); // Entry for the mode Menu - MenuAction* autopilot_e = new MenuAction("Autopilot", auto_m); - MenuAction* captureRoute_e = new MenuAction("Capture Route", cap_m); - MenuAction* consolControl_e = new MenuAction("Consol Control", dummy); - MenuAction* manualControl_e = new MenuAction("Manual Control", man_m); - MenuAction* testMode_e = new MenuAction("Test Mode", testM_m); - MenuAction* caliComp_e = new MenuAction("Gauge Compass", comp_m); - mode_m->addEntry(manualControl_e); - mode_m->addEntry(captureRoute_e); - mode_m->addEntry(autopilot_e); - mode_m->addEntry(caliComp_e); - mode_m->addEntry(testMode_e); - mode_m->addEntry(consolControl_e); + mode_m->addEntry(new MenuAction("Manual Control", man_m)); + mode_m->addEntry(new MenuAction("Capture Route", cap_m)); + mode_m->addEntry(new MenuAction("Autopilot", auto_m)); + mode_m->addEntry(new MenuAction("Gauge Compass", comp_m)); + mode_m->addEntry(new MenuAction("Test Mode", testM_m)); + mode_m->addEntry(new MenuAction("Consol Control", dummy)); // Entry for the PID Menu - MenuAction* pidl_e = new MenuAction("Left", pidl_m); - MenuAction* pidr_e = new MenuAction("Right", pidr_m); - pid_m->addEntry(pidl_e); - pid_m->addEntry(pidr_e); + pid_m->addEntry(new MenuAction("Left", pidl_m)); + pid_m->addEntry(new MenuAction("Right", pidr_m)); // Other config pidl_m->setMinMax(0, UINT8_MAX); diff --git a/src/network.cpp b/src/network.cpp index 8d1a1e0..baaa932 100644 --- a/src/network.cpp +++ b/src/network.cpp @@ -113,6 +113,19 @@ bool Network::connectEspNow(recieveCallbackPtr reci, sendCallbackPtr send) { return true; } +uint8_t Network::getCurrentChannel() { + uint8_t channel; + wifi_second_chan_t secondChannel; + if (esp_wifi_get_channel(&channel, &secondChannel) != ESP_OK) { + std::cout << "Network::getCurrentChannel - Error!" << std::endl; + return -1; + } + // std::cout << "Network::getCurrentChannel - Current WiFi channel: " + // << (int) channel << " second channel: " << (int) secondChannel + // << std::endl; + return channel; +} + bool Network::checkMQTT() { static uint64_t lastReconnectAttempt = 0; if (!mqtt_client->connected()) {