Merge branch 'main' of git.kleiax.de:kleiax/Projektarbeit-Rover into main

.vscode/settings.json

@@ -77,7 +77,47 @@
         "istream": "cpp",
         "streambuf": "cpp",
         "functional": "cpp",
-        "cmath": "cpp"
+        "cmath": "cpp",
+        "atomic": "cpp",
+        "cctype": "cpp",
+        "chrono": "cpp",
+        "clocale": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdint": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "exception": "cpp",
+        "algorithm": "cpp",
+        "iterator": "cpp",
+        "map": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "random": "cpp",
+        "ratio": "cpp",
+        "string_view": "cpp",
+        "system_error": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "limits": "cpp",
+        "ostream": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "cinttypes": "cpp",
+        "typeinfo": "cpp"
     },
     "cSpell.words": [
         "Ahnung",

doc/Notes and TODOs/SPIpushRawData.txt

@@ -1,9 +0,0 @@
-Added this code in SparkFun_u-blox_GNSS_Arduino_Libary.cpp in the function pushRawData.
-
-_spiPort->beginTransaction(SPISettings(_spiSpeed, MSBFIRST, SPI_MODE0));
-digitalWrite(_csPin, LOW);
-for (uint16_t i = 0; i < numDataBytes; i++) {
-    spiTransfer(dataBytes[i]);
-}
-digitalWrite(_csPin, HIGH);
-_spiPort->endTransaction();

doc/Notes and TODOs/ps3 reconnect.txt

@@ -1,26 +0,0 @@
-ps3.h
-void ps3ResetGlobals();
-
-ps3.c
-void ps3ResetGlobals() {
-    // Own Code
-    is_active = false;
-    ps3_connection_cb = NULL;
-    ps3_connection_object_cb = NULL;
-    ps3_connection_object = NULL;
-    ps3_event_cb = NULL;
-    ps3_event_object_cb = NULL;
-    ps3_event_object = NULL;
-}
-
-Ps3Controller.h
-private
-void resetGlobals();
-
-Ps3Controller.cpp
-void Ps3Controller::resetGlobals() {
-    ps3ResetGlobals();
-}
-
-in -> Ps3Controller::begin
-this->resetGlobals();

doc/PinBelegungen.txt

@@ -1,30 +0,0 @@
-ESP32 Rover Pinbelegung
-
-		3V3				GND
-		x				23		DirR1
-		x				22		PWMR
-		x				TX		PC
-		x				RX		PC				Connector I2C
-BATTERY	35				21		SDA					SDA
-INTL1	32				GND							GND
-INTL2	33				19		SCL					SCL
-INTR1	25				18							VCC
-INTR2	26				5		SPI_CSK
-DirL1	27				17		SPI_CS
-DirR2	14				16		SPI_COPI
-DirL2	12				4		SPI_CIPO
-		GND				x
-PWML	13				2		Probleme beim flashen
-		x				15		WS2812 LED ?
-		x				x
-		CMD				x
-		5V	   USB		x
-		
-
-Connector Encoder
-
--	2	1	+
-OB	4	3	-
-3V3	6  [5]	OA
-	8	7
-	10	9

doc/TODO allgemein.txt

@@ -0,0 +1,12 @@
+Für irgendwann:
+    -> Program underfloorLighting
+    -> Add an beeper
+    -> Program the beeper
+    -> Update GNSS Lib to v3
+
+Besser zügig:
+    Speedometer buffer ergibt kaum Sinn
+    Abweichung max 25 cm bevor zu ungenau damit nie mehr als 50 cm gesamt.
+    Doxygen Kommentare aktualisieren
+    Motortreiber vielleicht bei wenigster Leistung ohne Kurve
+    

doc/controller.txt

@@ -1,17 +0,0 @@
-Controller
-
-PS3-Button:     Change Mode
-
-Mode: ManualControl
-    L3: Joystick Mode
-        Left Joystick:  Drive
-
-    R3: Shoulder Trigger Mode
-        L2:             Spin left side
-        R2:             Spin right side
-
-Mode: Autopilot
-
-Mode: CaptureRoute
-
-Mode: ConsolControl

include/Version.h

@@ -0,0 +1,9 @@
+
+    // AUTO GENERATED FILE, DO NOT EDIT
+    #ifndef VERSION
+        #define VERSION "0.8.36"
+    #endif
+    #ifndef BUILD_TIMESTAMP
+        #define BUILD_TIMESTAMP "2023-08-13 15:13:34.491991"
+    #endif
+    

include/network.h

@@ -16,6 +16,7 @@
 #include <WiFi.h>
 #include <PubSubClient.h>
 #include <esp_now.h>
+#include <esp_wifi.h>
 
 #include "networkConfig.h"
 
@@ -44,6 +45,7 @@ class Network {
          */
         static bool connectWifi();
         static bool connectEspNow(recieveCallbackPtr reci, sendCallbackPtr send);
+        static uint8_t getCurrentChannel();
 
         /**
          * @brief Set all general settings to connect to a broker.

lib/MotorControl/motorControl.cpp

@@ -17,9 +17,9 @@ MotorControl::MotorControl() {
     this->setMaxPwm(PWMMAX);
 }
 
-void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t dir_1, uint8_t dir_2) {
+void MotorControl::init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8_t dir_2) {
-    this->pwm_pin = pwm_pin;
+    this->pwmPin = pwmPin;
-    this->pwm_channel = pwm_channel;
+    this->pwmChannel = pwmChannel;
     this->dir_1 = dir_1;
     this->dir_2 = dir_2;
 
@@ -29,17 +29,17 @@ void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t dir_1, uin
     digitalWrite(this->dir_1, LOW);
     digitalWrite(this->dir_2, LOW);
 
-    ledcSetup(this->pwm_channel, PWMFREQ, this->pwm_res);
+    ledcSetup(this->pwmChannel, PWMFREQ, this->pwmRes);
-    ledcAttachPin(this->pwm_pin, this->pwm_channel);
+    ledcAttachPin(this->pwmPin, this->pwmChannel);
-    ledcWrite(this->pwm_channel, 0);
+    ledcWrite(this->pwmChannel, 0);
 }
 
 uint16_t MotorControl::loop() {
     uint32_t time = millis();
     uint16_t elapsed_time = time - this->lastMillis;
 
-    //Cancel if delay is not reached
+    //Cancel if delayLoop is not reached
-    if (elapsed_time < delay)
+    if (elapsed_time < delayLoop)
         return elapsed_time;
 
     runMotorControl();
@@ -48,14 +48,14 @@ uint16_t MotorControl::loop() {
 }
 
 void MotorControl::runMotorControl() {
-    // Absolute difference between target_power and power
+    // Absolute difference between targetPower and power
-    uint8_t abs_difference = abs(this->target_power - this->power);
+    uint8_t abs_difference = abs(this->targetPower - this->power);
 
-    // Difference between target_power and power
+    // Difference between targetPower and power
-    int16_t difference = this->target_power - this->power;
+    int16_t difference = this->targetPower - this->power;
 
     // Check that the target speed is close to 0 and that the abs_difference is lower than powersteps
-    if (abs(this->target_power) <  powersteps && abs_difference < powersteps) {
+    if (abs(this->targetPower) <  powersteps && abs_difference < powersteps) {
         this->setRealPower(0);
         return;
     }
@@ -66,7 +66,7 @@ void MotorControl::runMotorControl() {
     }
 
     // Positive or negative tagret speed
-    if (this->target_power >= 0) {
+    if (this->targetPower >= 0) {
         // Positive or negative speed
         if (this->power >= 0) {
             if (difference > 0) {
@@ -95,36 +95,36 @@ void MotorControl::runMotorControl() {
 void MotorControl::setMinPwm(uint8_t min) {
     if (min > 80) min = 80;
     //transform percentage to real pwm value
-    min = (uint8_t) (((1 << pwm_res) - 1) * (min / 100.0));
+    min = (uint8_t) (((1 << pwmRes) - 1) * (min / 100.0));
-    this->dutycycle_min = min;
+    this->dutycycleMin = min;
 }
 
 void MotorControl::setMaxPwm(uint8_t max) {
     if (max > 100) max = 100;
     //transform percentage to real pwm value
-    max = (uint8_t) (((1 << pwm_res) - 1) * (max / 100.0));
+    max = (uint8_t) (((1 << pwmRes) - 1) * (max / 100.0));
-    this->dutycycle_max = max;
+    this->dutycycleMax = max;
 }
 
 uint16_t MotorControl::setPowerSteps(uint8_t increment) {
     this->powersteps = increment;
-    return (uint16_t) (delay * ( 100 / powersteps ));
+    return (uint16_t) (delayLoop * ( 100 / powersteps ));
 }
 
 void MotorControl::setTargetPower(int8_t power) {
     if (power <= 100 && power >= -100)
-        this->target_power = power;
+        this->targetPower = power;
     else
         std::cout << " MotorControl::setTargetPower: Invalid Argument - Power: " << power << std::endl;
 }
 
-uint16_t MotorControl::setDelay(uint8_t delay) {
+uint16_t MotorControl::setDelay(uint8_t delayLoop) {
-    this->delay = delay;
+    this->delayLoop = delayLoop;
-    return (uint16_t) (delay * ( 100 / powersteps ));
+    return (uint16_t) (delayLoop * ( 100 / powersteps ));
 }
 
 void MotorControl::stop() {
-    this->target_power = 0;
+    this->targetPower = 0;
 }
 
 void MotorControl::emergencyStop() {
@@ -136,23 +136,23 @@ int8_t MotorControl::getPower() {
 }
 
 int8_t MotorControl::getTargetPower() {
-    return this->target_power;
+    return this->targetPower;
 }
 
 bool MotorControl::isTargetPowerReached() {
-    if (this->target_power == this->power)
+    if (this->targetPower == this->power)
         return true;
     return false;
 }
 
 bool MotorControl::isAccelerationPositive() {
-    if (power < target_power)
+    if (power < targetPower)
         return true;
     return false;
 }
 
 bool MotorControl::isAccelerationNegative() {
-    if (power > target_power)
+    if (power > targetPower)
         return true;
     return false;
 }
@@ -169,12 +169,12 @@ void MotorControl::setRealPower(int8_t power) {
         this->direction = 0;
         digitalWrite(this->dir_1, LOW);
         digitalWrite(this->dir_2, LOW);
-        ledcWrite(this->pwm_channel, 0);
+        ledcWrite(this->pwmChannel, 0);
         this->dutycycle = 0;
         return;
     }
 
-    uint8_t pwm_val = map(abs(power), 0, 100, this->dutycycle_min, this->dutycycle_max);
+    uint8_t pwm_val = map(abs(power), 0, 100, this->dutycycleMin, this->dutycycleMax);
 
     if ((this->direction == 1 || this->direction == 0) && power < 0){  // new direction backward
         this->direction = 2;
@@ -186,7 +186,7 @@ void MotorControl::setRealPower(int8_t power) {
         digitalWrite(this->dir_2, LOW);
     }
 
-    ledcWrite(this->pwm_channel, pwm_val);
+    ledcWrite(this->pwmChannel, pwm_val);
     this->dutycycle = pwm_val;
 }
 

lib/MotorControl/motorControl.h

@@ -21,7 +21,7 @@
 #define PWMRES 8
 #define POWERSTEPS 2       //  A total of 20 levels ( 100 / SPEED_STEPS ) * RUN_MOTOR_CONTROL_DELAY = 500ms
 #define PWMMIN 55
-#define PWMMAX 80         // Max 98% of 2^PWM_RES
+#define PWMMAX 94         // Max 98% of 2^PWM_RES
 
 /**
  * @brief A class which use PWM to control the power of DC Motor
@@ -35,17 +35,17 @@ class MotorControl {
         /**
          * @brief Initialize the motorController
          * 
-         * @param pwm_pin The output pin for the signal on the esp.
+         * @param pwmPin The output pin for the signal on the esp.
-         * @param pwm_channel One of the pwm channels from the esp.
+         * @param pwmChannel One of the pwm channels from the esp.
          * @param dir_1 First direction pin for the H-Bridge.
          * @param dir_2 Second direction pin for the H-Bridge.
          */
-        void init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t dir_1, uint8_t dir_2);
+        void init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8_t dir_2);
 
         /**
          * @brief Calls runMotorControl() to update the pwm signal
          * 
-         * This function should be called every mainloop. If the delay is not reached, than the
+         * This function should be called every mainloop. If the delayLoop is not reached, than the
          * functions returns immediately.
          * @see runMotorControl()
          * @see DELAY
@@ -81,10 +81,10 @@ class MotorControl {
          * 
          * Set the increment of the steps with which the dutycycle is
          * increased or decreased. Note the dependency between the increment
-         * and delay().
+         * and delayLoop().
          * 
          * The formula for the time between 0% and 100% power is:
-         *      time[ms] = delay * ( 100 / increment )
+         *      time[ms] = delayLoop * ( 100 / increment )
          *      500 ms are recommended
          * 
          * @see setDelay()
@@ -105,17 +105,17 @@ class MotorControl {
         void setTargetPower(int8_t power);
 
         /**
-         * @brief Set the min delay between each loop
+         * @brief Set the min delayLoop between each loop
          * 
-         * Note the dependency between delay and
+         * Note the dependency between delayLoop and
          * setPowerSteps().
          * 
          * @see setPowerSteps()
          * 
-         * @param delay time in Milliseconds
+         * @param delayLoop time in Milliseconds
          * @return time from 0% power to 100% power in Milliseconds
          */
