added description and video to readme

- fix gyroscope
- fix init screen

.vscode/settings.json

@@ -1,5 +1,6 @@
 {
     "files.insertFinalNewline": true,
+    "editor.wordWrap": "on",
     "cSpell.ignoreWords": [
         "ledc",
         "write"
@@ -129,10 +130,10 @@
         "carr",
         "CIPO",
         "COPI",
-        "deadzone",
         "Doxygen",
         "Dutycycle",
         "gast",
+        "gnss",
         "GNSS",
         "GPGGA",
         "HSPI",
@@ -141,9 +142,12 @@
         "kleiax",
         "Lebennig",
         "microcontroller",
+        "Mqtt",
         "MQTT",
         "NMEA",
         "NMEAGPGGA",
+        "ntrip",
+        "Ntrip",
         "NTRIP",
         "pcnt",
         "pidl",
@@ -151,7 +155,8 @@
         "Punica",
         "RHEDE",
         "Rtcm",
-        "Sats",
+        "sapos",
+        "SAPOS",
         "Schalke",
         "Soln",
         "Systeminformation",

README.md

@@ -0,0 +1,5 @@
+# Autonomer Rover
+Testfahrt auf Youtube: https://youtu.be/GigIyo-vghE?si=jhProl1DQFUrcz7w
+
+# Beschreibung
+Vollständige Softwarelösung und Hardwareerweiterungen für einen Rover mit Skid-Antrieb. Das System wurde in C++ geschrieben und es wurden erweiterte Konzepte der Objektorientierung wie Polymorphie eingesetzt. Die Datenhaltung wurde erfolgreich von der Logik getrennt, hierfür wurde unter anderem eine API mit Express.js implementiert. Hardwareseitig wurde PWM für die Motorsteuerung, IC2 für verschiedene Sensoren und SPI für das GNSS-Modul eingesetzt. Zusätzlich wurden Encoder an den Motoren verbaut, um diese präzise zu steuern. Überdies wurde eine RTK-Referenzstation geplant und umgesetzt, dafür wurde die Antenne, ein weiteres GNSS-Modul und ein Embedded-Server installiert und konfiguriert. Durch das Zusammenspiel dieser Systeme kann der Rover Routen im niedrigen einstelligen Zentimeterbereich aufzeichnen und abfahren.

debugMqtt/hw1/main.py

@@ -0,0 +1,25 @@
+import paho.mqtt.client as mqtt
+
+
+def on_connect(client, userdata, flags, rc):
+    print("Connected with result code "+str(rc))
+
+    # Subscribing in on_connect() means that if we lose the connection and
+    # reconnect then subscriptions will be renewed.
+    client.subscribe("Rover/Info")
+    client.subscribe("Rover/Warn")
+    client.subscribe("Rover/Error")
+
+
+def on_message(client, userdata, msg):
+    print(msg.topic+"   -> "+str(msg.payload)[2:-1])
+
+
+if __name__ == '__main__':
+    client = mqtt.Client()
+    client.on_connect = on_connect
+    client.on_message = on_message
+
+    client.connect("172.22.64.216", 1883, 60)
+
+    client.loop_forever()

debugMqtt/hw1/main_debug.py

@@ -0,0 +1,23 @@
+import paho.mqtt.client as mqtt
+
+
+def on_connect(client, userdata, flags, rc):
+    print("Connected with result code "+str(rc))
+
+    # Subscribing in on_connect() means that if we lose the connection and
+    # reconnect then subscriptions will be renewed.
+    client.subscribe("Rover/Debug")
+
+
+def on_message(client, userdata, msg):
+    print(msg.topic+"   -> "+str(msg.payload)[2:-1])
+
+
+if __name__ == '__main__':
+    client = mqtt.Client()
+    client.on_connect = on_connect
+    client.on_message = on_message
+
+    client.connect("172.22.64.216", 1883, 60)
+
+    client.loop_forever()

debugMqtt/rhede/main.py

@@ -0,0 +1,27 @@
+import paho.mqtt.client as mqtt
+
+
+def on_connect(client, userdata, flags, rc):
+    print("Connected with result code "+str(rc))
+
+    # Subscribing in on_connect() means that if we lose the connection and
+    # reconnect then subscriptions will be renewed.
+    client.subscribe("Rover/Info")
+    client.subscribe("Rover/Warn")
+    client.subscribe("Rover/Error")
+    client.subscribe("Rover/Info")
+
+
+def on_message(client, userdata, msg):
+    print(msg.topic+"   -> "+str(msg.payload)[2:-1])
+
+
+if __name__ == '__main__':
+    client = mqtt.Client()
+    client.on_connect = on_connect
+    client.on_message = on_message
+
+    client.username_pw_set("kleiax", password="p?{$_~5%hBM7wrcFkr55KWr#")
+    client.connect("192.168.1.7", 1883, 60)
+
+    client.loop_forever()

debugMqtt/rhede/main_debug.py

@@ -0,0 +1,24 @@
+import paho.mqtt.client as mqtt
+
+
+def on_connect(client, userdata, flags, rc):
+    print("Connected with result code "+str(rc))
+
+    # Subscribing in on_connect() means that if we lose the connection and
+    # reconnect then subscriptions will be renewed.
+    client.subscribe("Rover/Debug")
+
+
+def on_message(client, userdata, msg):
+    print(msg.topic+"   -> "+str(msg.payload)[2:-1])
+
+
+if __name__ == '__main__':
+    client = mqtt.Client()
+    client.on_connect = on_connect
+    client.on_message = on_message
+
+    client.username_pw_set("kleiax", password="p?{$_~5%hBM7wrcFkr55KWr#")
+    client.connect("192.168.1.7", 1883, 60)
+
+    client.loop_forever()

doc/Diagrams/classdiagramm.uxf

@@ -0,0 +1,157 @@
+<diagram program="umletino" version="14.4.0-SNAPSHOT"><zoom_level>10</zoom_level><element><id>UMLClass</id><coordinates><x>750</x><y>10</y><w>220</w><h>650</h></coordinates><panel_attributes>&lt;&lt;hardware&gt;&gt;
+MotorControl
+--
+- target_power: int8_t
+- power: int8_t
+- direction: uint8_t
+
+
+- pwm_pin: uint8_t
+- pwm_channel: uint8_t
+- pwm_res: uint8_t
+- dutycycle_min: uint8_t
+- dutycycle_max: uint8_t
+- direction_1: uint8_t
+- direction_2: uint8_t
+- delay: uint8_t
+- powersteps: uint8_t
+
+- last_millis: uint32_t
+--
++ MotorControl()
++ init(pwm_pin: uint8_t,
+	  pwm_channel: uint8_t,
+	  dir_1: uint8_t, dir_2: uint8_t)
++ loop(): uint16_t
++ runMotorControl()
+
++ setMinPwm(uint8_t)
++ setMaxPwm(uint8_t)
++ setPowerSteps(uint8_t): uint16_t
++ setTargetPower(uint8_t)
++ setDelay(uint8_t): uint16_t
+
++ stop()
++ emergencyStop()
++ toString()
++ getPower(): int8_t
++ getTargetPower(): int8_t
++ isTargetPowerReached(): bool
++ isAccelerationPositive(): bool
++ isAccelerationNegative(): bool
+
+- setRealPower(power: int8_t)
+- increasePower(power: int8_t)
+
+</panel_attributes><additional_attributes></additional_attributes></element><element><id>UMLClass</id><coordinates><x>80</x><y>380</y><w>220</w><h>330</h></coordinates><panel_attributes>&lt;&lt;hardware&gt;&gt;
+Speedometer
+--
+- encoder: ESP32Encoder
+- debug: DebugMqtt*
+- name: String
+- speed: double = 0
+- last_millis: uint64_t = 0;
+- bufPos: uint8_t
+- buf: int16_t[BUF_SIZE]
+--
++ Speedometer()
++ init(pinA: uint8_t,
+	   pinB: uint8_t, 
+	  	 name: String)
++ runSpeedometer()
++ setTargetPower(power: int8_t)
++ getSpeed(): double
++ getDirection(): uint8_t
+- addValToBuf(val: int16_t)
+- getAverage(): int16_t</panel_attributes><additional_attributes></additional_attributes></element><element><id>UMLClass</id><coordinates><x>70</x><y>0</y><w>270</w><h>370</h></coordinates><panel_attributes>MoveControl
+--
+- debug: DebugMqtt*
+- left_motor: MotorControl*
+- right_motor: MotorControl*
+- left_speedometer: Speedometer*
+- right_speedometer: Speedometer*
+- driving_status: DrivingStatus = stop
+- x_speed: double = 0
+- rotation_speed: double = 0
+- wheelspeed_left_target: double = 0
+- wheelspeed_right_target: double = 0
+--
++ MoveControl()
++ init(left_motor: MotorControl*,
+	   right_motor: MotorControl*, 
+	   left_speedometer: Speedometer*,
+	   right_speedometer: Speedometer*)
++ runMoveControl()
++ setDrivingStatus(status: DrivingStatus)
++ setSpeed(speed: double)
++ setRotationspeed(speed: double)
+- calcWheelSpeed()
+- regulateMotors()</panel_attributes><additional_attributes></additional_attributes></element><element><id>UMLClass</id><coordinates><x>370</x><y>10</y><w>220</w><h>330</h></coordinates><panel_attributes>&lt;&lt;driveModi&gt;&gt;
+ManualControl
+--
+- encoder: ESP32Encoder
+- debug: DebugMqtt*
+- name: String
+- speed: double = 0
+- last_millis: uint64_t = 0;
+- bufPos: uint8_t
+- buf: int16_t[BUF_SIZE]
+--
++ Speedometer()
++ init(pinA: uint8_t,
+	   pinB: uint8_t, 
+	  	 name: String)
++ runSpeedometer()
++ setTargetPower(power: int8_t)
++ getSpeed(): double
++ getDirection(): uint8_t
+- addValToBuf(val: int16_t)
+- getAverage(): int16_t</panel_attributes><additional_attributes></additional_attributes></element><element><id>UMLClass</id><coordinates><x>1270</x><y>10</y><w>210</w><h>240</h></coordinates><panel_attributes>MenuControl
+--
+# parentMenu: MenuControl*
+# lcd: LiquidCrystal_I2C*
+--
+/+down()/
+/+up()/
+/+right()/
+/+left()/
+/+yes()/
+/+no()/
+
+/+update()/
+/+printMenu()/
+
++ setParentMenu(MenuControl*)
++ setLcd(LiquidCrystal_I2C*)</panel_attributes><additional_attributes></additional_attributes></element><element><id>UMLClass</id><coordinates><x>980</x><y>10</y><w>280</w><h>270</h></coordinates><panel_attributes>MenuAction
+--
+- name: char*
+
+- *function: void
+- *callback: void
+
+- isMenu: bool
+- menu: MenuControl
+--
++ MenuAction(const char*, (*function), (*callback))
++ MenuAction(const char*, (*function))
++ MenuAction(const char*, MenuControl* menu)
+
++ runAction()
+
++ getName(): const char*
++ getIsMenu(): bool 
++ getMenu(): MenuControl*
+</panel_attributes><additional_attributes></additional_attributes></element><element><id>UMLClass</id><coordinates><x>1220</x><y>360</y><w>280</w><h>160</h></coordinates><panel_attributes>Menu
+--
+- inSubmenu: bool
+
+- entrys: list&lt;MenuAction*&gt;
+- selectedEntry: list&lt;MenuAction*&gt;::iterator
+
+- isMenu: bool
+- menu: MenuControl
+--
++ addEntry(MenuAction* entry)
+</panel_attributes><additional_attributes></additional_attributes></element><element><id>Relation</id><coordinates><x>1350</x><y>240</y><w>30</w><h>140</h></coordinates><panel_attributes>lt=&lt;&lt;-</panel_attributes><additional_attributes>10;10;10;120</additional_attributes></element><element><id>Relation</id><coordinates><x>1080</x><y>270</y><w>160</w><h>190</h></coordinates><panel_attributes>lt=-
+m1=*
+m2=0..1</panel_attributes><additional_attributes>10;10;10;160;140;160</additional_attributes></element></diagram>

doc/Diagrams/statesAutopilot.uxf

@@ -0,0 +1,390 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<diagram program="umlet" version="14.3.0">
+  <zoom_level>10</zoom_level>
+  <element>
+    <id>UMLSpecialState</id>
+    <coordinates>
+      <x>40</x>
+      <y>20</y>
+      <w>20</w>
+      <h>20</h>
+    </coordinates>
+    <panel_attributes>type=initial</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>40</x>
+      <y>20</y>
+      <w>130</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>110.0;10.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>150</x>
+      <y>10</y>
+      <w>100</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>search next point
+bg=green</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLSpecialState</id>
+    <coordinates>
+      <x>180</x>
+      <y>90</y>
+      <w>40</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>bg=green
+type=decision</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>190</x>
+      <y>40</y>
+      <w>30</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>10.0;50.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>150</x>
+      <y>180</y>
+      <w>100</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>detect alignment
+bg=green</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>190</x>
+      <y>120</y>
+      <w>130</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-
+[distance &lt;= 5m]</panel_attributes>
+    <additional_attributes>10.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLObject</id>
+    <coordinates>
+      <x>20</x>
+      <y>0</y>
+      <w>840</w>
+      <h>590</h>
+    </coordinates>
+    <panel_attributes>Autopilot                                            
+
+valign=top</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>210</x>
+      <y>90</y>
+      <w>140</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-
+[distance &gt; 5m]</panel_attributes>
+    <additional_attributes>120.0;20.0;10.0;20.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>330</x>
+      <y>90</y>
+      <w>100</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>Error
+bg=red</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>150</x>
+      <y>270</y>
+      <w>100</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>correct alignment
+bg=green</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>150</x>
+      <y>360</y>
+      <w>100</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>drive to point
+bg=green</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>190</x>
+      <y>210</y>
+      <w>30</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>10.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>190</x>
+      <y>300</y>
+      <w>30</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>10.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>90</x>
+      <y>280</y>
+      <w>80</w>
+      <h>120</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>60.0;10.0;10.0;10.0;10.0;100.0;60.0;100.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>440</x>
+      <y>10</y>
+      <w>410</w>
+      <h>300</h>
+    </coordinates>
+    <panel_attributes>detect alignment
+valign=top</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLSpecialState</id>
+    <coordinates>
+      <x>460</x>
+      <y>60</y>
+      <w>20</w>
+      <h>20</h>
+    </coordinates>
+    <panel_attributes>type=final</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>470</x>
+      <y>60</y>
+      <w>90</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>lt=-&gt;</panel_attributes>
+    <additional_attributes>10.0;10.0;70.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>540</x>
+      <y>50</y>
+      <w>110</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>drive 1m forward</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>540</x>
+      <y>140</y>
+      <w>110</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>drive 2m backward</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>540</x>
+      <y>230</y>
+      <w>110</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>drive 1m forward</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>700</x>
+      <y>230</y>
+      <w>120</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>calculate straight line</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>580</x>
+      <y>80</y>
+      <w>30</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>10.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>580</x>
+      <y>170</y>
+      <w>30</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>10.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLSpecialState</id>
+    <coordinates>
+      <x>750</x>
+      <y>150</y>
+      <w>20</w>
+      <h>20</h>
+    </coordinates>
+    <panel_attributes>type=termination</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>750</x>
+      <y>160</y>
+      <w>30</w>
+      <h>90</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;70.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>440</x>
+      <y>320</y>
+      <w>410</w>
+      <h>260</h>
+    </coordinates>
+    <panel_attributes>correct alignment
+valign=top</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>540</x>
+      <y>370</y>
+      <w>190</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>calculate relativ target position</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>470</x>
+      <y>380</y>
+      <w>90</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>lt=-&gt;</panel_attributes>
+    <additional_attributes>10.0;10.0;70.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLSpecialState</id>
+    <coordinates>
+      <x>460</x>
+      <y>380</y>
+      <w>20</w>
+      <h>20</h>
+    </coordinates>
+    <panel_attributes>type=final</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>580</x>
+      <y>460</y>
+      <w>120</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>rotate x degree</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>630</x>
+      <y>400</y>
+      <w>30</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>10.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>630</x>
+      <y>490</y>
+      <w>30</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>10.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLSpecialState</id>
+    <coordinates>
+      <x>630</x>
+      <y>550</y>
+      <w>20</w>
+      <h>20</h>
+    </coordinates>
+    <panel_attributes>type=termination</panel_attributes>
+    <additional_attributes/>
+  </element>
+</diagram>

doc/Doxygen/Doxyfile

@@ -44,7 +44,7 @@ PROJECT_NUMBER         =
 # for a project that appears at the top of each page and should give viewer a
 # quick idea about the purpose of the project. Keep the description short.
 
-PROJECT_BRIEF          = "A small verhicle which should be drive a route with GPS"
+PROJECT_BRIEF          = "A small verhicle which should be drive a route with GNNS and RTK"
 
 # With the PROJECT_LOGO tag one can specify a logo or an icon that is included
 # in the documentation. The maximum height of the logo should not exceed 55

doc/TODO allgemein.txt

@@ -1,8 +1,45 @@
-Do now:
-Code:
-Latex:
-    Overfull H-Boxen
-    Test Motor, Zeiten kontrolieren. Wenn einfach so dann bezug auf fehler bei Timing und freeRtos
-    Was mit den nicht erfüllten Anforderungen machen?
-    Hardware doppelt erklärt
+Maybe:
+    -> Time Measurement in component interface
+    -> Time warnings
+    -> Blocking Battery Function in Battery class
+        - Callback function for output and shutdown things
+
+Do later:
+    -> Program underfloorLighting
+    -> Add an beeper
+    -> Program the beeper
+    -> Engine slow down without curve in motorControl
+    -> Network clean up (Mqtt remove?)
+    -> Extra class for maneuver, autopilot should inherit from int16_t
+    -> Remote Control
+        - Leds for gnss rtk etc
+        - what happens exactly when no data is arriving
+    -> Test Menu for big curve driving
+    -> Api with Names and show on Maps in Browser
+    -> 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
+    -> Menü für Einstellungen
+        - WiFi (save in Flash)
+    -> Battery
+        - tabelle mit eigenen Werten übergeben und nicht in header (wiederverwendbarkeit)
+        - kalibrierungsmethode mit Menü
+        - Daten in flash speichern können
+    -> Check speration between Ui and Route (RouteMenu)
+    -> update sparkfun gnss auf v3 für SPI korrekturdatenüvbetragung
+    -> Rover Objekt mit Error zustand freeze, damit das wenn möglich auch auf dem Display angezeigt wird.
+    -> ESP und Sensoren in den DeepSleep für Auschalten oder Akkuschutz
+    -> Doxygen comments
+        - navigation
+        - autopilot
+
+Have to:
+    -> check new distance calculation between points
+    -> check new course calculation between points
+    -> menu route data und ui disconnecten
+    -> use routeCache in captureRoute and autopilot
+    -> make ntrip client useable without user and password
+    -> understand gyroscope offsets
+
 

include/moveControl.h

@@ -87,7 +87,6 @@ public:
      * @param status
      */
     void setDrivingStatus(Status status);
-    Status getDrivingStatus() const { return this->driving_status; }
 
     /**
      * @brief Stops the engine immediately

include/networkConfig.h

@@ -14,16 +14,18 @@
 
 #include <stdint.h>
 
-#define HOTSPOT
+#define ZUHAUSE
+#define SAPOS_NI
+// #define RTK2GO
 
 #ifdef HOTSPOT
 namespace NetworkConfig
 {
-    const char ssid[] = "KleiaxHandy";
+    const char ssid[] = "Nubsi";
     const char password[] = "12345677";
-    const char ip[] = "192.168.43.4";
+    const char ip[] = "192.168.72.7";
     const char subnet[] = "255.255.255.0";
-    const char gateway[] = "192.168.43.1";
+    const char gateway[] = "192.168.72.228";
     const char dns[] = "8.8.8.8";
     const bool mqtt = false;
 }
@@ -56,6 +58,19 @@ namespace NetworkConfig
 }
 #endif // RHEDE
 
+#ifdef ZUHAUSE
+namespace NetworkConfig
+{
+    const char ssid[] = "Tuedelkram";
+    const char password[] = "3!Kaesehoch";
+    const char ip[] = "192.168.178.13";
+    const char subnet[] = "255.255.255.0";
+    const char gateway[] = "192.168.178.1";
+    const char dns[] = "8.8.8.8";
+    const bool mqtt = false;
+}
+#endif // ZUHAUSE
+
 namespace MqttConfig
 {
     const char server[] = "192.168.1.7";
@@ -64,13 +79,28 @@ namespace MqttConfig
     const char password[] = "p?{$_~5%hBM7wrcFkr55KWr#";
 }
 
+#ifdef RTK2GO
 namespace NtripConfig
 {
     const char host[] = "rtk2go.com";
     const uint16_t port = 2101;
-    // const char mountPoint[] = "GER-Dortmund";
+    const char mountPoint[] = "GER-Dortmund";
     // const char mountPoint[] = "GER-Papenburg";
-    const char mountPoint[] = "Rtkramerntrip";
+    // const char mountPoint[] = "Rtkramerntrip";
     const char user[] = "alklein1@gmx.de";
     const char password[] = "none";
+    const bool sendOwnPosition = false;
 }
+#endif // RTK2GO
+
+#ifdef SAPOS_NI
+namespace NtripConfig
+{
+    const char host[] = "sapos-ni-ntrip.de";
+    const uint16_t port = 2101;
+    const char mountPoint[] = "VRS_3_4G_NI";
+    const char user[] = "ni_FHDo01";
+    const char password[] = "ALdx-1-3e49";
+    const bool sendOwnPosition = true;
+}
+#endif // RTK2GO

lib/CalcAzimuth/calcAzimuth.cpp

@@ -0,0 +1,121 @@
+/**
+ * @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)
+    : lastChangePoint{point}, currentPosition{point}
+{
+    Component::loopDelay = CalcAzimuth::loopDelay;
+}
+
+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;
+}
+
+String CalcAzimuth::stateToString(State state)
+{
+    switch (state)
+    {
+    case State::Invalid:
+        return "Invalid";
+
+    case State::Bad:
+        return "Bad";
+
+    case State::Ok:
+        return "Ok";
+
+    case State::Good:
+        return "Good";
+
+    case State::Super:
+        return "Super";
+
+    default:
+        return "UNKOWN";
+    }
+}
+
+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 = INT16_MIN;
+        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) > this->minDistanceForBetterQuality)
+    {
+        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,93 @@
+/**
+ * @file calcAzimuth.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains a class that calculates the azimuth from a last and a current position
+ * @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"
+
+/**
+ * @brief A class to calculate an azimuth
+ *
+ * This class calculates the current Azimuth with the last position
+ * where the rover has been rotated and the current position
+ */
+class CalcAzimuth : public Component
+{
+public:
+    /**
+     * @brief States which represent the quality of the current calculated azimuth
+     */
+    enum State
+    {
+        Invalid,
+        Bad,
+        Ok,
+        Good,
+        Super
+    };
+
+    /**
+     * @brief Construct a new Calc Azimuth object
+     *
+     * @param point current position
+     */
+    CalcAzimuth(Point point);
+
+    /**
+     * @brief Have to be called if the rover rotates
+     *
+     * @param point current position
+     */
+    void drivingDirectionChange(Point point);
+
+    /**
+     * @brief update the current position
+     *
+     * This function should be called if the rover has moved in
+     * a straight direction, to calculated the current Azimuth.
+     * More distance to the point given to drivingDirectionChange()
+     * increase the accuracy of the calculation.
+     *
+     * @param point current position
+     */
+    void updateCurrentPosition(Point point);
+    void disableCalcAzimuth() { this->directionChangeMode = false; }
+
+    int16_t getAzimuth() const { return this->calcAzimuth; }
+
+    /**
+     * @brief Get the State struct
+     *
+     * @return State current quality of the calculation
+     */
+    State getState() const { return this->state; }
+
+    static String stateToString(State 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;
+    double minDistanceForBetterQuality = 2;
+
+    static constexpr uint8_t loopDelay = 50;
+};
+
+#endif // CALC_AZIMUTH_H

lib/Component/component.h

@@ -62,7 +62,7 @@ public:
      * If the component is deactivated the call of loop() ha no effect
      */
     void deactivate() { this->active = false; }
-    void activate() { this->active = false; }
+    void activate() { this->active = true; }
 
 protected:
     /**

lib/LcdWrapper/LcdWrapper.cpp

@@ -14,7 +14,7 @@
 LcdWrapper::LcdWrapper(LiquidCrystal_I2C *lcd)
     : lcd{lcd}, changed{false}
 {
-    this->clear();
+    // this->clear();
 }
 
 void LcdWrapper::run()

lib/MotorControl/motorControl.cpp

@@ -33,7 +33,6 @@ void MotorControl::init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8
     ledcSetup(this->pwmChannel, MotorControl::pwmFreq, MotorControl::pwmRes);
     ledcAttachPin(this->pwmPin, this->pwmChannel);
     ledcWrite(this->pwmChannel, 0);
-    std::cout << "Init pwm" << std::endl;
 }
 
 void MotorControl::run()
@@ -57,7 +56,7 @@ void MotorControl::run()
         return;
     }
 
-    // Positive or negative target speed
+    // Positive or negative tagret speed
     if (this->targetPower >= 0)
     {
         // Positive or negative speed
@@ -98,6 +97,28 @@ void MotorControl::run()
     }
 }
 
+void MotorControl::setMinPwm(uint8_t min)
+{
+    if (min > MotorControl::maxPwmMin)
+    {
+        min = MotorControl::maxPwmMin;
+    }
+    // transform percentage to real pwm value
+    min = static_cast<uint8_t>(((static_cast<uint8_t>(1) << pwmRes) - 1) * (min / 100.0));
+    this->dutycycleMin = min;
+}
+
+void MotorControl::setMaxPwm(uint8_t max)
+{
+    if (max > 100)
+    {
+        max = 100;
+    }
+    // transform percentage to real pwm value
+    max = static_cast<uint8_t>(((static_cast<uint8_t>(1) << pwmRes) - 1) * (max / 100.0));
+    this->dutycycleMax = max;
+}
+
 void MotorControl::setTargetPower(int8_t power)
 {
     if (power <= 100 && power >= -100)
@@ -137,6 +158,7 @@ bool MotorControl::isAccelerationNegative() const
 
 void MotorControl::setRealPower(int8_t power)
 {
+    // TODO: Exceptionhandling
     if (power <= 100 && power >= -100)
     {
         this->power = power;
@@ -153,11 +175,10 @@ void MotorControl::setRealPower(int8_t power)
         digitalWrite(this->dir_2, LOW);
         ledcWrite(this->pwmChannel, 0);
         this->dutycycle = 0;
-        std::cout << "abort" << std::endl;
         return;
     }
 
-    const uint8_t pwm_val = map(abs(power), 0, 100, MotorControl::minPwmVal, MotorControl::maxPwmVal);
+    const 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
@@ -172,13 +193,15 @@ void MotorControl::setRealPower(int8_t power)
         digitalWrite(this->dir_2, LOW);
     }
 
-    std::cout << "MotorControl::setRealPower pwm_val: " << (int) pwm_val << " channel:" << (int) this->pwmChannel << std::endl;
     ledcWrite(this->pwmChannel, pwm_val);
+    std::cout << "MotorControl::serRealPower - pwm: " << static_cast<int>(pwm_val) << std::endl;
     this->dutycycle = pwm_val;
 }
 
 void MotorControl::increasePower(int8_t power)
 {
+    // TODO: Exceptionhandling
+    // TODO: make a stop befor a direction change
     if (abs(power) > 2 * MotorControl::powerSteps)
     {
         Serial.println("Invalid Argument in MotorControl::increasePower");

lib/MotorControl/motorControl.h

@@ -38,6 +38,20 @@ public:
      */
     void init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8_t dir_2);
 
+    /**
+     * @brief Set the minimum duty cycle
+     *
+     * @param min duty cycle in percent
+     */
+    void setMinPwm(uint8_t min);
+
+    /**
+     * @brief Set the maximum duty cycle
+     *
+     * @param max duty cycle in percent
+     */
+    void setMaxPwm(uint8_t max);
+
     /**
      * @brief Set the Target Power
      *
@@ -89,8 +103,6 @@ private:
     static constexpr uint8_t pwmRes = 8;
     static constexpr uint8_t powerSteps = 2; //  A total of 20 levels ( 100 / SPEED_STEPS ) * RUN_MOTOR_CONTROL_DELAY = 500ms
     static constexpr uint8_t maxPwmMin = 80;
-    static constexpr uint8_t maxPwmVal = 250;
-    static constexpr uint8_t minPwmVal = 55; // Max 98% of 2^PWM_RES
 
     int8_t targetPower = 0;
     int8_t power = 0;
@@ -99,6 +111,8 @@ private:
     uint8_t pwmPin = 0;
     uint8_t pwmChannel = 0;
     uint16_t dutycycle = 0;
+    uint8_t dutycycleMin = 55;
+    uint8_t dutycycleMax = 98; // Max 98% of 2^PWM_RES
     uint8_t dir_1 = 0;
     uint8_t dir_2 = 0;
 };

lib/Navigation/navigation.cpp

@@ -0,0 +1,178 @@
+/**
+ * @file navigation.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains the implementation of the class Navigation
+ * @version 0.1
+ * @date 2022-01-31
+ *
+ * @copyright Copyright (c) 2022
+ *
+ */
+
+#include "navigation.h"
+
+Navigation::Navigation(const SensorData *sensorData, Route *route)
+    : sensorData{sensorData}
+{
+    this->init(route);
+}
+
+void Navigation::init(Route *route)
+{
+    if (static_cast<bool>(route))
+    {
+        this->route = route;
+    }
+    else
+    {
+        this->route = new Route();
+    }
+
+    Component::loopDelay = Navigation::loopDelay;
+}
+
+Navigation::~Navigation()
+{
+    delete this->route;
+}
+
+void Navigation::run() {}
+
+void Navigation::newRoute()
+{
+    delete this->route;
+    this->route = new Route();
+}
+
+bool Navigation::startNavigation()
+{
+    const Point newTargetPoint = this->route->startRoute();
+    this->navigationStarted = this->setTargetPoint(newTargetPoint);
+    if (this->navigationStarted)
+    {
+        this->navigationFinished = false;
+    }
+    return this->navigationStarted;
+}
+
+Navigation::Status Navigation::getCourseCorrection(CourseCorrection &correction, bool forceUpdate)
+{
+    if (this->navigationFinished)
+    {
+        return Status::Complete;
+    }
+
+    if (this->currentPosition.getAccuracy() <= this->minAccuracy)
+    {
+        return Status::InsufficientAccuracy;
+    }
+
+    // Check if the Rover has moved, if the Rover hasnt moved this function will be return.
+    if (this->currentPosition.distanceTo(this->lastPointCalcCorrection) < (this->minDistanceToReachPoint) / 2 && !forceUpdate)
+    {
+        correction.correction = this->calculateCourseCorrection(this->lastPointCalcCorrection);
+        correction.distance = this->lastPointCalcCorrection.distanceTo(this->targetPoint);
+        return Status::Unchanged;
+    }
+
+    double distance = this->currentPosition.distanceTo(this->targetPoint);
+
+    // Check if I need a new Point
+    if (distance < this->minDistanceToReachPoint && this->preventNextPoint == false)
+    {
+        if (!this->nextPoint())
+        {
+            this->navigationFinished = true;
+            this->navigationStarted = false;
+            return Status::Complete; // End of navigation
+        }
+        distance = this->currentPosition.distanceTo(this->targetPoint);
+    }
+
+    correction.correction = this->calculateCourseCorrection(this->currentPosition);
+    correction.distance = distance;
+    this->lastPointCalcCorrection = this->currentPosition;
+    return Status::Updated;
+}
+
+Navigation::Status Navigation::addCurrentPosToRoute()
+{
+    if (this->currentPosition.getAccuracy() <= this->minAccuracy)
+    {
+        return Status::InsufficientAccuracy;
+    }
+
+    // First Point
+    if (this->route->getRouteInfo().totalPoints == 0)
+    {
+        this->route->addPointToRoute(this->currentPosition);
+        this->lastPointRouteInsert = this->currentPosition;
+        return Status::Updated;
+    }
+
+    // Every Point after the first
+    const double distance = this->currentPosition.distanceTo(this->lastPointRouteInsert);
+    if (Navigation::minDisBetweenPoints <= distance && Navigation::maxDisBetweenPoints >= distance)
+    {
+        this->route->addPointToRoute(this->currentPosition);
+        this->lastPointRouteInsert = this->currentPosition;
+        return Status::Updated;
+    }
+    return Status::Unchanged;
+}
+
+int16_t Navigation::calculateCourseCorrection(Point &point)
+{
+    const int16_t targetCourse = point.courseTo(this->targetPoint);
+    int16_t correctionCourse = 0;
+
+    if (this->sensorData->getCalcAzimuthState() == CalcAzimuth::State::Good || this->sensorData->getCalcAzimuthState() == CalcAzimuth::State::Super)
+    {
+        correctionCourse = targetCourse - this->sensorData->getCalcAzimuth();
+        this->lastUsedCalcAzimuth = true;
+    }
+    else
+    {
+        correctionCourse = targetCourse - this->sensorData->getRealAzimuth();
+        this->lastUsedCalcAzimuth = false;
+    }
+
+    return Navigation::fixDegree(correctionCourse);
+}
+
+bool Navigation::nextPoint()
+{
+    if (!this->navigationStarted)
+    {
+        return false;
+    }
+    return this->setTargetPoint(this->route->getNextPoint());
+}
+
+bool Navigation::setTargetPoint(Point target)
+{
+    if (target.isInit())
+    {
+        this->targetPoint = target;
+        return true;
+    }
+    return false;
+}
+
+int16_t Navigation::fixDegree(int16_t degree)
+{
+    static constexpr uint16_t fullCircle = 360;
+
+    while (degree < -fullCircle / 2 || degree > fullCircle / 2)
+    {
+        if (degree > fullCircle / 2)
+        {
+            degree -= fullCircle;
+        }
+        else if (degree < -fullCircle / 2)
+        {
+            degree += fullCircle;
+        }
+    }
+    return degree;
+}

lib/Navigation/navigation.h

@@ -0,0 +1,163 @@
+/**
+ * @file navigation.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains a class which navigate an object by the given route
+ * @version 0.1
+ * @date 2022-01-10
+ *
+ * @copyright Copyright (c) 2022
+ *
+ */
+
+#ifndef NAVIGATION_H
+#define NAVIGATION_H
+
+#include <Arduino.h>
+#include <iostream>
+
+#include "route.h"
+#include "sensorData.h"
+#include "component.h"
+
+/**
+ * @brief This struct inherits the result of the navigation
+ *
+ * The drive get objects of this struct and should
+ * correct the direction in dependency on this.
+ *
+ */
+struct CourseCorrection
+{
+    int16_t correction;
+    double distance;
+};
+
+/**
+ * @brief This class navigate an object
+ *
+ * The class use the given Route and the gnss device
+ * to tell the driver in which direction he have to
+ * be drive and the distance to the next checkpoint.
+ *
+ */
+class Navigation : public Component
+{
+public:
+    enum Status
+    {
+        InsufficientAccuracy,
+        Unchanged,
+        Updated,
+        Complete
+    };
+
+    /**
+     * @brief Construct a new Navigation object
+     *
+     * @param sensorData
+     * @param route
+     */
+    Navigation(const SensorData *sensorData, Route *route = nullptr);
+
+    /**
+     * @brief Destroy the Navigation object
+     *
+     */
+    ~Navigation();
+
+    /**
+     * @brief creates a new empty route
+     *
+     */
+    void newRoute();
+
+    /**
+     * @brief Tries to start the route
+     *
+     * For example the route can not be started
+     * if there are no Points or wrong Points.
+     *
+     * @return true route is started
+     * @return false route can not be started
+     */
+    bool startNavigation();
+    void freezeTargetPoint(bool val = true) { this->preventNextPoint = val; };
+
+    double increaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint += 0.1; }
+    double decreaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint -= 0.1; }
+
+    // TODO: Dokumentation korrigieren.
+    /**
+     * @brief Get the Course Correction object
+     *
+     * @param correction
+     * @param forceUpdate
+     * @return Status
+     */
+    Status getCourseCorrection(CourseCorrection &correction, bool forceUpdate = false);
+
+    // TODO: Dokumentation korrigieren.
+    /**
+     * @brief Tries to add the current Position to the route
+     *
+     * This can be go wrong if there is no valid GPS signal
+     *
+     * @return true successful added point
+     * @return false no point added to route
+     */
+    Status addCurrentPosToRoute();
+
+    /**
+     * @brief Get the Route Info object
+     *
+     * This object contains information about the route.
+     * For example the stored points.
+     *
+     * @return RouteInfo
+     */
+    RouteInfo getRouteInfo() const { return this->route->getRouteInfo(); }
+    Route *getRoute() const { return this->route; }
+    Point getCurrentPosition() const { return this->currentPosition; }
+
+    Point::Accuracy getMinAccuracy() const { return this->minAccuracy; }
+    void setMinAccuracy(Point::Accuracy accuracy) { this->minAccuracy = accuracy; }
+
+    // map input in range from -180 to 180 degree
+    static int16_t fixDegree(int16_t degree);
+
+private:
+    void run() override;
+    void init(Route *route);
+    bool nextPoint();
+    bool setTargetPoint(Point target);
+    int16_t calculateCourseCorrection(Point &point);
+
+    static constexpr uint8_t loopDelay = 20;
+    static constexpr uint8_t maxDisBetweenPoints = 10;
+    static constexpr float minDisBetweenPoints = 0.3;
+
+    const SensorData *sensorData;
+    Route *route = nullptr;
+
+    Point lastPointRouteInsert;
+    Point lastPointCalcCorrection;
+    Point lastPointDrivingDirectionChange;
+    Point targetPoint;
+    Point currentPosition;
+    Point::Accuracy minAccuracy = Point::Accuracy::twoDigOfCM;
+
+    bool navigationStarted = false;
+    bool navigationFinished = false;
+    bool isNtripInit = false;
+    bool preventNextPoint = false;
+    bool directionChangeMode = false;
+    bool lastUsedCalcAzimuth = false;
+
+    uint8_t timeToWait = 200;
+    uint32_t lastMillis = 0;
+    uint32_t ubxUpdateTime = 0;
+
+    double minDistanceToReachPoint = 0.5;
+};
+
+#endif // NAVIGATION_H

