Merge remote-tracking branch 'origin/projektarbeit' into testProjekttoBachelor but only motor things

- deactivate a debug print
- activate init screen - update network config
2024-07-19 15:57:54 +02:00 · 2024-07-19 15:33:50 +02:00 · 2024-07-19 12:15:44 +02:00 · 2024-07-12 22:57:37 +02:00 · 2024-07-03 12:51:42 +02:00 · 2024-07-03 09:22:16 +02:00
43 changed files with 4212 additions and 51 deletions
@@ -129,7 +129,6 @@
        "carr",
        "CIPO",
        "COPI",
-        "deadzone",
        "Doxygen",
        "Dutycycle",
        "gast",
@@ -151,7 +150,6 @@
        "Punica",
        "RHEDE",
        "Rtcm",
-        "Sats",
        "Schalke",
        "Soln",
        "Systeminformation",
@@ -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()
@@ -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()
@@ -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()
@@ -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()
@@ -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>
@@ -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>
@@ -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
+

@@ -19,11 +19,11 @@
 #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 +56,19 @@ namespace NetworkConfig
 }
 #endif // RHEDE

+#ifdef ZUHAUSE
+namespace NetworkConfig
+{
+    const char ssid[] = "Tuedelkram";
+    const char password[] = "3!Kaesehoch";
+    const char ip[] = "192.168.178.11";
+    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";
@@ -68,9 +81,9 @@ 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";
 }
@@ -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;
+        }
+    }
+}
@@ -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
@@ -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:
    /**
@@ -14,7 +14,7 @@
 LcdWrapper::LcdWrapper(LiquidCrystal_I2C *lcd)
    : lcd{lcd}, changed{false}
 {
-    this->clear();
+    // this->clear();
 }

 void LcdWrapper::run()
@@ -153,7 +153,7 @@ void MotorControl::setRealPower(int8_t power)
        digitalWrite(this->dir_2, LOW);
        ledcWrite(this->pwmChannel, 0);
        this->dutycycle = 0;
-        std::cout << "abort" << std::endl;
+        // std::cout << "abort" << std::endl;
        return;
    }

@@ -174,6 +174,7 @@ void MotorControl::setRealPower(int8_t power)

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

@@ -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;
+}
@@ -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
@@ -0,0 +1,404 @@
+/**
+ * @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->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, 10);
+}
+
+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\n\
+                 Host: %s:%u\r\n\
+                 User-Agent: NTRIP KleiaxNtripClient/0.1\r\n\
+                 Accept: */*\r\n\
+                 Connection: 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\n\
+                 Host: %s:%u\r\n\
+                 Ntrip-Version: Ntrip/2.0\r\n\
+                 User-Agent: NTRIP KleiaxNtripClient/0.1\r\n\
+                 Accept: */*\r\n\
+                 Connection: close\r\n"),
+                 this->mountPoint,
+                 this->host,
+                 this->port);
+    }
+
+    // Add own Position if activated
+    if (this->transmitLocation && !this->useNtripRev1)
+    {
+        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\r\n"),
+                     data->nmea);
+            strncat(static_cast<char *>(serverRequest), static_cast<const char *>(positionUpdate), this->bufferSize);
+        }
+        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)
+    {
+        return;
+    }
+
+    if (millis() - this->lastGPGGAPushTime < this->pushGPGGATime)
+    {
+        return;
+    }
+    this->lastGPGGAPushTime = millis();
+
+    if (!this->ntripClient->connected())
+    {
+        std::cout << "Failed to pushing GGA to server: " << std::endl;
+    }
+
+    auto *data = new NMEA_GGA_data_t;
+    const uint8_t res = this->gnss->getLatestNMEAGPGGA(data);
+    if (res == 2) // 2 means fresh data
+    {
+        char positionUpdate[this->bufferSizePushGPGGA];
+        snprintf(static_cast<char *>(positionUpdate),
+                 this->bufferSizePushGPGGA,
+                 static_cast<const char *>("%s\r\n"),
+                 data->nmea);
+        this->ntripClient->write(positionUpdate, strlen(positionUpdate));
+        std::cout << "Position update: " << positionUpdate << std::endl;
+    }
+    delete data;
+}
+
+bool NTRIPClient::isConnected()
+{
+    return this->state == NTRIPClientStates::pushingData;
+}
@@ -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 = 10000;
+
+    static constexpr uint8_t loopDelay = 20;
+};
+
+#endif
@@ -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
@@ -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};
@@ -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;
+}
@@ -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
@@ -20,6 +20,21 @@ 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)
+{
+    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->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 +68,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 +78,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 +235,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()
@@ -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);
+
    /**
     * @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);
@@ -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
+; 
+; 
+; 
@@ -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]);
+}
@@ -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
@@ -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);
+}
@@ -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
@@ -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))
@@ -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();
+}
@@ -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
@@ -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();
+}
@@ -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
@@ -48,6 +48,7 @@ void MenuDriveMode::left()

    this->firstPrint = true;
    this->configureOnLeave();
+    delete this->menuSpeed;
    this->driveManager->changeModus();
    MenuInformationSites::left();
 }
@@ -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();
+}
@@ -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
@@ -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;
+}
@@ -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
@@ -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);
@@ -14,6 +14,7 @@

 #include "moveControl.h"
 #include "driveModi/driveModi.h"
+#include "navigation.h"
 #include "config.h"
 #include "controlPadInput.h"
 #include "component.h"
@@ -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,12 @@ 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));
+    sensorData->enableGyroscope();
    driveManager = new DriveManager(&moveController, sensorData, controlPad->getControlPadDataPtr());

    char wifiIndicator = 'X';
@@ -129,16 +138,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 +160,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 +193,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 +254,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 +357,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;
 }
Author	SHA1	Message	Date
kleiax	00c504fb12	Merge remote-tracking branch 'origin/projektarbeit' into testProjekttoBachelor but only motor things	2024-07-19 15:57:54 +02:00
kleiax	f0f75d3fd3	- deactivate a debug print - activate init screen - update network config	2024-07-19 15:33:50 +02:00
kleiax	73fde34e48	- fix ntripClient - fix gyroscope - fix init screen	2024-07-19 12:15:44 +02:00
kleiax	87743c8e67	refactoring ntripClient	2024-07-12 22:57:37 +02:00
kleiax	c866bc0d60	little optical fix	2024-07-03 12:51:42 +02:00
kleiax	dbb646b5a5	start implementation of routeCache	2024-07-03 09:22:16 +02:00
kleiax	dd304cf787	- new implementation for course and distance calculation	2024-07-03 09:05:06 +02:00
kleiax	717daef21e	add new todo	2024-04-09 21:37:32 +02:00
kleiax	16e04afebd	fix copy paste mistake	2024-04-09 21:31:54 +02:00
kleiax	f227a2f569	WiFi for Aschendorf	2024-04-05 14:02:17 +02:00
kleiax	9b6889b68c	- remove typo - add menu to dependencies	2024-04-04 21:26:25 +02:00
kleiax	155975df33	Start working on Bachelor	2024-04-04 21:12:37 +02:00