-        uint16_t setDelay(uint8_t delay);
+        uint16_t setDelay(uint8_t delayLoop);
 
         /**
          * @brief Stops the motor like setTargetPower() to 0
@@ -152,19 +152,19 @@ class MotorControl {
         void setRealPower(int8_t power);
         void increasePower(int8_t power);
 
-        int8_t target_power = 0;
+        int8_t targetPower = 0;
         int8_t power = 0;
         uint8_t direction = 0; // 0 = stop, 1 = forward, 2 = backward
 
-        uint8_t pwm_pin;
+        uint8_t pwmPin;
-        uint8_t pwm_channel;
+        uint8_t pwmChannel;
-        uint8_t pwm_res = PWMRES;
+        uint8_t pwmRes = PWMRES;
         uint16_t dutycycle = 0;
-        uint8_t dutycycle_min;
+        uint8_t dutycycleMin;
-        uint8_t dutycycle_max;
+        uint8_t dutycycleMax;
         uint8_t dir_1;
         uint8_t dir_2;
-        uint8_t delay = DELAY;
+        uint8_t delayLoop = DELAY;
         uint8_t powersteps = POWERSTEPS;
 
         uint32_t lastMillis = 0;

lib/Navigation/navigation.cpp

@@ -98,14 +98,11 @@ void Navigation::loop() {
         this->ntripClient->loop();
 
     if (millis() - this->lastMillis > AZIMUTH_UPDATE_DELAY) {
-    // if (millis() - this->lastMillis > 500) {
         this->compass->read();
-        this->azimuth = this->compass->getAzimuth();
+        this->realAzimuth = this->compass->getAzimuth();
-        // std::cout   << "Compass x: " << compass->getX() 
+
-        //             << " y: " << compass->getY() 
+        this->updateMagneticDeclination();
-        //             << " z: " << compass->getZ() 
+
-        //             << " Azi: " << compass->getAzimuth() 
-        //             << std::endl; 
         this->lastMillis = millis();
     }
 }
@@ -124,6 +121,15 @@ 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;
@@ -134,6 +140,7 @@ Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction,
     if (this->currentPosition.distanceTo(this->lastPointCalcCorrection) < (this->minDistanceToReachPoint / 2.0)
         && !forceUpdate) {
         correction.correction = this->calculateCourseCorrection(this->lastPointCalcCorrection);
+        correction.distance = this->lastPointCalcCorrection.distanceTo(this->targetPoint);
         return Status::Unchanged;
     }
 
@@ -189,21 +196,75 @@ 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);
-    // correction = targetCourse - currentCourse
+    int16_t correctionCourse;
-    int16_t signedAzimuth = this->azimuth > 180 ? this->azimuth - 360 : this->azimuth;
-    int16_t correctionCourse = targetCourse - signedAzimuth;
-    if (correctionCourse > 180)
-        correctionCourse -= 360;
-    else if (correctionCourse < -180)
-        correctionCourse += 360;
 
-    //Rotate result by 180° to corrigate Azimuth
+    if (this->calcAzimuthState == CalcAzimuthState::Good
-    correctionCourse += 180;
+        || this->calcAzimuthState == CalcAzimuthState::Super)
-    correctionCourse = correctionCourse > 180 ? correctionCourse -360 : correctionCourse;
+    {
+        correctionCourse = targetCourse - this->calcAzimuth;
+        this->lastUsedCalcAzimuth = true;
+    } else {
+        correctionCourse = targetCourse - this->realAzimuth;
+        this->lastUsedCalcAzimuth = false;
+    }
         
-    return correctionCourse;
+    return Navigation::fixDegree(correctionCourse);
 }
 
 bool Navigation::nextPoint() { 
@@ -224,6 +285,16 @@ void Navigation::setOutputStatusPrintPVTdata(bool status) {
     Navigation::outputStatusPrintPVTdata = status;
 }
 
+int16_t Navigation::fixDegree(int16_t degree) {
+    while (degree < -180 || degree > 180) {
+        if (degree > 180)
+            degree -= 360;
+        else if (degree < -180)
+            degree += 360;
+    }
+    return degree;
+}
+
 void Navigation::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
     if (!Navigation::outputStatusPrintPVTdata)
         return;

lib/Navigation/navigation.h

@@ -62,6 +62,14 @@ class Navigation {
             Complete
         };
 
+        enum CalcAzimuthState {
+            Invalid,
+            Bad,
+            Ok,
+            Good,
+            Super
+        };
+
 
         /**
          * @brief Construct a new Navigation object and using I2C
@@ -118,6 +126,8 @@ class Navigation {
          */
         bool startNavigation();
         void freezeTargetPoint(bool val = true) { this->preventNextPoint = val; };