lib/NtripClient/ntripClient.cpp

@@ -0,0 +1,400 @@
+/**
+ * @file NTRIPClient.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains the implementation of the class NTRIPClient.
+ * @version 0.1
+ * @date 2022-09-18
+ *
+ * @copyright Copyright (c) 2022
+ *
+ */
+
+#include "ntripClient.h"
+
+NTRIPClient::NTRIPClient(SFE_UBLOX_GNSS *gnss, const char *host, uint16_t port, const char *mountPoint, const char *user, const char *password)
+    : gnss{gnss}, port{port}, ntripClient{new WiFiClient}, state{NTRIPClientStates::closingConnection}
+{
+    strcpy(this->host, host);
+    strcpy(this->mountPoint, mountPoint);
+    strcpy(this->user, user);
+    strcpy(this->password, password);
+    Component::loopDelay = NTRIPClient::loopDelay;
+}
+
+NTRIPClient::~NTRIPClient()
+{
+    delete this->ntripClient;
+}
+
+void NTRIPClient::run()
+{
+    switch (this->state)
+    {
+    case NTRIPClientStates::openingConnection:
+        if (!this->activated)
+        {
+            this->state = NTRIPClientStates::closingConnection;
+            break;
+        }
+
+        std::cout << "Connecting to the NTRIP caster..." << std::endl;
+        if (this->beginClient())
+        {
+            std::cout << "Connected to the NTRIP caster!" << std::endl;
+            this->state = NTRIPClientStates::pushingData;
+        }
+        else
+        {
+            std::cout << "Failed!" << std::endl;
+            this->state = NTRIPClientStates::waiting;
+            this->lastReconnectTime = millis();
+            this->activated = false;
+        }
+        break;
+
+    case NTRIPClientStates::pushingData:
+        if (!processConnection() || !this->activated)
+        {
+            this->state = NTRIPClientStates::closingConnection;
+        }
+        break;
+
+    case NTRIPClientStates::closingConnection:
+        std::cout << "Closing the connection to the NTRIP caster..." << std::endl;
+        this->closeConnection();
+        state = NTRIPClientStates::waiting;
+        break;
+
+    case NTRIPClientStates::waiting:
+        if (this->activated)
+        {
+            this->state = NTRIPClientStates::openingConnection;
+        }
+        else
+        {
+            this->checkAutoReconnect();
+        }
+        break;
+
+    case NTRIPClientStates::notAvailable:
+        break;
+
+    default:
+        std::cout << "Wrong state in NTRIPClient.cpp..." << std::endl;
+        this->state = NTRIPClientStates::closingConnection;
+        break;
+    }
+}
+
+void NTRIPClient::runAsChild()
+{
+    this->pushGPGGA();
+}
+
+void NTRIPClient::gnssConfiguration()
+{
+    this->gnss->setSPIOutput(COM_TYPE_UBX | COM_TYPE_NMEA);
+    this->gnss->setPortInput(COM_PORT_SPI, COM_TYPE_UBX | COM_TYPE_NMEA | COM_TYPE_RTCM3);
+    // Set the differential mode - ambiguities are fixed whenever possible
+    this->gnss->setDGNSSConfiguration(SFE_UBLOX_DGNSS_MODE_FIXED);
+    this->gnss->setMainTalkerID(SFE_UBLOX_MAIN_TALKER_ID_GP);
+    this->gnss->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI);
+}
+
+bool NTRIPClient::setActivated(bool state)
+{
+    if (state && this->state != NTRIPClientStates::notAvailable)
+    {
+        this->activated = true;
+    }
+    else if (state)
+    {
+        return false;
+    }
+    else
+    {
+        this->activated = false;
+        this->autoReconnect = false;
+    }
+    return true;
+}
+
+void NTRIPClient::setAutoReconnect(bool state)
+{
+    if (state)
+    {
+        this->reconnectAttemps = 0;
+        this->autoReconnect = true;
+        return;
+    }
+
+    this->autoReconnect = false;
+}
+
+bool NTRIPClient::beginClient()
+{
+    static constexpr uint16_t httpError = 401;
+    static constexpr uint16_t httpCheck = 200;
+
+    std::cout << "Opening socket to " << this->host << std::endl;
+
+    char serverRequest[this->bufferSize];
+    char credentials[this->bufferSize];
+
+    if (!static_cast<bool>(this->ntripClient->connect(static_cast<const char *>(this->host), this->port)))
+    {
+        std::cout << "Connection to caster failed" << std::endl;
+        return false;
+    }
+    std::cout << "Connected to " << this->host << " : " << this->port << std::endl;
+    std::cout << "Requesting NTRIP Data from mount point " << this->mountPoint << std::endl;
+
+    // Generate the server request (GET)
+    if (this->useNtripRev1)
+    {
+        snprintf(static_cast<char *>(serverRequest),
+                 this->bufferSize,
+                 static_cast<const char *>("GET /%s HTTP/1.0\r\nHost: %s:%u\r\nUser-Agent: NTRIP KleiaxNtripClient/0.1\r\nAccept: */*\r\nConnection: close\r\n"),
+                 this->mountPoint,
+                 this->host,
+                 this->port);
+    }
+    else
+    {
+        snprintf(static_cast<char *>(serverRequest),
+                 this->bufferSize,
+                 static_cast<const char *>("GET /%s HTTP/1.1\r\nHost: %s:%u\r\nNtrip-Version: Ntrip/2.0\r\nUser-Agent: NTRIP KleiaxNtripClient/0.1\r\nAccept: */*\r\nConnection: close\r\n"),
+                 this->mountPoint,
+                 this->host,
+                 this->port);
+    }
+
+    // Add own Position if activated
+    if (this->transmitLocation && !this->useNtripRev1)
+    {
+        std::cout << "NTRIPClient::beginClient - Add own position to server request." << std::endl;
+        auto *data = new NMEA_GGA_data_t;
+        const uint8_t res = this->gnss->getLatestNMEAGPGGA(data);
+        if (res > 0) // valid data
+        {
+            char positionUpdate[this->bufferSizePushGPGGA];
+            snprintf(static_cast<char *>(positionUpdate),
+                     this->bufferSizePushGPGGA,
+                     static_cast<const char *>("Ntrip-GGA: %s"),
+                     data->nmea);
+            strncat(static_cast<char *>(serverRequest), static_cast<const char *>(positionUpdate), this->bufferSize);
+        }
+        else
+        {
+            std::cout << "NTRIPClient::beginClient - No valid data to send position to caster." << std::endl;
+        }
+        delete data;
+
+        this->lastGPGGAPushTime = millis();
+    }
+
+    if (this->user && this->password)
+    {
+        // Credentials
+        const uint8_t userCredentialsLength = strlen(this->user) + strlen(this->password) + 2;
+        auto *userCredentials = new char[userCredentialsLength];
+        snprintf(static_cast<char *>(userCredentials), userCredentialsLength, static_cast<const char *>("%s:%s"), this->user, this->password);
+
+        std::cout << "Sending credentials: " << userCredentials << std::endl;
+
+        // Encode
+        const base64 base;
+        const String strEncodedCredentials = base64::encode(userCredentials);
+        delete userCredentials;
+        char encodedCredentials[strEncodedCredentials.length() + 1];
+        strEncodedCredentials.toCharArray(static_cast<char *>(encodedCredentials), sizeof(encodedCredentials));
+
+        snprintf(credentials, sizeof(credentials), static_cast<const char *>("Authorization: Basic %s\r\n"), static_cast<const char *>(encodedCredentials));
+
+        // Add the encoded credentials to the server request
+        strncat(static_cast<char *>(serverRequest), static_cast<const char *>(credentials), this->bufferSize);
+    }
+
+    strncat(static_cast<char *>(serverRequest), static_cast<const char *>("\r\n"), this->bufferSize);
+
+    std::cout << static_cast<const char *>("serverRequest size: ")
+              << strlen(serverRequest)
+              << static_cast<const char *>(" of ")
+              << this->bufferSize
+              << static_cast<const char *>(" bytes available")
+              << std::endl;
+
+    // Send the server request
+    std::cout << "Sending server request: " << serverRequest << std::endl;
+    this->ntripClient->write(serverRequest, strlen(serverRequest));
+
+    // Wait up to 5 seconds for response
+    const uint32_t lastMillis = millis();
+    while (static_cast<bool>(!ntripClient->available()))
+    {
+        if (millis() - lastMillis > this->timeOut)
+        {
+            std::cout << "Caster timed out!" << std::endl;
+            this->ntripClient->stop();
+            return false;
+        }
+        delay(10);
+    }
+
+    // Check reply
+    uint16_t httpStatusCode = 0;
+    char response[this->bufferSize];
+    uint16_t responseIndex = 0;
+    while (static_cast<bool>(this->ntripClient->available()))
+    {
+        if (responseIndex == sizeof(response))
+        {
+            break;
+        }
+
+        response[responseIndex++] = ntripClient->read();
+
+        if (httpStatusCode == 0)
+        {
+            if (strstr(response, static_cast<const char *>("200")) != nullptr)
+            {
+                httpStatusCode = httpCheck;
+            }
+            if (strstr(response, static_cast<const char *>("401")) != nullptr)
+            {
+                httpStatusCode = httpError;
+            }
+        }
+    }
+    response[responseIndex] = '\0';
+
+    // std::cout << "Caster response: " << response << std::endl;
+
+    if (httpStatusCode != httpCheck)
+    {
+        std::cout << "Failed to connect to " << this->host << " - HTTP Code: " << (int)httpStatusCode
+                  << " Length of Response: " << responseIndex << std::endl;
+
+        if (httpStatusCode == 0)
+        {
+            std::cout << "Response: " << response << std::endl;
+        }
+        else if (httpStatusCode == httpError)
+        {
+            std::cout << "Statuscode 401 - Unauthorized" << std::endl;
+        }
+        return false;
+    }
+
+    std::cout << "Connected to: " << this->host << std::endl;
+    this->lastReceivedRtcmTime = millis();
+    return true;
+}
+
+void NTRIPClient::closeConnection()
+{
+    if (static_cast<bool>(this->ntripClient->connected()))
+    {
+        this->ntripClient->stop();
+    }
+    this->activated = false;
+    std::cout << "NtripClient disconnected from: " << this->host << std::endl;
+}
+
+bool NTRIPClient::processConnection()
+{
+    if (static_cast<bool>(this->ntripClient->connected()))
+    {
+        uint8_t rtcmData[this->bufferSize * 8];
+        uint16_t rtcmCount = 0;
+
+        while (static_cast<bool>(this->ntripClient->available()))
+        {
+            rtcmData[rtcmCount++] = ntripClient->read();
+            if (rtcmCount == sizeof(rtcmData))
+            {
+                break;
+            }
+        }
+
+        if (rtcmCount > 0)
+        {
+            this->lastReceivedRtcmTime = millis();
+            this->gnss->pushRawData(static_cast<uint8_t *>(rtcmData), rtcmCount);
+            // std::cout << "Pushed " << rtcmCount << " RTCM bytes to ZED." << std::endl;
+        }
+    }
+    else
+    {
+        std::cout << "Connection to " << this->host << " dropped!" << std::endl;
+        return false;
+    }
+
+    if (millis() - this->lastReceivedRtcmTime > this->timeOut)
+    {
+        std::cout << "RTCM timeout!" << std::endl;
+        return false;
+    }
+
+    return true;
+}
+
+void NTRIPClient::checkAutoReconnect()
+{
+    if (!this->autoReconnect)
+    {
+        return;
+    }
+
+    if (millis() - this->lastReconnectTime < this->reconnectDelayTime)
+    {
+        return;
+    }
+    this->lastReconnectTime = millis();
+
+    if (this->reconnectAttemps >= this->maxReconnectAttemps)
+    {
+        this->autoReconnect = false;
+        return;
+    }
+
+    this->activated = true;
+    this->reconnectAttemps++;
+}
+
+void NTRIPClient::pushGPGGA()
+{
+    if ((!this->transmitLocation || !this->activated) || !this->ntripClient->connected())
+    {
+        return;
+    }
+
+    if (millis() - this->lastGPGGAPushTime < this->pushGPGGATime)
+    {
+        return;
+    }
+    this->lastGPGGAPushTime = millis();
+
+    auto *data = new NMEA_GGA_data_t;
+    const uint8_t res = this->gnss->getLatestNMEAGPGGA(data);
+    if (res > 0) // 2 means fresh data
+    {
+        char positionUpdate[this->bufferSizePushGPGGA];
+        snprintf(static_cast<char *>(positionUpdate),
+                 this->bufferSizePushGPGGA,
+                 static_cast<const char *>("%s"),
+                 data->nmea);
+        this->ntripClient->write(positionUpdate, strlen(positionUpdate));
+        std::cout << "Position update: " << positionUpdate << std::endl;
+    }
+    else
+    {
+        std::cout << "NTRIPClient::pushGPGGA - No valid data to send position to caster." << std::endl;
+    }
+    delete data;
+}
+
+bool NTRIPClient::isConnected()
+{
+    return this->state == NTRIPClientStates::pushingData;
+}

