From e48f680fecbf4887386f5964bbd5a08cae634c3a Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Sat, 23 Sep 2023 22:47:07 +0200 Subject: [PATCH] quite some fixes --- include/networkConfig.h | 5 +++-- lib/Network/network.cpp | 6 +++-- lib/Sensors/sensorData.cpp | 22 ++++++++++++------- lib/Sensors/sensorData.h | 6 +++-- .../SensorData/menuSensorData.cpp | 17 +++++++------- src/SpecialMenus/driveModi/menuDriveMode.cpp | 4 ---- src/SpecialMenus/driveModi/menuDriveMode.h | 5 ----- src/driveModi/Modi/Autopilot/autopilot.cpp | 22 +++++++++++++------ src/driveModi/Modi/Autopilot/autopilot.h | 1 + .../Modi/CaptureRoute/captureRoute.cpp | 21 +++++++++++------- .../Modi/CaptureRoute/captureRoute.h | 5 ++--- src/driveModi/driveModi.cpp | 1 + src/driveModi/driveModi.h | 2 ++ src/main.cpp | 1 + 14 files changed, 69 insertions(+), 49 deletions(-) diff --git a/include/networkConfig.h b/include/networkConfig.h index ebb265b..4dafd0e 100644 --- a/include/networkConfig.h +++ b/include/networkConfig.h @@ -14,7 +14,7 @@ #include -#define HOTSPOT +#define RHEDE #ifdef HOTSPOT namespace NetworkConfig { @@ -51,8 +51,9 @@ namespace MqttConfig { namespace NtripConfig { const char host[] = "rtk2go.com"; const uint16_t port = 2101; - const char mountPoint[] = "GER-Dortmund"; + // const char mountPoint[] = "GER-Dortmund"; // const char mountPoint[] = "GER-Papenburg"; + const char mountPoint[] = "Rtkramerntrip"; const char user[] = "alklein1@gmx.de"; const char password[] = "none"; } diff --git a/lib/Network/network.cpp b/lib/Network/network.cpp index d67f65a..40456e0 100644 --- a/lib/Network/network.cpp +++ b/lib/Network/network.cpp @@ -19,6 +19,8 @@ Network::Network(const char *ssid, const char *passphrase) { } Network::Network(const char *ssid, const char *passphrase, NetworkAdresses adresses) { + this->adresses = adresses; + if (!WiFi.mode(WIFI_AP_STA)) std::cout << "Network::connectWiFi failed WiFi.mode" << std::endl; @@ -81,8 +83,8 @@ void Network::printIPs() { } std::cout << "WiFi is connected to" << std::endl; std::cout << "IP address: " << std::endl; - std::cout << this->adresses.localIP << std::endl; - std::cout << "WiFi MAC Address: " << WiFi.macAddress() << std::endl << std::endl; + std::cout << WiFi.localIP().toString().c_str() << std::endl; + std::cout << "WiFi MAC Address: " << WiFi.macAddress().c_str() << std::endl << std::endl; } uint8_t Network::getCurrentChannel() { diff --git a/lib/Sensors/sensorData.cpp b/lib/Sensors/sensorData.cpp index a3de617..7c62193 100644 --- a/lib/Sensors/sensorData.cpp +++ b/lib/Sensors/sensorData.cpp @@ -12,7 +12,6 @@ #include "sensorData.h" bool SensorData::outputStatusPrintPVTdata = false; -bool SensorData::newData = false; uint32_t SensorData::ubxUpdateTimeStatic = 0; UBX_NAV_PVT_data_t* SensorData::ubxDataStatic = nullptr; @@ -161,7 +160,6 @@ void SensorData::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) { void SensorData::savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) { SensorData::printPVTdata(ubxDataStruct); - SensorData::newData = true; SensorData::ubxDataStatic = ubxDataStruct; SensorData::ubxUpdateTimeStatic = millis(); } @@ -189,12 +187,9 @@ void SensorData::runAsChild() { if (this->gnss) { this->gnss->checkUblox(); this->gnss->checkCallbacks(); - } - - if (SensorData::newData) { - SensorData::newData = false; - // TODO: Update data - } + if (SensorData::ubxUpdateTimeStatic != this->lastUbxUpdate) + this->updateUbxData(); + } } void SensorData::initGnss() { @@ -210,3 +205,14 @@ void SensorData::initGnss() { this->gnss->setNavigationFrequency(1); this->gnss->setAutoPVT(true); } + +void SensorData::updateUbxData() { + this->gnssData = SensorData::ubxDataStatic; + this->lastUbxUpdate = SensorData::ubxUpdateTimeStatic; + + Point::Coordinates coords; + coords.lat = this->gnssData->lat / 10000000.0; + coords.lon = this->gnssData->lon / 10000000.0; + + this->currentPosition = Point(coords, this->gnssData->hAcc); +} diff --git a/lib/Sensors/sensorData.h b/lib/Sensors/sensorData.h index 71fcf54..29656e1 100644 --- a/lib/Sensors/sensorData.h +++ b/lib/Sensors/sensorData.h @@ -70,6 +70,8 @@ class SensorData : public Component { void run() override; void runAsChild() override; void initGnss(); + void updateUbxData(); + QMC5883LCompass* realCompass = nullptr; CalcAzimuth* calcCompass = nullptr; @@ -93,8 +95,9 @@ class SensorData : public Component { uint16_t port; int16_t realAzimuth = INT16_MAX; int16_t calcAzimuth = INT16_MAX; + uint32_t lastUbxUpdate = 0; - float yawPitchRoll[3]; + float yawPitchRoll[3] {0, 0, 0}; // static static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct); @@ -103,7 +106,6 @@ class SensorData : public Component { static UBX_NAV_PVT_data_t* ubxDataStatic; static uint32_t ubxUpdateTimeStatic; static bool outputStatusPrintPVTdata; - static bool newData; }; #endif //SENSOR_DATA_H diff --git a/src/SpecialMenus/SensorData/menuSensorData.cpp b/src/SpecialMenus/SensorData/menuSensorData.cpp index 13e74f2..2a6f86e 100644 --- a/src/SpecialMenus/SensorData/menuSensorData.cpp +++ b/src/SpecialMenus/SensorData/menuSensorData.cpp @@ -14,6 +14,7 @@ MenuSensorData::MenuSensorData(SensorData* sensorData) : MenuInformationSites(7) { this->sensorData = sensorData; + this->noEqualLeft = true; } void MenuSensorData::printPage() const { @@ -81,20 +82,20 @@ void MenuSensorData::printPage() const { lineTwo = "Ntrip: "; uint8_t carrSoln = gpsData->flags.bits.carrSoln; if (carrSoln == 0) - lineOne = "None"; + lineOne.concat("None"); else if (carrSoln == 1) - lineOne = "Floating"; + lineOne.concat("Floating"); else if (carrSoln == 2) - lineOne = "Fixed"; + lineOne.concat("Fixed"); else lineOne = "UNKNOWN"; NTRIPClientStates status = this->sensorData->getNtripState(); if (status == NTRIPClientStates::pushData) - lineTwo = "enabled"; + lineTwo.concat("enabled"); else if (status == NTRIPClientStates::notAvailable) - lineTwo = " N/A"; + lineTwo.concat(" N/A"); else - lineTwo = "disabled"; + lineTwo.concat("disabled"); } break; @@ -105,8 +106,8 @@ void MenuSensorData::printPage() const { lineOne.concat(gpsData->hAcc); lineTwo.concat(gpsData->numSV); } else { - lineOne = "-/-"; - lineTwo = "-/-"; + lineOne.concat("-/-"); + lineTwo.concat("-/-"); } break; diff --git a/src/SpecialMenus/driveModi/menuDriveMode.cpp b/src/SpecialMenus/driveModi/menuDriveMode.cpp index ab42129..0fa91ba 100644 --- a/src/SpecialMenus/driveModi/menuDriveMode.cpp +++ b/src/SpecialMenus/driveModi/menuDriveMode.cpp @@ -24,7 +24,3 @@ void MenuDriveMode::left() { this->leaved = true; } } - -void MenuDriveMode::no() { - this->left(); -} diff --git a/src/SpecialMenus/driveModi/menuDriveMode.h b/src/SpecialMenus/driveModi/menuDriveMode.h index 7e4ad87..ae94233 100644 --- a/src/SpecialMenus/driveModi/menuDriveMode.h +++ b/src/SpecialMenus/driveModi/menuDriveMode.h @@ -32,11 +32,6 @@ class MenuDriveMode : public MenuInformationSites { */ void left() override; - /** - * @brief Calls left. - */ - void no() override; - protected: void configureOnLeave() {} diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp index 7671518..9a1da0c 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.cpp +++ b/src/driveModi/Modi/Autopilot/autopilot.cpp @@ -18,20 +18,20 @@ DirectionChangeSignal::DirectionChangeSignal(Autopilot* pilot) { } DirectionChangeSignal::~DirectionChangeSignal() { - pilot->getSensorData()->getCalcCompass()->disableCalcAzimuth(); + CalcAzimuth* calcAzimuth = pilot->getSensorData()->getCalcCompass(); + if (calcAzimuth) + calcAzimuth->disableCalcAzimuth(); } void DirectionChangeSignal::action() { - pilot->getSensorData()->getCalcCompass()->drivingDirectionChange(pilot->getSensorData()->getCurrentPos()); + CalcAzimuth* calcAzimuth = pilot->getSensorData()->getCalcCompass(); + if (calcAzimuth) + calcAzimuth->drivingDirectionChange(pilot->getSensorData()->getCurrentPos()); } Autopilot::Autopilot() { - this->setInputMode(ManualControl::InputMode::Digital); - this->directionChangeSignal = new DirectionChangeSignal(this); - this->setDirectionChangeCallback(this->directionChangeSignal); - this->navigation = new Navigation(this->sensorData); - this->init(); + } Autopilot::~Autopilot() { @@ -247,3 +247,11 @@ void Autopilot::restartLoop() { this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection, true); this->updateDisplay = true; } + +void Autopilot::afterActivate() { + this->setInputMode(ManualControl::InputMode::Digital); + this->directionChangeSignal = new DirectionChangeSignal(this); + this->setDirectionChangeCallback(this->directionChangeSignal); + this->navigation = new Navigation(this->sensorData); + this->init(); +} diff --git a/src/driveModi/Modi/Autopilot/autopilot.h b/src/driveModi/Modi/Autopilot/autopilot.h index 3a709be..c6a81e4 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.h +++ b/src/driveModi/Modi/Autopilot/autopilot.h @@ -102,6 +102,7 @@ class Autopilot : public ManualControl { void askNavigationForOrder(); void selfDriving(); void restartLoop(); + void afterActivate() override; Navigation* navigation; CourseCorrection courseCorrection; diff --git a/src/driveModi/Modi/CaptureRoute/captureRoute.cpp b/src/driveModi/Modi/CaptureRoute/captureRoute.cpp index 703b201..cb58e4e 100644 --- a/src/driveModi/Modi/CaptureRoute/captureRoute.cpp +++ b/src/driveModi/Modi/CaptureRoute/captureRoute.cpp @@ -10,16 +10,14 @@ */ #include "driveModi/Modi/CaptureRoute/captureRoute.h" - -CaptureRoute::CaptureRoute() { - this->navigation = new Navigation(this->sensorData); - this->navigation->getRoute()->clear(); - this->sensorData->getNtripClient()->setAutoReconnect(true); - this->routeInfo = navigation->getRouteInfo(); -} +#include "captureRoute.h" CaptureRoute::~CaptureRoute() { - // this->navigation->getNTRIPClient()->setActivated(false); + if (this->navigation) + delete this->navigation; + + if (this->sensorData->getNtripClient()) + this->sensorData->getNtripClient()->setActivated(false); } void CaptureRoute::run() { @@ -34,6 +32,13 @@ void CaptureRoute::run() { } } +void CaptureRoute::afterActivate() { + this->navigation = new Navigation(this->sensorData); + this->navigation->getRoute()->clear(); + this->sensorData->getNtripClient()->setAutoReconnect(true); + this->routeInfo = navigation->getRouteInfo(); +} + double CaptureRoute::getDistanceToLastPoint() const { if (!this->lastSavedPoint.isInit()) return 0; diff --git a/src/driveModi/Modi/CaptureRoute/captureRoute.h b/src/driveModi/Modi/CaptureRoute/captureRoute.h index 44b3a4e..48caa1a 100644 --- a/src/driveModi/Modi/CaptureRoute/captureRoute.h +++ b/src/driveModi/Modi/CaptureRoute/captureRoute.h @@ -27,8 +27,6 @@ */ class CaptureRoute : public ManualControl { public: - CaptureRoute(); - /** * @brief Destroy the Capture Route object * @@ -68,8 +66,9 @@ class CaptureRoute : public ManualControl { bool shouldUpdate(); private: void run() override; + void afterActivate() override; - Navigation* navigation; + Navigation* navigation = nullptr; RouteInfo routeInfo; Point lastSavedPoint; Navigation::Status status = Navigation::Status::Complete; diff --git a/src/driveModi/driveModi.cpp b/src/driveModi/driveModi.cpp index fb0aa25..1cbed00 100644 --- a/src/driveModi/driveModi.cpp +++ b/src/driveModi/driveModi.cpp @@ -27,4 +27,5 @@ void DriveModi::activate(DriveModiParams params){ void DriveModi::init() { this->moveControl->setDrivingStatus(MoveControl::Status::Drive); this->loopDelay = 40; + this->afterActivate(); } diff --git a/src/driveModi/driveModi.h b/src/driveModi/driveModi.h index 46bbb5a..33d9f70 100644 --- a/src/driveModi/driveModi.h +++ b/src/driveModi/driveModi.h @@ -60,6 +60,8 @@ class DriveModi : public Component { double getMaxRotation() const { return this->maxRotationSpeed; } protected: + virtual void afterActivate() {} + MoveControl *moveControl; const ControlPadInput* input; const SensorData* sensorData; diff --git a/src/main.cpp b/src/main.cpp index 6646e51..9f7f5ab 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -114,6 +114,7 @@ void setup() { sensorData = new SensorData(); sensorData->enableGnss(spiPort, PinNumbers::gnssSpiCs); sensorData->enableRealCompass(); + sensorData->enableNtrip(NtripConfig::host, NtripConfig::port, NtripConfig::mountPoint, NtripConfig::user, NtripConfig::password); // sensorData->enableGyroskop(); driveManager = new DriveManager(&moveController, sensorData, controlPad->getControlPadDataPtr());