+        void drivingDirectionChange();
+        void dissableCalcAzimuth() { this->directionChangeMode = false; }
         
         double increaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint += 0.1; }
         double decreaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint -= 0.1; }
@@ -178,15 +188,21 @@ class Navigation {
         Point getCurrentPosition() const { return this->currentPosition; }
 
         /**
-         * @brief Get azimuth
+         * @brief Get realAzimuth
          * 
          * This value represents the angle between north and 
          * the line of sight. Clockwise.
          * 
          * @return uint16_t degree
          */
-        uint16_t getAzimuth() const { return this->azimuth; }
+        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; }
 
         /**
          * @brief Set the output status for PVTdata.
@@ -197,29 +213,16 @@ class Navigation {
          * @param status 
          */
         static void setOutputStatusPrintPVTdata(bool status);
+        // map input in range from -180 to 180 degree
+        static int16_t fixDegree(int16_t degree);
 
     private:
         void updateCurrentLocation();
-        int16_t calculateCourseCorrection(Point& point);
+        void updateMagneticDeclination();
-
-        /**
-         * @brief Set the next point as target
-         * 
-         * @return true
-         * @return false 
-         */
-        bool nextPoint();
-
-        /**
-         * @brief Set the target point
-         * 
-         * @param target 
-         * @return true 
-         * @return false 
-         */
-        bool setTargetPoint(Point target);
-
         void init(Route* route);
+        bool nextPoint();
+        bool setTargetPoint(Point target);
+        int16_t calculateCourseCorrection(Point& point);
 
         SFE_UBLOX_GNSS* gps;
         UBX_NAV_PVT_data_t* ubxData = nullptr;