lib/NtripClient/ntripClient.h

@@ -0,0 +1,138 @@
+/**
+ * @file ntripClient.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains the class NTRIPClient
+ * @version 0.1
+ * @date 2023-02-13
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+
+#ifndef NTRIP_CLIENT
+#define NTRIP_CLIENT
+
+#include <WiFiClient.h>
+#include <Arduino.h>
+#include <base64.h>
+#include <iostream>
+#include <SparkFun_u-blox_GNSS_Arduino_Library.h>
+
+#include "debugTimes.h"
+#include "component.h"
+
+/**
+ * @brief States for the state machine.
+ *
+ */
+enum NTRIPClientStates
+{
+    openingConnection,
+    pushingData,
+    closingConnection,
+    waiting,
+    notAvailable
+};
+
+/**
+ * @brief Ntrip Client
+ *
+ * This class can connect to a ntrip server to pull correction
+ * data and push it to a given gnss module. This module have to be compatible
+ * with the SparkFun u-blox GNSS Arduino Library.
+ */
+class NTRIPClient : public Component
+{
+public:
+    /**
+     * @brief Construct a new NTRIPClient object
+     *
+     * @param gnss The Gnss module
+     * @param host
+     * @param port
+     * @param mountPoint
+     * @param user
+     * @param password
+     */
+    NTRIPClient(SFE_UBLOX_GNSS *gnss, const char *host, uint16_t port, const char *mountPoint, const char *user = nullptr, const char *password = nullptr);
+    ~NTRIPClient();
+
+    /**
+     * @brief Configure the Gnss module to accept correction data
+     *
+     */
+    void gnssConfiguration();
+
+    /**
+     * @brief Activate or deactivate the location transmission
+     *
+     * Some server need the position of the Gnss module to send the
+     * right correction data.
+     *
+     * @param b
+     */
+    void setTransmitLocation(bool b) { this->transmitLocation = b; }
+
+    /**
+     * @brief Activate or deactivate the connection to the server.
+     *
+     * @param state
+     * @return true success
+     * @return false failure
+     */
+    bool setActivated(bool state);
+    void setAutoReconnect(bool state);
+    void useOutdatedNtripRev1 (bool rev1 = false ) { this->useNtripRev1 = rev1; }
+
+    bool isConnected();
+
+    /**
+     * @brief Get the Client State object
+     *
+     * Returns the state of the State machine
+     *
+     * @return NTRIPClientStates
+     */
+    NTRIPClientStates getClientState() { return this->state; }
+
+private:
+    void run() override;
+    void runAsChild() override;
+    void pushGPGGA();
+    bool beginClient();
+    void closeConnection();
+    bool processConnection();
+    void checkAutoReconnect();
+
+    SFE_UBLOX_GNSS *gnss;
+    WiFiClient *ntripClient;
+    NTRIPClientStates state = NTRIPClientStates::notAvailable;
+
+    bool transmitLocation = false;
+    bool activated = true;
+    bool autoReconnect = false;
+    bool useNtripRev1 = false;
+
+    uint8_t reconnectAttemps = 0;
+    uint16_t port;
+    uint32_t lastReceivedRtcmTime = 0;
+    // uint32_t lastNtripConnectTime = 0;  // can deleted?
+    uint32_t lastGPGGAPushTime = 0;
+    uint32_t lastReconnectTime = 0;
+
+    static const uint8_t connectionDataLength = 32;
+    char host[connectionDataLength];
+    char mountPoint[connectionDataLength];
+    char user[connectionDataLength];
+    char password[connectionDataLength];
+    const uint8_t maxReconnectAttemps = 10;
+    const uint16_t reconnectDelayTime = 5000;
+    const uint16_t timeOut = 10000;
+    const uint16_t bufferSize = 512;
+    const uint16_t bufferSizePushGPGGA = 128;
+    const uint16_t pushGPGGATime = 5000;
+
+    static constexpr uint8_t loopDelay = 20;
+};
+
+#endif

