From faaa3603a76ba091423154519c0cfcea7cd8fa65 Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Sun, 3 Sep 2023 12:36:08 +0200 Subject: [PATCH 1/3] - reafctor --- .../driveModi/Autopilot/menuAutopilot.cpp | 19 +++++++++-------- .../CalibrateCompass/menuCalibrateCompass.h | 3 ++- src/driveModi/driveManager.h | 21 ++++++------------- 3 files changed, 18 insertions(+), 25 deletions(-) diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp index 523e532..cb3bdd0 100644 --- a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp +++ b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp @@ -131,30 +131,31 @@ void MenuAutopilot::printPage() const { } case 4: - if (this->driveManager->getNavigation()->getCalcAzimuthState() == Navigation::CalcAzimuthState::Good - ||this->driveManager->getNavigation()->getCalcAzimuthState() == Navigation::CalcAzimuthState::Super) + + if (this->autopilot->getSensorData().getCalcAzimuth() == CalcAzimuth::CalcAzimuthState::Good + ||this->autopilot->getSensorData().getCalcAzimuth() == CalcAzimuth::CalcAzimuthState::Super) { lineOne = "Calc Azi: "; lineTwo = "State: "; - lineOne.concat(this->driveManager->getNavigation()->getCalcAzimuth()); - switch (this->driveManager->getNavigation()->getCalcAzimuthState()) { - case Navigation::CalcAzimuthState::Bad : + lineOne.concat(this->autopilot->getSensorData().getCalcAzimuth()); + switch (this->autopilot->getSensorData().getCalcAzimuthState()) { + case CalcAzimuth::CalcAzimuthState::Bad : lineTwo.concat("Bad"); break; - case Navigation::CalcAzimuthState::Good : + case CalcAzimuth::CalcAzimuthState::Good : lineTwo.concat("Good"); break; - case Navigation::CalcAzimuthState::Invalid : + case CalcAzimuth::CalcAzimuthState::Invalid : lineTwo.concat("Invalid"); break; - case Navigation::CalcAzimuthState::Ok : + case CalcAzimuth::CalcAzimuthState::Ok : lineTwo.concat("Ok"); break; - case Navigation::CalcAzimuthState::Super : + case CalcAzimuth::CalcAzimuthState::Super : lineTwo.concat("Super"); break; diff --git a/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h b/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h index f17438b..0eee4c2 100644 --- a/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h +++ b/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h @@ -13,6 +13,7 @@ #define MENU_CALIBRATE_COMPASS_H #include "SpecialMenus/driveModi/menuDriveMode.h" +#include "" #include "calibrateCompass.h" /** @@ -44,7 +45,7 @@ class MenuCalibrateCompass : public MenuDriveMode { void runCommand() const override; private: - ManualControl* manualControl; + CalibrateCompassM* caliCompassMode; CalibrateCompass* caliCompass; }; diff --git a/src/driveModi/driveManager.h b/src/driveModi/driveManager.h index 3841c75..c29bfcd 100644 --- a/src/driveModi/driveManager.h +++ b/src/driveModi/driveManager.h @@ -23,12 +23,12 @@ #include "debugTimes.h" #include "controlPadInput.h" #include "component.h" +#include "sensorData.h" // All Drive Modi #include "driveModi/Modi/ManualControl/manualControl.h" #include "driveModi/Modi/CaptureRoute/captureRoute.h" #include "driveModi/Modi/Autopilot/autopilot.h" -#include "driveModi/Modi/ConsolControl/consolControl.h" #include "driveModi/Modi/TestMode/testMode.h" /** @@ -40,7 +40,6 @@ enum class Modi { ManualControl, CaptureRoute, Autopilot, - ConsolControl, TestMode }; @@ -53,7 +52,8 @@ class DriveManager : public Component { * * @param moveControl */ - DriveManager(MoveControl *moveControl, SPIClass *spiPort, const ControlPadInput *input, bool wifi = false); + DriveManager(MoveControl *moveControl, SensorData* sensorData, ControlPadInput *input, bool wifi = false); + DriveManager(DriveModiParams params, bool wifi = false); /** * @brief Destroy the Drive Manager object @@ -61,14 +61,6 @@ class DriveManager : public Component { */ ~DriveManager(); - /** - * @brief Increment the DriveModi enum - * Than calls changeModus - * - * @see Modi - */ - void nextModus(); - /** * @brief Change the DriveModi to a specific value * @@ -106,14 +98,13 @@ class DriveManager : public Component { Modi getDriveModi() const { return this->currentModus; } private: - void run() override; + void run() override {}; + void init(bool wifi); Modi currentModus = Modi::Off; - MoveControl *moveControl; - const ControlPadInput* input; DriveModi *currentModusPtr = nullptr; Navigation* navigation; - SPIClass* spiPort; + DriveModiParams driveModiParams; }; From 5f650982a72bed9199588c0fe609cff86aad6f5f Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Sun, 3 Sep 2023 12:36:16 +0200 Subject: [PATCH 2/3] refactore --- doc/TODO allgemein.txt | 8 +- lib/CalcAzimuth/calcAzimuth.cpp | 92 ++++++++++++ lib/CalcAzimuth/calcAzimuth.h | 50 +++++++ lib/Navigation/navigation.cpp | 63 -------- lib/Navigation/navigation.h | 20 +-- lib/Point/point.cpp | 102 +++++++++++++ lib/Point/point.h | 139 ++++++++++++++++++ lib/Route/route.cpp | 93 ------------ lib/Route/route.h | 123 +--------------- lib/Sensors/senors.cpp | 26 ++-- lib/Sensors/sensorData.h | 40 +++++ lib/Sensors/sensors.h | 16 +- src/driveModi/Modi/Autopilot/autopilot.cpp | 4 +- src/driveModi/Modi/Autopilot/autopilot.h | 3 +- .../CalibrateCompass/calibrateCompassM.cpp | 24 +++ .../Modi/CalibrateCompass/calibrateCompassM.h | 29 ++++ .../Modi/CaptureRoute/captureRoute.cpp | 4 +- .../Modi/CaptureRoute/captureRoute.h | 2 +- .../Modi/ConsolControl/consolControl.cpp | 13 -- .../Modi/ConsolControl/consolControl.h | 20 --- .../Modi/ManualControl/manualControl.cpp | 21 +-- .../Modi/ManualControl/manualControl.h | 10 +- src/driveModi/Modi/Racing/racing.cpp | 0 src/driveModi/Modi/Racing/racing.h | 0 src/driveModi/Modi/TestMode/testMode.cpp | 4 +- src/driveModi/Modi/TestMode/testMode.h | 2 +- src/driveModi/driveManager.cpp | 51 +++---- src/driveModi/driveModi.cpp | 19 ++- src/driveModi/driveModi.h | 19 ++- src/main.cpp | 11 +- 30 files changed, 578 insertions(+), 430 deletions(-) create mode 100644 lib/CalcAzimuth/calcAzimuth.cpp create mode 100644 lib/CalcAzimuth/calcAzimuth.h create mode 100644 lib/Point/point.cpp create mode 100644 lib/Point/point.h create mode 100644 src/driveModi/Modi/CalibrateCompass/calibrateCompassM.cpp create mode 100644 src/driveModi/Modi/CalibrateCompass/calibrateCompassM.h delete mode 100644 src/driveModi/Modi/ConsolControl/consolControl.cpp delete mode 100644 src/driveModi/Modi/ConsolControl/consolControl.h delete mode 100644 src/driveModi/Modi/Racing/racing.cpp delete mode 100644 src/driveModi/Modi/Racing/racing.h diff --git a/doc/TODO allgemein.txt b/doc/TODO allgemein.txt index 443a7b0..0719e08 100644 --- a/doc/TODO allgemein.txt +++ b/doc/TODO allgemein.txt @@ -16,15 +16,12 @@ Do later: -> Menu structure mit add functions for each menu mit pointer return to config (additional not replace) -> Menu Display from parent as run() to make Menu as Component -> Racing Mode + -> ConsolControl Do now: Code: Doxygen Kommentare aktualisieren - Allgemeine Klasse für Sensordaten - DriveModi soll diese Klasse übergeben bekommen - DriveModi bekommt Benutzereinagebn Objekt / Oder extra Objekt was diese Daten speichert als extra Klasse damit weniger parameter Geschwindigkeiten außerhalb von Modi einstellen, Manager kann die Werte einstellen da Interface - Git Branch ohne Navigation Automat in MoveControl weil jetzt in Arbeit beschrieben Liste mit Betriebsmodi, automatisch in Menü einfügen Betriebmodi dem Drivemanger ohne switchCase geben, aus Liste oder so @@ -33,8 +30,9 @@ Code: battery methode für daten erzeugen und in eeporm speichern battery bruch mit R werten nur einmal berechnen lange kein daten von fernbedienung dann? - compass calibrieren eigener Betriebsmodus Input pointer von fernbedienung nicht veränderbar doppel const + trennen funktion und ui route menü + DriveManager Mode als Template übergeben Latex: Genaue Beschreibung von PulseCounter HardwareUnit wenn möglich diff --git a/lib/CalcAzimuth/calcAzimuth.cpp b/lib/CalcAzimuth/calcAzimuth.cpp new file mode 100644 index 0000000..e5b6e4c --- /dev/null +++ b/lib/CalcAzimuth/calcAzimuth.cpp @@ -0,0 +1,92 @@ +/** + * @file calcAzimuth.cpp + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2023-09-03 + * + * @copyright Copyright (c) 2023 + * + */ + +#include "calcAzimuth.h" + +CalcAzimuth::CalcAzimuth(Point point) { + this->lastChangePoint = point; + this->currentPosition = point; + this->loopDelay = 50; +} + +void CalcAzimuth::drivingDirectionChange(Point point) { + if (point.isInit() && point.isValid()) { + this->directionChangeMode = true; + this->lastChangePoint = point; + this->state = CalcAzimuthState::Invalid; + } +} + +void CalcAzimuth::updateCurrentPosition(Point point) { + this->currentPosition = point; + this->positionChanged = true; +} + +void CalcAzimuth::CalcAzimuth::run() { + if (!this->positionChanged) + return + + this->positionChanged = false; + this->updateAzimuth(); +} + +void CalcAzimuth::updateAzimuth() { + if (!this->directionChangeMode + || this->lastChangePoint.distanceTo(this->currentPosition) < 1.0) + { + this->state = CalcAzimuthState::Invalid; + this->calcAzimuth = 999; + return; + } + + this->calcAzimuth = this->lastChangePoint.courseTo(this->currentPosition); + + // Map point accuracy to CalcAzimuthState + if (this->lastChangePoint.getAccuracy() == Point::Accuracy::oneDigOfCM + || this->currentPosition.getAccuracy() == Point::Accuracy::oneDigOfCM) + { + this->state = CalcAzimuthState::Good; + } + else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::twoDigOfCM + || this->currentPosition.getAccuracy() == Point::Accuracy::twoDigOfCM) + { + this->state = CalcAzimuthState::Ok; + } + else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::threeDigOfCM + || this->currentPosition.getAccuracy() == Point::Accuracy::threeDigOfCM) + { + this->state = CalcAzimuthState::Bad; + } + else + { + this->state = CalcAzimuthState::Invalid; + } + + // Upgrade quality if the range grows up + if (this->lastChangePoint.distanceTo(this->currentPosition) > 2.0) { + switch (this->state) { + case CalcAzimuthState::Bad : + this->state = CalcAzimuthState::Ok; + break; + + case CalcAzimuthState::Ok : + this->state = CalcAzimuthState::Good; + break; + + case CalcAzimuthState::Good : + this->state = CalcAzimuthState::Super; + break; + + default: + break; + } + } +} diff --git a/lib/CalcAzimuth/calcAzimuth.h b/lib/CalcAzimuth/calcAzimuth.h new file mode 100644 index 0000000..157c233 --- /dev/null +++ b/lib/CalcAzimuth/calcAzimuth.h @@ -0,0 +1,50 @@ +/** + * @file calcAzimuth.h + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2023-09-03 + * + * @copyright Copyright (c) 2023 + * + */ + +#ifndef CALC_AZIMUTH_H +#define CALC_AZIMUTH_H + +#include "component.h" +#include "point.h" + +class CalcAzimuth : public Component { + public: + enum CalcAzimuthState { + Invalid, + Bad, + Ok, + Good, + Super + }; + + CalcAzimuth(Point point); + + void drivingDirectionChange(Point point); + void updateCurrentPosition(Point point); + void disableCalcAzimuth() { this->directionChangeMode = false; } + + int16_t getAzimuth() const { return this->calcAzimuth; } + CalcAzimuthState getState() const { return this->state; } + + private: + void run() override; + void updateAzimuth(); + + CalcAzimuthState state = CalcAzimuthState::Invalid; + Point lastChangePoint; + Point currentPosition; + + bool positionChanged = false; + bool directionChangeMode = false; + int16_t calcAzimuth = INT16_MAX; +}; + +#endif //CALC_AZIMUTH_H diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp index 5c505a4..8567fca 100644 --- a/lib/Navigation/navigation.cpp +++ b/lib/Navigation/navigation.cpp @@ -65,15 +65,6 @@ bool Navigation::startNavigation() { return this->navigationStarted; } -void Navigation::drivingDirectionChange() { - Point tmp = this->currentPosition; - if (tmp.isInit() && tmp.isValid()) { - this->directionChangeMode = true; - this->lastPointDrivingDirectionChange = tmp; - this->calcAzimuthState = CalcAzimuthState::Invalid; - } -} - Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction, bool forceUpdate) { if (this->navigationFinished) return Status::Complete; @@ -142,60 +133,6 @@ void Navigation::updateCurrentLocation() { this->currentPosition = Point(coords, this->ubxData->hAcc); } -void Navigation::updateMagneticDeclination() { - if (!this->directionChangeMode - || this->lastPointDrivingDirectionChange.distanceTo(this->currentPosition) < 1.0) - { - this->calcAzimuthState = CalcAzimuthState::Invalid; - this->calcAzimuth = 999; - return; - } - - this->calcAzimuth = this->lastPointDrivingDirectionChange.courseTo(this->currentPosition); - - // Map point accuracy to CalcAzimuthState - if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::oneDigOfCM - || this->currentPosition.getAccuracy() == Point::Accuracy::oneDigOfCM) - { - this->calcAzimuthState = CalcAzimuthState::Good; - } - else if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::twoDigOfCM - || this->currentPosition.getAccuracy() == Point::Accuracy::twoDigOfCM) - { - this->calcAzimuthState = CalcAzimuthState::Ok; - } - else if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::threeDigOfCM - || this->currentPosition.getAccuracy() == Point::Accuracy::threeDigOfCM) - { - this->calcAzimuthState = CalcAzimuthState::Bad; - } - else - { - this->calcAzimuthState = CalcAzimuthState::Invalid; - } - - // Upgrade quality if the range grows up - if (this->lastPointDrivingDirectionChange.distanceTo(this->currentPosition) > 2.0) { - switch (this->calcAzimuthState) { - case CalcAzimuthState::Bad : - this->calcAzimuthState = CalcAzimuthState::Ok; - break; - - case CalcAzimuthState::Ok : - this->calcAzimuthState = CalcAzimuthState::Good; - break; - - case CalcAzimuthState::Good : - this->calcAzimuthState = CalcAzimuthState::Super; - break; - - default: - break; - } - } - -} - int16_t Navigation::calculateCourseCorrection(Point& point) { int16_t targetCourse = point.courseTo(this->targetPoint); int16_t correctionCourse; diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h index 006e7da..8acc816 100644 --- a/lib/Navigation/navigation.h +++ b/lib/Navigation/navigation.h @@ -90,8 +90,7 @@ class Navigation : public Component { */ bool startNavigation(); void freezeTargetPoint(bool val = true) { this->preventNextPoint = val; }; - void drivingDirectionChange(); - void disableCalcAzimuth() { this->directionChangeMode = false; } + double increaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint += 0.1; } double decreaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint -= 0.1; } @@ -148,20 +147,6 @@ class Navigation : public Component { Route* getRoute() const { return this->route; } Point getCurrentPosition() const { return this->currentPosition; } - /** - * @brief Get realAzimuth - * - * This value represents the angle between north and - * the line of sight. Clockwise. - * - * @return uint16_t degree - */ - int16_t getAzimuth() const { return this->realAzimuth; } - QMC5883LCompass* getCompass() const { return this->compass; } - int16_t getCalcAzimuth() const { return this->calcAzimuth; } - CalcAzimuthState getCalcAzimuthState() const { return this->calcAzimuthState; } - bool getLastUsedCalcAzimuth() const { return this->lastUsedCalcAzimuth; } - Point::Accuracy getMinAccuracy() const { return this->minAccuracy; } void setMinAccuracy(Point::Accuracy accuracy) { this->minAccuracy = accuracy; } @@ -170,7 +155,6 @@ class Navigation : public Component { private: void run() override; - void updateMagneticDeclination(); void init(Route* route); bool nextPoint(); bool setTargetPoint(Point target); @@ -206,8 +190,6 @@ class Navigation : public Component { char* user; char* password; - int16_t calcAzimuth = INT16_MAX; - uint8_t timeToWait = 200; uint16_t port; uint32_t lastMillis = 0; diff --git a/lib/Point/point.cpp b/lib/Point/point.cpp new file mode 100644 index 0000000..5191dbc --- /dev/null +++ b/lib/Point/point.cpp @@ -0,0 +1,102 @@ +/** + * @file point.cpp + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2023-09-03 + * + * @copyright Copyright (c) 2023 + * + */ + +Point::Point(double lat, double lon, uint32_t horizontalAccuracy, uint32_t creationTime) { + this->coordinates.lat = lat; + this->coordinates.lon = lon; + this->init(horizontalAccuracy, creationTime); +} + +Point::Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy, uint32_t creationTime) { + this->coordinates.lat = lat / 10000000.0; + this->coordinates.lon = lon / 10000000.0; + this->init(horizontalAccuracy, creationTime); +} + +Point::Point(Coordinates coords, uint32_t horizontalAccuracy, uint32_t creationTime) { + this->coordinates = coords; + this->init(horizontalAccuracy, creationTime); +} + +Point::Point(Coordinates coords, bool imported) { + this->coordinates = coords; + if (imported) + this->init(UINT32_MAX, 0); + else + this->init(0, 0); +} + +Point::Point() { + this->coordinates.lat = 0; + this->coordinates.lon = 0; + this->init(0, 0); +} + +bool Point::operator==(const Point& rhs) const { + return this->coordinates == rhs.getCoordinates(); +} + + // distance = sqrt(dx * dx + dy * dy) + // mit distance: Entfernung in km + // dx = 111.3 * cos(lat) * (lon1 - lon2) + // lat = (lat1 + lat2) / 2 * 0.01745 + // dy = 111.3 * (lat1 - lat2) + // lat1, lat2, lon1, lon2: Breite, Länge in Grad +double Point::distanceTo(const Coordinates& point) const { + Coordinates begin = this->coordinates; + Coordinates end = point; + + double lat = (begin.lat + end.lat) / 2 * ROUTE_DEGREE_TO_RADIANT; + double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (begin.lat - end.lat); + double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (begin.lon - end.lon); + + return sqrt(dx * dx + dy * dy); +} + +double Point::distanceTo(const Point &point) const { + return this->distanceTo(point.getCoordinates()); +} + +int16_t Point::courseTo(const Coordinates& point) const { + Coordinates begin = this->coordinates; + Coordinates end = point; + + double phi = log( tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) ); + double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT); + + int16_t res = static_cast(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1; + + // if (res < 0) + // res += 360; + + return res; +} + +int16_t Point::courseTo(const Point &point) const { + return this->courseTo(point.getCoordinates()); +} + +void Point::init(uint32_t horizontalAccuracy, uint32_t creationTime) { + this->creationTime = creationTime; + + if (horizontalAccuracy == UINT32_MAX) + this->accuracy = Accuracy::imported; + else if (horizontalAccuracy > 9999) + this->accuracy = Accuracy::fourDigOfCM; + else if (horizontalAccuracy > 999) + this->accuracy = Accuracy::threeDigOfCM; + else if (horizontalAccuracy > 99) + this->accuracy = Accuracy::twoDigOfCM; + else if (horizontalAccuracy > 1) + this->accuracy = Accuracy::oneDigOfCM; + else + this->accuracy = Accuracy::none; +} diff --git a/lib/Point/point.h b/lib/Point/point.h new file mode 100644 index 0000000..9b7072e --- /dev/null +++ b/lib/Point/point.h @@ -0,0 +1,139 @@ +/** + * @file point.h + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2023-09-03 + * + * @copyright Copyright (c) 2023 + * + */ + +#ifndef POINT_H +#define POINT_H + +#include + +#define ROUTE_DEGREE_TO_RADIANT 0.01745 +#define ROUTE_DISTANCE_BETWEEN_LATITUDE 111300 + +/** + * @brief A to handle points on the earth + * + * The points inherits latidue and longitude as doubles + * + */ +class Point{ + public: + /** + * @brief Hold the data longitude and latitude + * + */ + struct Coordinates { + double lon; + double lat; + + bool operator==(const Coordinates rhs) const { + return ( this->lon == rhs.lon ) && ( this->lon == rhs.lon ); + } + }; + + /** + * @brief The Accuracy is set by the constructor + * + */ + enum Accuracy { + none, + fourDigOfCM, + threeDigOfCM, + twoDigOfCM, + oneDigOfCM, + imported + }; + + + /** + * @brief Construct a new Point object + * + * @param lat latitude + * @param lon longitude + * @param horizontalAccuracy mm + * @param coords Coordinates + * @param imported if true than highest accuracy + */ + Point(double lat, double lon, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0); + Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0); + Point(Coordinates coords, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0); + Point(Coordinates coords, bool imported); + Point(); + + /** + * @brief Checks if to points are equal. + * + * @param rhs + * @return true + * @return false + */ + bool operator==(const Point& rhs) const; + + /** + * @brief Checks if the point is initalized. + * + * @return true + * @return false + */ + bool isInit() const { return this->coordinates.lat + this->coordinates.lon; } + + /** + * @brief Checks if the point is valid. + * + * If the accuracy is higher than zero, true will be returned. + * + * @return true + * @return false + */ + bool isValid() const { return (this->accuracy > 0) ? true : false; } + + /** + * @brief Calculates the distance between to points. + * + * @param point + * @return double meter + */ + double distanceTo(const Coordinates& point) const; + double distanceTo(const Point& point) const; + + /** + * @brief Calculates the course to an other point. + * + * @param point + * @return int16_t degree + */ + int16_t courseTo(const Coordinates& point) const; + int16_t courseTo(const Point& point) const; + + uint32_t getCreationTime() const { return this->creationTime; } + double getLongitude() const { return this->coordinates.lon; } + double getLatitude() const { return this->coordinates.lat; } + Coordinates getCoordinates() const { return this->coordinates; } + + /** + * @brief Get the Accuracy object + * + * The higher the value, the greater the accuracy. + * You can check it by Accuracy. + * + * @return Accuracy + */ + Accuracy getAccuracy() const { return this->accuracy; } + + private: + void init(uint32_t horizontalAccuracy, uint32_t creationTime); + + Accuracy accuracy = Accuracy::none; + Coordinates coordinates; + + uint32_t creationTime = 0; +}; + +#endif //POINT_H diff --git a/lib/Route/route.cpp b/lib/Route/route.cpp index a9039fe..65d2cf2 100644 --- a/lib/Route/route.cpp +++ b/lib/Route/route.cpp @@ -11,99 +11,6 @@ #include "route.h" -Point::Point(double lat, double lon, uint32_t horizontalAccuracy, uint32_t creationTime) { - this->coordinates.lat = lat; - this->coordinates.lon = lon; - this->init(horizontalAccuracy, creationTime); -} - -Point::Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy, uint32_t creationTime) { - this->coordinates.lat = lat / 10000000.0; - this->coordinates.lon = lon / 10000000.0; - this->init(horizontalAccuracy, creationTime); -} - -Point::Point(Coordinates coords, uint32_t horizontalAccuracy, uint32_t creationTime) { - this->coordinates = coords; - this->init(horizontalAccuracy, creationTime); -} - -Point::Point(Coordinates coords, bool imported) { - this->coordinates = coords; - if (imported) - this->init(UINT32_MAX, 0); - else - this->init(0, 0); -} - -Point::Point() { - this->coordinates.lat = 0; - this->coordinates.lon = 0; - this->init(0, 0); -} - -bool Point::operator==(const Point& rhs) const { - return this->coordinates == rhs.getCoordinates(); -} - - // distance = sqrt(dx * dx + dy * dy) - // mit distance: Entfernung in km - // dx = 111.3 * cos(lat) * (lon1 - lon2) - // lat = (lat1 + lat2) / 2 * 0.01745 - // dy = 111.3 * (lat1 - lat2) - // lat1, lat2, lon1, lon2: Breite, Länge in Grad -double Point::distanceTo(const Coordinates& point) const { - Coordinates begin = this->coordinates; - Coordinates end = point; - - double lat = (begin.lat + end.lat) / 2 * ROUTE_DEGREE_TO_RADIANT; - double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (begin.lat - end.lat); - double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (begin.lon - end.lon); - - return sqrt(dx * dx + dy * dy); -} - -double Point::distanceTo(const Point &point) const { - return this->distanceTo(point.getCoordinates()); -} - -int16_t Point::courseTo(const Coordinates& point) const { - Coordinates begin = this->coordinates; - Coordinates end = point; - - double phi = log( tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) ); - double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT); - - int16_t res = static_cast(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1; - - // if (res < 0) - // res += 360; - - return res; -} - -int16_t Point::courseTo(const Point &point) const { - return this->courseTo(point.getCoordinates()); -} - -void Point::init(uint32_t horizontalAccuracy, uint32_t creationTime) { - this->creationTime = creationTime; - - if (horizontalAccuracy == UINT32_MAX) - this->accuracy = Accuracy::imported; - else if (horizontalAccuracy > 9999) - this->accuracy = Accuracy::fourDigOfCM; - else if (horizontalAccuracy > 999) - this->accuracy = Accuracy::threeDigOfCM; - else if (horizontalAccuracy > 99) - this->accuracy = Accuracy::twoDigOfCM; - else if (horizontalAccuracy > 1) - this->accuracy = Accuracy::oneDigOfCM; - else - this->accuracy = Accuracy::none; - -} - Route::Route() { } diff --git a/lib/Route/route.h b/lib/Route/route.h index 3aceff3..ab751b7 100644 --- a/lib/Route/route.h +++ b/lib/Route/route.h @@ -14,129 +14,8 @@ #include #include -#include -#define ROUTE_DEGREE_TO_RADIANT 0.01745 -#define ROUTE_DISTANCE_BETWEEN_LATITUDE 111300 - -/** - * @brief A to handle points on the earth - * - * The points inherits latidue and longitude as doubles - * - */ -class Point{ - public: - /** - * @brief Hold the data longitude and latitude - * - */ - struct Coordinates { - double lon; - double lat; - - bool operator==(const Coordinates rhs) const { - return ( this->lon == rhs.lon ) && ( this->lon == rhs.lon ); - } - }; - - /** - * @brief The Accuracy is set by the constructor - * - */ - enum Accuracy { - none, - fourDigOfCM, - threeDigOfCM, - twoDigOfCM, - oneDigOfCM, - imported - }; - - - /** - * @brief Construct a new Point object - * - * @param lat latitude - * @param lon longitude - * @param horizontalAccuracy mm - * @param coords Coordinates - * @param imported if true than highest accuracy - */ - Point(double lat, double lon, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0); - Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0); - Point(Coordinates coords, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0); - Point(Coordinates coords, bool imported); - Point(); - - /** - * @brief Checks if to points are equal. - * - * @param rhs - * @return true - * @return false - */ - bool operator==(const Point& rhs) const; - - /** - * @brief Checks if the point is initalized. - * - * @return true - * @return false - */ - bool isInit() const { return this->coordinates.lat + this->coordinates.lon; } - - /** - * @brief Checks if the point is valid. - * - * If the accuracy is higher than zero, true will be returned. - * - * @return true - * @return false - */ - bool isValid() const { return (this->accuracy > 0) ? true : false; } - - /** - * @brief Calculates the distance between to points. - * - * @param point - * @return double meter - */ - double distanceTo(const Coordinates& point) const; - double distanceTo(const Point& point) const; - - /** - * @brief Calculates the course to an other point. - * - * @param point - * @return int16_t degree - */ - int16_t courseTo(const Coordinates& point) const; - int16_t courseTo(const Point& point) const; - - uint32_t getCreationTime() const { return this->creationTime; } - double getLongitude() const { return this->coordinates.lon; } - double getLatitude() const { return this->coordinates.lat; } - Coordinates getCoordinates() const { return this->coordinates; } - - /** - * @brief Get the Accuracy object - * - * The higher the value, the greater the accuracy. - * You can check it by Accuracy. - * - * @return Accuracy - */ - Accuracy getAccuracy() const { return this->accuracy; } - - private: - void init(uint32_t horizontalAccuracy, uint32_t creationTime); - - Accuracy accuracy = Accuracy::none; - Coordinates coordinates; - - uint32_t creationTime = 0; -}; +#include "point.h" /** * @brief Holds some route information diff --git a/lib/Sensors/senors.cpp b/lib/Sensors/senors.cpp index 2e0a738..da55710 100644 --- a/lib/Sensors/senors.cpp +++ b/lib/Sensors/senors.cpp @@ -11,17 +11,8 @@ #include "sensors.h" -void Sensors::run() { - this->compass->read(); - this->realAzimuth = this->compass->getAzimuth(); -} - -void Sensors::runAsChild() { - this->gnss->checkUblox(); - this->gnss->checkCallbacks(); - - if (Sensors::newData) - Sensors::newData = false; +Sensors::Sensors() { + this->sensorData = new SensorData(this); } void Sensors::enableGnss(SPIClass* spiPort, uint8_t csPin) { @@ -59,6 +50,19 @@ void Sensors::setOutputStatusPrintPVTdata(bool status) { Sensors::outputStatusPrintPVTdata = status; } +void Sensors::run() { + this->compass->read(); + this->realAzimuth = this->compass->getAzimuth(); +} + +void Sensors::runAsChild() { + this->gnss->checkUblox(); + this->gnss->checkCallbacks(); + + if (Sensors::newData) + Sensors::newData = false; +} + void Sensors::initGnss() { uint8_t versionHigh = this->gnss->getProtocolVersionHigh(); uint8_t versionLow = this->gnss->getProtocolVersionLow(); diff --git a/lib/Sensors/sensorData.h b/lib/Sensors/sensorData.h index e69de29..b134240 100644 --- a/lib/Sensors/sensorData.h +++ b/lib/Sensors/sensorData.h @@ -0,0 +1,40 @@ +/** + * @file sensorData.h + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2023-09-02 + * + * @copyright Copyright (c) 2023 + * + */ + +#ifndef SENSOR_DATA_H +#define SENSOR_DATA_H + +#include "sensors.h" + +#include +#include +// Gyroskop +#include "calcAzimuth.h" + +class SensorData { + public: + SensorData(Sensors *senors); + + int16_t getRealAzimuth() const { return 0; } + int16_t getCalcAzimuth() const { return 0; } + CalcAzimuth::CalcAzimuthState getCalcAzimuthState() const { return CalcAzimuth::CalcAzimuthState::Invalid; } + const UBX_NAV_PVT_data_t* const getGnssData() const; + const void* const getGyroData() const; + + private: + Sensors* sensors; +}; + +SensorData::SensorData(Sensors* senors) { + this->sensors = sensors; +} + +#endif //SENSOR_DATA_H diff --git a/lib/Sensors/sensors.h b/lib/Sensors/sensors.h index cb6d9b1..4e40de3 100644 --- a/lib/Sensors/sensors.h +++ b/lib/Sensors/sensors.h @@ -25,24 +25,15 @@ class Sensors : public Component { public: - enum CalcAzimuthState { - Invalid, - Bad, - Ok, - Good, - Super - }; - Sensors(); - void run() override; - void runAsChild() override; - void enableGnss(SPIClass* spiPort, uint8_t csPin); void enableGnss(); void enableCompass(); void enableGyroskop(); + const SensorData const getSensorData() const { return this->sensorData; } + /** * @brief Set the output status for PVTdata. * @@ -54,10 +45,13 @@ class Sensors : public Component { static void setOutputStatusPrintPVTdata(bool status); private: + void run() override; + void runAsChild() override; void initGnss(); QMC5883LCompass* compass = nullptr; SFE_UBLOX_GNSS* gnss = nullptr; + SensorData* sensorData; CalcAzimuthState calcAzimuthState = CalcAzimuthState::Invalid; diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp index 2b40228..9d2557e 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.cpp +++ b/src/driveModi/Modi/Autopilot/autopilot.cpp @@ -26,8 +26,8 @@ void DirectionChangeSignal::action() { } -Autopilot::Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation) - : ManualControl(moveControl, input) { +Autopilot::Autopilot(DriveModiParams params, Navigation* navigation) + : ManualControl(params) { this->setInputMode(ManualControl::InputMode::Digital); this->directionChangeSignal = new DirectionChangeSignal(navigation); this->setDirectionChangeCallback(this->directionChangeSignal); diff --git a/src/driveModi/Modi/Autopilot/autopilot.h b/src/driveModi/Modi/Autopilot/autopilot.h index c3697f6..5f64bea 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.h +++ b/src/driveModi/Modi/Autopilot/autopilot.h @@ -14,7 +14,6 @@ #include "navigation.h" -#include "moveControl.h" #include "driveModi/Modi/ManualControl/manualControl.h" class DirectionChangeSignal : public DirectionChangeWrapper { @@ -56,7 +55,7 @@ class Autopilot : public ManualControl { * @param moveControl for ManualControl * @param navigation for route instructions */ - Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation); + Autopilot(DriveModiParams params, Navigation* navigation); /** * @brief Destroy the Autopilot object diff --git a/src/driveModi/Modi/CalibrateCompass/calibrateCompassM.cpp b/src/driveModi/Modi/CalibrateCompass/calibrateCompassM.cpp new file mode 100644 index 0000000..d2e195b --- /dev/null +++ b/src/driveModi/Modi/CalibrateCompass/calibrateCompassM.cpp @@ -0,0 +1,24 @@ +/** + * @file calibrateCompassM.cpp + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2023-09-03 + * + * @copyright Copyright (c) 2023 + * + */ + +#include "calibrateCompassM.h" + +CalibrateCompassM::CalibrateCompassM(DriveModiParams params) : ManualControl(params) {} + +void CalibrateCompassM::setCalibrateCompass(CalibrateCompass *caliCompass) { + if (caliCompass) { + this->caliCompass = caliCompass; + this->addChildComponent(this->caliCompass); + } else if (this->caliCompass) { + this->removeChildComponent(this->caliCompass); + this->caliCompass = caliCompass; + } +} diff --git a/src/driveModi/Modi/CalibrateCompass/calibrateCompassM.h b/src/driveModi/Modi/CalibrateCompass/calibrateCompassM.h new file mode 100644 index 0000000..b101721 --- /dev/null +++ b/src/driveModi/Modi/CalibrateCompass/calibrateCompassM.h @@ -0,0 +1,29 @@ +/** + * @file calibrateCompassM.h + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2023-09-03 + * + * @copyright Copyright (c) 2023 + * + */ + +#ifndef CALIBRATE_COMPASS_M_H +#define CALIBRATE_COMPASS_M_H + +#include "calibrateCompass.h" + +#include "driveModi/Modi/ManualControl/manualControl.h" + +class CalibrateCompassM : public ManualControl { + public: + CalibrateCompassM(DriveModiParams params); + + void setCalibrateCompass(CalibrateCompass* caliCompass = nullptr); + + private: + CalibrateCompass* caliCompass = nullptr; +}; + +#endif //CALIBRATE_COMPASS_M_H diff --git a/src/driveModi/Modi/CaptureRoute/captureRoute.cpp b/src/driveModi/Modi/CaptureRoute/captureRoute.cpp index b334e19..ae4cda9 100644 --- a/src/driveModi/Modi/CaptureRoute/captureRoute.cpp +++ b/src/driveModi/Modi/CaptureRoute/captureRoute.cpp @@ -11,8 +11,8 @@ #include "driveModi/Modi/CaptureRoute/captureRoute.h" -CaptureRoute::CaptureRoute(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation) - : ManualControl(moveControl, input) { +CaptureRoute::CaptureRoute(DriveModiParams params, Navigation* navigation) + : ManualControl(params) { this->navigation = navigation; this->navigation->getRoute()->clear(); this->navigation->getNTRIPClient()->setAutoReconnect(true); diff --git a/src/driveModi/Modi/CaptureRoute/captureRoute.h b/src/driveModi/Modi/CaptureRoute/captureRoute.h index c262a98..0916cee 100644 --- a/src/driveModi/Modi/CaptureRoute/captureRoute.h +++ b/src/driveModi/Modi/CaptureRoute/captureRoute.h @@ -33,7 +33,7 @@ class CaptureRoute : public ManualControl { * @param moveControl for ManualControl * @param navigation to add Points */ - CaptureRoute(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation); + CaptureRoute(DriveModiParams params, Navigation* navigation); /** * @brief Destroy the Capture Route object diff --git a/src/driveModi/Modi/ConsolControl/consolControl.cpp b/src/driveModi/Modi/ConsolControl/consolControl.cpp deleted file mode 100644 index 6b57026..0000000 --- a/src/driveModi/Modi/ConsolControl/consolControl.cpp +++ /dev/null @@ -1,13 +0,0 @@ -/** - * @file consolControl.cpp - * @author Alexander Klein (alex@kleiax.de) - * @brief - * @version 0.1 - * @date 2022-02-15 - * - * @copyright Copyright (c) 2022 - * - */ - -#include "consolControl.h" - diff --git a/src/driveModi/Modi/ConsolControl/consolControl.h b/src/driveModi/Modi/ConsolControl/consolControl.h deleted file mode 100644 index bf1b91f..0000000 --- a/src/driveModi/Modi/ConsolControl/consolControl.h +++ /dev/null @@ -1,20 +0,0 @@ -/** - * @file consolControl.h - * @author Alexander Klein (alex@kleiax.de) - * @brief - * @version 0.1 - * @date 2022-02-15 - * - * @copyright Copyright (c) 2022 - * - */ - -#ifndef CONSOL_CONTROL_H -#define CONSOL_CONTROL_H - -#include "driveModi/driveModi.h" -class ConsolControl : DriveModi{ - -}; - -#endif // CONSOL_CONTROL_H diff --git a/src/driveModi/Modi/ManualControl/manualControl.cpp b/src/driveModi/Modi/ManualControl/manualControl.cpp index a6cce5b..a6891ab 100644 --- a/src/driveModi/Modi/ManualControl/manualControl.cpp +++ b/src/driveModi/Modi/ManualControl/manualControl.cpp @@ -10,15 +10,6 @@ */ #include "manualControl.h" -ManualControl::ManualControl(MoveControl* moveControl, const ControlPadInput *input) - : DriveModi(moveControl) { - this->input = input; -} - -ManualControl::~ManualControl() { - -} - void ManualControl::run() { switch (this->inputMode) { case InputMode::Analog : @@ -34,16 +25,6 @@ void ManualControl::run() { } } -void ManualControl::setCalibrateCompass(CalibrateCompass *caliCompass) { - if (caliCompass) { - this->caliCompass = caliCompass; - this->addChildComponent(this->caliCompass); - } else if (this->caliCompass) { - this->removeChildComponent(this->caliCompass); - this->caliCompass = caliCompass; - } -} - void ManualControl::switchInputMode() { this->inputMode = (this->inputMode == InputMode::Analog) ? InputMode::Digital : InputMode::Analog; } @@ -51,7 +32,7 @@ void ManualControl::switchInputMode() { void ManualControl::analogControl() { // Have to be int16_t to avoid overflow (int8_t = 255 - 127 = -128) int16_t x = this->input->x - 127; - int16_t y = this->input->y - 127; + int16_t y = this-> input->y - 127; // static int counter = 0; // if (counter % 60 == 0) { diff --git a/src/driveModi/Modi/ManualControl/manualControl.h b/src/driveModi/Modi/ManualControl/manualControl.h index afbbdaa..138c573 100644 --- a/src/driveModi/Modi/ManualControl/manualControl.h +++ b/src/driveModi/Modi/ManualControl/manualControl.h @@ -11,10 +11,7 @@ #ifndef MANUAL_CONTROL_H #define MANUAL_CONTROL_H -#include "moveControl.h" #include "driveModi/driveModi.h" -#include "controlPadInput.h" -#include "calibrateCompass.h" class DirectionChangeWrapper { public: @@ -36,10 +33,7 @@ class ManualControl : public DriveModi { Digital }; - ManualControl(MoveControl *moveControl, const ControlPadInput *input); - ~ManualControl(); - - void setCalibrateCompass(CalibrateCompass* caliCompass = nullptr); + ManualControl(DriveModiParams params) : DriveModi(params){}; void switchInputMode(); void setInputMode(InputMode mode) { this->inputMode = mode; } @@ -51,14 +45,12 @@ class ManualControl : public DriveModi { protected: void run() override; - const ControlPadInput* input; private: void analogControl(); void digitalControl(); bool lastLoopTurned = false; - CalibrateCompass* caliCompass = nullptr; DirectionChangeWrapper* directionChangeWrapper = nullptr; InputMode inputMode = InputMode::Analog; }; diff --git a/src/driveModi/Modi/Racing/racing.cpp b/src/driveModi/Modi/Racing/racing.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/src/driveModi/Modi/Racing/racing.h b/src/driveModi/Modi/Racing/racing.h deleted file mode 100644 index e69de29..0000000 diff --git a/src/driveModi/Modi/TestMode/testMode.cpp b/src/driveModi/Modi/TestMode/testMode.cpp index 333bcf0..040ee37 100644 --- a/src/driveModi/Modi/TestMode/testMode.cpp +++ b/src/driveModi/Modi/TestMode/testMode.cpp @@ -10,8 +10,8 @@ */ #include "testMode.h" -TestMode::TestMode(MoveControl *moveControl, Navigation* navigation) - : DriveModi(moveControl) { +TestMode::TestMode(DriveModiParams params, Navigation* navigation) + : DriveModi(params) { this->navigation = navigation; } diff --git a/src/driveModi/Modi/TestMode/testMode.h b/src/driveModi/Modi/TestMode/testMode.h index 43a7b10..abfe21f 100644 --- a/src/driveModi/Modi/TestMode/testMode.h +++ b/src/driveModi/Modi/TestMode/testMode.h @@ -44,7 +44,7 @@ class TestMode : public DriveModi { * @param moveControl * @param navigation */ - TestMode(MoveControl *moveControl, Navigation* navigation); + TestMode(DriveModiParams params, Navigation* navigation); ~TestMode(); bool drive(int16_t cm = 0, int16_t degree = 0); diff --git a/src/driveModi/driveManager.cpp b/src/driveModi/driveManager.cpp index fe84f7a..46f86cc 100644 --- a/src/driveModi/driveManager.cpp +++ b/src/driveModi/driveManager.cpp @@ -11,33 +11,22 @@ #include "driveModi/driveManager.h" -Modi& operator++(Modi& m, int) { - return m = (m == Modi::TestMode) ? Modi::Off : static_cast(static_cast(m)+1); +DriveManager::DriveManager(MoveControl *moveControl, SensorData* sensorData, ControlPadInput *input, bool wifi) { + this->driveModiParams.input = input; + this->driveModiParams.moveControl = moveControl; + this->driveModiParams.sensorData = sensorData; + + this->init(wifi); } -DriveManager::DriveManager(MoveControl *moveControl, SPIClass *spiPort, const ControlPadInput *input, bool wifi) { - this->moveControl = moveControl; - this->input = input; - - this->spiPort = spiPort; - this->navigation = new Navigation(spiPort, PinNumbers::gnssSpiCs); - if (wifi) - this->navigation->initNtrip(NTRIP_HOST, NTRIP_PORT, NTRIP_MOUNT_POINT, NTRIP_USER, NTRIP_PASSWORD); +DriveManager::DriveManager(DriveModiParams params, bool wifi){ + this->driveModiParams = params; - this->addChildComponent(this->moveControl); - this->addChildComponent(this->navigation); - this->activateOnlyChilds(); + this->init(wifi); } DriveManager::~DriveManager() { delete this->navigation; - delete this->spiPort; -} - -void DriveManager::run() {} - -void DriveManager::nextModus() { - changeModus(this->currentModus++); } void DriveManager::changeModus(Modi modus) { @@ -56,26 +45,22 @@ void DriveManager::changeModus(Modi modus) { break; case Modi::ManualControl: { - this->currentModusPtr = new ManualControl(this->moveControl, this->input); + this->currentModusPtr = new ManualControl(this->driveModiParams); } break; case Modi::CaptureRoute: { - this->currentModusPtr = new CaptureRoute(this->moveControl, this->input, this->navigation); + this->currentModusPtr = new CaptureRoute(this->driveModiParams, this->navigation); } break; case Modi::Autopilot: { - this->currentModusPtr = new Autopilot(this->moveControl, this->input, this->navigation); + this->currentModusPtr = new Autopilot(this->driveModiParams, this->navigation); } break; - case Modi::ConsolControl: - this->currentModusPtr = nullptr; - break; - case Modi::TestMode: { - this->currentModusPtr = new TestMode(this->moveControl, this->navigation); + this->currentModusPtr = new TestMode(driveModiParams, this->navigation); } break; @@ -88,3 +73,13 @@ void DriveManager::changeModus(Modi modus) { this->addChildComponent(this->currentModusPtr); } +void DriveManager::init(bool wifi) { + this->navigation = new Navigation(); + if (wifi) + this->navigation->initNtrip(NTRIP_HOST, NTRIP_PORT, NTRIP_MOUNT_POINT, NTRIP_USER, NTRIP_PASSWORD); + + this->addChildComponent(this->driveModiParams.moveControl); + this->addChildComponent(this->navigation); + this->activateOnlyChilds(); +} + diff --git a/src/driveModi/driveModi.cpp b/src/driveModi/driveModi.cpp index 02a9d01..f07e6fe 100644 --- a/src/driveModi/driveModi.cpp +++ b/src/driveModi/driveModi.cpp @@ -1,9 +1,17 @@ #include "driveModi.h" -DriveModi::DriveModi(MoveControl* moveControl) { +DriveModi::DriveModi(MoveControl* moveControl, ControlPadInput* input, SensorData* sensorData) { this->moveControl = moveControl; - this->moveControl->setDrivingStatus(MoveControl::Status::Drive); - this->loopDelay = 40; + this->input = input; + this->sensorData = sensorData; + this->init(); +} + +DriveModi::DriveModi(DriveModiParams params){ + this->moveControl = params.moveControl; + this->input = params.input; + this->sensorData = params.sensorData; + this->init(); } DriveModi::~DriveModi() { @@ -11,3 +19,8 @@ DriveModi::~DriveModi() { this->moveControl->setRotationSpeed(0); this->moveControl->setDrivingStatus(MoveControl::Status::Stop); } + +void DriveModi::init() { + this->moveControl->setDrivingStatus(MoveControl::Status::Drive); + this->loopDelay = 40; +} diff --git a/src/driveModi/driveModi.h b/src/driveModi/driveModi.h index a78f0f4..32187cc 100644 --- a/src/driveModi/driveModi.h +++ b/src/driveModi/driveModi.h @@ -14,6 +14,14 @@ #include "component.h" #include "moveControl.h" +#include "controlPadInput.h" +#include "sensorData.h" + +struct DriveModiParams { + MoveControl* moveControl; + ControlPadInput* input; + SensorData* sensorData; +}; /** * @brief Baseclass to build DriveModi @@ -23,9 +31,12 @@ */ class DriveModi : public Component { public: - DriveModi(MoveControl* moveControl); + DriveModi(MoveControl* moveControl, ControlPadInput* input, SensorData* sensorData); + DriveModi(DriveModiParams); virtual ~DriveModi(); + SensorData getSensorData() const { return this->sensorData; } + /** * @brief Set the max speed * @@ -48,8 +59,12 @@ class DriveModi : public Component { protected: MoveControl *moveControl; + ControlPadInput* input; + SensorData* sensorData; double maxForwardSpeed = 1; double maxRotationSpeed = 7; - + + private: + void init(); }; #endif // DRIVEMODI_H diff --git a/src/main.cpp b/src/main.cpp index 49fd0cc..d91d4e3 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -31,6 +31,8 @@ #include "network.h" #include "debugMqtt.h" #include "battery.h" +#include "sensors.h" +#include "sensorData.h" #include "debugTimes.h" #include "controlPad.h" @@ -55,6 +57,8 @@ OutputBuf* outputBuf; DebugMqtt* debugMqtt = nullptr; Battery* mainBattery; SPIClass* spiPort; +Sensors* sensors; +SensorData* sensorData; ControlPad* controlPad; bool wifiIsActive; @@ -115,7 +119,11 @@ void setup() { std::cout << "Build timestamp: " << BUILD_TIMESTAMP << std::endl; std::cout << "All actions from the main program run on Core -> " << xPortGetCoreID() << std::endl; - driveManager = new DriveManager(&moveController, spiPort, controlPad->getControlPadDataPtr(), wifiIsActive); + sensors = new Sensors(); + sensors->enableGnss(spiPort, PinNumbers::gnssSpiCs); + sensors->enableCompass(); + sensorData = sensors->getSensorData(); + driveManager = new DriveManager(&moveController, sensorData, controlPad->getControlPadDataPtr(), wifiIsActive); outputBuf->activateMqtt(true); @@ -142,6 +150,7 @@ void loop() { #endif //MQTT wifiTime.stopConsol("WiFi-Time", 10); } + sensors->loop(); driveManager->loop(); controlPad->loop(); main_m->update(); From 6a76470f45a3cedbd91416f8b4b2c3e06e2babe4 Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Mon, 4 Sep 2023 13:30:34 +0200 Subject: [PATCH 3/3] compilation works again --- lib/CalcAzimuth/calcAzimuth.cpp | 30 ++--- lib/CalcAzimuth/calcAzimuth.h | 6 +- lib/ControllPad/controlPad.h | 2 +- lib/Navigation/navigation.cpp | 54 +++----- lib/Navigation/navigation.h | 46 +------ lib/Point/point.cpp | 2 + lib/Sensors/{senors.cpp => sensorData.cpp} | 115 +++++++++++------- lib/Sensors/sensorData.h | 88 ++++++++++++-- lib/Sensors/sensors.h | 71 ----------- src/SpecialMenus/GPS/menuGPS.cpp | 38 +++--- src/SpecialMenus/GPS/menuGPS.h | 9 +- .../driveModi/Autopilot/menuAutopilot.cpp | 24 ++-- .../CalibrateCompass/menuCalibrateCompass.cpp | 17 ++- .../CalibrateCompass/menuCalibrateCompass.h | 2 +- .../CaptureRoute/menuCaptureRoute.cpp | 6 +- src/driveModi/Modi/Autopilot/autopilot.cpp | 24 ++-- src/driveModi/Modi/Autopilot/autopilot.h | 20 +-- .../Modi/CaptureRoute/captureRoute.cpp | 2 +- src/driveModi/Modi/TestMode/testMode.cpp | 4 +- src/driveModi/driveManager.cpp | 14 +-- src/driveModi/driveManager.h | 7 +- src/driveModi/driveModi.h | 10 +- src/main.cpp | 15 +-- 23 files changed, 285 insertions(+), 321 deletions(-) rename lib/Sensors/{senors.cpp => sensorData.cpp} (57%) delete mode 100644 lib/Sensors/sensors.h diff --git a/lib/CalcAzimuth/calcAzimuth.cpp b/lib/CalcAzimuth/calcAzimuth.cpp index e5b6e4c..8db7a5d 100644 --- a/lib/CalcAzimuth/calcAzimuth.cpp +++ b/lib/CalcAzimuth/calcAzimuth.cpp @@ -21,7 +21,7 @@ void CalcAzimuth::drivingDirectionChange(Point point) { if (point.isInit() && point.isValid()) { this->directionChangeMode = true; this->lastChangePoint = point; - this->state = CalcAzimuthState::Invalid; + this->state = State::Invalid; } } @@ -30,9 +30,9 @@ void CalcAzimuth::updateCurrentPosition(Point point) { this->positionChanged = true; } -void CalcAzimuth::CalcAzimuth::run() { +void CalcAzimuth::run() { if (!this->positionChanged) - return + return; this->positionChanged = false; this->updateAzimuth(); @@ -42,47 +42,47 @@ void CalcAzimuth::updateAzimuth() { if (!this->directionChangeMode || this->lastChangePoint.distanceTo(this->currentPosition) < 1.0) { - this->state = CalcAzimuthState::Invalid; + this->state = State::Invalid; this->calcAzimuth = 999; return; } this->calcAzimuth = this->lastChangePoint.courseTo(this->currentPosition); - // Map point accuracy to CalcAzimuthState + // Map point accuracy to State if (this->lastChangePoint.getAccuracy() == Point::Accuracy::oneDigOfCM || this->currentPosition.getAccuracy() == Point::Accuracy::oneDigOfCM) { - this->state = CalcAzimuthState::Good; + this->state = State::Good; } else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::twoDigOfCM || this->currentPosition.getAccuracy() == Point::Accuracy::twoDigOfCM) { - this->state = CalcAzimuthState::Ok; + this->state = State::Ok; } else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::threeDigOfCM || this->currentPosition.getAccuracy() == Point::Accuracy::threeDigOfCM) { - this->state = CalcAzimuthState::Bad; + this->state = State::Bad; } else { - this->state = CalcAzimuthState::Invalid; + this->state = State::Invalid; } // Upgrade quality if the range grows up if (this->lastChangePoint.distanceTo(this->currentPosition) > 2.0) { switch (this->state) { - case CalcAzimuthState::Bad : - this->state = CalcAzimuthState::Ok; + case State::Bad : + this->state = State::Ok; break; - case CalcAzimuthState::Ok : - this->state = CalcAzimuthState::Good; + case State::Ok : + this->state = State::Good; break; - case CalcAzimuthState::Good : - this->state = CalcAzimuthState::Super; + case State::Good : + this->state = State::Super; break; default: diff --git a/lib/CalcAzimuth/calcAzimuth.h b/lib/CalcAzimuth/calcAzimuth.h index 157c233..b44cf8f 100644 --- a/lib/CalcAzimuth/calcAzimuth.h +++ b/lib/CalcAzimuth/calcAzimuth.h @@ -17,7 +17,7 @@ class CalcAzimuth : public Component { public: - enum CalcAzimuthState { + enum State { Invalid, Bad, Ok, @@ -32,13 +32,13 @@ class CalcAzimuth : public Component { void disableCalcAzimuth() { this->directionChangeMode = false; } int16_t getAzimuth() const { return this->calcAzimuth; } - CalcAzimuthState getState() const { return this->state; } + State getState() const { return this->state; } private: void run() override; void updateAzimuth(); - CalcAzimuthState state = CalcAzimuthState::Invalid; + State state = State::Invalid; Point lastChangePoint; Point currentPosition; diff --git a/lib/ControllPad/controlPad.h b/lib/ControllPad/controlPad.h index f843404..71412cf 100644 --- a/lib/ControllPad/controlPad.h +++ b/lib/ControllPad/controlPad.h @@ -24,7 +24,7 @@ class ControlPad : public Component { void setMenuControl(MenuControl* menuControl) { this->menuControl = menuControl; } - const ControlPadInput * getControlPadDataPtr() const { return &this->controlInput; } + const ControlPadInput* getControlPadDataPtr() const { return &this->controlInput; } bool isControlPadConnected() const { return this->connected; } diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp index 8567fca..fec6397 100644 --- a/lib/Navigation/navigation.cpp +++ b/lib/Navigation/navigation.cpp @@ -11,18 +11,12 @@ #include "navigation.h" -bool Navigation::outputStatusPrintPVTdata = false; -bool Navigation::newData = false; -uint32_t Navigation::ubxUpdateTimeStatic = 0; -UBX_NAV_PVT_data_t* Navigation::ubxDataStatic = nullptr; - -Navigation::Navigation(Route* route) { +Navigation::Navigation(const SensorData* sensorData, Route* route) { + this->sensorData = sensorData; this->init(route); } void Navigation::init(Route* route) { - - if (route) this->route = route; else @@ -33,23 +27,9 @@ void Navigation::init(Route* route) { Navigation::~Navigation() { delete this->route; - if (this->ntripClient) - delete this->ntripClient; -} - -void Navigation::initNtrip(String host, uint16_t port, String mountPoint, String user, String password) { - this->ntripClient = new NTRIPClient(this->gps, host.c_str(), port, mountPoint.c_str(), user.c_str(), password.c_str()); - this->ntripClient->gpsConfiguration(); - this->ntripClient->loop(); - this->ntripClient->setActivated(false); - this->isNtripInit = true; - this->addChildComponent(this->ntripClient); -} - -void Navigation::run() { - this->updateMagneticDeclination(); } +void Navigation::run() {} void Navigation::newRoute() { if (this->route) @@ -119,31 +99,31 @@ Navigation::Status Navigation::addCurrentPosToRoute() { return Status::Unchanged; } -void Navigation::updateCurrentLocation() { - if (Navigation::ubxUpdateTimeStatic == this->ubxUpdateTime) - return; +// void Navigation::updateCurrentLocation() { +// if (Navigation::ubxUpdateTimeStatic == this->ubxUpdateTime) +// return; - this->ubxData = Navigation::ubxDataStatic; - this->ubxUpdateTime = Navigation::ubxUpdateTimeStatic; +// this->ubxData = Navigation::ubxDataStatic; +// this->ubxUpdateTime = Navigation::ubxUpdateTimeStatic; - Point::Coordinates coords; - coords.lat = this->ubxData->lat / 10000000.0; - coords.lon = this->ubxData->lon / 10000000.0; +// Point::Coordinates coords; +// coords.lat = this->ubxData->lat / 10000000.0; +// coords.lon = this->ubxData->lon / 10000000.0; - this->currentPosition = Point(coords, this->ubxData->hAcc); -} +// this->currentPosition = Point(coords, this->ubxData->hAcc); +// } int16_t Navigation::calculateCourseCorrection(Point& point) { int16_t targetCourse = point.courseTo(this->targetPoint); int16_t correctionCourse; - if (this->calcAzimuthState == CalcAzimuthState::Good - || this->calcAzimuthState == CalcAzimuthState::Super) + if (this->sensorData->getCalcAzimuthState() == CalcAzimuth::State::Good + || this->sensorData->getCalcAzimuthState() == CalcAzimuth::State::Super) { - correctionCourse = targetCourse - this->calcAzimuth; + correctionCourse = targetCourse - this->sensorData->getCalcAzimuth(); this->lastUsedCalcAzimuth = true; } else { - correctionCourse = targetCourse - this->realAzimuth; + correctionCourse = targetCourse - this->sensorData->getRealAzimuth(); this->lastUsedCalcAzimuth = false; } diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h index 8acc816..55ae3ee 100644 --- a/lib/Navigation/navigation.h +++ b/lib/Navigation/navigation.h @@ -16,8 +16,7 @@ #include #include "route.h" -#include "NTRIPClient.h" - +#include "sensorData.h" #include "component.h" /** @@ -54,7 +53,7 @@ class Navigation : public Component { * * @param route with which to navigate */ - Navigation(Route* route = nullptr); + Navigation(const SensorData* sensorData, Route* route = nullptr); /** * @brief Destroy the Navigation object @@ -62,17 +61,6 @@ class Navigation : public Component { */ ~Navigation(); - /** - * @brief - * - * @param host - * @param port - * @param mountPoint - * @param user - * @param password - */ - void initNtrip(String host, uint16_t port, String mountPoint, String user, String password); - /** * @brief creates a new empty route * @@ -118,22 +106,6 @@ class Navigation : public Component { * @return false no point added to route */ Status addCurrentPosToRoute(); - - /** - * @brief Get the Ubx Data object - * - * This struct includes the most Data from the GNSS-Module. - * - * @return UBX_NAV_PVT_data_t* - */ - UBX_NAV_PVT_data_t* getUbxData() { return this->ubxData; } - - /** - * @brief Returns the NTRIPClient object - * - * @return NTRIPClient* - */ - NTRIPClient* getNTRIPClient() { return this->ntripClient; } /** * @brief Get the Route Info object @@ -163,12 +135,9 @@ class Navigation : public Component { static constexpr uint8_t loopDelay = 20; static constexpr uint8_t maxDisBetweenPoints = 10; static constexpr float minDisBetweenPoints = 0.3; - - - UBX_NAV_PVT_data_t* ubxData = nullptr; - Route* route = nullptr; - NTRIPClient* ntripClient = nullptr; + const SensorData* sensorData; + Route* route = nullptr; Point lastPointRouteInsert; Point lastPointCalcCorrection; @@ -177,7 +146,6 @@ class Navigation : public Component { Point currentPosition; Point::Accuracy minAccuracy = Point::Accuracy::twoDigOfCM; - bool navigationStarted = false; bool navigationFinished = false; bool isNtripInit = false; @@ -185,13 +153,7 @@ class Navigation : public Component { bool directionChangeMode = false; bool lastUsedCalcAzimuth = false; - char* host; - char* mountPoint; - char* user; - char* password; - uint8_t timeToWait = 200; - uint16_t port; uint32_t lastMillis = 0; uint32_t ubxUpdateTime = 0; diff --git a/lib/Point/point.cpp b/lib/Point/point.cpp index 5191dbc..eee438e 100644 --- a/lib/Point/point.cpp +++ b/lib/Point/point.cpp @@ -9,6 +9,8 @@ * */ +#include "point.h" + Point::Point(double lat, double lon, uint32_t horizontalAccuracy, uint32_t creationTime) { this->coordinates.lat = lat; this->coordinates.lon = lon; diff --git a/lib/Sensors/senors.cpp b/lib/Sensors/sensorData.cpp similarity index 57% rename from lib/Sensors/senors.cpp rename to lib/Sensors/sensorData.cpp index da55710..5ca4809 100644 --- a/lib/Sensors/senors.cpp +++ b/lib/Sensors/sensorData.cpp @@ -9,13 +9,32 @@ * */ -#include "sensors.h" +#include "sensorData.h" -Sensors::Sensors() { - this->sensorData = new SensorData(this); +bool SensorData::outputStatusPrintPVTdata = false; +bool SensorData::newData = false; +uint32_t SensorData::ubxUpdateTimeStatic = 0; +UBX_NAV_PVT_data_t* SensorData::ubxDataStatic = nullptr; + +SensorData::SensorData() { + this->loopDelay = 50; } -void Sensors::enableGnss(SPIClass* spiPort, uint8_t csPin) { +SensorData::~SensorData() { + if (this->ntripClient) + delete this->ntripClient; +} + +void SensorData::enableNtrip(String host, uint16_t port, String mountPoint, String user, String password) { + this->ntripClient = new NTRIPClient(this->gnss, host.c_str(), port, mountPoint.c_str(), user.c_str(), password.c_str()); + this->ntripClient->gpsConfiguration(); + this->ntripClient->loop(); + this->ntripClient->setActivated(false); + this->isNtripInit = true; + this->addChildComponent(this->ntripClient); +} + +void SensorData::enableGnss(SPIClass* spiPort, uint8_t csPin) { this->gnss = new SFE_UBLOX_GNSS(); if (this->gnss->begin(*spiPort, csPin, 4000000) == false) { std::cout << "u-blox GNSS not detected on SPI bus. Please check wiring. Freezing." << std::endl; @@ -24,7 +43,7 @@ void Sensors::enableGnss(SPIClass* spiPort, uint8_t csPin) { this->initGnss(); } -void Sensors::enableGnss() { +void SensorData::enableGnss() { this->gnss = new SFE_UBLOX_GNSS(); if (this->gnss->begin() == false) { std::cout << "u-blox GNSS not detected at default I2C address. Please check wiring. Freezing." << std::endl; @@ -33,52 +52,35 @@ void Sensors::enableGnss() { this->initGnss(); } -void Sensors::enableCompass() { - this->compass = new QMC5883LCompass(); +void SensorData::enableRealCompass() { + this->realCompass = new QMC5883LCompass(); // Init Compass Wire.beginTransmission(0x0d); Wire.write(0x0b); Wire.write(0x01); Wire.endTransmission(); - this->compass->setMode(0x01,0x0C,0x10,0X00); - CalibrateCompass caliCompass(this->compass); + this->realCompass->setMode(0x01,0x0C,0x10,0X00); + CalibrateCompass caliCompass(this->realCompass); caliCompass.loadData(); caliCompass.useData(); } -void Sensors::setOutputStatusPrintPVTdata(bool status) { - Sensors::outputStatusPrintPVTdata = status; +void SensorData::enableCalcCompass() { + } -void Sensors::run() { - this->compass->read(); - this->realAzimuth = this->compass->getAzimuth(); +void SensorData::enableGyroskop() { + } -void Sensors::runAsChild() { - this->gnss->checkUblox(); - this->gnss->checkCallbacks(); - - if (Sensors::newData) - Sensors::newData = false; +CalcAzimuth::State SensorData::getCalcAzimuthState() const { + if (this->calcCompass) + return this->calcCompass->getState(); + return CalcAzimuth::State::Invalid; } -void Sensors::initGnss() { - uint8_t versionHigh = this->gnss->getProtocolVersionHigh(); - uint8_t versionLow = this->gnss->getProtocolVersionLow(); - std::cout << "u-blox protocol version: " << unsigned(versionHigh) << "." << unsigned(versionLow) << std::endl; - - this->gnss->setSPIOutput(COM_TYPE_UBX); - this->gnss->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI, 10); - this->gnss->setUSBOutput(COM_TYPE_UBX | COM_TYPE_NMEA); - this->gnss->setAutoPVTcallbackPtr(&(Sensors::savePVTdata)); - // Sensors::setOutputStatusPrintPVTdata(true); - this->gnss->setNavigationFrequency(1); - this->gnss->setAutoPVT(true); -} - -void Sensors::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) { - if (!Sensors::outputStatusPrintPVTdata) +void SensorData::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) { + if (!SensorData::outputStatusPrintPVTdata) return; double latitude = (double) ubxDataStruct->lat / 10000000.0; @@ -124,10 +126,41 @@ void Sensors::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) { << " Horizontal Accuracy Estimate: " << hAcc << " mm" << std::endl; } -void Sensors::savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) { - Sensors::printPVTdata(ubxDataStruct); +void SensorData::savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) { + SensorData::printPVTdata(ubxDataStruct); - Sensors::newData = true; - Sensors::ubxDataStatic = ubxDataStruct; - Sensors::ubxUpdateTimeStatic = millis(); + SensorData::newData = true; + SensorData::ubxDataStatic = ubxDataStruct; + SensorData::ubxUpdateTimeStatic = millis(); +} + +void SensorData::setOutputStatusPrintPVTdata(bool status) { + SensorData::outputStatusPrintPVTdata = status; +} + +void SensorData::run() { + this->realCompass->read(); + this->realAzimuth = this->realCompass->getAzimuth(); +} + +void SensorData::runAsChild() { + this->gnss->checkUblox(); + this->gnss->checkCallbacks(); + + if (SensorData::newData) + SensorData::newData = false; +} + +void SensorData::initGnss() { + uint8_t versionHigh = this->gnss->getProtocolVersionHigh(); + uint8_t versionLow = this->gnss->getProtocolVersionLow(); + std::cout << "u-blox protocol version: " << unsigned(versionHigh) << "." << unsigned(versionLow) << std::endl; + + this->gnss->setSPIOutput(COM_TYPE_UBX); + this->gnss->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI, 10); + this->gnss->setUSBOutput(COM_TYPE_UBX | COM_TYPE_NMEA); + this->gnss->setAutoPVTcallbackPtr(&(SensorData::savePVTdata)); + // SensorData::setOutputStatusPrintPVTdata(true); + this->gnss->setNavigationFrequency(1); + this->gnss->setAutoPVT(true); } diff --git a/lib/Sensors/sensorData.h b/lib/Sensors/sensorData.h index b134240..fb4abe1 100644 --- a/lib/Sensors/sensorData.h +++ b/lib/Sensors/sensorData.h @@ -12,29 +12,91 @@ #ifndef SENSOR_DATA_H #define SENSOR_DATA_H -#include "sensors.h" +#include +#include + +#include "component.h" +#include "calibrateCompass.h" #include #include // Gyroskop #include "calcAzimuth.h" +#include "ntripClient.h" -class SensorData { +#include "point.h" +class Sensors; + +class SensorData : public Component { public: - SensorData(Sensors *senors); + SensorData(); + ~SensorData(); - int16_t getRealAzimuth() const { return 0; } - int16_t getCalcAzimuth() const { return 0; } - CalcAzimuth::CalcAzimuthState getCalcAzimuthState() const { return CalcAzimuth::CalcAzimuthState::Invalid; } - const UBX_NAV_PVT_data_t* const getGnssData() const; - const void* const getGyroData() const; + void enableNtrip(String host, uint16_t port, String mountPoint, String user, String password); + void enableGnss(SPIClass* spiPort, uint8_t csPin); + void enableGnss(); + void enableRealCompass(); + void enableCalcCompass(); + void enableGyroskop(); + + // Interface Const kram + int16_t getRealAzimuth() const { return this->realAzimuth; } + int16_t getCalcAzimuth() const { return this->calcAzimuth; } + CalcAzimuth::State getCalcAzimuthState() const; + + Point getCurrentPos() const { return this->currentPosition; } + const UBX_NAV_PVT_data_t* getGnssData() const { return this->gnssData; }; + const void* const getGyroData() const; + + CalcAzimuth* getCalcCompass() const { return this->calcCompass; } + QMC5883LCompass* getRealCompass() const { return this->realCompass; } + NTRIPClient* getNtripClient() const { return this->ntripClient; } + + // static + /** + * @brief Set the output status for PVTdata. + * + * If this is true, a lot of information from the gnss module will be printed in + * the interval of navigation frequency. + * + * @param status + */ + static void setOutputStatusPrintPVTdata(bool status); private: - Sensors* sensors; + void run() override; + void runAsChild() override; + void initGnss(); + + QMC5883LCompass* realCompass = nullptr; + CalcAzimuth* calcCompass = nullptr; + SFE_UBLOX_GNSS* gnss = nullptr; + NTRIPClient* ntripClient = nullptr; + + UBX_NAV_PVT_data_t* gnssData; + Point currentPosition; + + char* host; + char* mountPoint; + char* user; + char* password; + + bool isNtripInit = false; + + uint16_t port; + int16_t realAzimuth = INT16_MAX; + int16_t calcAzimuth = INT16_MAX; + + // static + static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct); + static void savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct); + + static UBX_NAV_PVT_data_t* ubxDataStatic; + + static uint32_t ubxUpdateTimeStatic; + + static bool outputStatusPrintPVTdata; + static bool newData; }; -SensorData::SensorData(Sensors* senors) { - this->sensors = sensors; -} - #endif //SENSOR_DATA_H diff --git a/lib/Sensors/sensors.h b/lib/Sensors/sensors.h deleted file mode 100644 index 4e40de3..0000000 --- a/lib/Sensors/sensors.h +++ /dev/null @@ -1,71 +0,0 @@ -/** - * @file sensors.h - * @author Alexander Klein (alex@kleiax.de) - * @brief - * @version 0.1 - * @date 2023-09-02 - * - * @copyright Copyright (c) 2023 - * - */ - -#ifndef SENSORS_H -#define SENSORS_H - -#include -#include -//Sensors -#include -#include -// Gyroskop - -#include "component.h" -#include "sensorData.h" -#include "calibrateCompass.h" - -class Sensors : public Component { - public: - Sensors(); - - void enableGnss(SPIClass* spiPort, uint8_t csPin); - void enableGnss(); - void enableCompass(); - void enableGyroskop(); - - const SensorData const getSensorData() const { return this->sensorData; } - - /** - * @brief Set the output status for PVTdata. - * - * If this is true, a lot of information from the gnss module will be printed in - * the interval of navigation frequency. - * - * @param status - */ - static void setOutputStatusPrintPVTdata(bool status); - - private: - void run() override; - void runAsChild() override; - void initGnss(); - - QMC5883LCompass* compass = nullptr; - SFE_UBLOX_GNSS* gnss = nullptr; - SensorData* sensorData; - - CalcAzimuthState calcAzimuthState = CalcAzimuthState::Invalid; - - int16_t realAzimuth = INT16_MAX; - - static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct); - static void savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct); - - static UBX_NAV_PVT_data_t* ubxDataStatic; - - static uint32_t ubxUpdateTimeStatic; - - static bool outputStatusPrintPVTdata; - static bool newData; -}; - -#endif // SENSORS_H diff --git a/src/SpecialMenus/GPS/menuGPS.cpp b/src/SpecialMenus/GPS/menuGPS.cpp index 338dbd2..878fd13 100644 --- a/src/SpecialMenus/GPS/menuGPS.cpp +++ b/src/SpecialMenus/GPS/menuGPS.cpp @@ -11,14 +11,14 @@ #include "menuGPS.h" -MenuGPS::MenuGPS(DriveManager* driveManager) : +MenuGPS::MenuGPS(SensorData* sensorData) : MenuInformationSites(10) { - this->navigation = driveManager->getNavigation(); - this->ntripClient = this->navigation->getNTRIPClient(); + this->sensorData = sensorData; + // this->ntripClient = this->navigation->getNTRIPClient(); } void MenuGPS::printPage() const { - UBX_NAV_PVT_data_t* gpsData = this->navigation->getUbxData(); + const UBX_NAV_PVT_data_t* gpsData = this->sensorData->getGnssData(); uint8_t fixType = 0; if (gpsData) fixType = gpsData->fixType; @@ -66,15 +66,15 @@ void MenuGPS::printPage() const { break; case 3: { - NTRIPClientStates status = this->ntripClient->getClientState(); - lineOne = "NTRIP Client is"; + // NTRIPClientStates status = this->ntripClient->getClientState(); + // lineOne = "NTRIP Client is"; - if (status == NTRIPClientStates::pushData) - lineTwo = "enabled"; - else if (status == NTRIPClientStates::notAvailable) - lineTwo = "not available"; - else - lineTwo = "disabled"; + // if (status == NTRIPClientStates::pushData) + // lineTwo = "enabled"; + // else if (status == NTRIPClientStates::notAvailable) + // lineTwo = "not available"; + // else + // lineTwo = "disabled"; } break; @@ -134,7 +134,7 @@ void MenuGPS::printPage() const { case 8: lineOne = "Azimuth: "; lineTwo = ""; - lineTwo.concat(this->navigation->getAzimuth()); + lineTwo.concat(this->sensorData->getRealAzimuth()); break; default: @@ -148,12 +148,12 @@ void MenuGPS::printPage() const { void MenuGPS::runCommand() const { switch (this->getCurrentPage()) { case 3: { - // std::cout << "MenuGPS::runCommand " << this->ntripClient->getClientState() << std::endl; - if (this->ntripClient->getClientState() == NTRIPClientStates::pushData) - this->ntripClient->setActivated(false); - else if (this->ntripClient->getClientState() != NTRIPClientStates::notAvailable) - this->ntripClient->setActivated(true); - break; + // // std::cout << "MenuGPS::runCommand " << this->ntripClient->getClientState() << std::endl; + // if (this->ntripClient->getClientState() == NTRIPClientStates::pushData) + // this->ntripClient->setActivated(false); + // else if (this->ntripClient->getClientState() != NTRIPClientStates::notAvailable) + // this->ntripClient->setActivated(true); } + break; } } diff --git a/src/SpecialMenus/GPS/menuGPS.h b/src/SpecialMenus/GPS/menuGPS.h index f431a58..e88f6fe 100644 --- a/src/SpecialMenus/GPS/menuGPS.h +++ b/src/SpecialMenus/GPS/menuGPS.h @@ -15,8 +15,7 @@ #include #include "menuInformationSites.h" -#include "driveModi/driveManager.h" -#include "navigation.h" +#include "sensorData.h" #include "ntripClient.h" /** @@ -30,14 +29,14 @@ class MenuGPS : public MenuInformationSites { * * @param gps */ - MenuGPS(DriveManager* driveManager); + MenuGPS(SensorData* sensorData); private: void printPage() const override; void runCommand() const override; - NTRIPClient* ntripClient; - Navigation* navigation; + // NTRIPClient* ntripClient; + SensorData* sensorData; }; #endif // MENU_GPS_H diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp index cb3bdd0..f146790 100644 --- a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp +++ b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp @@ -13,7 +13,7 @@ void MenuAutopilot::printPage() const { CourseCorrection correction = this->autopilot->getCourseCorrection(); - UBX_NAV_PVT_data_t* gpsData = this->driveManager->getNavigation()->getUbxData(); + const UBX_NAV_PVT_data_t* gpsData = this->autopilot->getSensorData()->getGnssData(); bool navigationStarted = this->autopilot->getState() >= Autopilot::State::NavigationStarted; String distanceString = ""; @@ -104,7 +104,7 @@ void MenuAutopilot::printPage() const { break; case 3: { - NTRIPClientStates status = this->driveManager->getNavigation()->getNTRIPClient()->getClientState(); + NTRIPClientStates status = this->autopilot->getSensorData()->getNtripClient()->getClientState(); uint8_t carrSoln = gpsData->flags.bits.carrSoln; lineOne = "GNSS: "; @@ -132,30 +132,30 @@ void MenuAutopilot::printPage() const { case 4: - if (this->autopilot->getSensorData().getCalcAzimuth() == CalcAzimuth::CalcAzimuthState::Good - ||this->autopilot->getSensorData().getCalcAzimuth() == CalcAzimuth::CalcAzimuthState::Super) + if (this->autopilot->getSensorData()->getCalcAzimuth() == CalcAzimuth::State::Good + ||this->autopilot->getSensorData()->getCalcAzimuth() == CalcAzimuth::State::Super) { lineOne = "Calc Azi: "; lineTwo = "State: "; - lineOne.concat(this->autopilot->getSensorData().getCalcAzimuth()); - switch (this->autopilot->getSensorData().getCalcAzimuthState()) { - case CalcAzimuth::CalcAzimuthState::Bad : + lineOne.concat(this->autopilot->getSensorData()->getCalcAzimuth()); + switch (this->autopilot->getSensorData()->getCalcAzimuthState()) { + case CalcAzimuth::State::Bad : lineTwo.concat("Bad"); break; - case CalcAzimuth::CalcAzimuthState::Good : + case CalcAzimuth::State::Good : lineTwo.concat("Good"); break; - case CalcAzimuth::CalcAzimuthState::Invalid : + case CalcAzimuth::State::Invalid : lineTwo.concat("Invalid"); break; - case CalcAzimuth::CalcAzimuthState::Ok : + case CalcAzimuth::State::Ok : lineTwo.concat("Ok"); break; - case CalcAzimuth::CalcAzimuthState::Super : + case CalcAzimuth::State::Super : lineTwo.concat("Super"); break; @@ -166,7 +166,7 @@ void MenuAutopilot::printPage() const { } else { lineOne = "Real Azi: "; lineTwo = ""; - lineOne.concat(this->driveManager->getNavigation()->getAzimuth()); + lineOne.concat(this->autopilot->getSensorData()->getRealAzimuth()); } break; diff --git a/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp b/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp index 013a5f5..9069641 100644 --- a/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp +++ b/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp @@ -10,13 +10,11 @@ */ #include "menuCalibrateCompass.h" -MenuCalibrateCompass::MenuCalibrateCompass(DriveManager* driveManager) : MenuDriveMode(driveManager) { - this->caliCompass = new CalibrateCompass(this->driveManager->getNavigation()->getCompass()); -} +MenuCalibrateCompass::MenuCalibrateCompass(DriveManager* driveManager) : MenuDriveMode(driveManager) {} MenuCalibrateCompass::~MenuCalibrateCompass() { delete this->caliCompass; - this->manualControl->setCalibrateCompass(); + this->caliCompassMode->setCalibrateCompass(); } void MenuCalibrateCompass::printPage() const { @@ -31,7 +29,7 @@ void MenuCalibrateCompass::printPage() const { case 1: lineOne = "Azimuth:"; - lineTwo.concat(this->driveManager->getNavigation()->getAzimuth()); + lineTwo.concat(this->caliCompassMode->getSensorData()->getRealAzimuth()); break; case 2: @@ -112,8 +110,9 @@ void MenuCalibrateCompass::printPage() const { void MenuCalibrateCompass::init() { this->firstPrint = false; - this->driveManager->changeModus(Modi::ManualControl); - this->manualControl = (ManualControl*) this->driveManager->getDriveModiPtr(); + this->driveManager->changeModus(Modi::CalibrateCompass); + this->caliCompassMode = (CalibrateCompassM*) this->driveManager->getDriveModiPtr(); + this->caliCompass = new CalibrateCompass(this->caliCompassMode->getSensorData()->getRealCompass()); this->setCountPages(11); this->updateDelay = 500; } @@ -123,12 +122,12 @@ void MenuCalibrateCompass::runCommand() const { case 2: switch (this->caliCompass->getState()) { case CalibrateCompass::State::Ready : - this->manualControl->setCalibrateCompass(this->caliCompass); + this->caliCompassMode->setCalibrateCompass(this->caliCompass); this->caliCompass->start(); break; case CalibrateCompass::State::Finished: - this->manualControl->setCalibrateCompass(); + this->caliCompassMode->setCalibrateCompass(); this->caliCompass->useData(); break; diff --git a/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h b/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h index 0eee4c2..284e200 100644 --- a/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h +++ b/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h @@ -13,7 +13,7 @@ #define MENU_CALIBRATE_COMPASS_H #include "SpecialMenus/driveModi/menuDriveMode.h" -#include "" +#include "driveModi/Modi/CalibrateCompass/calibrateCompassM.h" #include "calibrateCompass.h" /** diff --git a/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp b/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp index 94da0a9..5a81821 100644 --- a/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp +++ b/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp @@ -12,7 +12,7 @@ void MenuCaptureRoute::printPage() const { RouteInfo routeInfo = this->captureRoute->getRouteInfo(); - UBX_NAV_PVT_data_t* gpsData = this->driveManager->getNavigation()->getUbxData(); + const UBX_NAV_PVT_data_t* gpsData = this->captureRoute->getSensorData()->getGnssData(); String lineOne = ""; String lineTwo = ""; @@ -56,7 +56,7 @@ void MenuCaptureRoute::printPage() const { break; case 4: { - NTRIPClientStates status = this->driveManager->getNavigation()->getNTRIPClient()->getClientState(); + NTRIPClientStates status = this->captureRoute->getSensorData()->getNtripClient()->getClientState(); lineOne = "NTRIP Client is"; if (status == NTRIPClientStates::pushData) @@ -85,7 +85,7 @@ void MenuCaptureRoute::printPage() const { case 6: lineOne = "hAccuracy: "; lineTwo = "Azimuth: "; - lineTwo.concat(this->driveManager->getNavigation()->getAzimuth()); + lineTwo.concat(this->captureRoute->getSensorData()->getRealAzimuth()); if (gpsData->fixType) lineOne.concat(gpsData->hAcc); else diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp index 9d2557e..41b454f 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.cpp +++ b/src/driveModi/Modi/Autopilot/autopilot.cpp @@ -12,24 +12,24 @@ #include "driveModi/Modi/Autopilot/autopilot.h" #include "autopilot.h" -DirectionChangeSignal::DirectionChangeSignal(Navigation* navigation) { - this->navigation = navigation; +DirectionChangeSignal::DirectionChangeSignal(Autopilot* pilot) { + this->pilot = pilot; this->action(); } DirectionChangeSignal::~DirectionChangeSignal() { - navigation->disableCalcAzimuth(); + pilot->getSensorData()->getCalcCompass()->disableCalcAzimuth(); } void DirectionChangeSignal::action() { - this->navigation->drivingDirectionChange(); + pilot->getSensorData()->getCalcCompass()->drivingDirectionChange(pilot->getSensorData()->getCurrentPos()); } Autopilot::Autopilot(DriveModiParams params, Navigation* navigation) : ManualControl(params) { this->setInputMode(ManualControl::InputMode::Digital); - this->directionChangeSignal = new DirectionChangeSignal(navigation); + this->directionChangeSignal = new DirectionChangeSignal(this); this->setDirectionChangeCallback(this->directionChangeSignal); this->navigation = navigation; this->init(); @@ -119,7 +119,7 @@ void Autopilot::init() { else this->state = State::NoRoute; this->routeInfo = this->navigation->getRouteInfo(); - this->navigation->getNTRIPClient()->setAutoReconnect(true); + this->sensorData->getNtripClient()->setAutoReconnect(true); this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection, true); @@ -143,7 +143,7 @@ void Autopilot::beginRotate() { if (this->state != State::SelfDrivingRotate) { this->lastState = this->state; this->state = State::SelfDrivingRotate; - this->rotationAimAzimuth = this->navigation->getAzimuth() + this->courseCorrection.correction; + this->rotationAimAzimuth = this->getSensorData()->getRealAzimuth() + this->courseCorrection.correction; this->rotationAimAzimuth = Navigation::fixDegree(this->rotationAimAzimuth); this->moveControl->setSpeed(0); @@ -155,17 +155,17 @@ void Autopilot::beginRotate() { } void Autopilot::rotate() { - if (abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct) { + if (abs(this->getSensorData()->getRealAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct) { this->endRotate(); return; } - if ((abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct * 3) + if ((abs(this->getSensorData()->getRealAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct * 3) && ((this->courseCorrection.correction > 0 - && this->rotationAimAzimuth < this->navigation->getAzimuth()) + && this->rotationAimAzimuth < this->getSensorData()->getRealAzimuth()) || (this->courseCorrection.correction < 0 - && this->rotationAimAzimuth > this->navigation->getAzimuth()))) + && this->rotationAimAzimuth > this->getSensorData()->getRealAzimuth()))) { this->endRotate(); std::cout << "Autopilot::rotate: Rover rotated too far" << std::endl; @@ -177,7 +177,7 @@ void Autopilot::endRotate() { return; this->state = this->lastState; - this->navigation->drivingDirectionChange(); + this->sensorData->getCalcCompass()->drivingDirectionChange(this->sensorData->getCurrentPos()); this->moveControl->setRotationSpeed(0); this->moveControl->emergencyStop(); this->moveControl->setDrivingStatus(MoveControl::Status::Drive); diff --git a/src/driveModi/Modi/Autopilot/autopilot.h b/src/driveModi/Modi/Autopilot/autopilot.h index 5f64bea..ea64ac1 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.h +++ b/src/driveModi/Modi/Autopilot/autopilot.h @@ -16,15 +16,7 @@ #include "navigation.h" #include "driveModi/Modi/ManualControl/manualControl.h" -class DirectionChangeSignal : public DirectionChangeWrapper { - public: - DirectionChangeSignal(Navigation* navigation); - ~DirectionChangeSignal(); - void action() override; - - private: - Navigation* navigation; -}; +class DirectionChangeSignal; /** * @brief This class use the navigate class to drive automaticaly @@ -130,4 +122,14 @@ class Autopilot : public ManualControl { double minRemainingDistance = 0.25; }; +class DirectionChangeSignal : public DirectionChangeWrapper { + public: + DirectionChangeSignal(Autopilot* pilot); + ~DirectionChangeSignal(); + void action() override; + + private: + Autopilot* pilot; +}; + #endif // AUTOPILOT_H diff --git a/src/driveModi/Modi/CaptureRoute/captureRoute.cpp b/src/driveModi/Modi/CaptureRoute/captureRoute.cpp index ae4cda9..4ab4864 100644 --- a/src/driveModi/Modi/CaptureRoute/captureRoute.cpp +++ b/src/driveModi/Modi/CaptureRoute/captureRoute.cpp @@ -15,7 +15,7 @@ CaptureRoute::CaptureRoute(DriveModiParams params, Navigation* navigation) : ManualControl(params) { this->navigation = navigation; this->navigation->getRoute()->clear(); - this->navigation->getNTRIPClient()->setAutoReconnect(true); + this->sensorData->getNtripClient()->setAutoReconnect(true); this->routeInfo = navigation->getRouteInfo(); } diff --git a/src/driveModi/Modi/TestMode/testMode.cpp b/src/driveModi/Modi/TestMode/testMode.cpp index 040ee37..921e45e 100644 --- a/src/driveModi/Modi/TestMode/testMode.cpp +++ b/src/driveModi/Modi/TestMode/testMode.cpp @@ -21,7 +21,7 @@ TestMode::~TestMode() { void TestMode::run() { if (this->maneuver == Maneuver::Turn) { - uint16_t delta = abs(this->azimuth - this->navigation->getAzimuth()); + uint16_t delta = abs(this->azimuth - this->getSensorData()->getRealAzimuth()); if (delta > this->degree) this->abort = true; } @@ -50,7 +50,7 @@ bool TestMode::drive(int16_t cm, int16_t degree) { else if (degree > 0) this->moveControl->setRotationSpeed(this->maxRotationSpeed); - this->azimuth = this->navigation->getAzimuth(); + this->azimuth = this->getSensorData()->getRealAzimuth(); this->degree = degree; this->maneuverTime = 5 * 1000; diff --git a/src/driveModi/driveManager.cpp b/src/driveModi/driveManager.cpp index 46f86cc..5d7dc0f 100644 --- a/src/driveModi/driveManager.cpp +++ b/src/driveModi/driveManager.cpp @@ -11,18 +11,18 @@ #include "driveModi/driveManager.h" -DriveManager::DriveManager(MoveControl *moveControl, SensorData* sensorData, ControlPadInput *input, bool wifi) { +DriveManager::DriveManager(MoveControl *moveControl, const SensorData* sensorData, const ControlPadInput *input) { this->driveModiParams.input = input; this->driveModiParams.moveControl = moveControl; this->driveModiParams.sensorData = sensorData; - this->init(wifi); + this->init(); } -DriveManager::DriveManager(DriveModiParams params, bool wifi){ +DriveManager::DriveManager(DriveModiParams params){ this->driveModiParams = params; - this->init(wifi); + this->init(); } DriveManager::~DriveManager() { @@ -73,10 +73,8 @@ void DriveManager::changeModus(Modi modus) { this->addChildComponent(this->currentModusPtr); } -void DriveManager::init(bool wifi) { - this->navigation = new Navigation(); - if (wifi) - this->navigation->initNtrip(NTRIP_HOST, NTRIP_PORT, NTRIP_MOUNT_POINT, NTRIP_USER, NTRIP_PASSWORD); +void DriveManager::init() { + this->navigation = new Navigation(this->driveModiParams.sensorData); this->addChildComponent(this->driveModiParams.moveControl); this->addChildComponent(this->navigation); diff --git a/src/driveModi/driveManager.h b/src/driveModi/driveManager.h index c29bfcd..b0e3215 100644 --- a/src/driveModi/driveManager.h +++ b/src/driveModi/driveManager.h @@ -38,6 +38,7 @@ enum class Modi { Off, ManualControl, + CalibrateCompass, CaptureRoute, Autopilot, TestMode @@ -52,8 +53,8 @@ class DriveManager : public Component { * * @param moveControl */ - DriveManager(MoveControl *moveControl, SensorData* sensorData, ControlPadInput *input, bool wifi = false); - DriveManager(DriveModiParams params, bool wifi = false); + DriveManager(MoveControl *moveControl, const SensorData* sensorData, const ControlPadInput *input); + DriveManager(DriveModiParams params); /** * @brief Destroy the Drive Manager object @@ -99,7 +100,7 @@ class DriveManager : public Component { private: void run() override {}; - void init(bool wifi); + void init(); Modi currentModus = Modi::Off; DriveModi *currentModusPtr = nullptr; diff --git a/src/driveModi/driveModi.h b/src/driveModi/driveModi.h index 32187cc..b2267c3 100644 --- a/src/driveModi/driveModi.h +++ b/src/driveModi/driveModi.h @@ -19,8 +19,8 @@ struct DriveModiParams { MoveControl* moveControl; - ControlPadInput* input; - SensorData* sensorData; + const ControlPadInput* input; + const SensorData* sensorData; }; /** @@ -35,7 +35,7 @@ class DriveModi : public Component { DriveModi(DriveModiParams); virtual ~DriveModi(); - SensorData getSensorData() const { return this->sensorData; } + const SensorData* getSensorData() const { return this->sensorData; } /** * @brief Set the max speed @@ -59,8 +59,8 @@ class DriveModi : public Component { protected: MoveControl *moveControl; - ControlPadInput* input; - SensorData* sensorData; + const ControlPadInput* input; + const SensorData* sensorData; double maxForwardSpeed = 1; double maxRotationSpeed = 7; diff --git a/src/main.cpp b/src/main.cpp index d91d4e3..0044b6f 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -31,7 +31,6 @@ #include "network.h" #include "debugMqtt.h" #include "battery.h" -#include "sensors.h" #include "sensorData.h" #include "debugTimes.h" #include "controlPad.h" @@ -57,7 +56,6 @@ OutputBuf* outputBuf; DebugMqtt* debugMqtt = nullptr; Battery* mainBattery; SPIClass* spiPort; -Sensors* sensors; SensorData* sensorData; ControlPad* controlPad; @@ -119,11 +117,10 @@ void setup() { std::cout << "Build timestamp: " << BUILD_TIMESTAMP << std::endl; std::cout << "All actions from the main program run on Core -> " << xPortGetCoreID() << std::endl; - sensors = new Sensors(); - sensors->enableGnss(spiPort, PinNumbers::gnssSpiCs); - sensors->enableCompass(); - sensorData = sensors->getSensorData(); - driveManager = new DriveManager(&moveController, sensorData, controlPad->getControlPadDataPtr(), wifiIsActive); + sensorData = new SensorData(); + sensorData->enableGnss(spiPort, PinNumbers::gnssSpiCs); + sensorData->enableRealCompass(); + driveManager = new DriveManager(&moveController, sensorData, controlPad->getControlPadDataPtr()); outputBuf->activateMqtt(true); @@ -150,7 +147,7 @@ void loop() { #endif //MQTT wifiTime.stopConsol("WiFi-Time", 10); } - sensors->loop(); + sensorData->loop(); driveManager->loop(); controlPad->loop(); main_m->update(); @@ -225,7 +222,7 @@ void makeMenu() { MenuAutopilot* auto_m = new MenuAutopilot(driveManager); MenuTestMode* testM_m = new MenuTestMode(driveManager); MenuCalibrateCompass* comp_m = new MenuCalibrateCompass(driveManager); - MenuGPS* gps_m = new MenuGPS(driveManager); + MenuGPS* gps_m = new MenuGPS(sensorData); MenuSysteminformation* sys_m = new MenuSysteminformation(mainBattery); MenuRoute* rout_m = new MenuRoute(driveManager->getNavigation()->getRoute());