Merge branch 'main' into projektarbeit

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

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 = State::Invalid;
+    }
+}
+
+void CalcAzimuth::updateCurrentPosition(Point point) {
+    this->currentPosition = point;
+    this->positionChanged = true;
+}
+
+void 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 = State::Invalid;
+        this->calcAzimuth = 999;
+        return;
+    }
+
+    this->calcAzimuth = this->lastChangePoint.courseTo(this->currentPosition);
+
+    // Map point accuracy to State
+    if (this->lastChangePoint.getAccuracy() == Point::Accuracy::oneDigOfCM
+        || this->currentPosition.getAccuracy() == Point::Accuracy::oneDigOfCM) 
+    {
+        this->state = State::Good;
+    } 
+    else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::twoDigOfCM
+        || this->currentPosition.getAccuracy() == Point::Accuracy::twoDigOfCM) 
+    {
+        this->state = State::Ok;
+    } 
+    else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::threeDigOfCM
+        || this->currentPosition.getAccuracy() == Point::Accuracy::threeDigOfCM) 
+    {
+        this->state = State::Bad;
+    } 
+    else 
+    {
+        this->state = State::Invalid;
+    }
+
+    // Upgrade quality if the range grows up
+    if (this->lastChangePoint.distanceTo(this->currentPosition) > 2.0) {
+        switch (this->state) {
+            case State::Bad :
+                this->state = State::Ok;
+                break;
+            
+            case State::Ok :
+                this->state = State::Good;
+                break;
+
+            case State::Good :
+                this->state = State::Super;
+                break;
+
+            default:
+                break;
+        }
+    }
+}

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 State {
+            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; }
+        State getState() const { return this->state; }
+
+    private:
+        void run() override;
+        void updateAzimuth();
+
+        State state = State::Invalid;
+        Point lastChangePoint;
+        Point currentPosition;
+
+        bool positionChanged = false;
+        bool directionChangeMode = false;
+        int16_t calcAzimuth = INT16_MAX;
+};
+
+#endif //CALC_AZIMUTH_H

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; }
 

lib/Point/point.cpp

@@ -0,0 +1,104 @@
+/**
+ * @file point.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2023-09-03
+ * 
+ * @copyright Copyright (c) 2023
+ * 
+ */
+
+#include "point.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<int16_t>(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;   
+}

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 <cmath>   
+
+#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

lib/Sensors/sensorData.cpp

@@ -9,22 +9,32 @@
  * 
  */
 
-#include "sensors.h"
+#include "sensorData.h"
 