lib/Point/point.cpp

@@ -56,14 +56,29 @@ bool Point::operator==(const Point &rhs) const
 
 double Point::distanceTo(const Coordinates &point) const
 {
-    const Coordinates begin = this->coordinates;
-    const Coordinates end = point;
+    // Old implementation
+    // const Coordinates begin = this->coordinates;
+    // const Coordinates end = point;
 
-    const double lat = (begin.lat + end.lat) / 2 * ROUTE_DEGREE_TO_RADIANT;
-    const double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (begin.lat - end.lat);
-    const double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (begin.lon - end.lon);
+    // const double lat = (begin.lat + end.lat) / 2 * ROUTE_DEGREE_TO_RADIANT;
+    // const double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (begin.lat - end.lat);
+    // const double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (begin.lon - end.lon);
 
-    return sqrt(dx * dx + dy * dy);
+    // return sqrt(dx * dx + dy * dy);
+
+    double latStart = this->toRad(this->coordinates.lat);
+    double lonStart = this->toRad(this->coordinates.lon);
+    double latEnd = this->toRad(point.lat);
+    double lonEnd = this->toRad(point.lon);
+
+    const double earthRadius = 6371.0; //km
+    double cartesianX_1 = cos(latStart) * lonStart * earthRadius;
+    double cartesianY_1 = latStart * earthRadius;
+    double cartesianX_2 = cos(latEnd) * lonEnd * earthRadius;
+    double cartesianY_2 = latEnd * earthRadius;
+
+    return sqrt(pow(cartesianX_2 - cartesianX_1, 2)
+              + pow(cartesianY_2 - cartesianY_1, 2));
 }
 
 double Point::distanceTo(const Point &point) const
@@ -73,13 +88,24 @@ double Point::distanceTo(const Point &point) const
 
 int16_t Point::courseTo(const Coordinates &point) const
 {
-    const Coordinates begin = this->coordinates;
-    const Coordinates end = point;
+    // const Coordinates begin = this->coordinates;
+    // const Coordinates end = point;
 
-    const 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));
-    const double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT);
+    // const 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));
+    // const double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT);
 
-    return static_cast<int16_t>(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1;
+    // return static_cast<int16_t>(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1;
+
+    double latStart = this->toRad(this->coordinates.lat);
+    double lonStart = this->toRad(this->coordinates.lon);
+    double latEnd = this->toRad(point.lat);
+    double lonEnd = this->toRad(point.lon);
+
+    double y = sin(lonEnd - lonStart) * cos(latEnd);
+    double x = cos(latStart) * sin(latEnd) - sin(latStart) * cos(latEnd) * cos(lonEnd - lonStart);
+    double theta = atan2(y, x);
+
+    return static_cast<int16_t>(this->toDeg(theta)) % 360;
 }
 
 int16_t Point::courseTo(const Point &point) const

lib/Point/point.h

@@ -131,6 +131,17 @@ public:
 
 private:
     void init(uint32_t horizontalAccuracy, uint32_t creationTime);
+    double toRad (double degree) const
+    {
+        constexpr double radFactor = M_PI / 180.0;
+        return degree * radFactor;
+    };
+
+    double toDeg(double radiant) const
+    {
+        constexpr double degFactor = 180.0 / M_PI;
+        return radiant * degFactor;
+    };
 
     Accuracy accuracy = Accuracy::none;
     Coordinates coordinates{0, 0};

lib/Route/route.cpp

@@ -0,0 +1,107 @@
+/**
+ * @file route.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Implements the class Route and Point
+ * @version 0.1
+ * @date 2022-01-31
+ *
+ * @copyright Copyright (c) 2022
+ *
+ */
+
+#include "route.h"
+
+Route Route::routeCache = Route();
+
+void Route::addPointToRoute(Point point)
+{
+    this->points.push_back(point);
+}
+
+void Route::clear()
+{
+    this->points.clear();
+    this->currentPoint = 0;
+    this->started = false;
+}
+
+Point Route::startRoute()
+{
+    if (this->points.empty())
+    {
+        this->started = false;
+        return Point();
+    }
+
+    this->it = this->points.begin();
+    this->currentPoint = 1;
+    this->started = true;
+    return *this->it;
+}
+
+Point Route::endRoute()
+{
+    if (this->points.empty())
+    {
+        this->started = false;
+        return Point();
+    }
+
+    this->it = this->points.end();
+    // TODO: Understand why i have to decrement the iterator first to get realy the last element.
+    this->it--;
+    this->currentPoint = this->points.size();
+    this->started = true;
+    return *this->it;
+}
+
+Point Route::getNextPoint()
+{
+    Point point;
+    if (!this->started)
+    {
+        return point;
+    }
+
+    if (this->it != --this->points.end())
+    {
+        this->it++;
+        this->currentPoint++;
+        return *this->it;
+        // } else if (this->it == this->points.end() && this->currentPoint != this->points.size()) {
+        //     this->currentPoint++;
+        //     return *this->it;
+    }
+
+    return point;
+}
+
+Point Route::getPreviousPoint()
+{
+    Point point;
+    if (!this->started)
+    {
+        return point;
+    }
+
+    if (this->it != this->points.begin())
+    {
+        this->it--;
+        this->currentPoint--;
+        return *this->it;
+    }
+
+    return point;
+}
+
+RouteInfo Route::getRouteInfo()
+{
+    RouteInfo info{0, 0};
+    info.totalPoints = this->points.size();
+    info.currentPoint = this->currentPoint;
+    return info;
+}
+
+Route& Route::getRouteCache() {
+    return Route::routeCache;
+}

lib/Route/route.h

@@ -0,0 +1,115 @@
+/**
+ * @file route.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains the class Route
+ * @version 0.1
+ * @date 2022-01-31
+ *
+ * @copyright Copyright (c) 2022
+ *
+ */
+
+#ifndef ROUTE_H
+#define ROUTE_H
+
+#include <cstdint>
+#include <list>
+
+#include "point.h"
+
+/**
+ * @brief Holds some route information
+ *
+ */
+struct RouteInfo
+{
+    /**
+     * @brief Selected number of Points
+     */
+    uint16_t currentPoint;
+
+    /**
+     * @brief Total points stored in route
+     */
+    uint16_t totalPoints;
+};
+
+/**
+ * @brief A class to manage multiple points
+ *
+ * The list of point presents a route which can be driven
+ */
+class Route
+{
+public:
+    /**
+     * @brief Adds a point to the list.
+     *
+     * @param point
+     */
+    void addPointToRoute(Point point);
+
+    /**
+     * @brief Delete all points
+     */
+    void clear();
+
+    /**
+     * @brief Select the first point as target.
+     *
+     * @return Point
+     */
+    Point startRoute();
+
+    /**
+     * @brief Select the last point as target.
+     *
+     * @return Point
+     */
+    Point endRoute();
+
+    /**
+     * @brief Get the next point and set it as target.
+     *
+     * @return Point is zero if there are no more Points.
+     */
+    Point getNextPoint();
+
+    /**
+     * @brief Get the previous point and set it as target.
+     *
+     * @return Point
+     */
+    Point getPreviousPoint();
+
+    /**
+     * @brief Get the Route Info object
+     *
+     * @return RouteInfo
+     */
+    RouteInfo getRouteInfo();
+
+    /**
+     * @brief Get the Started object
+     *
+     * The Route will be marked as started when startRoute or
+     * endRoute has been called.
+     *
+     * @return true
+     * @return false
+     */
+    bool getStarted() const { return this->started; }
+
+    static Route& getRouteCache();
+
+private:
+    uint16_t currentPoint = 0;
+    bool started = false;
+
+    std::list<Point> points;
+    std::list<Point>::iterator it;
+
+    static Route routeCache;
+};
+
+#endif // ROUTE_H

lib/Sensors/sensorData.cpp

@@ -20,6 +20,22 @@ SensorData::SensorData()
     Component::loopDelay = SensorData::loopDelay;
 }
 
+SensorData::~SensorData()
+{
+    delete this->ntripClient;
+}
+
+void SensorData::enableNtrip(String host, uint16_t port, String mountPoint, String user, String password, bool sendOwnPosition)
+{
+    this->ntripClient = new NTRIPClient(this->gnss, host.c_str(), port, mountPoint.c_str(), user.c_str(), password.c_str());
+    this->ntripClient->gnssConfiguration();
+    this->ntripClient->loop();
+    this->ntripClient->setTransmitLocation(sendOwnPosition);
+    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();
@@ -53,6 +69,7 @@ void SensorData::enableRealCompass()
     this->realCompass = new QMC5883LCompass();
     // Init Compass
     Wire.beginTransmission(address);
+    // TODO: describe Bytes !!!
     Wire.write(0x0b);
     Wire.write(0x01);
     Wire.endTransmission();
@@ -62,6 +79,69 @@ void SensorData::enableRealCompass()
     caliCompass.useData();
 }
 
