Merge branch 'main' into projektarbeit

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() {
-    this->compass->read();
-    this->realAzimuth = this->compass->getAzimuth();
+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::runAsChild() {
-    this->gnss->checkUblox();
-    this->gnss->checkCallbacks();
-
-    if (Sensors::newData)
-        Sensors::newData = false;
+SensorData::~SensorData() {
+    if (this->ntripClient)
+        delete this->ntripClient;
 }
 
-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() {
-    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::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 SensorData::enableGyroskop() {
+    
 }
 
-void Sensors::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
-    if (!Sensors::outputStatusPrintPVTdata)
+CalcAzimuth::State SensorData::getCalcAzimuthState() const {
+    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) {
-    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);
 }

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