-void Sensors::run() {
+bool SensorData::outputStatusPrintPVTdata = false;
-    this->compass->read();
+bool SensorData::newData = false;
-    this->realAzimuth = this->compass->getAzimuth();
+uint32_t SensorData::ubxUpdateTimeStatic = 0;
+UBX_NAV_PVT_data_t* SensorData::ubxDataStatic = nullptr;
+
+SensorData::SensorData() {
+    this->loopDelay = 50;
 }
 
-void Sensors::runAsChild() {
+SensorData::~SensorData() {
-    this->gnss->checkUblox();
+    if (this->ntripClient)
-    this->gnss->checkCallbacks();
+        delete this->ntripClient;
-
-    if (Sensors::newData)
-        Sensors::newData = false;
 }
 
-void Sensors::enableGnss(SPIClass* spiPort, uint8_t csPin) {
+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;
@@ -33,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;
@@ -42,39 +52,35 @@ void Sensors::enableGnss() {
     this->initGnss();
 }
 
-void Sensors::enableCompass() {
+void SensorData::enableRealCompass() {
-    this->compass = new QMC5883LCompass();
+    this->realCompass = new QMC5883LCompass();
     // Init Compass
     Wire.beginTransmission(0x0d);
     Wire.write(0x0b);
     Wire.write(0x01);
     Wire.endTransmission();
-    this->compass->setMode(0x01,0x0C,0x10,0X00);
+    this->realCompass->setMode(0x01,0x0C,0x10,0X00);
-    CalibrateCompass caliCompass(this->compass);
+    CalibrateCompass caliCompass(this->realCompass);
     caliCompass.loadData();
     caliCompass.useData();
 }
 
-void Sensors::setOutputStatusPrintPVTdata(bool status) {
+void SensorData::enableCalcCompass() {
-    Sensors::outputStatusPrintPVTdata = status;
+
 }
 
-void Sensors::initGnss() {
+void SensorData::enableGyroskop() {
-    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) {
+CalcAzimuth::State SensorData::getCalcAzimuthState() const {
-    if (!Sensors::outputStatusPrintPVTdata)
+    if (this->calcCompass)
+        return this->calcCompass->getState();
+    return CalcAzimuth::State::Invalid;
+}
+
+void SensorData::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
+    if (!SensorData::outputStatusPrintPVTdata)
         return;
 
     double latitude = (double) ubxDataStruct->lat / 10000000.0;
@@ -120,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) {
+void SensorData::savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
-    Sensors::printPVTdata(ubxDataStruct);
+    SensorData::printPVTdata(ubxDataStruct);
 
-    Sensors::newData = true;
+    SensorData::newData = true;
-    Sensors::ubxDataStatic = ubxDataStruct;
+    SensorData::ubxDataStatic = ubxDataStruct;
-    Sensors::ubxUpdateTimeStatic = millis();
+    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);
 }

lib/Sensors/sensorData.h

@@ -0,0 +1,102 @@
+/**
+ * @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 <SPI.h>
+#include <iostream>
+
+#include "component.h"
+#include "calibrateCompass.h"
+
+#include <SparkFun_u-blox_GNSS_Arduino_Library.h>
+#include <QMC5883LCompass.h>
+// Gyroskop
+#include "calcAzimuth.h"
+#include "ntripClient.h"
+
+#include "point.h"
+class Sensors;
+
+class SensorData : public Component {
+    public:
+        SensorData();
+        ~SensorData();
+        
+        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:
+        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;
+};
+
+#endif //SENSOR_DATA_H

lib/Sensors/sensors.h

@@ -1,77 +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 <SPI.h>
-#include <iostream>
-//Sensors
-#include <SparkFun_u-blox_GNSS_Arduino_Library.h>
-#include <QMC5883LCompass.h>
-// Gyroskop
-
-#include "component.h"
-#include "sensorData.h"
-#include "calibrateCompass.h"
-
-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();
-
-        /**
-         * @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 initGnss();
-
-        QMC5883LCompass* compass = nullptr;
-        SFE_UBLOX_GNSS* gnss = nullptr;
-
-        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

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->sensorData = sensorData;
-    this->ntripClient = this->navigation->getNTRIPClient();
+    // 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();
+            // NTRIPClientStates status = this->ntripClient->getClientState();
-            lineOne = "NTRIP Client is";
+            // lineOne = "NTRIP Client is";
 
-            if (status ==  NTRIPClientStates::pushData)
+            // if (status ==  NTRIPClientStates::pushData)
-                lineTwo = "enabled";
+            //     lineTwo = "enabled";
-            else if (status == NTRIPClientStates::notAvailable)
+            // else if (status == NTRIPClientStates::notAvailable)
-                lineTwo = "not available";
+            //     lineTwo = "not available";
-            else
+            // else
-                lineTwo = "disabled";
+            //     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;
+            // // std::cout << "MenuGPS::runCommand " << this->ntripClient->getClientState() << std::endl;
-            if (this->ntripClient->getClientState() == NTRIPClientStates::pushData)
+            // if (this->ntripClient->getClientState() == NTRIPClientStates::pushData)
-                this->ntripClient->setActivated(false);
+            //     this->ntripClient->setActivated(false);
-            else if (this->ntripClient->getClientState() != NTRIPClientStates::notAvailable)
+            // else if (this->ntripClient->getClientState() != NTRIPClientStates::notAvailable)
-                this->ntripClient->setActivated(true);
+            //     this->ntripClient->setActivated(true);
+        }
         break;
     }
-    }
 }

src/SpecialMenus/GPS/menuGPS.h

@@ -15,8 +15,7 @@
 #include <SparkFun_u-blox_GNSS_Arduino_Library.h>
 
 #include "menuInformationSites.h"
-#include "driveModi/driveManager.h"
+#include "sensorData.h"
-#include "navigation.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;
+        // NTRIPClient* ntripClient;
-        Navigation* navigation;
+        SensorData* sensorData;
 };
 
 #endif // MENU_GPS_H

src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp

@@ -10,13 +10,11 @@
  */
 #include "menuCalibrateCompass.h"
 
-MenuCalibrateCompass::MenuCalibrateCompass(DriveManager* driveManager) : MenuDriveMode(driveManager) {
+MenuCalibrateCompass::MenuCalibrateCompass(DriveManager* driveManager) : MenuDriveMode(driveManager) {}
-    this->caliCompass = new CalibrateCompass(this->driveManager->getNavigation()->getCompass());
-}
 
 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->driveManager->changeModus(Modi::CalibrateCompass);
-    this->manualControl = (ManualControl*) this->driveManager->getDriveModiPtr();
+    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;
                 

src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h

@@ -13,6 +13,7 @@
 #define MENU_CALIBRATE_COMPASS_H
 
 #include "SpecialMenus/driveModi/menuDriveMode.h"
+#include "driveModi/Modi/CalibrateCompass/calibrateCompassM.h"
 #include "calibrateCompass.h"
 
 /**
@@ -44,7 +45,7 @@ class MenuCalibrateCompass : public MenuDriveMode {
         void runCommand() const override;
 
     private:
-        ManualControl* manualControl;
+        CalibrateCompassM* caliCompassMode;
         CalibrateCompass* caliCompass;
 };
 

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;
+    }
+}

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

src/driveModi/Modi/ConsolControl/consolControl.cpp

@@ -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"
-

src/driveModi/Modi/ConsolControl/consolControl.h

@@ -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

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) {

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(DriveModiParams params) : DriveModi(params){};
-        ~ManualControl();
-
-        void setCalibrateCompass(CalibrateCompass* caliCompass = nullptr);
 
         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;
 };

src/driveModi/Modi/TestMode/testMode.cpp

@@ -10,8 +10,8 @@
  */
 #include "testMode.h"
 
-TestMode::TestMode(MoveControl *moveControl,  Navigation* navigation) 
+TestMode::TestMode(DriveModiParams params,  Navigation* navigation) 
-        : DriveModi(moveControl) {
+        : DriveModi(params) {
     this->navigation = navigation;
 }
 
@@ -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;

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);

src/driveModi/driveManager.cpp

@@ -11,33 +11,22 @@
 
 #include "driveModi/driveManager.h"
 
-Modi& operator++(Modi& m, int) {
+DriveManager::DriveManager(MoveControl *moveControl, const SensorData* sensorData, const ControlPadInput *input) {
-    return m = (m == Modi::TestMode) ? Modi::Off : static_cast<Modi>(static_cast<int>(m)+1);
+    this->driveModiParams.input = input;
+    this->driveModiParams.moveControl = moveControl;
+    this->driveModiParams.sensorData = sensorData;
+
+    this->init();
 }
 
-DriveManager::DriveManager(MoveControl *moveControl, SPIClass *spiPort, const ControlPadInput *input, bool wifi) {
+DriveManager::DriveManager(DriveModiParams params){
-    this->moveControl = moveControl;
+    this->driveModiParams = params;
-    this->input = input;
     
-    this->spiPort = spiPort;
+    this->init();
-    this->navigation = new Navigation(spiPort, PinNumbers::gnssSpiCs);
-    if (wifi)
-        this->navigation->initNtrip(NTRIP_HOST, NTRIP_PORT, NTRIP_MOUNT_POINT, NTRIP_USER, NTRIP_PASSWORD);
-    
-    this->addChildComponent(this->moveControl);
-    this->addChildComponent(this->navigation);
-    this->activateOnlyChilds();
 }
 
 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,11 @@ void DriveManager::changeModus(Modi modus) {
         this->addChildComponent(this->currentModusPtr);
 }
 
+void DriveManager::init() {
+    this->navigation = new Navigation(this->driveModiParams.sensorData);
+    
+    this->addChildComponent(this->driveModiParams.moveControl);
+    this->addChildComponent(this->navigation);
+    this->activateOnlyChilds();
+}
+

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"
 
 /**
@@ -38,9 +38,9 @@
 enum class Modi {   
     Off,
     ManualControl,
+    CalibrateCompass,
     CaptureRoute,
     Autopilot,
-    ConsolControl,
     TestMode
 };
 
@@ -53,7 +53,8 @@ class DriveManager : public Component {
          * 
          * @param moveControl 
          */
-        DriveManager(MoveControl *moveControl, SPIClass *spiPort, const ControlPadInput *input, bool wifi = false);
+        DriveManager(MoveControl *moveControl, const SensorData* sensorData, const ControlPadInput *input);
+        DriveManager(DriveModiParams params);
 
         /**
          * @brief Destroy the Drive Manager object
@@ -61,14 +62,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 +99,13 @@ class DriveManager : public Component {
         Modi getDriveModi() const { return this->currentModus; }
 
     private:
-        void run() override;
+        void run() override {};
+        void init();
 
         Modi currentModus = Modi::Off;
-        MoveControl *moveControl;
-        const ControlPadInput* input;
         DriveModi *currentModusPtr = nullptr;
         Navigation* navigation;
-        SPIClass* spiPort;
+        DriveModiParams driveModiParams;
 };
 
 

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->input = input;
-    this->loopDelay = 40;
+    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;
+}

src/driveModi/driveModi.h

@@ -14,6 +14,14 @@
 
 #include "component.h"
 #include "moveControl.h"
+#include "controlPadInput.h"
+#include "sensorData.h"
+
+struct DriveModiParams {
+    MoveControl* moveControl;
+    const ControlPadInput* input;
+    const 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();
 
+        const SensorData* getSensorData() const { return this->sensorData; }
+
         /**
          * @brief Set the max speed
          * 
@@ -48,8 +59,12 @@ class DriveModi : public Component {
 
     protected:
         MoveControl *moveControl;
+        const ControlPadInput* input;
+        const SensorData* sensorData;
         double maxForwardSpeed = 1;
         double maxRotationSpeed = 7;
 
+    private:
+        void init();        
 };
 #endif // DRIVEMODI_H

src/main.cpp

@@ -31,6 +31,7 @@
 #include "network.h"
 #include "debugMqtt.h"
 #include "battery.h"
+#include "sensorData.h"
 #include "debugTimes.h"
 #include "controlPad.h"
 
@@ -55,6 +56,7 @@ OutputBuf* outputBuf;
 DebugMqtt* debugMqtt = nullptr;
 Battery* mainBattery;
 SPIClass* spiPort;
+SensorData* sensorData;
 ControlPad* controlPad;
 
 bool wifiIsActive;
@@ -115,7 +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;
 
-    driveManager = new DriveManager(&moveController, spiPort, controlPad->getControlPadDataPtr(), wifiIsActive);
+    sensorData = new SensorData();
+    sensorData->enableGnss(spiPort, PinNumbers::gnssSpiCs);
+    sensorData->enableRealCompass();
+    driveManager = new DriveManager(&moveController, sensorData, controlPad->getControlPadDataPtr());
 
     outputBuf->activateMqtt(true);
 
@@ -142,6 +147,7 @@ void loop() {
         #endif //MQTT
         wifiTime.stopConsol("WiFi-Time", 10);
     }
+    sensorData->loop();
     driveManager->loop();
     controlPad->loop();
     main_m->update();
@@ -216,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());