+void SensorData::enableCalcCompass()
+{
+    // TODO: !!! implementieren
+}
+
+void SensorData::enableGyroscope()
+{
+    this->gyroscope = new MPU6050();
+    this->gyroscope->initialize();
+    if (!this->gyroscope->testConnection())
+    {
+        std::cout << "SensorData::enableGyroscope: Gyroscope is not connected. Freeze!" << std::endl;
+        while (true)
+        {
+        }
+    }
+
+    const uint8_t deviceStatus = this->gyroscope->dmpInitialize();
+
+    // TODO: !!! MagicNumer 6x
+    this->gyroscope->setXGyroOffset(220);
+    this->gyroscope->setYGyroOffset(76);
+    this->gyroscope->setZGyroOffset(-85);
+    this->gyroscope->setZAccelOffset(1788);
+
+    if (deviceStatus == 0)
+    {
+        this->gyroscope->CalibrateAccel(6);
+        this->gyroscope->CalibrateGyro(6);
+        // this->gyroscope->PrintActiveOffsets();
+        this->gyroscope->setDMPEnabled(true);
+    }
+    else
+    {
+        // ERROR!
+        // 1 = initial memory load failed
+        // 2 = DMP configuration updates failed
+        // (if it's going to break, usually the code will be 1)
+        std::cout << "SensorData::enableGyroscope: DMP Initialization failed (code" << static_cast<int>(deviceStatus) << "). Freeze!" << std::endl;
+        while (true)
+        {
+        }
+    }
+}
+
+CalcAzimuth::State SensorData::getCalcAzimuthState() const
+{
+    if (static_cast<bool>(this->calcCompass))
+    {
+        return this->calcCompass->getState();
+    }
+    return CalcAzimuth::State::Invalid;
+}
+
+NTRIPClientStates SensorData::getNtripState() const
+{
+    if (static_cast<bool>(this->ntripClient))
+    {
+        return this->ntripClient->getClientState();
+    }
+    return NTRIPClientStates::notAvailable;
+}
+
 void SensorData::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct)
 {
     static constexpr uint8_t stringSize = 32;
@@ -156,6 +236,13 @@ void SensorData::run()
         this->realCompass->read();
         this->realAzimuth = this->realCompass->getAzimuth();
     }
+
+    if (static_cast<bool>(this->gyroscope) && this->gyroscope->dmpGetCurrentFIFOPacket(static_cast<uint8_t *>(this->gyroBuffer)))
+    {
+        this->gyroscope->dmpGetQuaternion(&this->quaternion, static_cast<uint8_t *>(this->gyroBuffer));
+        this->gyroscope->dmpGetGravity(&this->gravity, &this->quaternion);
+        this->gyroscope->dmpGetYawPitchRoll(static_cast<float *>(this->yawPitchRoll), &this->quaternion, &this->gravity);
+    }
 }
 
 void SensorData::runAsChild()
@@ -178,7 +265,7 @@ void SensorData::initGnss()
     std::cout << "u-blox protocol version: " << static_cast<int>(versionHigh) << "." << static_cast<int>(versionLow) << std::endl;
 
     this->gnss->setSPIOutput(COM_TYPE_UBX);
-    this->gnss->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI, 10);
+    this->gnss->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI);
     this->gnss->setUSBOutput(COM_TYPE_UBX | COM_TYPE_NMEA);
     this->gnss->setAutoPVTcallbackPtr(&(SensorData::savePVTdata));
     // SensorData::setOutputStatusPrintPVTdata(true);

lib/Sensors/sensorData.h

@@ -13,6 +13,7 @@
 #define SENSOR_DATA_H
 
 #include <SPI.h>
+#include <I2Cdev.h>
 #include <iostream>
 
 #include "component.h"
@@ -20,6 +21,9 @@
 
 #include <SparkFun_u-blox_GNSS_Arduino_Library.h>
 #include <QMC5883LCompass.h>
+#include <MPU6050_6Axis_MotionApps612.h>
+#include "calcAzimuth.h"
+#include "ntripClient.h"
 
 #include "point.h"
 
@@ -34,6 +38,8 @@ public:
     SensorData();
     ~SensorData();
 
+    void enableNtrip(String host, uint16_t port, String mountPoint, String user, String password, bool sendOwnPosition = false);
+
     /**
      * @brief Enable the gnss module over spi
      *
@@ -49,6 +55,7 @@ public:
 
     void enableRealCompass();
     void enableCalcCompass();
+    void enableGyroscope();
 
     // Interface Const
     /**
@@ -58,8 +65,45 @@ public:
      */
     int16_t getRealAzimuth() const { return this->realAzimuth; }
 
+    /**
+     * @brief Get the azimuth calculated by CalcAzimuth
+     *
+     * Consider to call getCalcAzimuthState() to check, if the data is valid.
+     *
+     * @return int16_t
+     */
+    int16_t getCalcAzimuth() const { return this->calcAzimuth; }
+
+    /**
+     * @brief Get the CalcAzimuth::State object
+     *
+     * Needed to check the quality of calculated azimuth
+     *
+     * @return CalcAzimuth::State
+     */
+    CalcAzimuth::State getCalcAzimuthState() const;
+
     Point getCurrentPos() const { return this->currentPosition; }
     const UBX_NAV_PVT_data_t *getGnssData() const { return this->gnssData; };
+    NTRIPClientStates getNtripState() const;
+
+    /**
+     * @brief Get the data from the gyroscope
+     *
+     * The returned float pointer is an array of 3 floats
+     * - Yaw
+     * - Pitch
+     * - Roll
+     *
+     * @return const float*
+     */
+    const float *getGyroData() const { return this->yawPitchRoll; }
+
+    /**
+     * @brief Get the CalcCompass object
+     * @return CalcAzimuth*
+     */
+    CalcAzimuth *getCalcCompass() const { return this->calcCompass; }
 
     /**
      * @brief Get the RealCompass object
@@ -67,6 +111,18 @@ public:
      */
     QMC5883LCompass *getRealCompass() const { return this->realCompass; }
 
+    /**
+     * @brief Get the NTRIPClient object
+     * @return NTRIPClient*
+     */
+    NTRIPClient *getNtripClient() const { return this->ntripClient; }
+
+    /**
+     * @brief Get the Gyroscope object
+     * @return MPU6050*
+     */
+    MPU6050 *getGyroscope() const { return this->gyroscope; }
+
     // static
     /**
      * @brief Set the output status for PVTdata.
@@ -85,14 +141,31 @@ private:
     void updateUbxData();
 
     QMC5883LCompass *realCompass = nullptr;
+    CalcAzimuth *calcCompass = nullptr;
     SFE_UBLOX_GNSS *gnss = nullptr;
+    NTRIPClient *ntripClient = nullptr;
+    MPU6050 *gyroscope = nullptr;
 
     UBX_NAV_PVT_data_t *gnssData = nullptr;
     Point currentPosition;
+    Quaternion quaternion;
+    VectorFloat gravity;
 
+    char *host = nullptr;
+    char *mountPoint = nullptr;
+    char *user = nullptr;
+    char *password = nullptr;
+
+    bool isNtripInit = false;
+
+    uint8_t gyroBuffer[64];
+    uint16_t port = 0;
     int16_t realAzimuth = INT16_MAX;
+    int16_t calcAzimuth = INT16_MAX;
     uint32_t lastUbxUpdate = 0;
 
+    float yawPitchRoll[3]{0, 0, 0};
+
     // static
     static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
     static void savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);

platformio.ini

@@ -15,14 +15,17 @@ board_build.partitions = no_ota.csv
 framework = arduino
 monitor_speed = 115200
 upload_speed = 921600
-monitor_port = COM6
+; monitor_port = COM6
 lib_deps = 
 	https://git.kleiax.de/PlatformIO-Libs/SparkFunGNSS.git
+	https://git.kleiax.de/PlatformIO-Libs/Menu.git
 	knolleary/PubSubClient@^2.8
 	br3ttb/PID@^1.2.1
 	marcoschwartz/LiquidCrystal_I2C@^1.1.4
+	bblanchon/ArduinoJson@^6.20.0
 	mprograms/QMC5883LCompass@^1.2.0
-upload_port = COM6
+	jrowberg/I2Cdevlib-MPU6050@^1.0.0
+; upload_port = COM6
 test_ignore = test_desktop
 build_type = debug
 monitor_filters = esp32_exception_decoder
@@ -40,3 +43,10 @@ test_ignore = test_embedded
 
 [platformio]
 description = A Rover who should be drive a route by gps.
+
+
+; Bei Möglichkeit folgendes aus clang tidy entfernen
+; cppcoreguidelines-owning-memory
+; 
+; 
+; 

src/SpecialMenus/Route/menuRoute.cpp

@@ -0,0 +1,375 @@
+/**
+ * @file menuRoute.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains the implementation of the classes MenuActionRoute, MenuRoute and MenuRouteWrapper.
+ * @version 0.1
+ * @date 2022-12-28
+ *
+ * @copyright Copyright (c) 2022
+ *
+ */
+
+#include "menuRoute.h"
+
+MenuActionRoute::MenuActionRoute(MenuRoute *menuRoute, DataFunction dataFunction)
+    : menuRoute{menuRoute}, dataFunction{dataFunction}
+{
+}
+
+void MenuActionRoute::action()
+{
+    (this->menuRoute->*this->dataFunction)(0);
+}
+
+MenuRoute::MenuRoute(Route *route)
+    : route{route}
+{
+}
+
+MenuRoute::~MenuRoute()
+{
+    if (isInit)
+    {
+        delete this->mainMenu;
+    }
+}
+
+void MenuRoute::printMenu()
+{
+    if (!this->isInit)
+    {
+        this->isInit = true;
+        this->init();
+    }
+
+    this->mainMenu->printMenu();
+}
+
+void MenuRoute::down()
+{
+    if (blockInput)
+    {
+        return;
+    }
+    this->mainMenu->down();
+}
+
+void MenuRoute::up()
+{
+    if (blockInput)
+    {
+        return;
+    }
+    this->mainMenu->up();
+}
+
+void MenuRoute::right()
+{
+    if (blockInput)
+    {
+        return;
+    }
+    this->mainMenu->right();
+}
+
+void MenuRoute::left()
+{
+    if (blockInput)
+    {
+        return;
+    }
+    if (this->mainMenu->isInSubmenu())
+    {
+        this->mainMenu->left();
+    }
+    else
+    {
+        this->parentMenu->printMenu();
+    }
+}
+
+void MenuRoute::yes()
+{
+    if (blockInput)
+    {
+        return;
+    }
+    this->mainMenu->yes();
+}
+
+void MenuRoute::no()
+{
+    if (blockInput)
+    {
+        return;
+    }
+    if (this->mainMenu->isInSubmenu())
+    {
+        this->mainMenu->no();
+    }
+    else
+    {
+        this->left();
+    }
+}
+
+void MenuRoute::init()
+{
+    // auto dummy = []() {
+    //     std::cout << "Dummy in Action" <<std::endl;
+    // };
+
+    this->mainMenu = new Menu;
+    auto *pointsMenu = new MenuRoutePoints(this->route);
+
+    this->mainMenu->setLcd(this->lcd);
+    pointsMenu->setLcd(this->lcd);
+
+    auto *importWrapper = new MenuIntInput(1, new MenuRouteWrapper(this, &MenuRoute::importRoute));
+    auto *exportWrapper = new MenuIntInput(1, new MenuRouteWrapper(this, &MenuRoute::exportRoute));
+    auto *deleteWrapper = new MenuIntInput(1, new MenuRouteWrapper(this, &MenuRoute::deleteRoute));
+    auto *clearWrapper = new MenuActionRoute(this, &MenuRoute::clearRoute);
+
+    importWrapper->setLcd(this->lcd);
+    importWrapper->setMinMax(0, MenuRoute::maxRouteNumber);
+    importWrapper->setPrintParentMenu(false);
+    importWrapper->setEntry(0, "Import Route");
+
+    exportWrapper->setLcd(this->lcd);
+    exportWrapper->setMinMax(0, MenuRoute::maxRouteNumber);
+    exportWrapper->setPrintParentMenu(false);
+    exportWrapper->setEntry(0, "Export Route");
+
+    deleteWrapper->setLcd(this->lcd);
+    deleteWrapper->setMinMax(0, MenuRoute::maxRouteNumber);
+    deleteWrapper->setPrintParentMenu(false);
+    deleteWrapper->setEntry(0, "Delete Route");
+
+    this->mainMenu->addEntry(new MenuAction("Points", pointsMenu));
+    this->mainMenu->addEntry(new MenuAction("Clear", clearWrapper));
+    this->mainMenu->addEntry(new MenuAction("Import", importWrapper));
+    this->mainMenu->addEntry(new MenuAction("Export", exportWrapper));
+    this->mainMenu->addEntry(new MenuAction("Delete", deleteWrapper));
+}
+
+void MenuRoute::importRoute(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);
+    const int httpResponseCode = http.GET();
+
+    lineOne = "Import complete";
+    lineTwo = "Code: ";
+
+    if (httpResponseCode == MenuRoute::httpValidImport)
+    {
+        deserializeJson(doc, http.getStream());
+    }
+    else
+    {
+        lineOne = "HTTP Error";
+    }
+
+    lineTwo.concat(httpResponseCode);
+    this->print(lineOne, lineTwo);
+
+    if (httpResponseCode <= 0)
+    {
+        this->blockInput = false;
+        return;
+    }
+
+    uint16_t totalPoints = doc["amountPoints"];
+
+    this->route->clear();
+    for (uint16_t i = 0; i < totalPoints; i++)
+    {
+        Point::Coordinates coords;
+        coords.lat = doc["points"][i][0].as<double>();
+        coords.lon = doc["points"][i][1].as<double>();
+        this->route->addPointToRoute(Point(coords, true));
+    }
+
+    this->blockInput = false;
+    http.end();
+}
+
+void MenuRoute::exportRoute(uint8_t routeNumber)
+{
+    this->blockInput = true;
+    uint16_t totalPoints = this->route->getRouteInfo().totalPoints;
+
+    String lineOne = "";
+    String lineTwo = "";
+
+    if (WiFiSTAClass::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;
+    }
+
+    DynamicJsonDocument doc(JSON_DOCUMENT_SIZE_ROUTE);
+    doc["number"] = routeNumber;
+    doc["amountPoints"] = totalPoints;
+
+    Point::Coordinates coords = this->route->startRoute().getCoordinates();
+    doc["points"][0][0] = coords.lat;
+    doc["points"][0][1] = coords.lon;
+    for (uint16_t i = 1; i < totalPoints; i++)
+    {
+        coords = this->route->getNextPoint().getCoordinates();
+        doc["points"][i][0] = coords.lat;
+        doc["points"][i][1] = coords.lon;
+    }
+    String jsonData;
+    serializeJson(doc, jsonData);
+
+    WiFiClient client;
+    HTTPClient http;
+
+    http.begin(client, "http://rover.kleiax.de/api/");
+    http.addHeader("Content-Type", "application/json");
+    const int httpResponseCode = http.POST(jsonData);
+
+    if (httpResponseCode == MenuRoute::httpValidExport)
+    {
+        lineOne = "Export complete";
+    }
+    else
+    {
+        lineOne = "HTTP Error";
+    }
+    lineTwo = "Code: ";
+    lineTwo.concat(httpResponseCode);
+    this->print(lineOne, lineTwo);
+    this->blockInput = false;
+
+    http.end();
+}
+
+void MenuRoute::deleteRoute(uint8_t routeNumber)
+{
+    this->blockInput = true;
+
+    String lineOne = "";
+    String lineTwo = "";
+
+    if (WiFiSTAClass::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);
+    const int httpResponseCode = http.sendRequest("DELETE");
+
+    lineOne = "Delete complete";
+    lineTwo = "Code: ";
+
+    if (httpResponseCode == MenuRoute::httpValidDelete)
+    {
+        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)
+{
+    // The parameter exist only for compability
+    none;
+    this->route->clear();
+    this->print("Currente route", "deleted...");
+}
+
+MenuRouteWrapper::MenuRouteWrapper(MenuRoute *menuRoute, DataFunction dataFunction)
+    : menuRoute{menuRoute}, dataFunction{dataFunction}
+{
+}
+
+void MenuRouteWrapper::action(int16_t *values, uint8_t length)
+{
+    if (length < 1)
+    {
+        std::cout << "Error in MenuRouteWrapper::action" << std::endl;
+        return;
+    }
+
+    if (values[0] < 0)
+    {
+        values[0] = 0;
+    }
+
+    if (values[0] > UINT8_MAX)
+    {
+        values[0] = UINT8_MAX;
+    }
+
+    (this->menuRoute->*this->dataFunction)(values[0]);
+}

src/SpecialMenus/Route/menuRoute.h

@@ -0,0 +1,173 @@
+/**
+ * @file menuRoute.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains a classes to handles routes with the user input.
+ * @version 0.1
+ * @date 2022-12-28
+ *
+ * @copyright Copyright (c) 2022
+ *
+ */
+
+#ifndef MENU_ROUTE_H
+#define MENU_ROUTE_H
+
+#include <Arduino.h>
+#include <ArduinoJson.h>
+#include <WiFi.h>
+#include <HTTPClient.h>
+
+#include "route.h"
+#include "menu.h"
+#include "menuControl.h"
+#include "menuIntInput.h"
+#include "menuRoutePoints.h"
+
+#define JSON_DOCUMENT_SIZE_ROUTE 4096
+
+class MenuRoute;
+typedef void (MenuRoute::*DataFunction)(uint8_t);
+
+/**
+ * @brief MenuActionWrapper to call DataFunctions
+ *
+ * This class should be used to call importRoute,
+ * exportRoute and clearRoute over the Menu.
+ */
+class MenuActionRoute : public MenuActionWrapper
+{
+public:
+    /**
+     * @brief Construct a new Menu Action Route object
+     *
+     * @param menuRoute
+     * @param dataFunction like exportRoute
+     */
+    MenuActionRoute(MenuRoute *menuRoute, DataFunction dataFunction);
+
+    /**
+     * @brief Runs the given DataFunction
+     */
+    void action() override;
+
+private:
+    MenuRoute *menuRoute;
+    DataFunction dataFunction;
+};
+
+/**
+ * @brief Handles routes
+ *
+ * With this menu the user can import, export and delete routes.
+ */
+class MenuRoute : public MenuControl
+{
+public:
+    /**
+     * @brief Construct a new Menu Route object
+     *
+     * @param route
+     */
+    MenuRoute(Route *route);
+    ~MenuRoute();
+
+    /**
+     * @brief Prints the last informations
+     *
+     * On first call this function calls the init function.
+     * On every call this functions call the printMenu function from
+     * the mainMenu of this class.
+     */
+    void printMenu() override;
+
+    /**
+     * @name User Inputs
+     * @brief Inputs given by the parentMenu
+     */
+    ///@{
+    void down() override;
+    void up() override;
+    void right() override;
+    void left() override;
+    void yes() override;
+    void no() override;
+    ///@}
+
+    /**
+     * @brief Import a Route.
+     *
+     * The functions tries to pull the given route id from the RoverApi.
+     *
+     * @param routeNumber id
+     */
+    void importRoute(uint8_t routeNumber);
+
+    /**
+     * @brief Export a Route.
+     *
+     * The functions tries to push the current route to the RoverApi.
+     *
+     * @param routeNumber id
+     */
+    void exportRoute(uint8_t routeNumber);
+
+    /**
+     * @brief Delete a Route
+     *
+     * The functions tries to delete the current route from the RoverApi.
+     *
+     * @param routeNumber
+     */
+    void deleteRoute(uint8_t routeNumber);
+
+    /**
+     * @brief Delete the current Route.
+     *
+     * @param none this param is not be used.
+     */
+    void clearRoute(uint8_t none);
+
+private:
+    void init();
+
+    Route *route;
+    Menu *mainMenu = nullptr;
+
+    bool isInit = false;
+    bool blockInput = false;
+
+    static constexpr uint8_t maxRouteNumber = 100;
+    static constexpr uint16_t httpValidExport = 201;
+    static constexpr uint16_t httpValidImport = 202;
+    static constexpr uint16_t httpValidDelete = 202;
+};
+
+/**
+ * @brief MenuRouteWrapper to call DataFunctions with numeric user input
+ *
+ */
+class MenuRouteWrapper : public MenuIntInputWrapper
+{
+public:
+    /**
+     * @brief Construct a new Menu Route Wrapper object.
+     *
+     * @param menuRoute
+     * @param dataFunction
+     */
+    MenuRouteWrapper(MenuRoute *menuRoute, DataFunction dataFunction);
+
+    /**
+     * @brief Calls the given DataFunction.
+     *
+     * @param values The value to be given to the DataFunction.
+     * @param length Number of values, should be 1.
+     */
+    void action(int16_t *values, uint8_t length) override;
+
+private:
+    MenuRoute *menuRoute;
+    DataFunction dataFunction = nullptr;
+};
+
+#endif // MENU_ROUTE_H

src/SpecialMenus/Route/menuRoutePoints.cpp

@@ -0,0 +1,101 @@
+/**
+ * @file menuRoutePoints.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains the implementation of the class MenuRoutePoints.
+ * @version 0.1
+ * @date 2022-12-28
+ *
+ * @copyright Copyright (c) 2022
+ *
+ */
+
+#include "menuRoutePoints.h"
+
+MenuRoutePoints::MenuRoutePoints(Route *route)
+    : route{route}
+{
+}
+
+void MenuRoutePoints::init()
+{
+    const uint16_t amountPoints = this->route->getRouteInfo().totalPoints;
+    if (amountPoints > UINT8_MAX)
+    {
+        this->error = true;
+    }
+    else
+    {
+        this->setCountPages(amountPoints);
+    }
+}
+
+void MenuRoutePoints::printPage() const
+{
+    static constexpr int8_t width = 9;
+    static constexpr int8_t prec = 7;
+    static constexpr int8_t bufSize = 20;
+
+    const uint8_t currentPage = this->getCurrentPage();
+    if (this->lastPageNumber == currentPage)
+    {
+        return;
+    }
+
+    String lineOne = "N ";
+    String lineTwo = "E ";
+
+    if (this->error)
+    {
+        lineOne = "Error: To much";
+        lineTwo = "points are given";
+        this->print(lineOne, lineTwo);
+        return;
+    }
+
+    RouteInfo info = this->route->getRouteInfo();
+    if (info.totalPoints == 0)
+    {
+        lineOne = "No Points are";
+        lineTwo = "available";
+        this->print(lineOne, lineTwo);
+        return;
+    }
+
+    Point point;
+    if (currentPage == 0)
+    {
+        point = this->route->startRoute();
+    }
+    else if (this->lastPageNumber - 1 == currentPage)
+    {
+        point = this->route->getPreviousPoint();
+    }
+    else if (this->lastPageNumber + 1 == currentPage)
+    {
+        point = this->route->getNextPoint();
+    }
+    else if (currentPage == this->getCountPages() - 1)
+    {
+        point = this->route->endRoute();
+    }
+    else
+    {
+        std::cout << "Error in: MenuRoutePoints::printPage()" << std::endl;
+    }
+
+    char buf[bufSize];
+    char *string;
+
+    info = this->route->getRouteInfo();
+    string = dtostrf(point.getLatitude(), width, prec, static_cast<char *>(buf));
+    lineOne.concat(string);
+    lineOne.concat(" ");
+    lineOne.concat(info.currentPoint);
+    // lineOne.concat(currentPage);
+    string = dtostrf(point.getLongitude(), width, prec, static_cast<char *>(buf));
+    lineTwo.concat(string);
+    lineTwo.concat(" ");
+    lineTwo.concat(info.totalPoints);
+    // std::cout << "MenuRoutePoints::printPage currentPage: " << (int) currentPage << std::endl;
+    this->print(lineOne, lineTwo);
+}

src/SpecialMenus/Route/menuRoutePoints.h

@@ -0,0 +1,50 @@
+/**
+ * @file menuRoutePoints.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains a class to print the coordinates of a Route.
+ * @version 0.1
+ * @date 2022-12-28
+ *
+ * @copyright Copyright (c) 2022
+ *
+ */
+
+#ifndef MENU_ROUTE_POINTS_H
+#define MENU_ROUTE_POINTS_H
+
+#include "navigation.h"
+#include "route.h"
+#include "menuInformationSites.h"
+
+/**
+ * @brief Prints coordinates of Route side by side.
+ *
+ */
+class MenuRoutePoints : public MenuInformationSites
+{
+public:
+    /**
+     * @brief Construct a new Menu Route Points object
+     *
+     * @param route
+     */
+    MenuRoutePoints(Route *route);
+
+    /**
+     * @brief Configure the base class MenuInformationSites
+     *
+     * Set the amount of pages.
+     */
+    void init() override;
+
+    /**
+     * @brief Prints the the coordinates of the current Point.
+     */
+    void printPage() const override;
+
+private:
+    Route *route;
+    bool error = false;
+};
+
+#endif // MENU_ROUTE_POINTS_H

src/SpecialMenus/SensorData/menuSensorData.cpp

@@ -31,11 +31,27 @@ void MenuSensorData::printPage() const
     switch (this->getCurrentPage())
     {
     case 0:
+        lineOne = " Y     P     R :";
+        lineTwo.concat(this->sensorData->getGyroData()[0]);
+        lineTwo.concat(" ");
+        lineTwo.concat(this->sensorData->getGyroData()[1]);
+        lineTwo.concat(" ");
+        lineTwo.concat(this->sensorData->getGyroData()[2]);
+        break;
+
+    case 1:
         lineOne = "Real Azimuth:";
         lineTwo.concat(this->sensorData->getRealAzimuth());
         break;
 
-    case 1:
+    case 2:
+        lineOne = "Calc Azimuth:";
+        lineTwo.concat(this->sensorData->getCalcAzimuth());
+        lineTwo.concat(" ");
+        lineTwo.concat(CalcAzimuth::stateToString(this->sensorData->getCalcAzimuthState()));
+        break;
+
+    case 3:
     {
         lineOne = "Lat:";
         lineTwo = "Lon:";
@@ -45,7 +61,7 @@ void MenuSensorData::printPage() const
     }
     break;
 
-    case 2:
+    case 4:
         lineOne = "Time: ";
         lineTwo = "";
         if (static_cast<bool>(fixType))
@@ -75,7 +91,44 @@ void MenuSensorData::printPage() const
 
         break;
 
-    case 3:
+    case 5:
+    {
+        lineOne = "CarSol: ";
+        lineTwo = "Ntrip: ";
+        const uint8_t carrSoln = gpsData->flags.bits.carrSoln;
+        if (carrSoln == 0)
+        {
+            lineOne.concat("None");
+        }
+        else if (carrSoln == 1)
+        {
+            lineOne.concat("Floating");
+        }
+        else if (carrSoln == 2)
+        {
+            lineOne.concat("Fixed");
+        }
+        else
+        {
+            lineOne = "UNKNOWN";
+        }
+        const NTRIPClientStates status = this->sensorData->getNtripState();
+        if (status == NTRIPClientStates::pushingData)
+        {
+            lineTwo.concat("enabled");
+        }
+        else if (status == NTRIPClientStates::notAvailable)
+        {
+            lineTwo.concat(" N/A");
+        }
+        else
+        {
+            lineTwo.concat("disabled");
+        }
+    }
+    break;
+
+    case 6:
         lineOne = "HAcc: ";
         lineTwo = "Sats: ";
         if (static_cast<bool>(fixType))
@@ -97,3 +150,28 @@ void MenuSensorData::printPage() const
 
     this->print(lineOne, lineTwo);
 }
+
+void MenuSensorData::runCommand()
+{
+    switch (this->getCurrentPage())
+    {
+    case 5:
+        {
+            NTRIPClient* client = this->sensorData->getNtripClient();
+            NTRIPClientStates state = client->getClientState();
+            if (state == NTRIPClientStates::pushingData)
+            {
+                client->deactivate();
+            }
+            else if (state == NTRIPClientStates::waiting)
+            {
+                client->activate();
+            }
+        }
+        break;
+        
+    default:
+        
+        break;
+    }
+}

src/SpecialMenus/SensorData/menuSensorData.h

@@ -30,6 +30,7 @@ public:
 
 private:
     void printPage() const override;
+    void runCommand() override;
 
     SensorData *sensorData;
 };