@@ -229,24 +232,31 @@ class Navigation {
 
         Point lastPointRouteInsert;
         Point lastPointCalcCorrection;
+        Point lastPointDrivingDirectionChange;
         Point targetPoint;
         Point currentPosition;
         Point::Accuracy minAccuracy = Point::Accuracy::twoDigOfCM;
+        CalcAzimuthState calcAzimuthState = CalcAzimuthState::Invalid;
+
 
 
         bool navigationStarted = false;
         bool navigationFinished = false;
         bool isNtripInit = false;
         bool preventNextPoint = false;
+        bool directionChangeMode = false;
+        bool lastUsedCalcAzimuth = false;
 
         char* host;
         char* mountPoint;
         char* user;
         char* password;
 
+        int16_t realAzimuth = INT16_MAX;
+        int16_t calcAzimuth = INT16_MAX;
+
         uint8_t timeToWait = 200;
         uint16_t port;
-        uint16_t azimuth = UINT16_MAX;
         uint32_t lastMillis = 0;
         uint32_t ubxUpdateTime = 0;
 

lib/NtripClient/ntripClient.cpp

@@ -221,7 +221,7 @@ void NTRIPClient::closeConnection() {
 
 bool NTRIPClient::processConnection() {
     if (this->ntripClient->connected()) {
-        uint8_t rtcmData[this->bufferSize * 4];
+        uint8_t rtcmData[this->bufferSize * 8];
         uint16_t rtcmCount = 0;
 
         while (this->ntripClient->available()) {

lib/NtripClient/ntripClient.h

@@ -129,7 +129,7 @@ class NTRIPClient {
         const uint8_t delayTime = 20;
         const uint8_t maxReconnectAttemps = 10;
         const uint16_t reconnectDelayTime = 1000;
-        const uint16_t timeOut = 5000;
+        const uint16_t timeOut = 10000;
         const uint16_t bufferSize = 512;
         const uint16_t pushGPGGATime = 10000;
 };

lib/Route/route.cpp

@@ -11,35 +11,35 @@
 
 #include "route.h"
 
-Point::Point(double lat, double lon, uint32_t horizontalAccuracy) {
+Point::Point(double lat, double lon, uint32_t horizontalAccuracy, uint32_t creationTime) {
     this->coordinates.lat = lat;
     this->coordinates.lon = lon; 
-    this->init(horizontalAccuracy);
+    this->init(horizontalAccuracy, creationTime);
 }
 
-Point::Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy) {
+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);
+    this->init(horizontalAccuracy, creationTime);
 }
 
-Point::Point(Coordinates coords, uint32_t horizontalAccuracy) {
+Point::Point(Coordinates coords, uint32_t horizontalAccuracy, uint32_t creationTime) {
     this->coordinates = coords;
-    this->init(horizontalAccuracy);
+    this->init(horizontalAccuracy, creationTime);
 }
 
 Point::Point(Coordinates coords, bool imported) {
     this->coordinates = coords;
     if (imported)
-        this->init(UINT32_MAX);
+        this->init(UINT32_MAX, 0);
     else
-        this->init(0);
+        this->init(0, 0);
 }
 
 Point::Point() {
     this->coordinates.lat = 0;
     this->coordinates.lon = 0; 
-    this->init(0);
+    this->init(0, 0);
 }
 
 bool Point::operator==(const Point& rhs) const {
@@ -74,7 +74,7 @@ int16_t Point::courseTo(const Coordinates& point) const {
     double phi = log( tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) );
     double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT);
 
-    int16_t res = (int16_t) atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT;
+    int16_t res = static_cast<int16_t>(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1;
 
     // if (res < 0)
     //     res += 360; 
@@ -86,8 +86,8 @@ int16_t Point::courseTo(const Point &point) const {
     return this->courseTo(point.getCoordinates());
 }
 
-void Point::init(uint32_t horizontalAccuracy) {
+void Point::init(uint32_t horizontalAccuracy, uint32_t creationTime) {
-    this->creationTime = millis();
+    this->creationTime = creationTime;
 
     if (horizontalAccuracy == UINT32_MAX)
         this->accuracy = Accuracy::imported;

lib/Route/route.h

@@ -12,8 +12,6 @@
 #ifndef ROUTE_H
 #define ROUTE_H
 
-#include <Arduino.h>
-
 #include <cstdint>
 #include <list> 
 #include <cmath>   
@@ -66,9 +64,9 @@ class Point{
          * @param coords Coordinates
          * @param imported if true than highest accuracy
          */
-        Point(double lat, double lon, uint32_t horizontalAccuracy = 0);
+        Point(double lat, double lon, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0);
-        Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy = 0);
+        Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0);
-        Point(Coordinates coords, uint32_t horizontalAccuracy = 0);
+        Point(Coordinates coords, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0);
         Point(Coordinates coords, bool imported);
         Point();
 
@@ -133,7 +131,7 @@ class Point{
         Accuracy getAccuracy() { return this->accuracy; }
 
     private:
-        void init(uint32_t horizontalAccuracy);
+        void init(uint32_t horizontalAccuracy, uint32_t creationTime);
 
         Accuracy accuracy = Accuracy::none;
         Coordinates coordinates;

lib/calibrateCompass/calibrateCompass.cpp

@@ -73,7 +73,7 @@ void CalibrateCompass::start() {
 
     this->clearData();
     this->state = State::Calibrating;
-    this->compass->removeCalibration();
+    this->compass->clearCalibration();
     this->lastChange = millis();
 }
 
@@ -95,7 +95,7 @@ void CalibrateCompass::useData() {
 }
 
 void CalibrateCompass::removeCalibration() {
-    this->compass->removeCalibration();
+    this->compass->clearCalibration();
 }
 
 void CalibrateCompass::reset() {

lib/calibrateCompass/calibrateCompass.h

@@ -52,6 +52,6 @@ class CalibrateCompass {
         void clearData();
 
         bool dataValid = false;
-        const uint16_t maxTimeWithoutChange = 5000;
+        const uint16_t maxTimeWithoutChange = 10000;
         uint32_t lastChange = 0;
 };

platformio.ini

@@ -8,7 +8,7 @@
 ; Please visit documentation for the other options and examples
 ; https://docs.platformio.org/page/projectconf.html
 
-[env]
+[env:embedded]
 platform = espressif32
 board = esp32doit-devkit-v1
 board_build.partitions = no_ota.csv
@@ -22,18 +22,21 @@ lib_deps =
 	br3ttb/PID@^1.2.1
 	marcoschwartz/LiquidCrystal_I2C@^1.1.4
 	bblanchon/ArduinoJson@^6.20.0
-	mprograms/QMC5883LCompass@^1.1.1
+	mprograms/QMC5883LCompass@^1.2.0
 	https://git.kleiax.de/PlatformIO-Libs/Menu.git
 	nrf24/RF24@^1.4.5
 upload_port = COM3
-
+extra_scripts = 
-[env:release]
+   pre:autoVersionIncrement/version_increment_pre.py
-build_type = release
+   post:autoVersionIncrement/version_increment_post.py
-
+test_ignore = test_desktop
-[env:debug]
 build_type = debug
 monitor_filters = esp32_exception_decoder
 check_tool = clangtidy
 
+[env:native]
+platform = native
+test_ignore = test_embedded
+
 [platformio]
 description = A Rover who should be drive a route by gps.

src/SpecialMenus/Route/menuRoute.cpp

@@ -83,18 +83,13 @@ void MenuRoute::init() {
 
     this->mainMenu = new Menu;
     MenuRoutePoints* pointsMenu = new MenuRoutePoints(this->route);
-    Menu* importMenu = new Menu;
-    Menu* exportMenu = new Menu;
-    Menu* clearMenu = new Menu;
 
     this->mainMenu->setLcd(this->lcd);
     pointsMenu->setLcd(this->lcd);
-    importMenu->setLcd(this->lcd);
-    exportMenu->setLcd(this->lcd);
-    clearMenu->setLcd(this->lcd);
 
     MenuIntInput* importWrapper = new MenuIntInput(1, new MenuRouteWrapper(this, &MenuRoute::importRoute));
     MenuIntInput* exportWrapper = new MenuIntInput(1, new MenuRouteWrapper(this, &MenuRoute::exportRoute));
+    MenuIntInput* deleteWrapper = new MenuIntInput(1, new MenuRouteWrapper(this, &MenuRoute::deleteRoute));
     MenuActionRoute* clearWrapper = new MenuActionRoute(this, &MenuRoute::clearRoute);
 
     importWrapper->setLcd(this->lcd);
@@ -107,15 +102,16 @@ void MenuRoute::init() {
     exportWrapper->setPrintParentMenu(false);
     exportWrapper->setEntry(0, "Export Route");
 
-    MenuAction* pointsAction = new MenuAction("Points", pointsMenu);
+    deleteWrapper->setLcd(this->lcd);
-    MenuAction* importAction = new MenuAction("Import", importWrapper);
+    deleteWrapper->setMinMax(0, 100);
-    MenuAction* exportAction = new MenuAction("Export", exportWrapper);
+    deleteWrapper->setPrintParentMenu(false);
-    MenuAction* clearAction = new MenuAction("Clear", clearWrapper);
+    deleteWrapper->setEntry(0, "Delete Route");
 
-    this->mainMenu->addEntry(pointsAction);
+    this->mainMenu->addEntry(new MenuAction("Points", pointsMenu));
-    this->mainMenu->addEntry(importAction);
+    this->mainMenu->addEntry(new MenuAction("Clear", clearWrapper));
-    this->mainMenu->addEntry(exportAction);
+    this->mainMenu->addEntry(new MenuAction("Import", importWrapper));
-    this->mainMenu->addEntry(clearAction);
+    this->mainMenu->addEntry(new MenuAction("Export", exportWrapper));
+    this->mainMenu->addEntry(new MenuAction("Delete", deleteWrapper));
 }
 
 void MenuRoute::importRoute(uint8_t routeNumber) {
@@ -236,6 +232,52 @@ void MenuRoute::exportRoute(uint8_t routeNumber) {
     http.end();
 }
 
+void MenuRoute::deleteRoute(uint8_t routeNumber) {
+    this->blockInput = true;
+
+    String lineOne = "";
+    String lineTwo = "";
+
+    if (WiFi.status() != WL_CONNECTED) {
+        lineOne = "Not connected to";
+        lineTwo = "the WiFi.";
+        this->print(lineOne, lineTwo);
+        this->blockInput = false;
+        return;
+    }
+
+    if (ESP.getMaxAllocHeap() < JSON_DOCUMENT_SIZE_ROUTE) {
+        lineOne = "Not enough mem";
+        lineTwo = "for Json obj";
+        this->print(lineOne, lineTwo);
+        this->blockInput = false;
+        return;
+    }
+
+    WiFiClient client;
+    HTTPClient http;
+    DynamicJsonDocument doc(JSON_DOCUMENT_SIZE_ROUTE);
+    
+    String host = "http://rover.kleiax.de/api/";
+    host.concat(routeNumber);
+    http.begin(client, host);
+    int httpResponseCode = http.sendRequest("DELETE");
+
+    lineOne = "Delete complete";
+    lineTwo = "Code: ";
+
+    if (httpResponseCode == 202)
+        deserializeJson(doc, http.getStream());
+    else
+        lineOne = "HTTP Error";
+
+    lineTwo.concat(httpResponseCode);
+    this->print(lineOne, lineTwo);
+
+    this->blockInput = false;
+    http.end();
+}
+
 void MenuRoute::clearRoute(uint8_t none) {
     this->route->clear();
     this->print("Currente route", "deleted...");

src/SpecialMenus/Route/menuRoute.h

@@ -109,6 +109,8 @@ class MenuRoute : public MenuControl {
          */
         void exportRoute(uint8_t routeNumber);
 
+        void deleteRoute(uint8_t routeNumber);
+
         /**
          * @brief Delete the current Route.
          * 

src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp

@@ -12,7 +12,7 @@
 #include "menuSysteminformation.h"
 
 MenuSysteminformation::MenuSysteminformation(Battery* mainBattery) 
-            : MenuInformationSites(5) {
+            : MenuInformationSites(8) {
     this->mainBattery = mainBattery;
 }
 
@@ -44,6 +44,22 @@ void MenuSysteminformation::printPage() const {
             break;
 
         case 4:
+            lineOne = "Current WiFi";
+            lineTwo = "channel: ";
+            lineTwo.concat(Network::getCurrentChannel());
+            break;
+
+        case 5:
+            lineOne = "Software verion:";
+            lineTwo = VERSION;
+            break;
+
+        case 6:
+            lineOne = "Build timestamp:";
+            lineTwo = String(BUILD_TIMESTAMP).substring(0, 16);
+            break;
+
+        case 7:
             lineOne = "Kleiax Rover by";
             lineTwo = "Alexander Klein";
             break;

src/SpecialMenus/Systeminformation/menuSysteminformation.h

@@ -14,9 +14,12 @@
 
 #include <Arduino.h>
 
+#include "Version.h"
+
 #include "controlPadInput.h"
 #include "menuInformationSites.h"
 #include "battery.h"
+#include "network.h"
 
 /**
  * @brief Prints information about the current system status.

src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp

@@ -43,7 +43,7 @@ void MenuAutopilot::printPage() const {
             lineTwo = "";
             switch (this->autopilot->getState()) {
                 case Autopilot::State::InsufficientAccuarcy :
-                    lineTwo = "Err: No NTRIP";
+                    lineTwo = "Err: LowAccuracy";
                     break;
 
                 case Autopilot::State::NoRoute :
@@ -66,6 +66,10 @@ void MenuAutopilot::printPage() const {
                     lineTwo = "Autopilot active";
                     break;
 
+                case Autopilot::State::SelfDrivingRotate :
+                    lineTwo = "Rotating";
+                    break;
+
                 case Autopilot::State::TargetReached :
                     lineTwo = "Target reached";
                     break;
@@ -158,6 +162,65 @@ void MenuAutopilot::printPage() const {
             lineTwo.concat(" - Decrease");
             break;
 
+        case 10:
+            lineOne = "Current minimal";
+            if (this->driveManager->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM)
+                lineTwo = "accuracy is high";
+            else 
+                lineTwo = "accuracy is low";
+            break;
+
+        case 11:
+            lineOne = "CalcAzi: ";
+            lineTwo = "State: ";
+            lineOne.concat(this->driveManager->getNavigation()->getCalcAzimuth());
+            switch (this->driveManager->getNavigation()->getCalcAzimuthState()) {
+                case Navigation::CalcAzimuthState::Bad :
+                    lineTwo.concat("Bad");
+                    break;
+                
+                case Navigation::CalcAzimuthState::Good :
+                    lineTwo.concat("Good");
+                    break;
+
+                case Navigation::CalcAzimuthState::Invalid :
+                    lineTwo.concat("Invalid");
+                    break;
+
+                case Navigation::CalcAzimuthState::Ok :
+                    lineTwo.concat("Ok");
+                    break;
+
+                case Navigation::CalcAzimuthState::Super :
+                    lineTwo.concat("Super");
+                    break;
+
+                default:
+                    lineTwo.concat("Unkown");
+                    break;
+            }
+            break;
+
+        case 12:
+            lineOne = "Test rotate";
+            lineTwo = "180 degree";
+            break;
+
+        case 13:
+            lineOne = "Test rotate";
+            lineTwo = "270 degree";
+            break;
+
+        case 14:
+            lineOne = "Test rotate";
+            lineTwo = "45 degree";
+            break;
+
+        case 15:
+            lineOne = "Test rotate";
+            lineTwo = "20 degree";
+            break;
+
         default:
             this->printDefault();
             return;
@@ -190,6 +253,29 @@ void MenuAutopilot::runCommand() const {
             this->minDistance = this->driveManager->getNavigation()->decreaseMinDistanceToReachPoint();
             break;
 
+        case 10:
+            if (this->driveManager->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM)
+                this->driveManager->getNavigation()->setMinAccuracy(Point::Accuracy::none);
+            else 
+                this->driveManager->getNavigation()->setMinAccuracy(Point::Accuracy::twoDigOfCM);
+            break;
+
+        case 12:
+            this->autopilot->testRotate(180);
+            break;
+
+        case 13:
+            this->autopilot->testRotate(270);
+            break;
+
+        case 14:
+            this->autopilot->testRotate(45);
+            break;
+
+        case 15:
+            this->autopilot->testRotate(20);
+            break;
+        
         default:
             break;
     }
@@ -204,7 +290,7 @@ void MenuAutopilot::update() {
 }
 
 void MenuAutopilot::init() {
-    this->setCountPages(10);
+    this->setCountPages(16);
     this->driveManager->changeModus(Modi::Autopilot);
     this->autopilot = (Autopilot*) this->driveManager->getDriveModiPtr();
     this->routeInfo = this->autopilot->getRouteInfo();

src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp

@@ -77,20 +77,25 @@ void MenuCalibrateCompass::printPage() const {
             break;
 
         case 7:
+            lineOne = "Reset for new";
+            lineTwo = "calibration run";
+            break;
+
+        case 8:
             lineOne = "X min: ";
             lineOne.concat(this->caliCompass->getCallibrationData().data[0][0]);
             lineTwo = "X max: ";
             lineTwo.concat(this->caliCompass->getCallibrationData().data[0][1]);
             break;
 
-        case 8:
+        case 9:
             lineOne = "Y min: ";
             lineOne.concat(this->caliCompass->getCallibrationData().data[1][0]);
             lineTwo = "Y max: ";
             lineTwo.concat(this->caliCompass->getCallibrationData().data[1][1]);
             break;
 
-        case 9:
+        case 10:
             lineOne = "Z min: ";
             lineOne.concat(this->caliCompass->getCallibrationData().data[2][0]);
             lineTwo = "Z max: ";
@@ -109,7 +114,7 @@ void MenuCalibrateCompass::init() {
     this->firstPrint = false;
     this->driveManager->changeModus(Modi::ManualControl);
     this->manualControl = (ManualControl*) this->driveManager->getDriveModiPtr();
-    this->setCountPages(10);
+    this->setCountPages(11);
     this->updateDelay = 500; 
 }
 
@@ -125,7 +130,6 @@ void MenuCalibrateCompass::runCommand() const {
                 case CalibrateCompass::State::Finished:
                     this->manualControl->setCalibrateCompass();
                     this->caliCompass->useData();
-                    this->caliCompass->reset();
                     break;
                 
                 default:
@@ -149,6 +153,10 @@ void MenuCalibrateCompass::runCommand() const {
             this->caliCompass->useData();
             break;     
 
+        case 7:
+            this->caliCompass->reset();
+            break;   
+        
         default:
             break;
     }

src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp

@@ -92,6 +92,14 @@ void MenuCaptureRoute::printPage() const {
                 lineOne.concat("0");
             break;
 
+        case 7:
+            lineOne = "Current minimal";
+            if (this->driveManager->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM)
+                lineTwo = "accuracy is high";
+            else 
+                lineTwo = "accuracy is low";
+            break;
+        
         default:
             this->printDefault();
             return;
@@ -106,8 +114,23 @@ void MenuCaptureRoute::update() {
     this->printMenu();
 }
 
+void MenuCaptureRoute::runCommand() const {
+    switch (this->getCurrentPage())
+    {
+    case 7:
+        if (this->driveManager->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM)
+            this->driveManager->getNavigation()->setMinAccuracy(Point::Accuracy::none);
+        else 
+            this->driveManager->getNavigation()->setMinAccuracy(Point::Accuracy::twoDigOfCM);
+        break;
+    
+    default:
+        break;
+    }
+}
+
 void MenuCaptureRoute::init() {
-    this->setCountPages(7);
+    this->setCountPages(8);
     this->updateDelay = 500;
     this->driveManager->changeModus(Modi::CaptureRoute);
     this->captureRoute = (CaptureRoute*) this->driveManager->getDriveModiPtr();

src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h

@@ -42,6 +42,8 @@ class MenuCaptureRoute : public MenuDriveMode {
          */
         void update() override;
 
+        void runCommand() const override;
+
     private:
         void init() override;
 

src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp

@@ -27,44 +27,66 @@ void MenuManualControl::printPage() const {
             break;
 
         case 1:
-            lineOne = "Speed: ";
+            lineOne = "Input mode:";
-            lineOne.concat(this->manualControl->getMaxSpeed());
+            if (this->manualControl->getInputMode() == ManualControl::InputMode::Analog)
-            lineTwo = "Increase by 0.1";
+                lineTwo = "Analog";
+            else
+                lineTwo = "Digital";
             break;
 
         case 2:
             lineOne = "Speed: ";
             lineOne.concat(this->manualControl->getMaxSpeed());
-            lineTwo = "Decrease by 0.1";
+            lineTwo = "Increase by 0.1";
             break;
 
         case 3:
-            lineOne = "RotSpeed: ";
+            lineOne = "Speed: ";
-            lineOne.concat(this->manualControl->getMaxRotation());
+            lineOne.concat(this->manualControl->getMaxSpeed());
-            lineTwo = "Increase by 0.1";
+            lineTwo = "Decrease by 0.1";
             break;
 
         case 4:
             lineOne = "RotSpeed: ";
             lineOne.concat(this->manualControl->getMaxRotation());
-            lineTwo = "Decrease by 0.1";
+            lineTwo = "Increase by 0.1";
             break;
 
         case 5:
+            lineOne = "RotSpeed: ";
+            lineOne.concat(this->manualControl->getMaxRotation());
+            lineTwo = "Decrease by 0.1";
+            break;
+
+        case 6:
             lineOne = "Dutycycle Left:";
             lineTwo.concat(this->manualControl->getDutycycleLeft());
             break;
 
-        case 6:
+        case 7:
             lineOne = "Dutycycle Right:";
             lineTwo.concat(this->manualControl->getDutycycleRight());
             break;
 
-        case 7:
+        case 8:
             lineOne = "Azimuth:";
             lineTwo.concat(this->driveManager->getNavigation()->getAzimuth());
             break;
 
+        case 9: {
+            UBX_NAV_PVT_data_t* gpsData = this->driveManager->getNavigation()->getUbxData();
+            lineOne = "magDec: ";
+            lineTwo = "magAcc: ";
+            if (gpsData->valid.bits.validMag) {
+                lineOne.concat(gpsData->magDec);
+                lineTwo.concat(gpsData->magAcc);
+            } else {
+                lineOne.concat("invalid");
+                lineTwo.concat("invalid");   
+            }
+            break;
+        }
+
         default:
             this->printDefault();
         return;
@@ -77,29 +99,32 @@ void MenuManualControl::init() {
     this->firstPrint = false;
     this->driveManager->changeModus(Modi::ManualControl);
     this->manualControl = (ManualControl*) this->driveManager->getDriveModiPtr();
-    this->setCountPages(8);
+    this->setCountPages(10);
     this->updateDelay = 500; 
 }
 
 void MenuManualControl::runCommand() const {
     switch (this->getCurrentPage()) {
-
         case 1:
-            this->manualControl->increaseMaxSpeed();
+            this->manualControl->switchInputMode();
             break;
 
         case 2:
-            this->manualControl->decreaseMaxSpeed();
+            this->manualControl->increaseMaxSpeed();
             break;
 
         case 3:
-            this->manualControl->increaseMaxRotation();
+            this->manualControl->decreaseMaxSpeed();
             break;
 
         case 4:
             this->manualControl->increaseMaxRotation();
             break;
 
+        case 5:
+            this->manualControl->increaseMaxRotation();
+            break;
+        
         default:
             break;
     }

src/driveModi/Modi/Autopilot/autopilot.cpp

@@ -12,13 +12,31 @@
 #include "driveModi/Modi/Autopilot/autopilot.h"
 #include "autopilot.h"
 
+DirectionChangeSignal::DirectionChangeSignal(Navigation* navigation) {
+    this->navigation = navigation;
+    this->action();
+}
+
+DirectionChangeSignal::~DirectionChangeSignal() {
+    navigation->dissableCalcAzimuth();
+}
+
+void DirectionChangeSignal::action() {
+    this->navigation->drivingDirectionChange();
+}
+
+
 Autopilot::Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation)
     : ManualControl(moveControl, input) {
+        this->setInputMode(ManualControl::InputMode::Digital);
+        this->directionChangeSignal = new DirectionChangeSignal(navigation);
+        this->setDirectionChangeCallback(this->directionChangeSignal);
         this->navigation = navigation;
         this->init();
 }
 
 Autopilot::~Autopilot() {
+    delete this->directionChangeSignal;
     // this->navigation->getNTRIPClient()->setActivated(false);
 }
 
@@ -53,6 +71,7 @@ void Autopilot::runAutopilot() {
 
         case State::NavigationStarted:
             this->askNavigationForOrder();
+            this->checkButtonInput();
             break;
 
         case State::GetToStartPoint:
@@ -72,6 +91,11 @@ void Autopilot::runAutopilot() {
             this->selfDriving();
             break;
 
+        case SelfDrivingRotate:
+            this->checkButtonInput();
+            this->rotate();
+            break;
+
         case State::TargetReached:
             break;
         
@@ -92,6 +116,14 @@ bool Autopilot::shouldUpdate() {
     return false;
 }
 
+void Autopilot::testRotate(int16_t degree) {
+    if (!degree)
+        return;
+
+    this->courseCorrection.correction = degree;
+    this->beginRotate();
+}
+
 void Autopilot::init() {
     if (this->navigation->startNavigation())
             this->state = State::NavigationStarted;
@@ -115,29 +147,63 @@ void Autopilot::drive() {
         this,moveControl->setSpeed(0);
 }
 
-void Autopilot::rotate() {
+void Autopilot::beginRotate() {
+    if (this->state != State::SelfDrivingRotate) {
+        this->lastState = this->state;
+        this->state = State::SelfDrivingRotate;
+        this->rotationAimAzimuth = this->navigation->getAzimuth() + this->courseCorrection.correction;
+        this->rotationAimAzimuth = Navigation::fixDegree(this->rotationAimAzimuth);
+
         this->moveControl->setSpeed(0);
         if (this->courseCorrection.correction > 0)
-        this->moveControl->setRotationSpeed(this->rotationSpeed);
-    else
-
             this->moveControl->setRotationSpeed(-this->rotationSpeed);
+        else
+            this->moveControl->setRotationSpeed(this->rotationSpeed);
+    }
+}
+
+void Autopilot::rotate() {
+    if (abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < 5) {
+        this->endRotate();
+        return;
+    }
+
+    if ((abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < 15)
+        &&
+        ((this->courseCorrection.correction > 0 
+        && this->rotationAimAzimuth < this->navigation->getAzimuth())
+        || (this->courseCorrection.correction < 0
+        && this->rotationAimAzimuth > this->navigation->getAzimuth())))
+    {
+        this->endRotate();
+        std::cout << "Autopilot::rotate: Rover rotated too far" << std::endl;
+    }    
+}
+
+void Autopilot::endRotate() {
+    if (this->state != State::SelfDrivingRotate)
+        return;
+
+    this->state = this->lastState;
+    this->navigation->drivingDirectionChange();
+    this->moveControl->setRotationSpeed(0);
+    this->moveControl->emergencyStop();
+    this->moveControl->setDrivingStatus(MoveControl::Status::Drive);
 }
 
 void Autopilot::checkButtonInput() {
     if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)
         && millis() - this->lastAutopilotChangeMillis > this->autopilotChangeDelayMillis) {
         
-        if (this->state  == State::SelfDrivingAvailable) {
+        if (this->state  == State::SelfDrivingAvailable)
             this->state = State::SelfDriving;
-            this->updateDisplay = true;
+        else if (this->state == State::SelfDriving || this->state == State::SelfDrivingRotate)
-            this->lastAutopilotChangeMillis = millis();
-        } else if (this->state == State::SelfDriving) {
             this->state = State::SelfDrivingAvailable;
+        else if (this->state == State::NavigationStarted)
+            this->state = State::GetToStartPoint;
         this->updateDisplay = true;
         this->lastAutopilotChangeMillis = millis();
     }
-    }
 }
 
 void Autopilot::askNavigationForOrder() {
@@ -172,10 +238,11 @@ void Autopilot::askNavigationForOrder() {
 }
 
 void Autopilot::selfDriving() {
-    if (this->lastOrderStatus == Navigation::Status::Updated) {
+    if (this->state != State::SelfDriving)
+        return;
+
     if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBerforeAct)
-            this->rotate();
+        this->beginRotate();
     else
         this->drive();
-    }
 }

src/driveModi/Modi/Autopilot/autopilot.h

@@ -17,6 +17,16 @@
 #include "moveControl.h"
 #include "driveModi/Modi/ManualControl/manualControl.h"
 
+class DirectionChangeSignal : public DirectionChangeWrapper {
+    public:
+        DirectionChangeSignal(Navigation* navigation);
+        ~DirectionChangeSignal();
+        void action() override;
+
+    private:
+        Navigation* navigation;
+};
+
 /**
  * @brief This class use the navigate class to drive automaticaly
  * 
@@ -36,6 +46,7 @@ class Autopilot : public ManualControl {
             GetToStartPoint,
             SelfDrivingAvailable,
             SelfDriving,
+            SelfDrivingRotate,
             TargetReached
         };
 
@@ -66,15 +77,6 @@ class Autopilot : public ManualControl {
          */
         void loop();
 
-        /**
-         * @brief Manges the autopilot
-         * 
-         * If the first Point is near to current location you can turn
-         * the autopilot on.
-         * Gets the course correction and decide what to do.
-         */
-        void runAutopilot();
-
         void restart();
 
         /**
@@ -105,11 +107,15 @@ class Autopilot : public ManualControl {
          * @return false 
          */
         bool shouldUpdate();
+        void testRotate(int16_t degree);
+        void endRotate();
 
     private:
         void init();
         void drive();
+        void beginRotate();
         void rotate();
+        void runAutopilot();
         void checkButtonInput();
         void askNavigationForOrder();
         void selfDriving();
@@ -120,6 +126,7 @@ class Autopilot : public ManualControl {
         State state = State::None;
         State lastState = State::None;
         Navigation::Status lastOrderStatus;
+        DirectionChangeSignal* directionChangeSignal;
 
         bool updateDisplay = false;
 
@@ -131,8 +138,10 @@ class Autopilot : public ManualControl {
         uint32_t loopLastMillis = 0;
         uint32_t lastAutopilotChangeMillis = 0;
 
+        int16_t rotationAimAzimuth;
+
         double drivingSpeed = 1;
-        double rotationSpeed = 3;
+        double rotationSpeed = 4.5;
         double minRemainingDistance = 0.25;
 };
 

src/driveModi/Modi/ManualControl/manualControl.cpp

@@ -26,14 +26,30 @@ void ManualControl::loop() {
     if (this->caliCompass)
         this->caliCompass->loop();
 
-    if (millis() - this->lastMillis < delay) {
+    if (millis() - this->lastMillis < delay)
         return;
+    
+    switch (this->inputMode) {
+        case InputMode::Analog :
+            this->analogControl();
+            break;
+
+        case InputMode::Digital :
+            this->digitalControl();
+            break;
+        
+        default:
+            break;
     }
-    this->runManualControl();
+
     this->lastMillis = millis();
 }
 
-void ManualControl::runManualControl() {
+void ManualControl::switchInputMode() {
+    this->inputMode = (this->inputMode == InputMode::Analog) ? InputMode::Digital : InputMode::Analog;
+}
+
+void ManualControl::analogControl() {
     // Have to be int16_t to avoid overflow (int8_t = 255 - 127 = -128)
     int16_t y = this->input->x - 127;
     int16_t x = this->input->y - 127;
@@ -49,3 +65,30 @@ void ManualControl::runManualControl() {
     value_per_step = this->max_rotation * 2 / UINT8_MAX;
     this->moveControl->setRotationSpeed(x * value_per_step);
 }
+
+void ManualControl::digitalControl() {
+    int16_t y = this->input->x - 127;
+    int16_t x = this->input->y - 127;
+
+    if (y > 120)
+        this->moveControl->setSpeed(-this->max_speed);
+    else if (y < -120)
+        this->moveControl->setSpeed(this->max_speed);
+    else
+        this->moveControl->setSpeed(0);
+
+    if (x > 120)
+        this->moveControl->setRotationSpeed(this->max_rotation);
+    else if (x < -120)
+        this->moveControl->setRotationSpeed(-this->max_rotation);
+    else
+        this->moveControl->setRotationSpeed(0);
+
+    if (this->directionChangeWrapper && (x > 120 || x < -120))
+        this->lastLoopTurned = true;
+    else if (this->lastLoopTurned) {
+        if (this->directionChangeWrapper)
+            this->directionChangeWrapper->action();
+        this->lastLoopTurned = false;
+    }      
+}

src/driveModi/Modi/ManualControl/manualControl.h

@@ -16,6 +16,11 @@
 #include "controlPadInput.h"
 #include "calibrateCompass.h"
 
+class DirectionChangeWrapper {
+    public:
+        virtual void action() = 0;
+};
+
 /**
  * @brief Drive the Rover with a Joystick
  * 
@@ -26,6 +31,11 @@
  */
 class ManualControl : public DriveModi {
     public:
+        enum class InputMode : uint8_t {
+            Analog,
+            Digital
+        };
+
         ManualControl(MoveControl *moveControl, const ControlPadInput *input);
 
         /**
@@ -35,7 +45,7 @@ class ManualControl : public DriveModi {
         ~ManualControl();
 
         /**
-         * @brief Calls runManualControl() to update all values.
+         * @brief Calls analogControl() to update all values.
          * 
          * This function should be called every mainloop. If the delay is not reached, than the
          * functions returns immediately.
@@ -44,12 +54,6 @@ class ManualControl : public DriveModi {
          */
         void loop() override;
 
-        /**
-         * @brief Noramly called repeatedly by loop() to calcluate new values.
-         * Set new values for speed and rotation in moveControl
-         */
-        void runManualControl();
-
         /**
          * @brief Set the min delay between each loop
          * 
@@ -79,6 +83,11 @@ class ManualControl : public DriveModi {
 
         void setCalibrateCompass(CalibrateCompass* val = nullptr) { this->caliCompass = val; }
 
+        void switchInputMode();
+        void setInputMode(InputMode mode) { this->inputMode = mode; }
+        InputMode getInputMode() const { return this->inputMode; }
+        void setDirectionChangeCallback(DirectionChangeWrapper* callback) { this->directionChangeWrapper = callback; }
+
         uint16_t getDutycycleLeft() const { return this->moveControl->getDutycycleLeft(); }
         uint16_t getDutycycleRight() const { return this->moveControl->getDutycycleRight(); }
 
@@ -87,11 +96,21 @@ class ManualControl : public DriveModi {
         const ControlPadInput* input;
 
     private:
-        uint32_t lastMillis = 0;
+        /**
+         * @brief Noramly called repeatedly by loop() to calcluate new values.
+         * Set new values for speed and rotation in moveControl
+         */
+        void analogControl();
+        void digitalControl();
+
+        bool lastLoopTurned = false;
         uint8_t delay = 10;
+        uint32_t lastMillis = 0;
         double max_speed = 1;
         double max_rotation = 7;
         CalibrateCompass* caliCompass = nullptr;
+        DirectionChangeWrapper* directionChangeWrapper = nullptr;
+        InputMode inputMode = InputMode::Analog;
 };
 
 #endif // MANUAL_CONTROL_H

src/main.cpp

@@ -22,6 +22,8 @@
 #include <LiquidCrystal_I2C.h>
 #include <SPI.h>
 
+#include "Version.h"
+
 #include "driveModi/driveManager.h"
 #include "OutputBuf/outputBuf.h"
 #include "LcdWrapper.h"
@@ -109,6 +111,8 @@ void setup() {
     Network::connectEspNow(receiveCallback, sendCallback);
 
     std::cout << "Welcome to Kleiax-Rover" << std::endl;
+    std::cout << "Project verion: " << VERSION << std::endl;
+    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);
@@ -118,7 +122,7 @@ void setup() {
     lcd->backlight();
     lcd->printf("%c Kleiax-Rover %c", wifiIndicator, wifiIndicator);
     lcd->setCursor(0, 1);
-    lcd->print("Press PS-Button");
+    lcd->printf("WiFi channel %u", Network::getCurrentChannel());
     lcdWrapper = new LcdWrapper(lcd);
     lcdWrapper->setCallback(lcdWrapperCallback);
 
@@ -231,41 +235,25 @@ void makeMenu() {
     sys_m->setUpdateDelay(1500);
     rout_m->setLcd(lcdWrapper);
 
-
-
     // Entry for the main menu
-    MenuAction* mode_e = new MenuAction("Mode", mode_m);
+    main_m->addEntry(new MenuAction("Mode", mode_m));
-    MenuAction* gps_e = new MenuAction("GPS", gps_m);
+    main_m->addEntry(new MenuAction("GPS", gps_m));
-    MenuAction* pid_e = new MenuAction("PID", pid_m);
+    main_m->addEntry(new MenuAction("Route", rout_m));
-    MenuAction* restart_e = new MenuAction("Restart", restart);
+    main_m->addEntry(new MenuAction("PID", pid_m));
-    MenuAction* sys_e = new MenuAction("Systeminfo", sys_m);
+    main_m->addEntry(new MenuAction("Systeminfo", sys_m));
-    MenuAction* rout_e = new MenuAction("Route", rout_m);
+    main_m->addEntry(new MenuAction("Restart", restart));
-    main_m->addEntry(mode_e);
-    main_m->addEntry(gps_e);
-    main_m->addEntry(rout_e);
-    main_m->addEntry(pid_e);
-    main_m->addEntry(sys_e);
-    main_m->addEntry(restart_e);
 
     // Entry for the mode Menu
-    MenuAction* autopilot_e = new MenuAction("Autopilot", auto_m);
+    mode_m->addEntry(new MenuAction("Manual Control", man_m));
-    MenuAction* captureRoute_e = new MenuAction("Capture Route", cap_m);
+    mode_m->addEntry(new MenuAction("Capture Route", cap_m));
-    MenuAction* consolControl_e = new MenuAction("Consol Control", dummy);
+    mode_m->addEntry(new MenuAction("Autopilot", auto_m));
-    MenuAction* manualControl_e = new MenuAction("Manual Control", man_m);
+    mode_m->addEntry(new MenuAction("Gauge Compass", comp_m));
-    MenuAction* testMode_e = new MenuAction("Test Mode", testM_m);
+    mode_m->addEntry(new MenuAction("Test Mode", testM_m));
-    MenuAction* caliComp_e = new MenuAction("Gauge Compass", comp_m);
+    mode_m->addEntry(new MenuAction("Consol Control", dummy));
-    mode_m->addEntry(manualControl_e);
-    mode_m->addEntry(captureRoute_e);
-    mode_m->addEntry(autopilot_e);
-    mode_m->addEntry(caliComp_e);
-    mode_m->addEntry(testMode_e);
-    mode_m->addEntry(consolControl_e);
 
     // Entry for the PID Menu
-    MenuAction* pidl_e = new MenuAction("Left", pidl_m);
+    pid_m->addEntry(new MenuAction("Left", pidl_m));
-    MenuAction* pidr_e = new MenuAction("Right", pidr_m);
+    pid_m->addEntry(new MenuAction("Right", pidr_m));
-    pid_m->addEntry(pidl_e);
-    pid_m->addEntry(pidr_e);
 
     // Other config
     pidl_m->setMinMax(0, UINT8_MAX);

src/moveControl.cpp

@@ -187,9 +187,9 @@ void MoveControl::calcTargetWheelSpeed() {
                \ 1 -b / \ T /   \ Xr /  */
 
     // (1 / r) * 1
-    const static double A = 15.82278481;
+    constexpr double A = 15.82278481;
     // (1 / r) * b
-    const static double B = 2.096518987;
+    constexpr double B = 2.096518987;
 
     this->wheelspeed_right_target = (A * this->x_speed + B * this->rotation_speed) * (WHEEL_DIAMETER / 2);
     this->wheelspeed_left_target  = (A * this->x_speed + (-B) * this->rotation_speed) * (WHEEL_DIAMETER / 2);

src/network.cpp

@@ -113,6 +113,19 @@ bool Network::connectEspNow(recieveCallbackPtr reci, sendCallbackPtr send) {
     return true;
 }
 
+uint8_t Network::getCurrentChannel() {
+    uint8_t channel;
+    wifi_second_chan_t secondChannel;
+    if (esp_wifi_get_channel(&channel, &secondChannel) != ESP_OK) {
+        std::cout << "Network::getCurrentChannel - Error!" << std::endl;
+        return -1;
+    }
+    // std::cout << "Network::getCurrentChannel - Current WiFi channel: " 
+    //           << (int) channel << " second channel: " << (int) secondChannel 
+    //           << std::endl;
+    return channel;
+}
+
 bool Network::checkMQTT() {
     static uint64_t lastReconnectAttempt = 0;
     if (!mqtt_client->connected()) {

test/test_desktop/bootstrap.cpp

@@ -0,0 +1,22 @@
+#include "courseCalculation.hpp"
+#include <unity.h>
+
+void setUp() {
+
+}
+
+void tearDown() {
+
+}
+
+void uselessTest() {
+  TEST_ASSERT_EQUAL(12, 12);
+}
+
+int main(int argc, char **argv) {
+  UNITY_BEGIN();
+  RUN_TEST(testCourseCalculation);
+  UNITY_END();
+
+  return 0;
+}

test/test_desktop/courseCalculation.hpp

@@ -0,0 +1,15 @@
+#include "route.h"
+#include <unity.h>
+#include <iostream>
+
+void testCourseCalculation(void) {
+    // Point pointA(53.106725, 7.248069); // 45
+    // Point pointA(53.112188, 7.264404); // 90
+    Point pointA(53.112072, 7.266619); // -90
+    Point pointB(53.112123, 7.265354); //Home
+    int res = pointA.courseTo(pointB);
+    std::cout << "" << std::endl;
+    std::cout << "" << std::endl;
+    std::cout << "Return Value: " << res << std::endl;
+    TEST_ASSERT_LESS_OR_EQUAL(10, res);
+}

version

@@ -0,0 +1 @@
+0.8.36