From 5f650982a72bed9199588c0fe609cff86aad6f5f Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Sun, 3 Sep 2023 12:36:16 +0200 Subject: [PATCH] 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();