src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp

@@ -0,0 +1,308 @@
+/**
+ * @file menuAutopilot.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains an implementation of the class MenuAutopilot
+ * @version 0.1
+ * @date 2022-02-03
+ *
+ * @copyright Copyright (c) 2022
+ *
+ */
+
+#include "menuAutopilot.h"
+
+void MenuAutopilot::printPage() const
+{
+    const CourseCorrection correction = this->autopilot->getCourseCorrection();
+    const UBX_NAV_PVT_data_t *gpsData = this->autopilot->getSensorData()->getGnssData();
+    const bool navigationStarted = this->autopilot->getState() >= Autopilot::State::NavigationStarted;
+
+    String distanceString = "";
+    if (navigationStarted)
+    {
+        uint16_t distance = 0;
+        if (correction.distance < 1)
+        {
+            distance = static_cast<uint16_t>(correction.distance * 100);
+            distanceString.concat(distance);
+            distanceString.concat("cm");
+        }
+        else if (correction.distance < 1000)
+        {
+            distance = static_cast<uint16_t>(correction.distance);
+            distanceString.concat(distance);
+            distanceString.concat("m");
+        }
+        else
+        {
+            distance = static_cast<uint16_t>(correction.distance / 1000);
+            distanceString.concat(distance);
+            distanceString.concat("km");
+        }
+    }
+
+    String lineOne = "No information";
+    String lineTwo = "available";
+
+    switch (this->getCurrentPage())
+    {
+    case 0:
+    {
+        lineOne = "Status: ";
+        lineTwo = "";
+        switch (this->autopilot->getState())
+        {
+        case Autopilot::State::InsufficientAccuracy:
+            lineTwo = "Err: LowAccuracy";
+            break;
+
+        case Autopilot::State::NoRoute:
+            lineTwo = "Err: No route";
+            break;
+
+        case Autopilot::State::NavigationStarted:
+            lineTwo = "Nav started";
+            break;
+
+        case Autopilot::State::GetToStartPoint:
+            lineTwo = "Drive to start";
+            break;
+
+        case Autopilot::State::SelfDrivingAvailable:
+            lineTwo = "Autopilot ready";
+            break;
+
+        case Autopilot::State::SelfDriving:
+            lineTwo = "Autopilot active";
+            break;
+
+        case Autopilot::State::SelfDrivingRotate:
+            lineTwo = "Rotating";
+            break;
+
+        case Autopilot::State::TargetReached:
+            lineTwo = "Target reached";
+            break;
+
+        default:
+            lineTwo = "UNKOWN - ";
+            lineTwo.concat(static_cast<int>(this->autopilot->getState()));
+
+            break;
+        }
+        break;
+    }
+
+    case 1:
+        if (!navigationStarted)
+        {
+            lineOne = "Navigation is";
+            lineTwo = "not started";
+            break;
+        }
+        lineOne = "Distance: ";
+        lineOne.concat(distanceString);
+        lineTwo = "Turn: ";
+        lineTwo.concat(correction.correction);
+        break;
+
+    case 2:
+        lineOne = "Target waypoint";
+        lineTwo = "";
+        lineTwo.concat(this->autopilot->getRouteInfo().currentPoint);
+        lineTwo.concat(" from ");
+        lineTwo.concat(this->autopilot->getRouteInfo().totalPoints);
+        break;
+
+    case 3:
+    {
+        NTRIPClientStates status = this->autopilot->getSensorData()->getNtripClient()->getClientState();
+        const uint8_t carrSoln = gpsData->flags.bits.carrSoln;
+        lineOne = "GNSS: ";
+
+        if (status == NTRIPClientStates::pushingData)
+        {
+            if (carrSoln == 0)
+            {
+                lineOne.concat("None");
+            }
+            else if (carrSoln == 1)
+            {
+                lineOne.concat("Floating");
+            }
+            else if (carrSoln == 2)
+            {
+                lineOne.concat("Fixed");
+            }
+            else
+            {
+                lineOne.concat("UNKNOWN");
+            }
+        }
+        else if (status == NTRIPClientStates::notAvailable)
+            lineOne.concat("No WiFi");
+        else
+        {
+            lineOne.concat("Offline");
+        }
+
+        lineTwo = "hAcc: ";
+        if (static_cast<bool>(gpsData->fixType))
+        {
+            lineTwo.concat(gpsData->hAcc);
+        }
+        else
+        {
+            lineTwo.concat("0");
+        }
+        break;
+    }
+
+    case 4:
+
+        if (this->autopilot->getSensorData()->getCalcAzimuth() == CalcAzimuth::State::Good || this->autopilot->getSensorData()->getCalcAzimuth() == CalcAzimuth::State::Super)
+        {
+            lineOne = "Calc Azi: ";
+            lineTwo = "State: ";
+            lineOne.concat(this->autopilot->getSensorData()->getCalcAzimuth());
+            switch (this->autopilot->getSensorData()->getCalcAzimuthState())
+            {
+            case CalcAzimuth::State::Bad:
+                lineTwo.concat("Bad");
+                break;
+
+            case CalcAzimuth::State::Good:
+                lineTwo.concat("Good");
+                break;
+
+            case CalcAzimuth::State::Invalid:
+                lineTwo.concat("Invalid");
+                break;
+
+            case CalcAzimuth::State::Ok:
+                lineTwo.concat("Ok");
+                break;
+
+            case CalcAzimuth::State::Super:
+                lineTwo.concat("Super");
+                break;
+
+            default:
+                lineTwo.concat("Unkown");
+                break;
+            }
+        }
+        else
+        {
+            lineOne = "Real Azi: ";
+            lineTwo = "";
+            lineOne.concat(this->autopilot->getSensorData()->getRealAzimuth());
+        }
+        break;
+
+    case 5:
+        lineOne = "Loop mode is";
+        if (this->autopilot->getLoopMode())
+        {
+            lineTwo = "enabled";
+        }
+        else
+        {
+            lineTwo = "disabled";
+        }
+        break;
+
+    case 6:
+        lineOne = "Freeze target is";
+        if (this->targetFreezed)
+        {
+            lineTwo = "activated";
+        }
+        else
+        {
+            lineTwo = "deactivated";
+        }
+        break;
+
+    case 7:
+        lineOne = "MinDisToPoint:";
+        lineTwo = "<- ";
+        lineTwo.concat(this->minDistance);
+        lineTwo.concat(" ->");
+        break;
+
+    default:
+        this->printDefault();
+        return;
+    }
+
+    this->print(lineOne, lineTwo);
+}
+
+void MenuAutopilot::runCommand()
+{
+    switch (this->getCurrentPage())
+    {
+    case 5:
+        this->autopilot->switchLoopMode();
+        break;
+
+    case 6:
+        if (this->targetFreezed)
+        {
+            this->targetFreezed = false;
+            this->autopilot->getNavigation()->freezeTargetPoint(false);
+        }
+        else
+        {
+            this->targetFreezed = true;
+            this->autopilot->getNavigation()->freezeTargetPoint();
+        }
+        break;
+
+    case 7:
+        this->minDistance = this->autopilot->getNavigation()->increaseMinDistanceToReachPoint();
+        break;
+
+    default:
+        break;
+    }
+}
+
+void MenuAutopilot::runCommandNo()
+{
+    switch (this->getCurrentPage())
+    {
+    case 7:
+        this->minDistance = this->autopilot->getNavigation()->decreaseMinDistanceToReachPoint();
+        break;
+
+    default:
+        break;
+    }
+}
+
+void MenuAutopilot::update()
+{
+    if (!this->autopilot->shouldUpdate())
+    {
+        return;
+    }
+
+    this->routeInfo = this->autopilot->getRouteInfo();
+    this->printMenu();
+}
+
+void MenuAutopilot::init()
+{
+    this->setCountPages(8);
+    this->autopilot = new Autopilot();
+    this->driveManager->changeModus(this->autopilot);
+    this->routeInfo = this->autopilot->getRouteInfo();
+
+    // dirty hack to get the val, can be better!!! TODO
+    this->autopilot->getNavigation()->increaseMinDistanceToReachPoint();
+    this->minDistance = this->autopilot->getNavigation()->decreaseMinDistanceToReachPoint();
+
+    MenuDriveMode::init();
+}

src/SpecialMenus/driveModi/Autopilot/menuAutopilot.h

@@ -0,0 +1,60 @@
+/**
+ * @file menuAutopilot.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains a class to print informations about the Autopilot
+ * @version 0.1
+ * @date 2022-02-03
+ *
+ * @copyright Copyright (c) 2022
+ *
+ */
+
+#ifndef MENU_AUTOPILOT_DRIVE_H
+#define MENU_AUTOPILOT_DRIVE_H
+
+#include "SpecialMenus/driveModi/menuDriveMode.h"
+#include "driveModi/Modi/Autopilot/autopilot.h"
+
+/**
+ * @brief A class to print informations about the Autopilot
+ *
+ */
+class MenuAutopilot : public MenuDriveMode
+{
+public:
+    /**
+     * @brief Construct a new Menu Autopilot object
+     *
+     * @param driveManager for MenuDriveMode
+     */
+    MenuAutopilot(DriveManager *driveManager) : MenuDriveMode(driveManager) {}
+
+    /**
+     * @brief Prints the Information to display and console
+     *
+     * The informations are only printed to the display if it
+     * set.
+     *
+     * Changes the DriveModi to Autopilot if it is the first time called
+     * and save the Autopilot object.
+     */
+    void printPage() const override;
+
+    /**
+     * @brief can be called to update shown data
+     */
+    void update() override;
+
+private:
+    void init() override;
+    void runCommand() override;
+    void runCommandNo() override;
+
+    bool mutable targetFreezed = false;
+    double mutable minDistance;
+
+    Autopilot *autopilot;
+    RouteInfo routeInfo;
+};
+
+#endif // MENU_AUTOPILOT_DRIVE_H

src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp

@@ -0,0 +1,174 @@
+/**
+ * @file menuCaptureRoute.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains an implementation of the class MenuCaptureRoute
+ * @version 0.1
+ * @date 2022-01-31
+ *
+ * @copyright Copyright (c) 2022
+ *
+ */
+#include "menuCaptureRoute.h"
+
+void MenuCaptureRoute::printPage() const
+{
+    const RouteInfo routeInfo = this->captureRoute->getRouteInfo();
+    const UBX_NAV_PVT_data_t *gpsData = this->captureRoute->getSensorData()->getGnssData();
+
+    String lineOne = "";
+    String lineTwo = "";
+
+    switch (this->getCurrentPage())
+    {
+    case 0:
+        lineOne = "Capture Route";
+        lineTwo = "You can drive";
+        break;
+
+    case 1:
+        lineOne = "Saved waypoints";
+        lineTwo.concat(routeInfo.totalPoints);
+        break;
+
+    case 2:
+        lineOne = "Last status:";
+        switch (this->captureRoute->getLastStatus())
+        {
+        case Navigation::Status::InsufficientAccuracy:
+            lineTwo = "Poor Accuracy";
+            break;
+
+        case Navigation::Status::Updated:
+            lineTwo = "Point added";
+            break;
+
+        case Navigation::Status::Unchanged:
+            lineTwo = "Point too close";
+            break;
+
+        default:
+            lineTwo = "---";
+            break;
+        }
+        break;
+
+    case 3:
+        lineOne = "Distance to last";
+        lineTwo = "point: ";
+        lineTwo.concat(this->captureRoute->getDistanceToLastPoint());
+        break;
+
+    case 4:
+    {
+        NTRIPClientStates status = this->captureRoute->getSensorData()->getNtripClient()->getClientState();
+        lineOne = "NTRIP Client is";
+
+        if (status == NTRIPClientStates::pushingData)
+        {
+            lineTwo = "enabled";
+        }
+        else if (status == NTRIPClientStates::notAvailable)
+        {
+            lineTwo = "not available";
+        }
+        else
+        {
+            lineTwo = "disabled";
+        }
+        break;
+    }
+
+    case 5:
+    {
+        lineOne = "Carrier Solution";
+        uint8_t carrSoln = gpsData->flags.bits.carrSoln;
+        if (carrSoln == 0)
+        {
+            lineTwo = "None";
+        }
+        else if (carrSoln == 1)
+        {
+            lineTwo = "Floating";
+        }
+        else if (carrSoln == 2)
+        {
+            lineTwo = "Fixed";
+        }
+        else
+        {
+            lineTwo = "UNKNOWN";
+        }
+        break;
+    }
+
+    case 6:
+        lineOne = "hAccuracy: ";
+        lineTwo = "Azimuth: ";
+        lineTwo.concat(this->captureRoute->getSensorData()->getRealAzimuth());
+        if (gpsData->fixType)
+        {
+            lineOne.concat(gpsData->hAcc);
+        }
+        else
+        {
+            lineOne.concat("0");
+        }
+        break;
+
+    case 7:
+        lineOne = "Current minimal";
+        if (this->captureRoute->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM)
+        {
+            lineTwo = "accuracy is high";
+        }
+        else
+        {
+            lineTwo = "accuracy is low";
+        }
+        break;
+
+    default:
+        this->printDefault();
+        return;
+    }
+
+    this->print(lineOne, lineTwo);
+}
+
+void MenuCaptureRoute::update()
+{
+    if (!this->captureRoute->shouldUpdate())
+    {
+        return;
+    }
+    this->printMenu();
+}
+
+void MenuCaptureRoute::runCommand()
+{
+    switch (this->getCurrentPage())
+    {
+    case 7:
+        if (this->captureRoute->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM)
+        {
+            this->captureRoute->getNavigation()->setMinAccuracy(Point::Accuracy::none);
+        }
+        else
+        {
+            this->captureRoute->getNavigation()->setMinAccuracy(Point::Accuracy::twoDigOfCM);
+        }
+        break;
+
+    default:
+        break;
+    }
+}
+
+void MenuCaptureRoute::init()
+{
+    this->setCountPages(8);
+    MenuDriveMode::updateDelay = MenuCaptureRoute::updateDelay;
+    this->captureRoute = new CaptureRoute();
+    this->driveManager->changeModus(this->captureRoute);
+    MenuDriveMode::init();
+}

src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h

@@ -0,0 +1,56 @@
+/**
+ * @file menuCaptureRoute.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains a class to print informations about CaptureRoute
+ * @version 0.1
+ * @date 2022-01-31
+ *
+ * @copyright Copyright (c) 2022
+ *
+ */
+#ifndef MENU_CAPTURE_ROUTE_H
+#define MENU_MANUAL_DRIVE_H
+
+#include "SpecialMenus/driveModi/menuDriveMode.h"
+#include "driveModi/Modi/CaptureRoute/captureRoute.h"
+
+/**
+ * @brief A class to print informations about the Autopilot
+ *
+ */
+class MenuCaptureRoute : public MenuDriveMode
+{
+public:
+    /**
+     * @brief Construct a new Menu Capture Route object
+     *
+     * @param driveManager
+     */
+    MenuCaptureRoute(DriveManager *driveManager) : MenuDriveMode(driveManager) {}
+
+    /**
+     * @brief Prints the Information to display and console
+     *
+     * The informations are only printed to the display if it
+     * set.
+     *
+     * Changes the DriveModi to CaptureRoute if it is the first time called
+     * and save the CaptureRoute object.
+     */
+    void printPage() const override;
+
+    /**
+     * @brief can be called to update shown data
+     */
+    void update() override;
+
+private:
+    void runCommand() override;
+    void init() override;
+
+    CaptureRoute *captureRoute = nullptr;
+
+    static constexpr uint16_t updateDelay = 500;
+};
+
+#endif // MENU_MANUAL_DRIVE_H

src/SpecialMenus/driveModi/menuDriveMode.cpp

@@ -48,6 +48,7 @@ void MenuDriveMode::left()
 
     this->firstPrint = true;
     this->configureOnLeave();
+    delete this->menuSpeed;
     this->driveManager->changeModus();
     MenuInformationSites::left();
 }

src/driveModi/Modi/Autopilot/autopilot.cpp

@@ -0,0 +1,304 @@
+/**
+ * @file autopilot.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains the implementation of the class Autopilot
+ * @version 0.1
+ * @date 2022-02-02
+ *
+ * @copyright Copyright (c) 2022
+ *
+ */
+
+#include "driveModi/Modi/Autopilot/autopilot.h"
+#include "autopilot.h"
+
+DirectionChangeSignal::DirectionChangeSignal(Autopilot *pilot)
+    : pilot {pilot}
+{
+    this->action();
+}
+
+DirectionChangeSignal::~DirectionChangeSignal()
+{
+    CalcAzimuth *calcAzimuth = pilot->getSensorData()->getCalcCompass();
+    if (static_cast<bool>(calcAzimuth))
+    {
+        calcAzimuth->disableCalcAzimuth();
+    }
+}
+
+void DirectionChangeSignal::action()
+{
+    CalcAzimuth *calcAzimuth = pilot->getSensorData()->getCalcCompass();
+    if (static_cast<bool>(calcAzimuth))
+    {
+        calcAzimuth->drivingDirectionChange(pilot->getSensorData()->getCurrentPos());
+    }
+}
+
+Autopilot::~Autopilot()
+{
+    delete this->directionChangeSignal;
+    // this->navigation->getNTRIPClient()->setActivated(false);
+}
+
+void Autopilot::run()
+{
+    this->routeInfo = this->navigation->getRouteInfo();
+
+    switch (this->state)
+    {
+    case State::InsufficientAccuracy:
+        this->askNavigationForOrder();
+        return;
+
+    case State::NoRoute:
+        return;
+
+    case State::None:
+        return;
+
+    case State::NavigationStarted:
+        this->askNavigationForOrder();
+        this->checkButtonInput();
+        break;
+
+    case State::GetToStartPoint:
+        ManualControl::run();
+        this->askNavigationForOrder();
+        if (this->routeInfo.currentPoint >= 2){
+            this->state = State::SelfDrivingAvailable;
+        }
+        break;
+
+    case State::SelfDrivingAvailable:
+        ManualControl::run();
+        this->askNavigationForOrder();
+        this->checkButtonInput();
+        break;
+
+    case State::SelfDriving:
+        this->askNavigationForOrder();
+        this->checkButtonInput();
+        this->selfDriving();
+        break;
+
+    case State::SelfDrivingRotate:
+        this->checkButtonInput();
+        this->rotate();
+        break;
+
+    case State::TargetReached:
+        if (this->loopMode){
+            this->restartLoop();
+        }
+        break;
+
+    default:
+        break;
+    }
+}
+
+void Autopilot::restart()
+{
+    this->init();
+}
+
+bool Autopilot::shouldUpdate()
+{
+    if (this->updateDisplay)
+    {
+        this->updateDisplay = false;
+        return true;
+    }
+    return false;
+}
+
+void Autopilot::testRotate(int16_t degree)
+{
+    if (!static_cast<bool>(degree))
+    {
+        return;
+    }
+
+    this->courseCorrection.correction = degree;
+    this->beginRotate();
+}
+
+void Autopilot::init()
+{
+    if (this->navigation->startNavigation())
+    {
+        this->state = State::NavigationStarted;
+    }
+    else
+    {
+        this->state = State::NoRoute;
+    }
+    this->routeInfo = this->navigation->getRouteInfo();
+    this->sensorData->getNtripClient()->setAutoReconnect(true);
+
+    this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection, true);
+
+    this->courseCorrection.correction = 0;
+    this->courseCorrection.distance = 0;
+    this->updateDisplay = true;
+}
+
+void Autopilot::drive()
+{
+    DrivingSpeeds speeds = {0, 0};
+    if (this->courseCorrection.distance >= this->minRemainingDistance)
+    {
+        speeds.x = this->maxSpeeds.x;
+    }
+    this->moveControl->setSpeeds(speeds);
+}
+
+void Autopilot::beginRotate()
+{
+    if (this->state != State::SelfDrivingRotate)
+    {
+        this->lastState = this->state;
+        this->state = State::SelfDrivingRotate;
+        this->rotationAimAzimuth = this->getSensorData()->getRealAzimuth() + this->courseCorrection.correction;
+        this->rotationAimAzimuth = Navigation::fixDegree(this->rotationAimAzimuth);
+
+        DrivingSpeeds speeds = {0, 0};
+        if (this->courseCorrection.correction > 0)
+        {
+            speeds.rot = -this->maxSpeeds.rot;
+        }
+        else
+        {
+            speeds.rot = this->maxSpeeds.rot;
+        }
+
+        this->moveControl->setSpeeds(speeds);
+    }
+}
+
+void Autopilot::rotate()
+{
+    if (abs(this->getSensorData()->getRealAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct)
+    {
+        this->endRotate();
+        return;
+    }
+
+    if ((abs(this->getSensorData()->getRealAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct * 3) &&
+        ((this->courseCorrection.correction > 0 && this->rotationAimAzimuth < this->getSensorData()->getRealAzimuth()) || (this->courseCorrection.correction < 0 && this->rotationAimAzimuth > this->getSensorData()->getRealAzimuth())))
+    {
+        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->sensorData->getCalcCompass()->drivingDirectionChange(this->sensorData->getCurrentPos());
+    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)
+        {
+            this->state = State::SelfDriving;
+        }
+        else if (this->state == State::SelfDriving || this->state == State::SelfDrivingRotate)
+        {
+            this->state = State::SelfDrivingAvailable;
+        }
+        else if (this->state == State::NavigationStarted)
+        {
+            this->state = State::GetToStartPoint;
+        }
+        this->updateDisplay = true;
+        this->lastAutopilotChangeMillis = millis();
+    }
+}
+
+void Autopilot::askNavigationForOrder()
+{
+    this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection);
+    switch (this->lastOrderStatus)
+    {
+    case Navigation::Status::Complete:
+        this->state = State::TargetReached;
+        this->moveControl->setSpeed(0);
+        this->moveControl->setRotationSpeed(0);
+        break;
+
+    case Navigation::Status::InsufficientAccuracy:
+        if (this->state == State::InsufficientAccuracy){
+            break;
+        }
+        this->lastState = this->state;
+        this->state = State::InsufficientAccuracy;
+        this->moveControl->setSpeed(0);
+        this->moveControl->setRotationSpeed(0);
+        break;
+
+    case Navigation::Status::Unchanged:
+        if (this->state == State::InsufficientAccuracy){
+            this->state = this->lastState;
+        }
+        break;
+
+    case Navigation::Status::Updated:
+        if (this->state == State::InsufficientAccuracy){
+            this->state = this->lastState;
+        }
+        break;
+
+    default:
+        break;
+    }
+}
+
+void Autopilot::selfDriving()
+{
+    if (this->state != State::SelfDriving)
+    {
+        return;
+    }
+
+    if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBeforeAct)
+    {
+        this->beginRotate();
+    }
+    else
+    {
+        this->drive();
+    }
+}
+
+void Autopilot::restartLoop()
+{
+    this->navigation->startNavigation();
+    this->state = State::SelfDriving;
+    this->routeInfo = this->navigation->getRouteInfo();
+    this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection, true);
+    this->updateDisplay = true;
+}
+
+void Autopilot::afterActivate()
+{
+    this->setInputMode(ManualControl::InputMode::Digital);
+    this->directionChangeSignal = new DirectionChangeSignal(this);
+    this->setDirectionChangeCallback(this->directionChangeSignal);
+    this->navigation = new Navigation(this->sensorData);
+    this->init();
+}

src/driveModi/Modi/Autopilot/autopilot.h

@@ -0,0 +1,131 @@
+/**
+ * @file autopilot.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains a class which use the navigate class to drive automaticaly
+ * @version 0.1
+ * @date 2022-02-02
+ *
+ * @copyright Copyright (c) 2022
+ *
+ */
+
+#ifndef AUTOPILOT_H
+#define AUTOPILOT_H
+
+#include "navigation.h"
+#include "driveModi/Modi/ManualControl/manualControl.h"
+
+class DirectionChangeSignal;
+
+/**
+ * @brief This class use the navigate class to drive automaticaly
+ *
+ * This class get the information from the navigate class. When an
+ * object of this class is constructed the rover can be driven manually.
+ * When the Rover is near to the first position of the Route, you can
+ * switch to automatic drive.
+ *
+ */
+class Autopilot : public ManualControl
+{
+public:
+    enum State
+    {
+        InsufficientAccuracy = -2,
+        NoRoute = -1,
+        None = 0,
+        NavigationStarted,
+        GetToStartPoint,
+        SelfDrivingAvailable,
+        SelfDriving,
+        SelfDrivingRotate,
+        TargetReached
+    };
+
+    /**
+     * @brief Destroy the Autopilot object
+     *
+     * Disconnect the NTRIP-Client
+     */
+    ~Autopilot();
+
+    void restart();
+
+    /**
+     * @brief Get the Route Info object
+     *
+     * @return RouteInfo
+     */
+    RouteInfo getRouteInfo() const { return this->routeInfo; }
+
+    /**
+     * @brief Get the Course Correction object
+     *
+     * @return CourseCorrection
+     */
+    CourseCorrection getCourseCorrection() const { return this->courseCorrection; }
+
+    /**
+     * @brief Get the State object
+     *
+     * @return State
+     */
+    State getState() const { return this->state; }
+    Navigation *getNavigation() const { return this->navigation; }
+
+    /**
+     * @brief Tells if there are new informations to display
+     *
+     * @return true
+     * @return false
+     */
+    bool shouldUpdate();
+    void testRotate(int16_t degree);
+    void endRotate();
+    void switchLoopMode() { this->loopMode = !this->loopMode; }
+    bool getLoopMode() const { return this->loopMode; }
+
+private:
+    void init();
+    void drive();
+    void beginRotate();
+    void rotate();
+    void run() override;
+    void checkButtonInput();
+    void askNavigationForOrder();
+    void selfDriving();
+    void restartLoop();
+    void afterActivate() override;
+
+    Navigation *navigation = nullptr;
+    CourseCorrection courseCorrection{0, 0};
+    RouteInfo routeInfo{0, 0};
+    State state = State::None;
+    State lastState = State::None;
+    Navigation::Status lastOrderStatus;
+    DirectionChangeSignal *directionChangeSignal = nullptr;
+
+    bool updateDisplay = false;
+    bool loopMode = false;
+
+    uint8_t maxCourseDeviationBeforeAct = 5;
+    uint16_t autopilotChangeDelayMillis = 500;
+    uint32_t lastAutopilotChangeMillis = 0;
+
+    int16_t rotationAimAzimuth = 0;
+
+    double minRemainingDistance = 0.25;
+};
+
+class DirectionChangeSignal : public DirectionChangeWrapper
+{
+public:
+    DirectionChangeSignal(Autopilot *pilot);
+    ~DirectionChangeSignal();
+    void action() override;
+
+private:
+    Autopilot *pilot;
+};
+
+#endif // AUTOPILOT_H

src/driveModi/Modi/CaptureRoute/captureRoute.cpp

@@ -0,0 +1,65 @@
+/**
+ * @file captureRoute.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains the implementation of the class CaptureRoute
+ * @version 0.1
+ * @date 2022-02-15
+ *
+ * @copyright Copyright (c) 2022
+ *
+ */
+
+#include "driveModi/Modi/CaptureRoute/captureRoute.h"
+#include "captureRoute.h"
+
+CaptureRoute::~CaptureRoute()
+{
+    delete this->navigation;
+
+    if (static_cast<bool>(this->sensorData->getNtripClient()))
+    {
+        this->sensorData->getNtripClient()->setActivated(false);
+    }
+}
+
+void CaptureRoute::run()
+{
+    ManualControl::run();
+    if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action))
+    {
+        this->status = this->navigation->addCurrentPosToRoute();
+        if (this->status == Navigation::Status::Updated)
+        {
+            this->lastSavedPoint = this->navigation->getCurrentPosition();
+            this->routeInfo = navigation->getRouteInfo();
+            this->updateDisplay = true;
+        }
+    }
+}
+
+void CaptureRoute::afterActivate()
+{
+    this->navigation = new Navigation(this->sensorData);
+    this->navigation->getRoute()->clear();
+    this->sensorData->getNtripClient()->setAutoReconnect(true);
+    this->routeInfo = navigation->getRouteInfo();
+}
+
+double CaptureRoute::getDistanceToLastPoint() const
+{
+    if (!this->lastSavedPoint.isInit())
+    {
+        return 0;
+    }
+    return this->lastSavedPoint.distanceTo(this->navigation->getCurrentPosition());
+}
+
+bool CaptureRoute::shouldUpdate()
+{
+    if (this->updateDisplay)
+    {
+        this->updateDisplay = false;
+        return true;
+    }
+    return false;
+}

src/driveModi/Modi/CaptureRoute/captureRoute.h

@@ -0,0 +1,75 @@
+/**
+ * @file captureRoute.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains a class to capture a driven route
+ * @version 0.1
+ * @date 2022-02-15
+ *
+ * @copyright Copyright (c) 2022
+ *
+ */
+
+#ifndef CAPTURE_ROUTE_H
+#define CAPTURE_ROUTE_H
+
+#include <iostream>
+
+#include "driveModi/Modi/ManualControl/manualControl.h"
+#include "navigation.h"
+
+/**
+ * @brief A class to capture a driven class
+ *
+ * This class inherits ManualControl so you can drive
+ * normally as in ManualControl. If GPS signal is valid
+ * you can add a Point everytime you want.
+ *
+ */
+class CaptureRoute : public ManualControl
+{
+public:
+    /**
+     * @brief Destroy the Capture Route object
+     *
+     * Disconnect the NTRIP-Client
+     */
+    ~CaptureRoute();
+
+    /**
+     * @brief Get the Route Info object
+     *
+     * @return RouteInfo
+     */
+    RouteInfo getRouteInfo() const { return this->routeInfo; }
+
+    Navigation::Status getLastStatus() const { return this->status; }
+    Navigation *getNavigation() const { return this->navigation; }
+
+    /**
+     * @brief Get the distance to the last saved oint
+     *
+     * @return double in meters
+     */
+    double getDistanceToLastPoint() const;
+
+    /**
+     * @brief Tells if there are new informations to display
+     *
+     * @return true
+     * @return false
+     */
+    bool shouldUpdate();
+
+private:
+    void run() override;
+    void afterActivate() override;
+
+    Navigation *navigation = nullptr;
+    RouteInfo routeInfo;
+    Point lastSavedPoint;
+    Navigation::Status status = Navigation::Status::Complete;
+
+    bool updateDisplay = false;
+};
+
+#endif // CAPTURE_ROUTE_H

src/driveModi/Modi/ManualControl/manualControl.cpp

@@ -47,7 +47,7 @@ void ManualControl::analogControl()
 
 void ManualControl::digitalControl()
 {
-    static constexpr uint8_t deadzone = 110;
+    static constexpr uint8_t deadzone = 120;
     const int16_t yAxis = this->input->x - 127;
     const int16_t xAxis = this->input->y - 127;
 

src/driveModi/Modi/TestMode/testMode.cpp

@@ -82,6 +82,8 @@ bool TestMode::drive(int16_t cmDistance, int16_t degree)
     }
     else
     {
+        // forward or backward and left or right
+        //  TODO: Calculate roationspeed
         if (cmDistance < 0)
         {
             this->moveControl->setSpeed(-this->maxSpeeds.x);

src/driveModi/driveManager.h

@@ -14,6 +14,7 @@
 
 #include "moveControl.h"
 #include "driveModi/driveModi.h"
+#include "navigation.h"
 #include "config.h"
 #include "controlPadInput.h"
 #include "component.h"

src/main.cpp

@@ -39,10 +39,13 @@
 #include "menu.h"
 #include "menuAction.h"
 #include "SpecialMenus/driveModi/ManualDrive/menuManualDrive.h"
+#include "SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h"
+#include "SpecialMenus/driveModi/Autopilot/menuAutopilot.h"
 #include "SpecialMenus/driveModi/TestMode/menuTestMode.h"
 #include "SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h"
 #include "SpecialMenus/Systeminformation/menuSysteminformation.h"
 #include "SpecialMenus/PID/menuPidSettings.h"
+#include "SpecialMenus/Route/menuRoute.h"
 #include "SpecialMenus/SensorData/menuSensorData.h"
 #include "SpecialMenus/CalibrateBattery/menuCalibrateBattery.h"
 
@@ -122,6 +125,13 @@ void setup()
     sensorData = new SensorData();
     sensorData->enableGnss(spiPort, PinNumbers::gnssSpiCs);
     sensorData->enableRealCompass();
+    sensorData->enableNtrip(static_cast<const char *>(NtripConfig::host),
+                            NtripConfig::port,
+                            static_cast<const char *>(NtripConfig::mountPoint),
+                            static_cast<const char *>(NtripConfig::user),
+                            static_cast<const char *>(NtripConfig::password),
+                            NtripConfig::sendOwnPosition);
+    sensorData->enableGyroscope();
     driveManager = new DriveManager(&moveController, sensorData, controlPad->getControlPadDataPtr());
 
     char wifiIndicator = 'X';
@@ -129,16 +139,12 @@ void setup()
     {
         wifiIndicator = '-';
     }
-
-    lcdWrapper = new LcdWrapper(lcd);
-    lcdWrapper->setCallback(lcdWrapperCallback);
-    lcdWrapper->loop();
-
     lcd->backlight();
-    lcd->setCursor(0, 0);
     lcd->printf("%c Kleiax-Rover %c", wifiIndicator, wifiIndicator);
     lcd->setCursor(0, 1);
     lcd->printf("WiFi channel %u", Network::getCurrentChannel());
+    lcdWrapper = new LcdWrapper(lcd);
+    lcdWrapper->setCallback(lcdWrapperCallback);
 
     makeMenu();
 
@@ -155,7 +161,7 @@ void loop()
     lcdWrapper->loop();
     mainBattery->loop();
 
-    // new Value every 0.5s
+    // new Value ervery 0.5s
     if (mainBattery->isNewValue())
     {
         static uint8_t batteryLowCounter = 0;
@@ -188,14 +194,14 @@ void loop()
 void i2cScanner()
 {
     constexpr uint8_t checkForLength = 16;
-    constexpr uint8_t maxAddresses = UINT8_MAX / 2;
+    constexpr uint8_t maxAdresses = UINT8_MAX / 2;
     std::cout << "\nI2C Scanner" << std::endl;
 
     byte error = 0;
     byte address = 0;
     int nDevices = 0;
     std::cout << "Scanning..." << std::endl;
-    for (address = 1; address < maxAddresses; address++)
+    for (address = 1; address < maxAdresses; address++)
     {
         Wire.beginTransmission(address);
         error = Wire.endTransmission();
@@ -249,24 +255,32 @@ void makeMenu()
     auto *pidr_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(1)));
     auto *speed_m = new MenuIntInput(2, new MenuSpeed(driveManager->getDrivingSpeedsRef()));
     auto *man_m = new MenuManualControl(driveManager);
+    auto *cap_m = new MenuCaptureRoute(driveManager);
+    auto *auto_m = new MenuAutopilot(driveManager);
     auto *testM_m = new MenuTestMode(driveManager);
     auto *comp_m = new MenuCalibrateCompass(driveManager);
     auto *sys_m = new MenuSysteminformation(mainBattery);
     auto *sen_m = new MenuSensorData(sensorData);
+    // TODO: Wie bekommt jeder die dumme Route?
+    auto *rout_m = new MenuRoute(new Route());
     auto *bat_m = new MenuCalibrateBattery(mainBattery);
 
+    auto_m->setUpdateDelay(displayUpdateDelay);
     sys_m->setUpdateDelay(displayUpdateDelay);
     sen_m->setUpdateDelay(displayUpdateDelay);
 
     // Entry for the main menu
     main_m->addEntry(new MenuAction("Mode", mode_m));
     main_m->addEntry(new MenuAction("Sensor", sen_m));
+    main_m->addEntry(new MenuAction("Route", rout_m));
     main_m->addEntry(new MenuAction("Settings", set_m));
     main_m->addEntry(new MenuAction("Systeminfo", sys_m));
     main_m->addEntry(new MenuAction("Restart", restart));
 
     // Entry for the mode Menu
     mode_m->addEntry(new MenuAction("Manual Control", man_m));
+    mode_m->addEntry(new MenuAction("Capture Route", cap_m));
+    mode_m->addEntry(new MenuAction("Autopilot", auto_m));
     mode_m->addEntry(new MenuAction("Gauge Compass", comp_m));
     mode_m->addEntry(new MenuAction("Test Mode", testM_m));
     mode_m->addEntry(new MenuAction("Consol Control", dummy));
@@ -344,16 +358,16 @@ void lcdWrapperCallback(const char data[][LcdWrapper::totalRows], uint8_t lines,
 
 NetworkAddresses setIPs()
 {
-    NetworkAddresses addresses;
-    addresses.localIP.fromString(static_cast<const char *>(NetworkConfig::ip));
-    addresses.subnet.fromString(static_cast<const char *>(NetworkConfig::subnet));
-    addresses.gateway.fromString(static_cast<const char *>(NetworkConfig::gateway));
-    addresses.dnsServer.fromString(static_cast<const char *>(NetworkConfig::dns));
+    NetworkAddresses adresses;
+    adresses.localIP.fromString(static_cast<const char *>(NetworkConfig::ip));
+    adresses.subnet.fromString(static_cast<const char *>(NetworkConfig::subnet));
+    adresses.gateway.fromString(static_cast<const char *>(NetworkConfig::gateway));
+    adresses.dnsServer.fromString(static_cast<const char *>(NetworkConfig::dns));
     if (NetworkConfig::mqtt)
     {
-        addresses.mqttServer.fromString(static_cast<const char *>(MqttConfig::server));
-        addresses.mqttPort = MqttConfig::port;
+        adresses.mqttServer.fromString(static_cast<const char *>(MqttConfig::server));
+        adresses.mqttPort = MqttConfig::port;
     }
 
-    return addresses;
+    return adresses;
 }