Merge branch 'main' into projektarbeit

.gitignore

@@ -3,11 +3,10 @@
 .vscode/c_cpp_properties.json
 .vscode/launch.json
 .vscode/ipch
+rover-cppcheck-build-dir
 
 doc/Doxygen/html*
 doc/Doxygen/generated*
 
-
-
 workspace.code-workspace
 projektarbeit.code-workspace

.vscode/settings.json

@@ -123,11 +123,14 @@
         "Ahnung",
         "akku",
         "ardui",
+        "Baseclass",
         "blox",
         "bluedroid",
         "carr",
         "CIPO",
         "COPI",
+        "Doxygen",
+        "Dutycycle",
         "gast",
         "GNSS",
         "GPGGA",
@@ -136,9 +139,12 @@
         "Keine",
         "kleiax",
         "Lebennig",
+        "microcontroller",
+        "MQTT",
         "NMEA",
         "NMEAGPGGA",
         "NTRIP",
+        "pcnt",
         "pidl",
         "pidr",
         "Punica",
@@ -146,8 +152,10 @@
         "Rtcm",
         "Schalke",
         "Soln",
+        "Systeminformation",
         "TPMOBIL",
         "UBLOX",
+        "wheelspeed",
         "ZLPJ"
     ],
     "cmake.configureOnOpen": false

doc/Doxygen/Doxyfile

@@ -51,7 +51,7 @@ PROJECT_BRIEF          = "A small verhicle which should be drive a route with GP
 # pixels and the maximum width should not exceed 200 pixels. Doxygen will copy
 # the logo to the output directory.
 
-PROJECT_LOGO           = C:/Users/alex1/git/projektarbeit/doc/Doxygen/logo.png
+PROJECT_LOGO           = logo.png
 
 # The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path
 # into which the generated documentation will be written. If a relative path is
@@ -606,7 +606,7 @@ HIDE_COMPOUND_REFERENCE= NO
 # will show which file needs to be included to use the class.
 # The default value is: YES.
 
-SHOW_HEADERFILE        = YES
+# SHOW_HEADERFILE        = YES
 
 # If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of
 # the files that are included by a file in the documentation of that file.
@@ -824,7 +824,7 @@ WARN_IF_DOC_ERROR      = YES
 # parameters have no documentation without warning.
 # The default value is: YES.
 
-WARN_IF_INCOMPLETE_DOC = YES
+# WARN_IF_INCOMPLETE_DOC = YES
 
 # This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that
 # are documented, but have no documentation for their parameters or return
@@ -1608,7 +1608,7 @@ GENERATE_TREEVIEW      = YES
 # The default value is: NO.
 # This tag requires that the tag GENERATE_HTML is set to YES.
 
-FULL_SIDEBAR           = NO
+# FULL_SIDEBAR           = NO
 
 # The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that
 # doxygen will group on one line in the generated HTML documentation.
@@ -1691,7 +1691,7 @@ USE_MATHJAX            = NO
 # The default value is: MathJax_2.
 # This tag requires that the tag USE_MATHJAX is set to YES.
 
-MATHJAX_VERSION        = MathJax_2
+# MATHJAX_VERSION        = MathJax_2
 
 # When MathJax is enabled you can set the default output format to be used for
 # the MathJax output. For more details about the output format see MathJax

doc/TODO allgemein.txt

@@ -6,39 +6,42 @@ Do later:
     -> Program underfloorLighting
     -> Add an beeper
     -> Program the beeper
-    -> Update GNSS Lib to v3
     -> Engine slow down without curve in motorControl
     -> Network clean up (Mqtt remove?)
     -> Extra class for maneuver, autopilot should inherit from int16_t
-    -> Better remote Control with Leds for gnss rtk etc
+    -> 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
+
 
 Do now:
 Code:
-    Doxygen Kommentare aktualisieren
+    Fernbedienung!
-    Geschwindigkeiten außerhalb von Modi einstellen, Manager kann die Werte einstellen da Interface
-    Automat in MoveControl weil jetzt in Arbeit beschrieben
-    Liste mit Betriebsmodi, automatisch in Menü einfügen
-    Betriebmodi dem Drivemanger ohne switchCase geben, aus Liste oder so
-    bettery managment checken, buffer in battery und in Main
-    battery tabelle mit eigenen Werten übergeben und nicht in header (wiederverwendbarkeit)
-    battery methode für daten erzeugen und in eeporm speichern
-    battery bruch mit R werten nur einmal berechnen
-    lange kein daten von fernbedienung dann?
-    Input pointer von fernbedienung nicht veränderbar doppel const
-    trennen funktion und ui route menü
-    DriveManager Mode als Template übergeben
 
 Latex:
-    Genaue Beschreibung von PulseCounter HardwareUnit wenn möglich
     Anhang: Liste mit allen Komponenten und Kurzbeschreibung
-
+    Unterschied funktionale und nicht funktionale Anforderungen
-3D:
+    Overfull H-Boxen
-    Oberrahmen soll nur aussehen wie ein A
+    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
 

doc/latexSnippet.cpp

@@ -1,9 +0,0 @@
-void KomponenteA::loop() {
-    this->komponenteB->loop();
-    this->komponenteC->loop();
-
-    if (millis() - this->lastMillis < this->delayMillis)
-        return;
-
-    this->doSomething();
-}

include/Version.h

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

include/config.h

@@ -1,7 +1,7 @@
 /**
  * @file config.h
  * @author Alexander Klein (alex@kleiax.de)
- * @brief Some defines to configere the project.
+ * @brief Some defines to configure the project.
  * @version 0.1
  * @date 2022-02-15
  *
@@ -11,42 +11,64 @@
 
 #pragma once
 
-namespace PinNumbers {
+/**
+ * @brief The numbers of pins to be used
+ *
+ */
+namespace PinNumbers
+{
     constexpr uint8_t spiCopi = 16;
     constexpr uint8_t spiCipo = 4;
     constexpr uint8_t spiSck = 5;
     constexpr uint8_t gnssSpiCs = 17;
+    constexpr uint8_t sda = 21;
+    constexpr uint8_t scl = 19;
+    constexpr uint8_t battery = 35;
 
-    namespace LeftMotor {
+    namespace LeftMotor
+    {
         constexpr uint8_t dir1 = 27;
         constexpr uint8_t dir2 = 12;
         constexpr uint8_t pwm = 13;
-        constexpr uint8_t encoder = 26;
+        constexpr uint8_t encoder = 32;
         constexpr uint8_t pmwChannel = 0;
     }
 
-    namespace RightMotor {
+    namespace RightMotor
+    {
         constexpr uint8_t dir1 = 14;
         constexpr uint8_t dir2 = 23;
         constexpr uint8_t pwm = 22;
         constexpr uint8_t encoder = 33;
-        constexpr uint8_t pmwChannel = 0;
+        constexpr uint8_t pmwChannel = 1;
     }
 }
 
-namespace Settings {
+/**
-    constexpr float wheelDiameter = 0.1263;
+ * @brief Default values for different components
-    constexpr uint16_t encoderSteps = 360;
+ *
+ */
+namespace Settings
+{
+    constexpr uint32_t baudRate = 115200;
+    constexpr uint32_t i2cSpeed = 400000;
 
-    namespace Pid {
+    constexpr float wheelDiameter = 0.105;
-        namespace Left {
+    constexpr float wheelDistance = 0.255;
-            constexpr uint8_t P = 75;
+    constexpr uint16_t encoderSteps = 384;
+
+    namespace Pid
+    {
+        namespace Left
+        {
+            constexpr uint8_t P = 5;
             constexpr uint8_t I = 0;
             constexpr uint8_t D = 0;
         }
 
-        namespace Right {
+        namespace Right
-            constexpr uint8_t P = 75;
+        {
+            constexpr uint8_t P = 5;
             constexpr uint8_t I = 0;
             constexpr uint8_t D = 0;
         }

include/moveControl.h

@@ -22,6 +22,15 @@
 #include "debugTimes.h"
 #include "component.h"
 
+/**
+ * @brief A struct to easy set the speed
+ *
+ */
+struct DrivingSpeeds
+{
+    double x;
+    double rot;
+};
 
 /**
  * @brief This class manages the motors and the encoders
@@ -31,24 +40,33 @@
  * given target speed to calculate a new duty cycle for the motors.
  *
  */
-class MoveControl : public Component {
+class MoveControl : public Component
-    public:
+{
+public:
     /**
-         * @brief used to set driving status
+     * @brief Used to set driving status
      *
      * When set to stop all motors are set to halt
      *
+     * When set to Raw the PIDs are not used. The target
+     * values must be given withe setRawPowerLeft() and
+     * setRawPowerRight().
+     *
+     * Drive is the normal working mode.
+     *
      */
-        enum Status {Stop,
+    enum Status
+    {
+        Stop,
         Drive,
-                    Raw};
+        Raw
-
+    };
 
     /**
      * @brief Construct a new Move Control object
      *
      * Initialize the motors, encoders and PIDs with given
-         * values in moveControlConfig.h
+     * values in config.h
      */
     MoveControl();
 
@@ -62,6 +80,9 @@ class MoveControl : public Component {
     /**
      * @brief Set the Status
      *
+     * Control if this component should work normally or with
+     * raw values for the motors.
+     *
      * @see Status
      * @param status
      */
@@ -83,27 +104,40 @@ class MoveControl : public Component {
     void setSpeed(double speed);
 
     /**
-         * @brief Set the rotationspeed
+     * @brief Set the rotationSpeed
      *
-         * If the given rotationspeed is close to zero, then the
+     * If the given rotationSpeed is close to zero, then the
-         * target rotationspeed is set to zero.
+     * target rotationSpeed is set to zero.
      * @param speed rad/s
      */
     void setRotationSpeed(double speed);
 
     /**
-         * @brief Set Raw Power Left
+     * @brief Set the speeds
      *
-         * This value has only an effect if Status is raw.
+     * This function is a combination of the functions setSpeed() and
+     * setRotationSpeed(). The struct DrivingSpeeds
+     * is used for this.
+     *
+     *
+     * @param drivingSpeeds
+     */
+    void setSpeeds(DrivingSpeeds drivingSpeeds);
+
+    /**
+     * @brief Set raw power left
+     *
+     * This value has only an effect if the status is raw.
+     * @see setDrivingStatus
      *
      * @param power between -100 and 100
      */
     void setRawPowerLeft(int16_t power);
 
     /**
-         * @brief Set the Raw Power Right
+     * @brief Set the raw power right
      *
-         * This value has only an effect if Status is raw.
+     * This value has only an effect if status is raw.
      *
      * @param power between -100 and 100
      */
@@ -113,11 +147,11 @@ class MoveControl : public Component {
      * @brief Set the pid tunings
      *
      * @param side 0 -> left, 1 -> right
-         * @param p 
+     * @param pPart
-         * @param i 
+     * @param iPart
-         * @param d 
+     * @param dPart
      */
-        void setPidTunings(uint8_t side, double p, double i, double d);
+    void setPidTunings(uint8_t side, double pPart, double iPart, double dPart);
 
     /**
      * @brief Returns the PID object of the choosen side.
@@ -125,54 +159,45 @@ class MoveControl : public Component {
      * @param side 0 -> left, 1 -> right
      * @return PID*
      */
-        PID* getPID(uint8_t side) const;
+    PID *getPID(uint8_t side) const;
 
-        /**
+    Speedometer *getSpeedometerLeft() const { return this->leftSpeedometer; }
-         * @brief Get the Speedometer Left object
+    Speedometer *getSpeedometerRight() const { return this->rightSpeedometer; }
-         * 
-         * @return Speedometer* 
-         */
-        Speedometer* getSpeedometerLeft() const { return this->left_speedometer; }
 
-        /**
+    uint16_t getDutycycleLeft() const { return this->leftMotor->getDutycycle(); }
-         * @brief Get the Speedometer Right object
+    uint16_t getDutycycleRight() const { return this->rightMotor->getDutycycle(); }
-         * 
-         * @return Speedometer* 
-         */
-        Speedometer* getSpeedometerRight() const { return this->right_speedometer; }
 
-        uint16_t getDutycycleLeft() const { return this->left_motor->getDutycycle(); }
+private:
-        uint16_t getDutycycleRight() const { return this->right_motor->getDutycycle(); }
-
-    private:
     void run() override;
-        void setSpeedometerDirection(Speedometer *speedometer, double value);
+    static void setSpeedometerDirection(Speedometer *speedometer, double value);
     void calcTargetWheelSpeed();
     void regulateMotors();
     void updateCurrentWheelSpeed();
 
-        MotorControl *left_motor;
+    MotorControl *leftMotor;
-        MotorControl *right_motor;
+    MotorControl *rightMotor;
-        Speedometer *left_speedometer;
+    Speedometer *leftSpeedometer;
-        Speedometer *right_speedometer;
+    Speedometer *rightSpeedometer;
-        PID *left_pid;
+    PID *leftPid;
-        PID *right_pid;
+    PID *rightPid;
 
     Status driving_status = Status::Stop;
+    DrivingSpeeds drivingSpeeds = {0, 0};
 
-        double x_speed = 0;
+    double wheelspeedLeftTarget = 0;
-        double rotation_speed = 0;
+    double wheelspeedRightTarget = 0;
-
+    double wheelspeedLeft = 0;
-        double wheelspeed_left_target = 0;
+    double wheelspeedRight = 0;
-        double wheelspeed_right_target = 0;
+    double leftPidOut = 0;
-        double wheelspeed_left = 0;
+    double rightPidOut = 0;
-        double wheelspeed_right = 0;
-        double left_pid_out;
-        double right_pid_out;
 
     uint8_t overTimeCounter = 0;
     uint8_t overTimeMax = 100;
     int8_t rawPowerLeft = 0;
     int8_t rawPowerRight = 0;
+
+    static constexpr int8_t maxPercentage = 100;
+    static constexpr float minSpeed = 0.2;
+    static constexpr uint8_t loopDelay = 20;
 };
 #endif // MOVE_CONTROL_H

include/network.h

@@ -1,107 +0,0 @@
-/**
- * @file network.h
- * @author Alexander Klein (alex@kleiax.de)
- * @brief Contains the Network class.
- * @version 0.1
- * @date 2021-12-14
- * 
- * @copyright Copyright (c) 2021
- * 
- */
-
-#ifndef NETWORK_H
-#define NETWORK_H
-
-#include <iostream>
-#include <WiFi.h>
-#include <PubSubClient.h>
-#include <esp_now.h>
-#include <esp_wifi.h>
-
-#include "networkConfig.h"
-
-typedef void (*recieveCallbackPtr) (const uint8_t * mac, const uint8_t *incomingData, int len); 
-typedef void (*sendCallbackPtr) (const uint8_t *mac_addr, esp_now_send_status_t status);
-
-/**
- * @brief This class handles all network stuff
- * 
- * This class only inherits static functions and members and use
- * only the configuration data is given by networkConfig.h
- * 
- */
-class Network {
-    public:
-        /**
-         * @brief Set the all IPs needed by this class
-         */
-        static void setIps();
-
-        /**
-         * @brief Establish a connection to the WiFi.
-         * 
-         * @return true Wifi is connected
-         * @return false Timeout
-         */
-        static bool connectWifi();
-        static bool connectEspNow(recieveCallbackPtr reci, sendCallbackPtr send);
-        static uint8_t getCurrentChannel();
-
-        /**
-         * @brief Set all general settings to connect to a broker.
-         */
-        static void setupMQTT();
-
-        /**
-         * @brief Checks if mqtt is still connected
-         * 
-         * If mqtt is not connect, this function try a reconnect
-         * with the function connectMQTT. This function should be
-         * called every mainloop.
-         * 
-         * @return true if connected
-         * @return false if not connected
-         */
-        static bool checkMQTT();
-        static bool initMQTT();
-
-        /**
-         * @brief Checks if WiFi is still connected
-         * 
-         * If WiFi is not connected, this function try a reconnect.
-         * This function should be called every mainloop.
-         */
-        static void checkWiFi();
-
-        /**
-         * @brief Get the Mqtt Client object
-         * 
-         * @return PubSubClient* 
-         */
-        static PubSubClient* getMqttClient();
-        static const uint8_t * getBroadcastAddress() { return broadcastAddress; }
-
-    private:
-        /**
-         * @brief Tries a connect to the broker
-         * 
-         * @return true if the connect attemp was successful
-         * @return false if the connect attemp was unsuccessful
-         */
-        static bool connectMQTT();
-        static int32_t getWiFiChannel(const char *ssid);
-
-        static esp_now_peer_info_t peerInfo;
-        static uint8_t broadcastAddress[6];
-
-        static IPAddress local_IP;
-        static IPAddress gateway;
-        static IPAddress subnet;
-        static IPAddress mqtt_server;
-        static IPAddress dnsServer;
-
-        static WiFiClient wifi_client;
-        static PubSubClient* mqtt_client;
-};
-
-#endif // NETWORK_H

include/networkConfig.h

@@ -12,94 +12,65 @@
  *
  */
 
-// General config
+#include <stdint.h>
-#define MQTT_TIME_RECONNECT 2500
 
-/**
+#define HW1
- * @brief Wlan connect timeout
- * 
- * This number multiplied by WLAN_CONNECT_LOOP_TIME is the 
- * timeout in milliseconds.
- */
-#define WLAN_CONNECT_TIMEOUT 20
 
-/**
- * @brief Wlan connect loop time
- * 
- * The time in milliseconds until the next connection attempt.
- */
-#define WLAN_CONNECT_LOOP_TIME 500
-#define WLAN_DNS_SERVER     "8.8.8.8"
-#define ESP_NOW_CONTROLER_MAC {0xC8, 0xC9, 0xA3, 0xC8, 0x57, 0x10}
-
-#define NTRIP_RTK2GO
-
-// NTRIP config NRW
-#ifdef NTRIP_NRW
-#define NTRIP_HOST          "www.sapos-nw-ntrip.de"
-#define NTRIP_PORT          2101
-#define NTRIP_MOUNT_POINT   "VRS_3_3G_NW"
-#define NTRIP_USER          "nw-916771"
-#define NTRIP_PASSWORD      "Schalke#246"
-#endif // NTRIP_NRW
-
-// NTRIP config NS
-#ifdef NTRIP_NS
-#define NTRIP_HOST          "openservice-sapos.niedersachsen.de"
-#define NTRIP_PORT          2101
-#define NTRIP_MOUNT_POINT   "VRS_3_2G_NI"
-#define NTRIP_USER          "ni_FHDo01"
-#define NTRIP_PASSWORD      "ALdx-1-3e49"
-#endif // NTRIP_NS
-
-// NTRIP config RTK2Go
-#ifdef NTRIP_RTK2GO
-#define NTRIP_HOST          "rtk2go.com"
-#define NTRIP_PORT          2101
-#define NTRIP_MOUNT_POINT   "GER-Dortmund"
-#define NTRIP_USER          "alklein1@gmx.de"
-#define NTRIP_PASSWORD      "none"
-#endif // NTRIP_RTK2GO
-
-
-#define HOTSPOT
-
-//Network config Hotspot
 #ifdef HOTSPOT
-#define WLAN_SSID "Kleiax Handy"
+namespace NetworkConfig
-#define WLAN_PASSWORD "12345677"
+{
-#define WLAN_IP "192.168.43.4"
+    const char ssid[] = "Kleiax Handy";
-#define WLAN_SUBNETMASK "255.255.128.0"
+    const char password[] = "12345677";
-#define WLAN_GATEWAY "192.168.43.1"
+    const char ip[] = "192.168.43.4";
-// Only for defines reasons
+    const char subnet[] = "255.255.255.0";
-    #define MQTT_SERVER "172.22.64.216"
+    const char gateway[] = "192.168.43.1";
-    #define MQTT_PORT 1883
+    const char dns[] = "8.8.8.8";
-#endif //HOTSPOT
+    const bool mqtt = false;
+}
+#endif // HOTSPOT
 
-//Network config Rhede
+#ifdef HW1
+namespace NetworkConfig
+{
+    const char ssid[] = "hw1_gast";
+    const char password[] = "KeineAhnung";
+    const char ip[] = "192.168.0.4";
+    const char subnet[] = "255.255.255.0";
+    const char gateway[] = "192.168.0.1";
+    const char dns[] = "8.8.8.8";
+    const bool mqtt = false;
+}
+#endif // HW1
+
+// Network config Rhede
 #ifdef RHEDE
-#define WLAN_SSID "LebennigHuus"
+namespace NetworkConfig
-#define WLAN_PASSWORD "Punica-699"
+{
-#define WLAN_IP "192.168.11.4"
+    const char ssid[] = "LebennigHuus";
-#define WLAN_SUBNETMASK "255.255.128.0"
+    const char password[] = "Punica-699";
-#define WLAN_GATEWAY "192.168.0.1"
+    const char ip[] = "192.168.11.4";
-#define MQTT
+    const char subnet[] = "255.255.128.0";
-#define MQTT_SERVER "192.168.1.7"
+    const char gateway[] = "192.168.0.1";
-#define MQTT_PORT 1883
+    const char dns[] = "8.8.8.8";
-#define MQTT_AUTH
+    const bool mqtt = true;
-#define MQTT_USER "kleiax"
+}
-#define MQTT_PASSWORD "p?{$_~5%hBM7wrcFkr55KWr#"
+#endif // RHEDE
-#endif //RHEDE
 
-//Network config Doro
+namespace MqttConfig
-#ifdef DORO
+{
-#define WLAN_SSID "Tuedelkram"
+    const char server[] = "192.168.1.7";
-#define WLAN_PASSWORD "3Kaesehoch!"
+    const uint16_t port = 1883;
-#define WLAN_IP "192.168.178.4"
+    const char user[] = "kleiax";
-#define WLAN_SUBNETMASK "255.255.255.0"
+    const char password[] = "p?{$_~5%hBM7wrcFkr55KWr#";
-#define WLAN_GATEWAY "192.168.178.1"
+}
-    // Only for defines reasons
-    #define MQTT_SERVER "172.22.64.216"
-    #define MQTT_PORT 1883
-#endif //DORO
 
+namespace NtripConfig
+{
+    const char host[] = "rtk2go.com";
+    const uint16_t port = 2101;
+    // const char mountPoint[] = "GER-Dortmund";
+    // const char mountPoint[] = "GER-Papenburg";
+    const char mountPoint[] = "Rtkramerntrip";
+    const char user[] = "alklein1@gmx.de";
+    const char password[] = "none";
+}

lib/Battery/battery.cpp

@@ -11,137 +11,228 @@
 
 #include "battery.h"
 
-Battery::Battery(uint8_t pin, uint32_t r1, uint32_t r2) {
+Battery::Battery(uint8_t pin, uint32_t firstResistor, uint32_t secondResistor)
-    this->pin = pin;
+    : pin{pin}, firstResistor{firstResistor}, secondResistor{secondResistor},
-    this->r1 = r1;
+      batteryVoltageFactor{firstResistor + secondResistor / static_cast<double>(secondResistor)}
-    this->r2 = r2;
+{
     this->initBuffer();
-    this->loopDelay = 100;
+    Component::loopDelay = Battery::loopDelay;
 }
 
-Battery::Battery(uint8_t pin) {
+Battery::Battery(uint8_t pin)
-    this->pin = pin;
+    : pin{pin}
+{
     this->initBuffer();
-    this->loopDelay = 100;
+    Component::loopDelay = Battery::loopDelay;
 }
 
-void Battery::run() {
+void Battery::run()
+{
+    if (this->calibrationState != CalibrationState::None)
+    {
+        this->runCalibration();
+        return;
+    }
+
     this->readAdcToBuf();
     this->loopCounter++;
 
-    if (this->loopCounter == this->calulationDelayMultiplier) {
+    if (this->loopCounter >= this->calculateDelayMultiplier)
+    {
         this->calculateBatteryVoltage();
         this->calculateBatteryPercent();
         this->loopCounter = 0;
-        this->calculatetNewValues = true;
+        this->calculatedNewValues = true;
+    }
+}
+
+void Battery::runCalibration()
+{
+    if (this->calibrationState != CalibrationState::Reading)
+    {
+        return;
     }
 
-    return;
+    this->readAdcToBuf();
+    if (this->bufferPos == 0)
+    {
+        const uint16_t res = this->getBufAvg();
+        this->newRawAdcVoltages[this->currentCalibrationVoltage] = res;
+        std::cout << "Index: "
+                  << (int)this->currentCalibrationVoltage
+                  << " Value: "
+                  << (int)res
+                  << std::endl;
+        this->currentCalibrationVoltage++;
+        this->calibrationState = CalibrationState::Waiting;
+
+        if (this->currentCalibrationVoltage == Battery::rawAdcVoltagesCount)
+        {
+            this->calibrationState = CalibrationState::Finished;
+        }
+    }
 }
 
-double Battery::getBatteryVoltage() const {
+double Battery::getBatteryVoltage() const
-    double res = this->batteryVoltage;
+{
-    return (int)(res*100+0.5)/100.0;
+    return static_cast<int>((this->batteryVoltage * 100 + 0.5)) / 100.0;
 }
 
-bool Battery::isBatteryLow(double voltage) const {
+bool Battery::isBatteryLow(double voltage) const
+{
     if (this->getBatteryVoltage() <= voltage && this->batteryVoltage > this->absurdLowVoltage)
+    {
         return true;
+    }
     return false;
 }
 
-bool Battery::isNewValue() {
+bool Battery::isNewValue()
-    if (!this->calculatetNewValues)
+{
+    if (!this->calculatedNewValues)
+    {
         return false;
+    }
 
-    this->calculatetNewValues = false;
+    this->calculatedNewValues = false;
     return true;
 }
 
-double Battery::calculateInputVoltage() {
+void Battery::nextVoltageIsReady()
+{
+    if (this->calibrationState == CalibrationState::Waiting)
+    {
+        this->calibrationState = CalibrationState::Reading;
+    }
+}
+
+void Battery::startCalibration()
+{
+    this->calibrationState = CalibrationState::Waiting;
+    this->currentCalibrationVoltage = 0;
+    this->bufferPos = 0;
+    Component::loopDelay = Battery::loopDelay / 2;
+    this->newRawAdcVoltages = new uint16_t[Battery::rawAdcVoltagesCount];
+}
+
+void Battery::finishCalibration()
+{
+    if (this->calibrationState != CalibrationState::None)
+    {
+        return;
+    }
+
+    delete[] this->newRawAdcVoltages;
+    this->calibrationState = CalibrationState::None;
+    Component::loopDelay = Battery::loopDelay;
+}
+
+double Battery::calculateInputVoltage()
+{
     // Reference voltage is 3v3 so maximum reading is 3v3 = 4095 in range 0 to 4095
     double reading = this->getBufAvg();
-    if(reading < 1 || reading > 4095) return 0;
+    if (reading < 1 || reading > Battery::adcMaxValue)
-    return  - 0.000000000000016 * pow(reading,4)
+    {
-            + 0.000000000118171 * pow(reading,3)
+        return 0;
-            - 0.000000301211691 * pow(reading,2)
+    }
-            + 0.001109019271794 * reading
+
-            + 0.034143524634089;
+    return -this->adcCurveCoefficient[0] * pow(reading, 4) + this->adcCurveCoefficient[1] * pow(reading, 3) - this->adcCurveCoefficient[2] * pow(reading, 2) + this->adcCurveCoefficient[3] * reading + this->adcCurveCoefficient[4];
 }
 
-void Battery::calculateBatteryVoltage() {
+void Battery::calculateBatteryVoltage()
-    if (this->r1 && this->r2) {
+{
-        this->batteryVoltage = (this->calculateInputVoltage() * (double) (this->r1 + this->r2)) / (double) this->r2;
+    if (this->firstResistor && this->secondResistor)
+    {
+        this->batteryVoltage = this->calculateInputVoltage() * this->batteryVoltageFactor;
         return;
     }
 
-    uint16_t adcValue = this->getBufAvg();
+    const uint16_t adcValue = this->getBufAvg();
-    if (adcValue < this->rawAdcVoltages[0]) {
+    if (adcValue < this->rawAdcVoltages[0])
+    {
         this->batteryVoltage = -1;
         return;
     }
 
-    if (adcValue > this->rawAdcVoltages[this->rawAdcVoltagesCount] + 50) {
+    uint8_t index = 1;
-        this->batteryVoltage = -2;
+    for (; index < this->rawAdcVoltagesCount; index++)
-        return;
+    {
-    }
-
-    uint8_t index;
-    for (index = 1; index < this->rawAdcVoltagesCount; index++) {
         if (adcValue < this->rawAdcVoltages[index])
+        {
             break;
         }
-    double indexDelta = this->rawAdcVoltages[index] - this->rawAdcVoltages[index - 1];
+    }
-    double valueDelta = this->rawAdcVoltages[index] - adcValue;
+    const double indexDelta = this->rawAdcVoltages[index] - this->rawAdcVoltages[index - 1];
+    const double valueDelta = this->rawAdcVoltages[index] - adcValue;
     double voltage = this->startVoltage + (index - 1) * this->stepVoltage;
     voltage += valueDelta / indexDelta * this->stepVoltage;
     this->batteryVoltage = voltage;
-
-    // std::cout << "Battery::calculateBatteryVoltage() - Voltage: " << voltage << " Index: " <<(int) index << " adcValue: " << (int) adcValue <<" iD: " << indexDelta << " vD: " << valueDelta << std::endl;
 }
 
-void Battery::calculateBatteryPercent() {
+void Battery::calculateBatteryPercent()
+{
     int8_t size = sizeof(this->capacityVoltages) / sizeof(*this->capacityVoltages);
-    uint8_t i;
+    uint8_t index = 0;
-    for (i = 0; i < size; i++) {
+    for (; index < size; index++)
-        if (this->batteryVoltage <= this->capacityVoltages[i])
+    {
+        if (this->batteryVoltage <= this->capacityVoltages[index])
+        {
             break;
         }
-    
-    if (i == 0) {
-        if (this->batteryVoltage > 6)
-            std::cout << "Critical low battery!" << std::endl;
-    } else if (i == size - 1) {
-
-    } else {
-        double diffToLowerVal = this->batteryVoltage - this->capacityVoltages[i - 1];
-        double diffToHigherVal = this->capacityVoltages[i] - this->batteryVoltage;
-        if (diffToLowerVal > diffToHigherVal)
-            i--;
     }
-    this->batteryPercent = i * (100 / (size - 1));
+
+    if (index == 0)
+    {
+        if (this->batteryVoltage > this->absurdLowVoltage)
+        {
+            std::cout << "Critical low battery!" << std::endl;
+        }
+    }
+    else if (index == size - 1)
+    {
+    }
+    else
+    {
+        const double diffToLowerVal = this->batteryVoltage - this->capacityVoltages[index - 1];
+        const double diffToHigherVal = this->capacityVoltages[index] - this->batteryVoltage;
+        if (diffToLowerVal > diffToHigherVal)
+        {
+            index--;
+        }
+    }
+    this->batteryPercent = index * (100 / (size - 1));
 }
 
-void Battery::readAdcToBuf() {
+void Battery::readAdcToBuf()
+{
     this->adcBuffer[this->bufferPos] = analogRead(this->pin);
     this->bufferPos++;
     if (this->bufferPos == Battery::bufferSize)
+    {
         this->bufferPos = 0;
+    }
 
     // std::cout << "Battery::readAdcToBuf added Value: " << this->adcBuffer[this->bufferPos] << std::endl;
 }
 
-void Battery::initBuffer() {
+void Battery::initBuffer()
+{
     for (uint8_t i = 0; i < Battery::bufferSize; i++)
+    {
         this->adcBuffer[i] = 0;
+    }
 }
 
-uint16_t Battery::getBufAvg() const {
+uint16_t Battery::getBufAvg() const
+{
     uint32_t res = 0;
     uint8_t emptyPos = 0;
-    for (uint8_t i = 0; i < Battery::bufferSize; i++) {
+    for (uint8_t i = 0; i < Battery::bufferSize; i++)
+    {
         if (this->adcBuffer[i] == 0)
+        {
             emptyPos++;
+        }
         res += this->adcBuffer[i];
     }
     return res / (Battery::bufferSize - emptyPos);

lib/Battery/battery.h

@@ -24,23 +24,50 @@
  *
  * This class reads the voltage from an analog pin to calculate the
  * charge level of a 3 Cell Li-Poly battery pack. The battery pack have
- * to be after a voltage diveder, so that maximum voltage for the
+ * to be after a voltage divider, so that maximum voltage for the
  * microcontroller is 3.3 Volt.
  */
-class Battery : public Component {
+class Battery : public Component
-    public:
+{
+public:
+    /**
+     * @brief States when the calibration modes is active
+     *
+     * - None means that no calibration is running
+     * - Reading means that the adc takes multiple values to calculate an average
+     * - Waiting means that the user has to set the new wanted voltage
+     * - Finished means that all measurements was taken
+     */
+    enum CalibrationState
+    {
+        None,
+        Reading,
+        Waiting,
+        Finished
+    };
+
     /**
      * @brief Construct a new Battery object
      *
-         * The voltage devider have to be calculated, so that the input
+     * The voltage divider have to be calculated, so that the input
      * voltage from 3.3 Volt is never exceeded. It is assumed that
      * the microcontroller is connected to the second resistor.
      *
      * @param pin The analog to read from.
-         * @param r1 First resistor of the voltage devider.
+     * @param firstResistor First resistor of the voltage divider.
-         * @param r2 Second resistor of the voltage devider.
+     * @param secondResistor Second resistor of the voltage divider.
+     */
+    Battery(uint8_t pin, uint32_t firstResistor, uint32_t secondResistor);
+
+    /**
+     * @brief Construct a new Battery object
+     *
+     * With this constructor the real voltage is not calculated with the
+     * voltage divider but with a table which contains the raw reading from
+     * the adc mapped to a specific voltage
+     *
+     * @param pin
      */
-        Battery(uint8_t pin, uint32_t r1, uint32_t r2);
     Battery(uint8_t pin);
 
     /**
@@ -70,10 +97,55 @@ class Battery : public Component {
      */
     bool isBatteryLow(double voltage) const;
 
+    /**
+     * @brief Check if a new voltage was been calculated
+     *
+     * @return true
+     * @return false
+     */
     bool isNewValue();
 
-    private:
+    // Calibration
+    CalibrationState getCalibrationState() const { return this->calibrationState; }
+
+    /**
+     * @brief Get the current calibration voltage target
+     *
+     * The returned value stand for the index of the table for this reason
+     * the real value have to be calculated. After the returned voltage has been set
+     * you have to call nextVoltageIsReady().
+     *
+     * @return uint8_t voltage multiply with 0,1 and add 7
+     */
+    uint8_t getCurrentCalibrationVoltage() const { return this->currentCalibrationVoltage; }
+
+    /**
+     * @brief Read next wanted voltage
+     *
+     * If this function is called, the calibration mode reads the new voltage
+     * and save the value in the table.
+     */
+    void nextVoltageIsReady();
+
+    /**
+     * @brief Calibrate the battery readings
+     *
+     * This calibration has only an effect if the Component
+     * uses the table with the raw adc values. The calibration gives
+     * the user different voltages that have to be set with a
+     * laboratory power supply. The power supply have to be connected
+     * instead of the battery.
+     */
+    void startCalibration();
+
+    /**
+     * @brief abort the calibration
+     */
+    void finishCalibration();
+
+private:
     void run() override;
+    void runCalibration();
     double calculateInputVoltage();
     void calculateBatteryVoltage();
     void calculateBatteryPercent();
@@ -81,38 +153,50 @@ class Battery : public Component {
     void initBuffer();
     uint16_t getBufAvg() const;
 
-        static const uint8_t bufferSize = 30;
+    static constexpr uint8_t bufferSize = 30;
+    static constexpr uint8_t loopDelay = 100;
+    static constexpr uint16_t adcMaxValue = 4095;
+
+    CalibrationState calibrationState = CalibrationState::None;
 
     uint8_t absurdLowVoltage = 5;
     uint8_t pin;
     uint8_t batteryPercent = 0;
     uint8_t batteryLowPercent = 10;
     uint8_t bufferPos = 0;
-        uint8_t calulationDelayMultiplier = 10;
+    uint8_t calculateDelayMultiplier = 5;
     uint8_t loopCounter = 0;
+    uint8_t currentCalibrationVoltage = 0; // *0.1 + 7
     uint16_t adcBuffer[bufferSize];
-        uint32_t r1 = 0;
+    uint16_t *newRawAdcVoltages = nullptr;
-        uint32_t r2 = 0;
+    uint32_t firstResistor = 0;
-        bool calculatetNewValues = false;
+    uint32_t secondResistor = 0;
+    bool calculatedNewValues = false;
     double batteryVoltage = 0;
+    double batteryVoltageFactor;
 
     const float capacityVoltages[21] = {9.82, 10.83, 11.06, 11.12,  //  0  5 10 15
                                         11.18, 11.24, 11.3, 11.36,  // 20 25 30 35
                                         11.39, 11.45, 11.51, 11.56, // 40 45 50 55
                                         11.62, 11.74, 11.86, 11.95, // 60 65 70 75
                                         12.07, 12.25, 12.33, 12.45, // 80 85 90 95
-                                            12.6 };
+                                        12.6};
 
-        const uint8_t rawAdcVoltagesCount = 60;
+    static constexpr uint8_t rawAdcVoltagesCount = 60;
     const double startVoltage = 7;
     const double stepVoltage = 0.1;
-        const uint16_t rawAdcVoltages[60] = // from 7.0V to 12.9V in 0.1V steps
+    const uint16_t rawAdcVoltages[rawAdcVoltagesCount] = // from 7.0V to 12.9V in 0.1V steps
-                           {1820, 1851, 1880, 1910, 1937, 1967, 1992, 2020, 2048, 2080,
+        {1992, 2021, 2056, 2090, 2118, 2145, 2177, 2208, 2241, 2272,
-                            2109, 2136, 2163, 2189, 2218, 2244, 2273, 2302, 2334, 2363,
+         2298, 2331, 2362, 2387, 2420, 2453, 2482, 2514, 2543, 2577,
-                            2391, 2415, 2441, 2471, 2499, 2531, 2557, 2587, 2617, 2646,
+         2607, 2640, 2670, 2703, 2736, 2763, 2794, 2826, 2858, 2890,
-                            2674, 2699, 2730, 2761, 2791, 2816, 2843, 2872, 2900, 2930,
+         2920, 2956, 2983, 3019, 3054, 3088, 3121, 3158, 3189, 3226,
-                            2958, 2991, 3017, 3049, 3080, 3115, 3144, 3178, 3208, 3242,
+         3264, 3300, 3339, 3379, 3414, 3453, 3500, 3544, 3598, 3636,
-                            3276, 3313, 3346, 3389, 3433, 3470, 3509, 3548, 3590, 3636};
+         3682, 3730, 3781, 3837, 3887, 3943, 3997, 4054, 4093, 4095};
+    const double adcCurveCoefficient[5] = {0.000000000000016,
+                                           0.000000000118171,
+                                           0.000000301211691,
+                                           0.001109019271794,
+                                           0.034143524634089};
 };
 
 #endif // BATTERY_H

lib/CalcAzimuth/calcAzimuth.cpp

@@ -11,57 +11,84 @@
 
 #include "calcAzimuth.h"
 
-CalcAzimuth::CalcAzimuth(Point point) {
+CalcAzimuth::CalcAzimuth(Point point)
-    this->lastChangePoint = point;
+    : lastChangePoint{point}, currentPosition{point}
-    this->currentPosition = point;
+{
-    this->loopDelay = 50;
+    Component::loopDelay = CalcAzimuth::loopDelay;
 }
 
-void CalcAzimuth::drivingDirectionChange(Point point) {
+void CalcAzimuth::drivingDirectionChange(Point point)
-    if (point.isInit() && point.isValid()) {
+{
+    if (point.isInit() && point.isValid())
+    {
         this->directionChangeMode = true;
         this->lastChangePoint = point;
         this->state = State::Invalid;
     }
 }
 
-void CalcAzimuth::updateCurrentPosition(Point point) {
+void CalcAzimuth::updateCurrentPosition(Point point)
+{
     this->currentPosition = point;
     this->positionChanged = true;
 }
 
-void CalcAzimuth::run() {
+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() {
+void CalcAzimuth::updateAzimuth()
-    if (!this->directionChangeMode
+{
-        || this->lastChangePoint.distanceTo(this->currentPosition) < 1.0) 
+    if (!this->directionChangeMode || this->lastChangePoint.distanceTo(this->currentPosition) < 1.0)
     {
         this->state = State::Invalid;
-        this->calcAzimuth = 999;
+        this->calcAzimuth = INT16_MIN;
         return;
     }
 
     this->calcAzimuth = this->lastChangePoint.courseTo(this->currentPosition);
 
     // Map point accuracy to State
-    if (this->lastChangePoint.getAccuracy() == Point::Accuracy::oneDigOfCM
+    if (this->lastChangePoint.getAccuracy() == Point::Accuracy::oneDigOfCM || this->currentPosition.getAccuracy() == Point::Accuracy::oneDigOfCM)
-        || this->currentPosition.getAccuracy() == Point::Accuracy::oneDigOfCM) 
     {
         this->state = State::Good;
     }
-    else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::twoDigOfCM
+    else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::twoDigOfCM || this->currentPosition.getAccuracy() == Point::Accuracy::twoDigOfCM)
-        || this->currentPosition.getAccuracy() == Point::Accuracy::twoDigOfCM) 
     {
         this->state = State::Ok;
     }
-    else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::threeDigOfCM
+    else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::threeDigOfCM || this->currentPosition.getAccuracy() == Point::Accuracy::threeDigOfCM)
-        || this->currentPosition.getAccuracy() == Point::Accuracy::threeDigOfCM) 
     {
         this->state = State::Bad;
     }
@@ -71,17 +98,19 @@ void CalcAzimuth::updateAzimuth() {
     }
 
     // Upgrade quality if the range grows up
-    if (this->lastChangePoint.distanceTo(this->currentPosition) > 2.0) {
+    if (this->lastChangePoint.distanceTo(this->currentPosition) > this->minDistanceForBetterQuality)
-        switch (this->state) {
+    {
-            case State::Bad :
+        switch (this->state)
+        {
+        case State::Bad:
             this->state = State::Ok;
             break;
 
-            case State::Ok :
+        case State::Ok:
             this->state = State::Good;
             break;
 
-            case State::Good :
+        case State::Good:
             this->state = State::Super;
             break;
 

lib/CalcAzimuth/calcAzimuth.h

@@ -1,7 +1,7 @@
 /**
  * @file calcAzimuth.h
  * @author Alexander Klein (alex@kleiax.de)
- * @brief 
+ * @brief Contains a class that calculates the azimuth from a last and a current position
  * @version 0.1
  * @date 2023-09-03
  *
@@ -15,9 +15,20 @@
 #include "component.h"
 #include "point.h"
 
-class CalcAzimuth : public Component {
+/**
-    public:
+ * @brief A class to calculate an azimuth
-        enum State {
+ *
+ * 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,
@@ -25,16 +36,45 @@ class CalcAzimuth : public Component {
         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; }
 
-    private:
+    static String stateToString(State state);
+
+private:
     void run() override;
     void updateAzimuth();
 
@@ -45,6 +85,9 @@ class CalcAzimuth : public Component {
     bool positionChanged = false;
     bool directionChangeMode = false;
     int16_t calcAzimuth = INT16_MAX;
+    double minDistanceForBetterQuality = 2;
+
+    static constexpr uint8_t loopDelay = 50;
 };
 
-#endif //CALC_AZIMUTH_H
+#endif // CALC_AZIMUTH_H

lib/CalibrateCompass/calibrateCompass.cpp

@@ -11,58 +11,71 @@
 
 #include "calibrateCompass.h"
 
-CalibrateCompass::CalibrateCompass(QMC5883LCompass* compass) {
+CalibrateCompass::CalibrateCompass(QMC5883LCompass *compass)
-    this->compass = compass;
+    : compass{compass}
+{
     this->state = State::Ready;
     this->clearData();
     this->activateOnlyChilds();
 }
 
-void CalibrateCompass::runAsChild() {
+void CalibrateCompass::runAsChild()
+{
     if (this->state != State::Calibrating)
+    {
         return;
+    }
 
     bool changed = false;
 
     this->compass->read();
-    int x = this->compass->getX();
+    const int xAxis = this->compass->getX();
-    int y = this->compass->getY();
+    const int yAxis = this->compass->getY();
-    int z = this->compass->getZ();
+    const int zAxis = this->compass->getZ();
 
-    if(x < this->data.data[0][0]) {
+    if (xAxis < this->data.data[0][0])
-        this->data.data[0][0] = x;
+    {
+        this->data.data[0][0] = xAxis;
         changed = true;
     }
 
-    if(x > this->data.data[0][1]) {
+    if (xAxis > this->data.data[0][1])
-        this->data.data[0][1] = x;
+    {
+        this->data.data[0][1] = xAxis;
         changed = true;
     }
 
-    if(y < this->data.data[1][0]) {
+    if (yAxis < this->data.data[1][0])
-        this->data.data[1][0] = y;
+    {
+        this->data.data[1][0] = yAxis;
         changed = true;
     }
 
-    if(y > this->data.data[1][1]) {
+    if (yAxis > this->data.data[1][1])
-        this->data.data[1][1] = y;
+    {
+        this->data.data[1][1] = yAxis;
         changed = true;
     }
 
-    if(z < this->data.data[2][0]) {
+    if (zAxis < this->data.data[2][0])
-        this->data.data[2][0] = z;
+    {
+        this->data.data[2][0] = zAxis;
         changed = true;
     }
 
-    if(z > this->data.data[2][1]) {
+    if (zAxis > this->data.data[2][1])
-        this->data.data[2][1] = z;
+    {
+        this->data.data[2][1] = zAxis;
         changed = true;
     }
 
     if (changed)
+    {
         this->lastChange = millis();
+    }
 
-    if (millis() - this->lastChange > this->maxTimeWithoutChange) {
+    if (millis() - this->lastChange > this->maxTimeWithoutChange)
+    {
         this->state = State::Finished;
         this->checkDataValidity();
     }
@@ -70,9 +83,12 @@ void CalibrateCompass::runAsChild() {
 
 void CalibrateCompass::run() {}
 
-void CalibrateCompass::start() {
+void CalibrateCompass::start()
+{
     if (this->state != State::Ready)
+    {
         return;
+    }
 
     this->clearData();
     this->state = State::Calibrating;
@@ -80,35 +96,41 @@ void CalibrateCompass::start() {
     this->lastChange = millis();
 }
 
-void CalibrateCompass::useData() {
+void CalibrateCompass::useData()
-    if (!this->dataValid) {
+{
+    if (!this->dataValid)
+    {
         std::cout << "CalibrateCompass::useData - Data not valid" << std::endl;
         return;
     }
 
-    this->compass->setCalibration(  this->data.data[0][0],
+    this->compass->setCalibration(this->data.data[0][0],
                                   this->data.data[0][1],
                                   this->data.data[1][0],
                                   this->data.data[1][1],
                                   this->data.data[2][0],
-                                    this->data.data[2][1]
+                                  this->data.data[2][1]);
-                                    );
 
     std::cout << "CalibrateCompass::useData " << *this << std::endl;
 }
 
-void CalibrateCompass::removeCalibration() {
+void CalibrateCompass::removeCalibration()
+{
     this->compass->clearCalibration();
 }
 
-void CalibrateCompass::reset() {
+void CalibrateCompass::reset()
+{
     this->clearData();
     this->state = State::Ready;
 }
 
-void CalibrateCompass::saveData() {
+void CalibrateCompass::saveData()
+{
     if (!this->dataValid)
+    {
         return;
+    }
 
     Preferences preferences;
     preferences.begin("compass", false);
@@ -123,7 +145,8 @@ void CalibrateCompass::saveData() {
     preferences.end();
 }
 
-void CalibrateCompass::loadData() {
+void CalibrateCompass::loadData()
+{
     Preferences preferences;
     preferences.begin("compass", true);
 
@@ -138,19 +161,22 @@ void CalibrateCompass::loadData() {
     this->checkDataValidity();
 }
 
-void CalibrateCompass::clearData() {
+void CalibrateCompass::clearData()
-    for (uint8_t i = 0; i < 3; i++) {
+{
+    for (uint8_t i = 0; i < 3; i++)
+    {
         this->data.data[i][0] = 0;
         this->data.data[i][1] = 0;
     }
     this->dataValid = false;
 }
 
-void CalibrateCompass::checkDataValidity() {
+void CalibrateCompass::checkDataValidity()
+{
     int sum = 0;
-    for (uint8_t i = 0; i < 3; i++) {
+    for (uint8_t i = 0; i < 3; i++)
-        if (this->data.data[i][0] > INT16_MAX || this->data.data[i][0] < INT16_MIN
+    {
-            || this->data.data[i][1] > INT16_MAX || this->data.data[i][1] < INT16_MIN) 
+        if (this->data.data[i][0] > INT16_MAX || this->data.data[i][0] < INT16_MIN || this->data.data[i][1] > INT16_MAX || this->data.data[i][1] < INT16_MIN)
         {
             this->dataValid = false;
             return;
@@ -158,22 +184,23 @@ void CalibrateCompass::checkDataValidity() {
         sum += this->data.data[i][0];
         sum += this->data.data[i][1];
     }
-    this->dataValid = sum;
+    this->dataValid = static_cast<bool>(sum);
 }
 
-std::ostream& operator<<(std::ostream& os, const CalibrateCompass& caliComp) {
+std::ostream &operator<<(std::ostream &stream, const CalibrateCompass &caliComp)
-    os << "(";
+{
-    os << caliComp.data.data[0][0];
+    stream << "(";
-    os << ", ";
+    stream << caliComp.data.data[0][0];
-    os << caliComp.data.data[0][1];
+    stream << ", ";
-    os << ", ";
+    stream << caliComp.data.data[0][1];
-    os << caliComp.data.data[1][0];
+    stream << ", ";
-    os << ", ";
+    stream << caliComp.data.data[1][0];
-    os << caliComp.data.data[1][1];
+    stream << ", ";
-    os << ", ";
+    stream << caliComp.data.data[1][1];
-    os << caliComp.data.data[2][0];
+    stream << ", ";
-    os << ", ";
+    stream << caliComp.data.data[2][0];
-    os << caliComp.data.data[2][1];
+    stream << ", ";
-    os << ")";
+    stream << caliComp.data.data[2][1];
-    return os;
+    stream << ")";
+    return stream;
 }

lib/CalibrateCompass/calibrateCompass.h

@@ -0,0 +1,110 @@
+/**
+ * @file calibrateCompass.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains a class to calibrate the compass module
+ * @version 0.1
+ * @date 2023-05-23
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+
+#pragma once
+
+#include <QMC5883LCompass.h>
+#include <Preferences.h>
+#include <iostream>
+
+#include "component.h"
+
+/**
+ * @brief A Class to calibrate the compass
+ *
+ * This class reads the raw values of the compass while
+ * the rove have to be moved. The lowest and highest values
+ * are used to calibrate the compass to the current location.
+ */
+class CalibrateCompass : public Component
+{
+public:
+    /**
+     * @brief State of the calibration process
+     */
+    enum State
+    {
+        Ready,
+        Calibrating,
+        Finished
+    };
+
+    /**
+     * @brief Type for calibration data
+     *
+     * The data consist of 6 values. For each for the 3 axis
+     * are to integer needed.
+     */
+    struct CalibrationData
+    {
+        int data[3][2];
+    };
+
+    CalibrateCompass(QMC5883LCompass *compass);
+
+    /**
+     * @brief Starts the calibration
+     */
+    void start();
+
+    /**
+     * @brief Use the measured calibration data
+     */
+    void useData();
+
+    /**
+     * @brief Remove the measured calibration data
+     */
+    void removeCalibration();
+
+    /**
+     * @brief Reset the calibration process to start again
+     */
+    void reset();
+
+    /**
+     * @brief Save the measured calibration data to the flash
+     */
+    void saveData();
+
+    /**
+     * @brief Load the measured calibration data from the flash
+     *
+     */
+    void loadData();
+
+    State getState() const { return this->state; }
+    CalibrationData getCalibrationData() const { return this->data; }
+
+    /**
+     * @brief Makes the calibration data printable with std::cout()
+     *
+     * @param stream
+     * @param caliComp
+     * @return std::ostream&
+     */
+    friend std::ostream &operator<<(std::ostream &stream, const CalibrateCompass &caliComp);
+
+private:
+    void runAsChild() override;
+    void run() override;
+    void checkDataValidity();
+
+    QMC5883LCompass *compass;
+    State state = State::Ready;
+    CalibrationData data{};
+
+    void clearData();
+
+    bool dataValid = false;
+    const uint16_t maxTimeWithoutChange = 10000;
+    uint32_t lastChange = 0;
+};

lib/Component/component.cpp

@@ -1,25 +1,35 @@
 #include "component.h"
 
-Component::Component(uint16_t loopDelay) {
+Component::Component(uint16_t loopDelay) : loopDelay{loopDelay}
-    this->loopDelay = loopDelay;
+{
 }
 
-void Component::loop() {
+void Component::loop()
+{
     if (!this->active)
+    {
         return;
+    }
 
-    if (this->childComponents.size()){
+    if (this->childComponents.size())
-        std::list<Component*>::iterator it;
+    {
+        std::list<Component *>::iterator it;
         for (it = this->childComponents.begin(); it != this->childComponents.end(); it++)
+        {
             (*it)->loop();
         }
-    this->runAsChild();
+    }
+    this->runAsChild();F
 
     if (this->onlyChilds)
+    {
         return;
+    }
 
-    if (this->loopDelay && millis() - this->lastMillis < this->loopDelay)
+    if (static_cast<bool>(this->loopDelay) && millis() - this->lastMillis < this->loopDelay)
+    {
         return;
+    }
 
     this->lastMillis = millis();
 
@@ -28,13 +38,17 @@ void Component::loop() {
     this->afterRun();
 
     if (this->timeUpdateAfter)
+    {
         this->lastMillis = millis();
+    }
 }
 
-void Component::addChildComponent(Component* child) {
+void Component::addChildComponent(Component *child)
+{
     this->childComponents.push_back(child);
 }
 
-void Component::removeChildComponent(Component* child) {
+void Component::removeChildComponent(Component *child)
+{
     this->childComponents.remove(child);
 }

lib/Component/component.h

@@ -1,7 +1,7 @@
 /**
  * @file component.h
  * @author Alexander Klein (alex@kleiax.de)
- * @brief 
+ * @brief Contains an interface to make non blocking components with delay.
  * @version 0.1
  * @date 2023-08-16
  *
@@ -15,34 +15,127 @@
 
 #include <list>
 
-class Component {
+/**
-    public:
+ * @brief An Interface to make components
+ *
+ * A component is a task that be called in a loop which not
+ * runs every loop, so every component has a non blocking delay.
+ *
+ * A component can manage other components which called children components.
+ */
+class Component
+{
+public:
+    /**
+     * @brief Construct a new Component object
+     *
+     * With the default constructor the created component is
+     * by default inactive. This does not affect the execution of
+     * the children components.
+     */
     Component() {}
+
+    /**
+     * @brief Construct a new Component object
+     *
+     * If the given parameter is zero, there are no differences to the
+     * default constructor.
+     *
+     * @param loopDelay the minimum time in milliseconds before the task runs
+     */
     Component(uint16_t loopDelay);
 
+    /**
+     * @brief Runs the children components and the task
+     *
+     * The loop() function of the children is called every time this
+     * loop is called.
+     *
+     * The run() function which presents the task of this component is
+     * only called if the delay is reached.
+     */
     void loop();
 
+    /**
+     * @brief Deactivate this component
+     *
+     * If the component is deactivated the call of loop() ha no effect
+     */
     void deactivate() { this->active = false; }
     void activate() { this->active = false; }
 
-    protected:
+protected:
+    /**
+     * @brief Override this function to avoid the delay
+     */
     virtual void runAsChild() {}
+
+    /**
+     * @brief Runs befor the run() function
+     *
+     * This function is only called, if the delay
+     * is reached.
+     * The function do nothing, except the function is overwritten
+     * by the class which inherits this class.
+     */
     virtual void beforeRun() {}
+
+    /**
+     * @brief The actual task
+     *
+     * This function have to be overwritten by the inheriting class.
+     */
     virtual void run() = 0;
+
+    /**
+     * @brief Runs after the run() function
+     *
+     * This function is only called, if the delay
+     * is reached.
+     * The function do nothing, except the function is overwritten
+     * by the class which inherits this class.
+     */
     virtual void afterRun() {}
 
-        void addChildComponent(Component* child);
+    /**
-        void removeChildComponent(Component* child);
+     * @brief Adds a child component
+     *
+     * The child component will be called every time the loop() function
+     * is called.
+     *
+     * @param child
+     */
+    void addChildComponent(Component *child);
+    void removeChildComponent(Component *child);
 
     void activateOnlyChilds() { this->onlyChilds = true; }
+
+    /**
+     * @brief Skip the actual task
+     *
+     * Same as set the loopDelay to zero.
+     */
     void deactivateOnlyChilds() { this->onlyChilds = false; }
 
+    /**
+     * @brief Set the timer after task
+     *
+     * If this function is called once the measurement of the delay
+     * starts after task has finished. The default is, that the
+     * measurement begins at the start of the task.
+     */
     void setTimerAfterTask() { this->timeUpdateAfter = true; }
 
+    /**
+     * @brief the minimum time in milliseconds before the task runs
+     *
+     * If this value is zero, the functions beforeRun(), run() and
+     * afterRun() would not be called
+     */
     uint16_t loopDelay = 0;
 
-    private:
+private:
-        std::list<Component*> childComponents;
+    std::list<Component *> childComponents;
 
     bool active = true;
     bool onlyChilds = false;

lib/ControlPad/controlPad.cpp

@@ -11,27 +11,33 @@
 
 #include "controlPad.h"
 
-ControlPad::ControlPad() {
+ControlPad::ControlPad() : Component(10), controlInput{0, 0, 0, 0} {}
-    this->loopDelay = 5;
-}
 
-void ControlPad::run() {
+void ControlPad::run()
-    if(!this->connected)
+{
+    if (!this->connected)
+    {
         return;
+    }
 
-    if (millis() - this->lastMessageReceive > this->disconnectTime) {
+    if (millis() - this->lastMessageReceive > this->disconnectTime)
+    {
         this->connected = false;
         this->controlInput.buttons = 0;
-        this->controlInput.x = 127;
+        this->controlInput.x = UINT8_MAX / 2;
-        this->controlInput.y = 127;
+        this->controlInput.y = UINT8_MAX / 2;
         return;
     }
 
     if (this->lastButtons != controlInput.buttons)
+    {
         this->updated = true;
+    }
 
-    if (this->menuControl && this->controlInput.buttons > 0 && this->updated) {
+    if (static_cast<bool>(this->menuControl) && this->controlInput.buttons > 0 && this->updated)
-        if (this->firstButtonPress) {
+    {
+        if (this->firstButtonPress)
+        {
             this->menuControl->printMenu();
             this->firstButtonPress = false;
             std::cout << "ControlPad::loop - First menu print" << std::endl;
@@ -39,38 +45,48 @@ void ControlPad::run() {
         }
 
         if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Left))
+        {
             this->menuControl->left();
+        }
         else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Right))
+        {
             this->menuControl->right();
+        }
         else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Up))
+        {
             this->menuControl->up();
+        }
         else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Down))
+        {
             this->menuControl->down();
+        }
         else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Yes))
+        {
             this->menuControl->yes();
+        }
         else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::No))
+        {
             this->menuControl->no();
+        }
 
         this->updated = false;
         this->lastButtons = controlInput.buttons;
     }
-    return;
 }
 
-void ControlPad::insertData(const uint8_t *data) {
+void ControlPad::insertData(const uint8_t *data)
+{
     this->connected = true;
     this->lastMessageReceive = millis();
 
-    uint8_t lastCount = this->controlInput.counter + 1;
+    const uint8_t lastCount = this->controlInput.counter + 1;
     memcpy(&(this->controlInput), data, sizeof(this->controlInput));
     if (lastCount != this->controlInput.counter)
         std::cout << "ControlPad::insertData counter wrong value" << std::endl;
 
-    if (this->controlInput.x > 127 - this->deadZoneX 
+    if (this->controlInput.x > UINT8_MAX / 2 - this->deadZoneX && this->controlInput.x < UINT8_MAX / 2 + this->deadZoneX)
-        && this->controlInput.x < 127 + this->deadZoneX)
+        this->controlInput.x = UINT8_MAX / 2;
-        this->controlInput.x = 127;
 
-    if (this->controlInput.y > 127 - this->deadZoneY 
+    if (this->controlInput.y > UINT8_MAX / 2 - this->deadZoneY && this->controlInput.y < UINT8_MAX / 2 + this->deadZoneY)
-        && this->controlInput.y < 127 + this->deadZoneY)
+        this->controlInput.y = UINT8_MAX / 2;
-        this->controlInput.y = 127;
 }

lib/ControlPad/controlPad.h

@@ -0,0 +1,80 @@
+/**
+ * @file controlPad.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains a class that gets the input data
+ * @version 0.1
+ * @date 2023-03-30
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+
+#pragma once
+
+#include <menuControl.h>
+
+#include <controlPadInput.h>
+#include <component.h>
+
+/**
+ * @brief A class to manage inputs
+ *
+ * This class converts the incoming data to the buttons and
+ * the axis from the joystick. The converted data will be send
+ * to the menu.
+ */
+class ControlPad : public Component
+{
+public:
+    ControlPad();
+
+    /**
+     * @brief Insert the incoming data to convert
+     *
+     * The data is converted to the ControlPadInput struct.
+     *
+     * @param data have to be 4 byte long
+     */
+    void insertData(const uint8_t *data);
+
+    /**
+     * @brief Set the MenuControl object
+     *
+     * The MenuControl object is used to control the Menu.
+     *
+     * @see Menu
+     * @see ControlPadInput
+     *
+     * @param menuControl
+     */
+    void setMenuControl(MenuControl *menuControl) { this->menuControl = menuControl; }
+
+    /**
+     * @brief Get the Control Pad Data
+     *
+     * The pointer holds the lates data from the ControlPad
+     *
+     * @return const ControlPadInput*
+     */
+    const ControlPadInput *getControlPadDataPtr() const { return &this->controlInput; }
+
+    bool isControlPadConnected() const { return this->connected; }
+
+private:
+    void run() override;
+
+    MenuControl *menuControl = nullptr;
+    ControlPadInput controlInput;
+
+    bool connected = false;
+    bool updated = false;
+    bool firstButtonPress = true;
+
+    uint8_t deadZoneX = 20;
+    uint8_t deadZoneY = 20;
+    uint8_t lastButtons = 0;
+
+    uint16_t disconnectTime = 100;
+
+    uint32_t lastMessageReceive = 0;
+};

lib/ControlPad/controlPadInput.h

@@ -0,0 +1,79 @@
+/**
+ * @file controlPadInput.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief
+ * @version 0.1
+ * @date 2023-03-30
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+
+#ifndef CONTROL_PAD_INPUT_H
+#define CONTROL_PAD_INPUT_H
+
+#include <stdint.h>
+
+struct ControlPadInput
+{
+    uint8_t buttons;
+    uint8_t x;
+    uint8_t y;
+    uint8_t counter;
+};
+
+class ControlPadButton
+{
+public:
+    enum PadButton
+    {
+        Left = 4,
+        Right = 8,
+        Up = 2,
+        Down = 1,
+        Yes = 32,
+        No = 16,
+        Action = 64
+    };
+
+    static bool isControlPadButtonPressed(const ControlPadInput *input, PadButton button)
+    {
+        uint8_t buttonNum = input->buttons;
+        switch (button)
+        {
+        case PadButton::Left:
+            return buttonNum & (uint8_t)PadButton::Left;
+            break;
+
+        case PadButton::Right:
+            return buttonNum & (uint8_t)PadButton::Right;
+            break;
+
+        case PadButton::Up:
+            return buttonNum & (uint8_t)PadButton::Up;
+            break;
+
+        case PadButton::Down:
+            return buttonNum & (uint8_t)PadButton::Down;
+            break;
+
+        case PadButton::Yes:
+            return buttonNum & (uint8_t)PadButton::Yes;
+            break;
+
+        case PadButton::No:
+            return buttonNum & (uint8_t)PadButton::No;
+            break;
+
+        case PadButton::Action:
+            return buttonNum & (uint8_t)PadButton::Action;
+            break;
+
+        default:
+            return false;
+            ;
+        }
+    }
+};
+
+#endif // CONTROL_PAD_INPUT_H

lib/ControllPad/controlPad.h

@@ -1,48 +0,0 @@
-/**
- * @file controlPad.h
- * @author Alexander Klein (alex@kleiax.de)
- * @brief 
- * @version 0.1
- * @date 2023-03-30
- * 
- * @copyright Copyright (c) 2023
- * 
- */
-
-#pragma once
-
-#include <menuControl.h>
-
-#include <controlPadInput.h>
-#include <component.h>
-
-class ControlPad : public Component {
-    public:
-        ControlPad();
-
-        void insertData(const uint8_t *data);
-
-        void setMenuControl(MenuControl* menuControl) { this->menuControl = menuControl; }
-
-        const ControlPadInput* getControlPadDataPtr() const { return &this->controlInput; }
-
-        bool isControlPadConnected() const { return this->connected; }
-
-    private:
-        void run() override;
-
-        MenuControl* menuControl = nullptr;
-        ControlPadInput controlInput;
-
-        bool connected = false;
-        bool updated = false;
-        bool firstButtonPress = true;
-
-        uint8_t deadZoneX = 20;
-        uint8_t deadZoneY = 20;
-        uint8_t lastButtons = 0;
-
-        uint16_t disconnectTime = 100;
-
-        uint32_t lastMessageReceive = 0;
-};

lib/ControllPad/controlPadInput.h

@@ -1,74 +0,0 @@
-/**
- * @file controlPadInput.h
- * @author Alexander Klein (alex@kleiax.de)
- * @brief 
- * @version 0.1
- * @date 2023-03-30
- * 
- * @copyright Copyright (c) 2023
- * 
- */
-
-#ifndef CONTROL_PAD_INPUT_H
-#define CONTROL_PAD_INPUT_H
-
-#include <stdint.h>
-
-struct ControlPadInput {
-    uint8_t buttons;
-    uint8_t x;
-    uint8_t y;
-    uint8_t counter;
-};
-
-
-class ControlPadButton {
-    public:
-        enum PadButton {
-            Left = 4,
-            Right = 8,
-            Up = 2,
-            Down = 1,
-            Yes = 32,
-            No = 16,
-            Action = 64
-        };
-
-        static bool isControlPadButtonPressed(const ControlPadInput *input, PadButton button) {
-            uint8_t buttonNum = input->buttons;
-            switch (button) {
-                case PadButton::Left :
-                    return buttonNum & (uint8_t) PadButton::Left;
-                    break;
-
-                case PadButton::Right :
-                    return buttonNum & (uint8_t) PadButton::Right;
-                    break;
-
-                case PadButton::Up :
-                    return buttonNum & (uint8_t) PadButton::Up;
-                    break;
-
-                case PadButton::Down :
-                    return buttonNum & (uint8_t) PadButton::Down;
-                    break;
-
-                case PadButton::Yes :
-                    return buttonNum & (uint8_t) PadButton::Yes;
-                    break;
-
-                case PadButton::No :
-                    return buttonNum & (uint8_t) PadButton::No;
-                    break;
-
-                case PadButton::Action :
-                    return buttonNum & (uint8_t) PadButton::Action;
-                    break;
-                
-                default:
-                    return false;;
-            }
-        }
-};
-
-#endif // CONTROL_PAD_INPUT_H

lib/Counter/counter.cpp

@@ -1,14 +1,14 @@
 #include "counter.h"
-#include <inttypes.h>
 
 uint8_t Counter::amountOfCounter = 0;
 
-Counter::Counter(uint8_t pin) {
+Counter::Counter(uint8_t pin)
-    this->pulsePin = pin;
+    : pulsePin{pin}, unit{static_cast<pcnt_unit_t>(Counter::amountOfCounter)}
-
+{
-    this->unit = static_cast<pcnt_unit_t>(Counter::amountOfCounter);
+    if (Counter::amountOfCounter <= Counter::maxCounter)
-    if (Counter::amountOfCounter < 7)
+    {
         Counter::amountOfCounter++;
+    }
 
     pcnt_config_t config;
     config.unit = this->unit;
@@ -24,33 +24,40 @@ Counter::Counter(uint8_t pin) {
     pcnt_unit_config(&config);
 }
 
-void Counter::pause() {
+void Counter::pause()
+{
     pcnt_counter_pause(this->unit);
 }
 
-void Counter::resume() {
+void Counter::resume()
+{
     pcnt_counter_resume(this->unit);
 }
 
-void Counter::clear() {
+void Counter::clear()
+{
     pcnt_counter_clear(this->unit);
 }
 
-int16_t Counter::getValue() const {
+int16_t Counter::getValue() const
-    int16_t res;
+{
+    int16_t res = 0;
     pcnt_get_counter_value(this->unit, &res);
     return res;
 }
 
-void Counter::setFilterValue(uint16_t value) {
+void Counter::setFilterValue(uint16_t value)
+{
     pcnt_set_filter_value(this->unit, value);
     this->filterEnable();
 }
 
-void Counter::filterEnable() {
+void Counter::filterEnable()
+{
     pcnt_filter_enable(this->unit);
 }
 
-void Counter::filterDisable() {
+void Counter::filterDisable()
+{
     pcnt_filter_disable(this->unit);
 }

lib/Counter/counter.h

@@ -1,29 +1,76 @@
+/**
+ * @file counter.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains a class that abstract the hardware counter from the esp32
+ * @version 0.1
+ * @date 2023-10-12
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
 #pragma once
 
 #include <Arduino.h>
 #include <driver/pcnt.h>
+#include <cinttypes>
 
-class Counter {
+/**
-    public:
+ * @brief A class that abstract the hardware counter from the esp32
+ */
+class Counter
+{
+public:
+    /**
+     * @brief Construct a new Counter object
+     *
+     * @param pin with incoming pulses
+     */
     Counter(uint8_t pin);
 
+    /**
+     * @brief Pause the pulse counting
+     */
     void pause();
+
+    /**
+     * @brief Resume the pulse counting
+     */
     void resume();
+
+    /**
+     * @brief Start counting by zero again
+     */
     void clear();
 
+    /**
+     * @brief Get the counted pulses
+     *
+     * @return int16_t
+     */
     int16_t getValue() const;
 
+    /**
+     * @brief Set the filter value
+     *
+     * The filter skip all pulses after a pulse for the filter time.
+     * The filter time depends on the frequency of the processor. The time
+     * for a whole tact multiplied with the filter value results the filter time.
+     *
+     * @param value max 1023
+     */
     void setFilterValue(uint16_t value);
+
     void filterEnable();
     void filterDisable();
 
-    private:
+private:
     static constexpr int16_t highLimit = INT16_MAX;
     static constexpr uint8_t lowLimit = 0;
+    static constexpr uint8_t maxCounter = 6;
 
     static uint8_t amountOfCounter;
 
-        bool initalised = false;
+    bool initalized = false;
 
     uint8_t pulsePin;
     pcnt_unit_t unit;

lib/LcdWrapper/LcdWrapper.cpp

@@ -1,5 +1,5 @@
 /**
- * @file displayWrapper.cpp
+ * @file LcdWrapper.cpp
  * @author Alexander Klein (alex@kleiax.de)
  * @brief Contains the implementation of the class LcdWrapper
  * @version 0.1
@@ -11,55 +11,73 @@
 
 #include "LcdWrapper.h"
 
-LcdWrapper::LcdWrapper(LiquidCrystal_I2C* lcd) {
+LcdWrapper::LcdWrapper(LiquidCrystal_I2C *lcd)
-    this->lcd = lcd;
+    : lcd{lcd}, changed{false}
+{
     this->clear();
-    this->changed = false;
 }
 
-void LcdWrapper::run() {
+void LcdWrapper::run()
+{
     if (!this->changed)
+    {
         return;
-
-    this->lcd->clear();
-    for (uint8_t i = 0; i < LcdWrapper::totalLines; i++) {
-        this->lcd->setCursor(0, i);
-        this->lcd->print(this->data[i]);
     }
 
-    if (this->callback)
+    this->lcd->clear();
+    for (uint8_t i = 0; i < LcdWrapper::totalLines; i++)
+    {
+        this->lcd->setCursor(0, i);
+        this->lcd->print(static_cast<const char *>(this->data[i]));
+    }
+
+    if (static_cast<bool>(this->callback))
+    {
         this->callback(this->data, LcdWrapper::totalLines, LcdWrapper::totalRows);
+    }
 
     this->changed = false;
 }
 
-void LcdWrapper::clear() {
+void LcdWrapper::clear()
-    for (uint8_t i = 0; i < LcdWrapper::totalLines; i++) {
+{
-        for (uint8_t j = 0; j < LcdWrapper::totalRows; j++) {
+    for (uint8_t i = 0; i < LcdWrapper::totalLines; i++)
+    {
+        for (uint8_t j = 0; j < LcdWrapper::totalRows; j++)
+        {
             data[i][j] = ' ';
         }
     }
     this->changed = true;
 }
 
-void LcdWrapper::setCursor(uint8_t row, uint8_t line) {
+void LcdWrapper::setCursor(uint8_t row, uint8_t line)
+{
     if (row > LcdWrapper::totalRows - 1)
+    {
         row = LcdWrapper::totalRows - 1;
+    }
 
     if (line > LcdWrapper::totalLines - 1)
+    {
         line = LcdWrapper::totalLines - 1;
+    }
 
     this->cursorRow = row;
     this->cursorLine = line;
 }
 
-void LcdWrapper::print(const char *str) {
+void LcdWrapper::print(const char *str)
+{
     uint8_t inputStringPosition = 0;
-    for (uint8_t i = this->cursorRow; i < LcdWrapper::totalRows; i++) {
+    for (uint8_t i = this->cursorRow; i < LcdWrapper::totalRows; i++)
+    {
         if (str[inputStringPosition] == '\0')
+        {
             break;
-        else
+        }
         this->data[this->cursorLine][i] = str[inputStringPosition];
+
         inputStringPosition++;
     }
     this->changed = true;

lib/LcdWrapper/LcdWrapper.h

@@ -1,5 +1,5 @@
 /**
- * @file displayWrapper.h
+ * @file LcdWrapper.h
  * @author Alexander Klein (alex@kleiax.de)
  * @brief Contains the LcdWrapper class
  * @version 0.1
@@ -17,7 +17,7 @@
 #include <displayWrapper.h>
 #include <component.h>
 
-typedef void (*LcdWrapperCallback) (const char data[][16], uint8_t lines, uint8_t rows);
+typedef void (*LcdWrapperCallback)(const char data[][16], uint8_t lines, uint8_t rows);
 /**
  * @brief A class for the Menu class to print information
  *
@@ -25,18 +25,18 @@ typedef void (*LcdWrapperCallback) (const char data[][16], uint8_t lines, uint8_
  * The class takes the information to print from any thread. The
  * loop function has to be called to print the data.
  */
-class LcdWrapper : public DisplayWrapper, public Component {
+class LcdWrapper : public DisplayWrapper, public Component
-    public:
+{
+public:
     /**
      * @brief Construct a new Lcd Wrapper object
      *
      * @param lcd
      */
-        LcdWrapper(LiquidCrystal_I2C* lcd);
+    LcdWrapper(LiquidCrystal_I2C *lcd);
 
     /**
      * @brief Empty the buffer
-         * 
      */
     void clear() override;
 
@@ -58,11 +58,11 @@ class LcdWrapper : public DisplayWrapper, public Component {
 
     static constexpr uint8_t totalRows = 16;
     static constexpr uint8_t totalLines = 2;
-    private:
+
+private:
     void run() override;
 
-    
+    LiquidCrystal_I2C *lcd;
-        LiquidCrystal_I2C* lcd;
     LcdWrapperCallback callback = nullptr;
     char data[LcdWrapper::totalLines][LcdWrapper::totalRows];
 

lib/MQTT/debugMqtt.cpp

@@ -11,93 +11,109 @@
  */
 #include "debugMqtt.h"
 
-PubSubClient* DebugMqtt::client;
+PubSubClient *DebugMqtt::client;
 Loglevel DebugMqtt::loglevel;
 bool DebugMqtt::isInit = false;
 char DebugMqtt::msg[MQTT_BUFFER_SIZE];
 char DebugMqtt::topic[MQTT_BUFFER_SIZE];
 
-
+DebugMqtt::DebugMqtt(const char *name, uint8_t bufSize)
-DebugMqtt::DebugMqtt(const char* name, uint8_t bufSize) {
+    : name{name}
-    this->name = name;
+{
     if (bufSize != 0)
+    {
         this->bufSize = bufSize;
+    }
     this->buf = new char[this->bufSize];
 }
 
-DebugMqtt::~DebugMqtt() {
+DebugMqtt::~DebugMqtt()
+{
     delete this->buf;
 }
 
-void DebugMqtt::sendMsg(Loglevel loglevel, String topic, String msg) {
+void DebugMqtt::sendMsg(Loglevel loglevel, String topic, String msg)
-    snprintf (DebugMqtt::msg, MQTT_BUFFER_SIZE, "%s: %s",this->name ,msg.c_str());
+{
-    this->sendData(loglevel, topic, DebugMqtt::msg);
+    snprintf(static_cast<char *>(DebugMqtt::msg), MQTT_BUFFER_SIZE, static_cast<const char *>("%s: %s"), this->name, msg.c_str());
+    this->sendData(loglevel, topic, static_cast<const char *>(DebugMqtt::msg));
 }
 
-void DebugMqtt::sendMsg(Loglevel loglevel, String msg) {
+void DebugMqtt::sendMsg(Loglevel loglevel, String msg)
+{
     this->sendMsg(loglevel, "", msg);
 }
 
-void DebugMqtt::sendData(Loglevel loglevel, String topic, String data) {
+void DebugMqtt::sendData(Loglevel loglevel, String topic, String data)
-    if (!DebugMqtt::isInit) {
+{
+    if (!DebugMqtt::isInit)
+    {
         return;
     }
 
-    if (loglevel <= DebugMqtt::loglevel && loglevel > Loglevel::none) {
+    if (loglevel <= DebugMqtt::loglevel && loglevel > Loglevel::none)
-        snprintf (DebugMqtt::topic, MQTT_BUFFER_SIZE, "%s%s", DebugMqtt::enum_to_string(loglevel).c_str(), topic.c_str());
+    {
-        snprintf (DebugMqtt::msg, MQTT_BUFFER_SIZE, "%s", data.c_str());
+        snprintf(static_cast<char *>(DebugMqtt::topic), MQTT_BUFFER_SIZE, static_cast<const char *>("%s%s"), DebugMqtt::enum_to_string(loglevel).c_str(), topic.c_str());
-        client->publish(DebugMqtt::topic, DebugMqtt::msg);
+        snprintf(static_cast<char *>(DebugMqtt::msg), MQTT_BUFFER_SIZE, static_cast<const char *>("%s"), data.c_str());
+        client->publish(DebugMqtt::topic, static_cast<const char *>(DebugMqtt::msg));
     }
 }
 
-void DebugMqtt::sendData(Loglevel loglevel, String data){
+void DebugMqtt::sendData(Loglevel loglevel, String data)
-    this->sendData(loglevel, "", data);
+{
+    DebugMqtt::sendData(loglevel, "", data);
 }
 
-void DebugMqtt::writeToInflux(String measurement_name, String field_set, float measurement, uint64_t nanos) {
+void DebugMqtt::writeToInflux(String measurement_name, String field_set, float measurement, uint64_t nanos)
+{
     // Example String: "weather temperature=82 1465839830100400200";
 
-    snprintf(DebugMqtt::msg, MQTT_BUFFER_SIZE, "%s %s=%f %llu", measurement_name.c_str(), field_set.c_str(), measurement, nanos);
+    snprintf(static_cast<char *>(DebugMqtt::msg), MQTT_BUFFER_SIZE, static_cast<const char *>("%s %s=%f %llu"), measurement_name.c_str(), field_set.c_str(), measurement, nanos);
-    this->sendData(Loglevel::influx, DebugMqtt::msg);
+    this->sendData(Loglevel::influx, static_cast<const char *>(DebugMqtt::msg));
 }
 
-void DebugMqtt::addCharacter(char c) {
+void DebugMqtt::addCharacter(char character)
-    this->buf[this->bufPos] = c;
+{
+    this->buf[this->bufPos] = character;
     this->bufPos++;
-    if (c == '\n' || this->bufPos >= this->bufSize - 1) {
+    if (character == '\n' || this->bufPos >= this->bufSize - 1)
+    {
         this->buf[this->bufPos - 1] = '\0';
         this->sendMsg(Loglevel::info, buf);
         this->bufPos = 0;
     }
 }
 
-void DebugMqtt::init(PubSubClient *client, Loglevel max_loglevel) {
+void DebugMqtt::init(PubSubClient *client, Loglevel max_loglevel)
+{
     DebugMqtt::client = client;
     DebugMqtt::loglevel = max_loglevel;
     DebugMqtt::isInit = true;
 }
 
-void DebugMqtt::changeLoglevel(Loglevel loglevel) {
+void DebugMqtt::changeLoglevel(Loglevel loglevel)
+{
     DebugMqtt::loglevel = loglevel;
 }
 
-String DebugMqtt::enum_to_string(Loglevel loglevel) {
+String DebugMqtt::enum_to_string(Loglevel loglevel)
+{
     String topic = "";
     topic += MQTT_DEBUG_TOPIC;
-    switch(loglevel){
+    switch (loglevel)
-    case Loglevel::error :
+    {
+    case Loglevel::error:
         topic += "/Error";
         break;
-    case Loglevel::warn :
+    case Loglevel::warn:
         topic += "/Warn";
         break;
-    case Loglevel::info :
+    case Loglevel::info:
         topic += "/Info";
         break;
-    case Loglevel::debug :
+    case Loglevel::debug:
         topic += "/Debug";
         break;
-    case Loglevel::influx :
+    case Loglevel::influx:
         topic += "/Influx";
         break;
     default:
@@ -105,5 +121,4 @@ String DebugMqtt::enum_to_string(Loglevel loglevel) {
         break;
     }
     return topic;
- }
+}
-

lib/MQTT/debugMqtt.h

@@ -14,7 +14,6 @@
 #include <iostream>
 #include <PubSubClient.h>
 
-
 /**
  * @brief
  *
@@ -35,7 +34,7 @@
  */
 #ifndef MQTT_BUFFER_SIZE
 #define MQTT_BUFFER_SIZE 128
-#endif //MQTT_BUFFER_SIZE
+#endif // MQTT_BUFFER_SIZE
 
 /**
  * @brief An enum to set the log level
@@ -48,12 +47,15 @@
  * @see sendData()
  *
  */
-enum Loglevel { none,
+enum Loglevel
+{
+    none,
     error,
     warn,
     info,
     debug,
-                influx};
+    influx
+};
 
 /**
  * @brief A class to send debug messages over MQTT
@@ -64,15 +66,16 @@ enum Loglevel { none,
  *
  * @see Loglevel
  */
-class DebugMqtt {
+class DebugMqtt
-    public:
+{
+public:
     /**
      * @brief Construct a new Debug Mqtt object.
      *
      * @param name A String with send with every Message.
      * @param bufSize for the addCharacter function.
      */
-        DebugMqtt(const char* name, uint8_t bufSize = 0);
+    DebugMqtt(const char *name, uint8_t bufSize = 0);
 
     ~DebugMqtt();
 
@@ -108,8 +111,8 @@ class DebugMqtt {
      * @param topic Additional topic behind loglevel
      * @param data The message to send as String
      */
-        void sendData(Loglevel loglevel, String topic, String data);
+    static void sendData(Loglevel loglevel, String topic, String data);
-        void sendData(Loglevel loglevel, String data);
+    static void sendData(Loglevel loglevel, String data);
 
     /**
      * @brief Send a Message via MQTT for InfluxDB
@@ -132,9 +135,9 @@ class DebugMqtt {
      *  1. when the buffer is full
      *  2. when the character is '\n'
      *
-         * @param c 
+     * @param character
      */
-        void addCharacter(char c);
+    void addCharacter(char character);
 
     /**
      * @brief Initialize debugMQTT for all instances
@@ -149,7 +152,7 @@ class DebugMqtt {
      * @param client PubSubClient
      * @param max_loglevel Max loglevel to send.
      */
-        static void init(PubSubClient* client, Loglevel max_loglevel);
+    static void init(PubSubClient *client, Loglevel max_loglevel);
 
     /**
      * @brief Change loglevel
@@ -160,15 +163,15 @@ class DebugMqtt {
      */
     static void changeLoglevel(Loglevel loglevel);
 
-    private:
+private:
-        const char* name;
+    const char *name;
-        char* buf;
+    char *buf;
     uint8_t bufPos = 0;
     uint8_t bufSize = 100;
 
     static String enum_to_string(Loglevel loglevel);
 
-        static PubSubClient* client;
+    static PubSubClient *client;
     static Loglevel loglevel;
     static bool isInit;
     static char msg[MQTT_BUFFER_SIZE];

lib/MotorControl/motorControl.cpp

@@ -12,13 +12,13 @@
 
 #include "motorControl.h"
 
-MotorControl::MotorControl() {
+MotorControl::MotorControl()
-    this->setMinPwm(MotorControl::pwmMin);
+{
-    this->setMaxPwm(MotorControl::pwmMax);
     Component::loopDelay = MotorControl::loopDelay;
 }
 
-void MotorControl::init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8_t dir_2) {
+void MotorControl::init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8_t dir_2)
+{
     this->pwmPin = pwmPin;
     this->pwmChannel = pwmChannel;
     this->dir_1 = dir_1;
@@ -35,107 +35,141 @@ void MotorControl::init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8
     ledcWrite(this->pwmChannel, 0);
 }
 
-void MotorControl::run() {
+void MotorControl::run()
+{
     // Absolute difference between targetPower and power
-    uint8_t abs_difference = abs(this->targetPower - this->power);
+    const uint8_t abs_difference = abs(this->targetPower - this->power);
 
     // Difference between targetPower and power
-    int16_t difference = this->targetPower - this->power;
+    const int16_t difference = this->targetPower - this->power;
 
     // Check that the target speed is close to 0 and that the abs_difference is lower than MotorControl::powerSteps
-    if (abs(this->targetPower) <  MotorControl::powerSteps && abs_difference < MotorControl::powerSteps) {
+    if (abs(this->targetPower) < MotorControl::powerSteps && abs_difference < MotorControl::powerSteps)
+    {
         this->setRealPower(0);
         return;
     }
 
     // Correct speed
-    if (abs_difference < MotorControl::powerSteps) {
+    if (abs_difference < MotorControl::powerSteps)
+    {
         return;
     }
 
     // Positive or negative tagret speed
-    if (this->targetPower >= 0) {
+    if (this->targetPower >= 0)
+    {
         // Positive or negative speed
-        if (this->power >= 0) {
+        if (this->power >= 0)
-            if (difference > 0) {
+        {
-                this->increasePower(MotorControl::powerSteps);
+            if (difference > 0)
-            } else {
+            {
-                this->increasePower(-MotorControl::powerSteps);
-            }
-        } else {
                 this->increasePower(MotorControl::powerSteps);
             }
-
+            else
-    } else {
+            {
-        // Positive or negative speed
-        if (this->power >= 0) {
                 this->increasePower(-MotorControl::powerSteps);
-        } else {
+            }
-            if (difference > 0) {
+        }
+        else
+        {
             this->increasePower(MotorControl::powerSteps);
-            } else {
+        }
+    }
+    else
+    {
+        // Positive or negative speed
+        if (this->power >= 0)
+        {
+            this->increasePower(-MotorControl::powerSteps);
+        }
+        else
+        {
+            if (difference > 0)
+            {
+                this->increasePower(MotorControl::powerSteps);
+            }
+            else
+            {
                 this->increasePower(-MotorControl::powerSteps);
             }
         }
     }
 }
 
-void MotorControl::setMinPwm(uint8_t min) {
+void MotorControl::setMinPwm(uint8_t min)
-    if (min > 80) min = 80;
+{
-    //transform percentage to real pwm value
+    if (min > MotorControl::maxPwmMin)
-    min = (uint8_t) (((1 << pwmRes) - 1) * (min / 100.0));
+    {
+        min = MotorControl::maxPwmMin;
+    }
+    // transform percentage to real pwm value
+    min = static_cast<uint8_t>(((static_cast<uint8_t>(1) << pwmRes) - 1) * (min / 100.0));
     this->dutycycleMin = min;
 }
 
-void MotorControl::setMaxPwm(uint8_t max) {
+void MotorControl::setMaxPwm(uint8_t max)
-    if (max > 100) max = 100;
+{
-    //transform percentage to real pwm value
+    if (max > 100)
-    max = (uint8_t) (((1 << pwmRes) - 1) * (max / 100.0));
+    {
+        max = 100;
+    }
+    // transform percentage to real pwm value
+    max = static_cast<uint8_t>(((static_cast<uint8_t>(1) << pwmRes) - 1) * (max / 100.0));
     this->dutycycleMax = max;
 }
 
-void MotorControl::setTargetPower(int8_t power) {
+void MotorControl::setTargetPower(int8_t power)
+{
     if (power <= 100 && power >= -100)
+    {
         this->targetPower = power;
+    }
     else
+    {
         std::cout << " MotorControl::setTargetPower: Invalid Argument - Power: " << power << std::endl;
+    }
 }
 
-void MotorControl::stop() {
+void MotorControl::stop()
+{
     this->targetPower = 0;
 }
 
-void MotorControl::emergencyStop() {
+void MotorControl::emergencyStop()
+{
     setRealPower(0);
 }
 
-bool MotorControl::isTargetPowerReached() const {
+bool MotorControl::isTargetPowerReached() const
-    if (this->targetPower == this->power)
+{
-        return true;
+    return this->targetPower == this->power;
-    return false;
 }
 
-bool MotorControl::isAccelerationPositive() const {
+bool MotorControl::isAccelerationPositive() const
-    if (power < targetPower)
+{
-        return true;
+    return power < targetPower;
-    return false;
 }
 
-bool MotorControl::isAccelerationNegative() const {
+bool MotorControl::isAccelerationNegative() const
-    if (power > targetPower)
+{
-        return true;
+    return power > targetPower;
-    return false;
 }
 
-void MotorControl::setRealPower(int8_t power) {
+void MotorControl::setRealPower(int8_t power)
-    //TODO: Exceptionhandling
+{
-    if (power <= 100 && power >= -100) {
+    // TODO: Exceptionhandling
+    if (power <= 100 && power >= -100)
+    {
         this->power = power;
-    } else {
+    }
+    else
+    {
         return;
     }
 
-    if (this->power == 0) {
+    if (this->power == 0)
+    {
         this->direction = 0;
         digitalWrite(this->dir_1, LOW);
         digitalWrite(this->dir_2, LOW);
@@ -144,13 +178,16 @@ void MotorControl::setRealPower(int8_t power) {
         return;
     }
 
-    uint8_t pwm_val = map(abs(power), 0, 100, this->dutycycleMin, this->dutycycleMax);
+    const uint8_t pwm_val = map(abs(power), 0, 100, this->dutycycleMin, this->dutycycleMax);
 
-    if ((this->direction == 1 || this->direction == 0) && power < 0){  // new direction backward
+    if ((this->direction == 1 || this->direction == 0) && power < 0)
+    { // new direction backward
         this->direction = 2;
         digitalWrite(this->dir_1, LOW);
         digitalWrite(this->dir_2, HIGH);
-    } else if ((this->direction == 2 || this->direction == 0) && power > 0){  // new direction forward
+    }
+    else if ((this->direction == 2 || this->direction == 0) && power > 0)
+    { // new direction forward
         this->direction = 1;
         digitalWrite(this->dir_1, HIGH);
         digitalWrite(this->dir_2, LOW);
@@ -160,10 +197,12 @@ void MotorControl::setRealPower(int8_t power) {
     this->dutycycle = pwm_val;
 }
 
-void MotorControl::increasePower(int8_t power) {
+void MotorControl::increasePower(int8_t power)
-    //TODO: Exceptionhandling
+{
-    //TODO: make a stop befor a direction change
+    // TODO: Exceptionhandling
-    if (abs(power) > 2 * MotorControl::powerSteps) {
+    // TODO: make a stop befor a direction change
+    if (abs(power) > 2 * MotorControl::powerSteps)
+    {
         Serial.println("Invalid Argument in MotorControl::increasePower");
         return;
     }

lib/MotorControl/motorControl.h

@@ -23,8 +23,9 @@
  * You can control the acceleration of the motor, for example to
  * prevent a damage on your H-Bridge.
  */
-class MotorControl : public Component {
+class MotorControl : public Component
-    public:
+{
+public:
     MotorControl();
 
     /**
@@ -92,7 +93,7 @@ class MotorControl : public Component {
     bool isAccelerationPositive() const;
     bool isAccelerationNegative() const;
 
-    private:
+private:
     void run() override;
     void setRealPower(int8_t power);
     void increasePower(int8_t power);
@@ -101,20 +102,19 @@ class MotorControl : public Component {
     static constexpr uint16_t pwmFreq = 16000;
     static constexpr uint8_t pwmRes = 8;
     static constexpr uint8_t powerSteps = 2; //  A total of 20 levels ( 100 / SPEED_STEPS ) * RUN_MOTOR_CONTROL_DELAY = 500ms
-        static constexpr uint8_t pwmMin = 55;
+    static constexpr uint8_t maxPwmMin = 80;
-        static constexpr uint8_t pwmMax = 98;           // Max 98% of 2^PWM_RES     
 
     int8_t targetPower = 0;
     int8_t power = 0;
     uint8_t direction = 0; // 0 = stop, 1 = forward, 2 = backward
 
-        uint8_t pwmPin;
+    uint8_t pwmPin = 0;
-        uint8_t pwmChannel;
+    uint8_t pwmChannel = 0;
     uint16_t dutycycle = 0;
-        uint8_t dutycycleMin;
+    uint8_t dutycycleMin = 55;
-        uint8_t dutycycleMax;
+    uint8_t dutycycleMax = 98; // Max 98% of 2^PWM_RES
-        uint8_t dir_1;
+    uint8_t dir_1 = 0;
-        uint8_t dir_2;        
+    uint8_t dir_2 = 0;
 };
 
 #endif // MOTOR_CONTROL_H

lib/Network/network.cpp

@@ -0,0 +1,201 @@
+/**
+ * @file network.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief
+ * @version 0.1
+ * @date 2023-09-18
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+
+#include "network.h"
+
+Network::Network(const char *ssid, const char *passphrase)
+{
+    if (!WiFiGenericClass::mode(WIFI_AP_STA))
+    {
+        std::cout << "Network::connectWiFi failed WiFi.mode" << std::endl;
+    }
+
+    this->init(ssid, passphrase);
+}
+
+Network::Network(const char *ssid, const char *passphrase, NetworkAddresses adresses)
+    : addresses{adresses}
+{
+    if (!WiFiGenericClass::mode(WIFI_AP_STA))
+    {
+        std::cout << "Network::connectWiFi failed WiFi.mode" << std::endl;
+    }
+
+    if (!WiFi.config(this->addresses.localIP,
+                     this->addresses.gateway,
+                     this->addresses.subnet,
+                     this->addresses.dnsServer))
+    {
+        std::cout << "STA Failed to configure" << std::endl;
+    }
+
+    this->init(ssid, passphrase);
+}
+
+Network::~Network()
+{
+    delete mqttClient;
+}
+
+bool Network::activateEspNow(receiveCallbackPtr reci, sendCallbackPtr send)
+{
+    if (esp_now_init() != ESP_OK)
+    {
+        std::cout << "Network::activateEspNow - Error initializing ESP-NOW" << std::endl;
+        return false;
+    }
+
+    esp_now_register_send_cb(send);
+
+    esp_now_peer_info_t peerInfo = {};
+    memcpy(static_cast<void *>(peerInfo.peer_addr), static_cast<const void *>(this->broadcastAddress), 6);
+    peerInfo.channel = 0;
+    peerInfo.encrypt = false;
+
+    if (esp_now_add_peer(&peerInfo) != ESP_OK)
+    {
+        std::cout << "Network::connectEspNow - Failed to add peer" << std::endl;
+        return false;
+    }
+
+    esp_now_register_recv_cb(reci);
+    return true;
+}
+
+bool Network::activateMqtt(const char *user, const char *passphrase)
+{
+    this->mqttUser = user;
+    this->mqttPassphrase = passphrase;
+
+    this->mqttClient = new PubSubClient(this->wifiClient);
+    this->mqttClient->setServer(this->addresses.mqttServer, this->addresses.mqttPort);
+    this->mqttClient->setSocketTimeout(1);
+
+    if (this->wifiConnected)
+    {
+        return this->connectMqtt();
+    }
+    return false;
+}
+
+const void Network::printIPs()
+{
+    std::cout << std::endl;
+    if (!this->wifiConnected)
+    {
+        std::cout << "WiFi is not connected." << std::endl;
+        return;
+    }
+    std::cout << "WiFi is connected to" << std::endl;
+    std::cout << "IP address: " << std::endl;
+    std::cout << WiFi.localIP().toString().c_str() << std::endl;
+    std::cout << "WiFi MAC Address: " << WiFi.macAddress().c_str() << std::endl
+              << std::endl;
+}
+
+uint8_t Network::getCurrentChannel()
+{
+    uint8_t channel = 0;
+    wifi_second_chan_t secondChannel = WIFI_SECOND_CHAN_NONE;
+    if (esp_wifi_get_channel(&channel, &secondChannel) != ESP_OK)
+    {
+        std::cout << "Network::getCurrentChannel - Error!" << std::endl;
+        return -1;
+    }
+
+    return channel;
+}
+
+void Network::runAsChild()
+{
+    if (!this->initSuccessful)
+    {
+        return;
+    }
+
+    this->checkWifi();
+
+    if (this->wifiConnected && static_cast<bool>(this->mqttClient))
+    {
+        this->checkMqtt();
+    }
+}
+
+void Network::init(const char *ssid, const char *passphrase)
+{
+    // Connect to Wi-Fi network with SSID and password
+    std::cout << "Connecting to " << ssid << std::endl;
+    WiFi.begin(ssid, passphrase);
+    uint8_t timeout = Network::wifiConnectTimeout;
+    while (WiFiSTAClass::status() != WL_CONNECTED)
+    {
+        delay(Network::wifiConnectLoopTime);
+        std::cout << "." << std::flush;
+        timeout--;
+        if (timeout == 0)
+        {
+            std::cout << std::endl;
+            std::cout << "WiFi NOT connected." << std::endl;
+            return;
+        }
+    }
+
+    this->wifiConnected = true;
+    this->printIPs();
+}
+
+void Network::checkWifi()
+{
+    if ((WiFiSTAClass::status() != WL_CONNECTED) && (millis() - this->lastWifiReconnectAttempt >= Network::wifiReconnectDelay))
+    {
+        std::cout << "Reconnecting to WiFi..." << std::endl;
+        WiFi.disconnect();
+        this->wifiConnected = WiFi.reconnect();
+        this->lastWifiReconnectAttempt = millis();
+    }
+}
+
+void Network::checkMqtt()
+{
+    if (!this->mqttClient->connected() && millis() - this->lastMqttReconnectAttempt > Network::mqttReconnectDelay)
+    {
+        this->mqttConnected = this->connectMqtt();
+        this->lastMqttReconnectAttempt = millis();
+    }
+
+    if (this->mqttConnected)
+    {
+        this->mqttClient->loop();
+    }
+}
+
+bool Network::connectMqtt()
+{
+    String clientId = "ESP32Rover-";
+    clientId += String(random(), HEX);
+
+    if (static_cast<bool>(this->mqttUser))
+    {
+        if (this->mqttClient->connect(clientId.c_str(), this->mqttUser, this->mqttPassphrase))
+        {
+            this->mqttClient->publish("Rover/Info", "Connected to Mqtt-Broker");
+        }
+    }
+    else
+    {
+        if (this->mqttClient->connect(clientId.c_str()))
+        {
+            this->mqttClient->publish("Rover/Info", "Connected to Mqtt-Broker");
+        }
+    }
+
+    return this->mqttClient->connected();
+}

lib/Network/network.h

@@ -0,0 +1,136 @@
+/**
+ * @file network.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief
+ * @version 0.1
+ * @date 2023-09-18
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+
+#ifndef NETWORK_H
+#define NETWORK_H
+
+#include "component.h"
+
+#include <iostream>
+#include <WiFi.h>
+#include <PubSubClient.h>
+#include <esp_now.h>
+#include <esp_wifi.h>
+
+/**
+ * @brief Typedef to easy handle the receive Callback
+ */
+typedef void (*receiveCallbackPtr)(const uint8_t *mac, const uint8_t *incomingData, int len);
+
+/**
+ * @brief Typedef to easy handle the send Callback
+ */
+typedef void (*sendCallbackPtr)(const uint8_t *mac_addr, esp_now_send_status_t status);
+
+/**
+ * @brief A Struct to hold all network addresses
+ */
+struct NetworkAddresses
+{
+    IPAddress localIP;
+    IPAddress gateway;
+    IPAddress subnet;
+    IPAddress dnsServer;
+    IPAddress mqttServer;
+    uint16_t mqttPort = 1883;
+};
+
+/**
+ * @brief A class to manage the wireless connections
+ */
+class Network : public Component
+{
+public:
+    /**
+     * @brief Construct a new Network object
+     *
+     * With this constructor the esp gets its ip from a Dhcp server
+     *
+     * @param ssid
+     * @param passphrase
+     */
+    Network(const char *ssid, const char *passphrase);
+
+    /**
+     * @brief Construct a new Network object
+     *
+     * This constructor is used for a static ip setup
+     *
+     * @param ssid
+     * @param passphrase
+     * @param addresses
+     */
+    Network(const char *ssid, const char *passphrase, NetworkAddresses addresses);
+
+    ~Network();
+
+    /**
+     * @brief Activate ESP-NOW to communicate with the remote Control
+     *
+     * @param receive
+     * @param send
+     * @return true
+     * @return false
+     */
+    bool activateEspNow(receiveCallbackPtr receive, sendCallbackPtr send);
+
+    /**
+     * @brief Activate MQTT to send debug messages
+     *
+     * @param user
+     * @param passphrase
+     * @return true
+     * @return false
+     */
+    bool activateMqtt(const char *user = nullptr, const char *passphrase = nullptr);
+
+    /**
+     * @brief Print the current used IPs
+     */
+    const void printIPs();
+
+    bool isWifiConnected() const { return this->wifiConnected; }
+    bool isMqttConnected() const { return this->mqttConnected; }
+    const uint8_t *getBroadcastAddress() const { return this->broadcastAddress; }
+    PubSubClient *getMqttClient() const { return this->mqttClient; }
+
+    static uint8_t getCurrentChannel();
+
+private:
+    void run() override{};
+    void runAsChild() override;
+    void init(const char *ssid, const char *passphrase);
+    void checkWifi();
+    void checkMqtt();
+    bool connectMqtt();
+
+    NetworkAddresses addresses;
+    WiFiClient wifiClient;
+    PubSubClient *mqttClient = nullptr;
+
+    bool initSuccessful = false;
+    bool wifiConnected = false;
+    bool mqttConnected = false;
+
+    const char *mqttUser = nullptr;
+    const char *mqttPassphrase = nullptr;
+
+    uint8_t broadcastAddress[6] = {0xC8, 0xC9, 0xA3, 0xC8, 0x57, 0x10};
+    uint32_t lastWifiReconnectAttempt = 0;
+    uint32_t lastMqttReconnectAttempt = 0;
+
+    static constexpr uint8_t wifiConnectTimeout = 20;
+    static constexpr uint16_t wifiConnectLoopTime = 500;
+    static constexpr uint16_t wifiReconnectDelay = 5000;
+    static constexpr uint16_t mqttReconnectDelay = 2500;
+};
+
+#endif // NETWORK_H

lib/Point/point.cpp

@@ -11,94 +11,108 @@
 
 #include "point.h"
 
-Point::Point(double lat, double lon, uint32_t horizontalAccuracy, uint32_t creationTime) {
+Point::Point(double lat, double lon, uint32_t horizontalAccuracy, uint32_t creationTime)
-    this->coordinates.lat = lat;
+    : coordinates{lat, lon}
-    this->coordinates.lon = lon; 
+{
     this->init(horizontalAccuracy, creationTime);
 }
 
-Point::Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy, uint32_t creationTime) {
+Point::Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy, uint32_t creationTime)
-    this->coordinates.lat = lat / 10000000.0;
+    : coordinates{lat / 10000000.0, lon / 10000000.0}
-    this->coordinates.lon = lon / 10000000.0;
+{
     this->init(horizontalAccuracy, creationTime);
 }
 
-Point::Point(Coordinates coords, uint32_t horizontalAccuracy, uint32_t creationTime) {
+Point::Point(Coordinates coords, uint32_t horizontalAccuracy, uint32_t creationTime)
-    this->coordinates = coords;
+    : coordinates{coords}
+{
     this->init(horizontalAccuracy, creationTime);
 }
 
-Point::Point(Coordinates coords, bool imported) {
+Point::Point(Coordinates coords, bool imported)
+{
     this->coordinates = coords;
     if (imported)
+    {
         this->init(UINT32_MAX, 0);
+    }
     else
+    {
         this->init(0, 0);
+    }
 }
 
-Point::Point() {
+Point::Point()
+{
     this->coordinates.lat = 0;
     this->coordinates.lon = 0;
     this->init(0, 0);
 }
 
-bool Point::operator==(const Point& rhs) const {
+bool Point::operator==(const Point &rhs) const
+{
     return this->coordinates == rhs.getCoordinates();
 }
 
-    // distance = sqrt(dx * dx + dy * dy)
+double Point::distanceTo(const Coordinates &point) const
-    // mit distance: Entfernung in km 
+{
-    // dx = 111.3 * cos(lat) * (lon1 - lon2)
+    const Coordinates begin = this->coordinates;
-    // lat = (lat1 + lat2) / 2 * 0.01745
+    const Coordinates end = point;
-    // dy = 111.3 * (lat1 - lat2)
-    // lat1, lat2, lon1, lon2: Breite, Länge in Grad
-double Point::distanceTo(const Coordinates& point) const {
-    Coordinates begin = this->coordinates;
-    Coordinates end = point;
 
-    double lat = (begin.lat + end.lat) / 2 * ROUTE_DEGREE_TO_RADIANT;
+    const double lat = (begin.lat + end.lat) / 2 * ROUTE_DEGREE_TO_RADIANT;
-    double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (begin.lat - end.lat);
+    const double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (begin.lat - end.lat);
-    double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (begin.lon - end.lon);
+    const double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (begin.lon - end.lon);
 
     return sqrt(dx * dx + dy * dy);
 }
 
-double Point::distanceTo(const Point &point) const {
+double Point::distanceTo(const Point &point) const
+{
     return this->distanceTo(point.getCoordinates());
 }
 
-int16_t Point::courseTo(const Coordinates& point) const {
+int16_t Point::courseTo(const Coordinates &point) const
-    Coordinates begin = this->coordinates;
+{
-    Coordinates end = point;
+    const Coordinates begin = this->coordinates;
+    const Coordinates end = point;
 
-    double phi = log( tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) );
+    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));
-    double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT);
+    const double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT);
 
-    int16_t res = static_cast<int16_t>(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1;
+    return static_cast<int16_t>(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1;
-
-    // if (res < 0)
-    //     res += 360; 
-    
-    return res;
 }
 
-int16_t Point::courseTo(const Point &point) const {
+int16_t Point::courseTo(const Point &point) const
+{
     return this->courseTo(point.getCoordinates());
 }
 
-void Point::init(uint32_t horizontalAccuracy, uint32_t creationTime) {
+void Point::init(uint32_t horizontalAccuracy, uint32_t creationTime)
+{
     this->creationTime = creationTime;
 
     if (horizontalAccuracy == UINT32_MAX)
+    {
         this->accuracy = Accuracy::imported;
+    }
     else if (horizontalAccuracy > 9999)
+    {
         this->accuracy = Accuracy::fourDigOfCM;
+    }
     else if (horizontalAccuracy > 999)
+    {
         this->accuracy = Accuracy::threeDigOfCM;
+    }
     else if (horizontalAccuracy > 99)
+    {
         this->accuracy = Accuracy::twoDigOfCM;
+    }
     else if (horizontalAccuracy > 1)
+    {
         this->accuracy = Accuracy::oneDigOfCM;
+    }
     else
+    {
         this->accuracy = Accuracy::none;
+    }
 }

lib/Point/point.h

@@ -18,23 +18,26 @@
 #define ROUTE_DISTANCE_BETWEEN_LATITUDE 111300
 
 /**
- * @brief A to handle points on the earth
+ * @brief A class to handle points on the earth
  *
- * The points inherits latidue and longitude as doubles
+ * The points inherits latitude and longitude as doubles
  *
  */
-class Point{
+class Point
-    public:
+{
+public:
     /**
      * @brief Hold the data longitude and latitude
      *
      */
-        struct Coordinates {
+    struct Coordinates
+    {
         double lon;
         double lat;
 
-            bool operator==(const Coordinates rhs) const {
+        bool operator==(const Coordinates rhs) const
-                return ( this->lon == rhs.lon ) && ( this->lon == rhs.lon );
+        {
+            return (this->lon == rhs.lon) && (this->lon == rhs.lon);
         }
     };
 
@@ -42,7 +45,8 @@ class Point{
      * @brief The Accuracy is set by the constructor
      *
      */
-        enum Accuracy {
+    enum Accuracy
+    {
         none,
         fourDigOfCM,
         threeDigOfCM,
@@ -51,15 +55,13 @@ class Point{
         imported
     };
 
-
     /**
      * @brief Construct a new Point object
      *
-         * @param lat latitude
+     * @param lat
-         * @param lon longitude
+     * @param lon
-         * @param horizontalAccuracy mm
+     * @param horizontalAccuracy
-         * @param coords Coordinates
+     * @param creationTime
-         * @param imported if true than highest accuracy
      */
     Point(double lat, double lon, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0);
     Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0);
@@ -74,7 +76,7 @@ class Point{
      * @return true
      * @return false
      */
-        bool operator==(const Point& rhs) const;
+    bool operator==(const Point &rhs) const;
 
     /**
      * @brief Checks if the point is initalized.
@@ -100,8 +102,8 @@ class Point{
      * @param point
      * @return double meter
      */
-        double distanceTo(const Coordinates& point) const;
+    double distanceTo(const Coordinates &point) const;
-        double distanceTo(const Point& point) const;
+    double distanceTo(const Point &point) const;
 
     /**
      * @brief Calculates the course to an other point.
@@ -109,8 +111,8 @@ class Point{
      * @param point
      * @return int16_t degree
      */
-        int16_t courseTo(const Coordinates& point) const;
+    int16_t courseTo(const Coordinates &point) const;
-        int16_t courseTo(const Point& point) const;
+    int16_t courseTo(const Point &point) const;
 
     uint32_t getCreationTime() const { return this->creationTime; }
     double getLongitude() const { return this->coordinates.lon; }
@@ -127,13 +129,13 @@ class Point{
      */
     Accuracy getAccuracy() const { return this->accuracy; }
 
-    private:
+private:
     void init(uint32_t horizontalAccuracy, uint32_t creationTime);
 
     Accuracy accuracy = Accuracy::none;
-        Coordinates coordinates;
+    Coordinates coordinates{0, 0};
 
     uint32_t creationTime = 0;
 };
 
-#endif //POINT_H
+#endif // POINT_H

lib/Sensors/sensorData.cpp

@@ -1,5 +1,5 @@
 /**
- * @file senors.cpp
+ * @file sensorData.cpp
  * @author Alexander Klein (alex@kleiax.de)
  * @brief
  * @version 0.1
@@ -12,96 +12,184 @@
 #include "sensorData.h"
 
 bool SensorData::outputStatusPrintPVTdata = false;
-bool SensorData::newData = false;
 uint32_t SensorData::ubxUpdateTimeStatic = 0;
-UBX_NAV_PVT_data_t* SensorData::ubxDataStatic = nullptr;
+UBX_NAV_PVT_data_t *SensorData::ubxDataStatic = nullptr;
 
-SensorData::SensorData() {
+SensorData::SensorData()
-    this->loopDelay = 50;
+{
+    Component::loopDelay = SensorData::loopDelay;
 }
 
-void SensorData::enableGnss(SPIClass* spiPort, uint8_t csPin) {
+void SensorData::enableGnss(SPIClass *spiPort, uint8_t csPin)
+{
     this->gnss = new SFE_UBLOX_GNSS();
-    if (this->gnss->begin(*spiPort, csPin, 4000000) == false) {
+    if (this->gnss->begin(*spiPort, csPin, 4000000) == false)
+    {
         std::cout << "u-blox GNSS not detected on SPI bus. Please check wiring. Freezing." << std::endl;
-        while (1);
+        while (true)
+        {
+        }
     }
     this->initGnss();
 }
 
-void SensorData::enableGnss() {
+void SensorData::enableGnss()
+{
     this->gnss = new SFE_UBLOX_GNSS();
-    if (this->gnss->begin() == false) {
+    if (this->gnss->begin() == false)
+    {
         std::cout << "u-blox GNSS not detected at default I2C address. Please check wiring. Freezing." << std::endl;
-        while (1);
+        while (true)
+        {
+        }
     }
     this->initGnss();
 }
 
-void SensorData::enableRealCompass() {
+void SensorData::enableRealCompass()
+{
+    static constexpr byte address = 0x0d;
+
     this->realCompass = new QMC5883LCompass();
     // Init Compass
-    Wire.beginTransmission(0x0d);
+    Wire.beginTransmission(address);
+    // TODO: describe Bytes !!!
     Wire.write(0x0b);
     Wire.write(0x01);
     Wire.endTransmission();
-    this->realCompass->setMode(0x01,0x0C,0x10,0X00);
+    this->realCompass->setMode(0x01, 0x0C, 0x10, 0X00);
     CalibrateCompass caliCompass(this->realCompass);
     caliCompass.loadData();
     caliCompass.useData();
 }
 
-void SensorData::enableCalcCompass() {
+void SensorData::enableCalcCompass()
-
+{
+    // TODO: !!! implementieren
 }
 
-void SensorData::enableGyroskop() {
+void SensorData::enableGyroscope()
+{
+    this->gyroscope = new MPU6050();
+    this->gyroscope->initialize();
+    if (!this->gyroscope->testConnection())
+    {
+        std::cout << "SensorData::enableGyroscope: Gyroskop is not conntected. 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 {
+CalcAzimuth::State SensorData::getCalcAzimuthState() const
-    if (this->calcCompass)
+{
+    if (static_cast<bool>(this->calcCompass))
+    {
         return this->calcCompass->getState();
+    }
     return CalcAzimuth::State::Invalid;
 }
 
-void SensorData::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
+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;
+
     if (!SensorData::outputStatusPrintPVTdata)
+    {
         return;
+    }
 
-    double latitude = (double) ubxDataStruct->lat / 10000000.0;
+    const double latitude = ubxDataStruct->lat / 10000000.0;
-    double longitude = (double) ubxDataStruct->lon / 10000000.0;
+    const double longitude = ubxDataStruct->lon / 10000000.0;
-    double altitude = (double) ubxDataStruct->hMSL / 1000.0;
+    const double altitude = ubxDataStruct->hMSL / 1000.0;
 
-    uint8_t fixType = ubxDataStruct->fixType;
+    const uint8_t fixType = ubxDataStruct->fixType;
-    char fixTypeString[32];
+    char fixTypeString[stringSize];
     if (fixType == 0)
-        strcpy(fixTypeString, "None");
+    {
+        strcpy(fixTypeString, static_cast<const char *>("None"));
+    }
     else if (fixType == 1)
-        strcpy(fixTypeString, "Dead Reckoning");
+    {
+        strcpy(fixTypeString, static_cast<const char *>("Dead Reckoning"));
+    }
     else if (fixType == 2)
-        strcpy(fixTypeString, "2D");
+    {
+        strcpy(fixTypeString, static_cast<const char *>("2D"));
+    }
     else if (fixType == 3)
-        strcpy(fixTypeString, "3D");
+    {
+        strcpy(fixTypeString, static_cast<const char *>("3D"));
+    }
     else if (fixType == 3)
-        strcpy(fixTypeString, "GNSS + Dead Reckoning");
+    {
+        strcpy(fixTypeString, static_cast<const char *>("GNSS + Dead Reckoning"));
+    }
     else if (fixType == 5)
-        strcpy(fixTypeString, "Time Only");
+    {
+        strcpy(fixTypeString, static_cast<const char *>("Time Only"));
+    }
     else
-        strcpy(fixTypeString, "UNKNOWN");
+    {
+        strcpy(fixTypeString, static_cast<const char *>("UNKNOWN"));
+    }
 
-    uint8_t carrSoln = ubxDataStruct->flags.bits.carrSoln;
+    const uint8_t carrSoln = ubxDataStruct->flags.bits.carrSoln;
-    char carrSolnString[16];
+    char carrSolnString[stringSize];
     if (carrSoln == 0)
-        strcpy(carrSolnString, "None");
+    {
+        strcpy(carrSolnString, static_cast<const char *>("None"));
+    }
     else if (carrSoln == 1)
-        strcpy(carrSolnString, "Floating");
+    {
+        strcpy(carrSolnString, static_cast<const char *>("Floating"));
+    }
     else if (carrSoln == 2)
-        strcpy(carrSolnString, "Fixed");
+    {
+        strcpy(carrSolnString, static_cast<const char *>("Fixed"));
+    }
     else
-        strcpy(carrSolnString, "UNKNOWN");
+    {
+        strcpy(carrSolnString, static_cast<const char *>("UNKNOWN"));
+    }
 
-    uint32_t hAcc = ubxDataStruct->hAcc;
+    const uint32_t hAcc = ubxDataStruct->hAcc;
 
     std::cout << "Lat: " << latitude
               << " Lng: " << longitude
@@ -112,35 +200,53 @@ void SensorData::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
               << " Horizontal Accuracy Estimate: " << hAcc << " mm" << std::endl;
 }
 
-void SensorData::savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
+void SensorData::savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct)
+{
     SensorData::printPVTdata(ubxDataStruct);
 
-    SensorData::newData = true;
     SensorData::ubxDataStatic = ubxDataStruct;
     SensorData::ubxUpdateTimeStatic = millis();
 }
 
-void SensorData::setOutputStatusPrintPVTdata(bool status) {
+void SensorData::setOutputStatusPrintPVTdata(bool status)
+{
     SensorData::outputStatusPrintPVTdata = status;
 }
 
-void SensorData::run() {
+void SensorData::run()
+{
+    if (static_cast<bool>(this->realCompass))
+    {
         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() {
+void SensorData::runAsChild()
+{
+    if (static_cast<bool>(this->gnss))
+    {
         this->gnss->checkUblox();
         this->gnss->checkCallbacks();
-
+        if (SensorData::ubxUpdateTimeStatic != this->lastUbxUpdate)
-    if (SensorData::newData)
+        {
-        SensorData::newData = false;
+            this->updateUbxData();
+        }
+    }
 }
 
-void SensorData::initGnss() {
+void SensorData::initGnss()
-    uint8_t versionHigh = this->gnss->getProtocolVersionHigh();
+{
-    uint8_t versionLow = this->gnss->getProtocolVersionLow();
+    const uint8_t versionHigh = this->gnss->getProtocolVersionHigh();
-    std::cout << "u-blox protocol version: " << unsigned(versionHigh) << "." << unsigned(versionLow) << std::endl;
+    const uint8_t versionLow = this->gnss->getProtocolVersionLow();
+    std::cout << "u-blox protocol version: " << static_cast<int>(versionHigh) << "." << static_cast<int>(versionLow) << std::endl;
 
     this->gnss->setSPIOutput(COM_TYPE_UBX);
     this->gnss->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI, 10);
@@ -150,3 +256,15 @@ void SensorData::initGnss() {
     this->gnss->setNavigationFrequency(1);
     this->gnss->setAutoPVT(true);
 }
+
+void SensorData::updateUbxData()
+{
+    this->gnssData = SensorData::ubxDataStatic;
+    this->lastUbxUpdate = SensorData::ubxUpdateTimeStatic;
+
+    Point::Coordinates coords{0, 0};
+    coords.lat = this->gnssData->lat / 10000000.0;
+    coords.lon = this->gnssData->lon / 10000000.0;
+
+    this->currentPosition = Point(coords, this->gnssData->hAcc);
+}

lib/Sensors/sensorData.h

@@ -1,7 +1,7 @@
 /**
  * @file sensorData.h
  * @author Alexander Klein (alex@kleiax.de)
- * @brief 
+ * @brief Contains the SensorData class
  * @version 0.1
  * @date 2023-09-02
  *
@@ -13,6 +13,7 @@
 #define SENSOR_DATA_H
 
 #include <SPI.h>
+#include <I2Cdev.h>
 #include <iostream>
 
 #include "component.h"
@@ -20,33 +21,106 @@
 
 #include <SparkFun_u-blox_GNSS_Arduino_Library.h>
 #include <QMC5883LCompass.h>
-// Gyroskop
+#include <MPU6050_6Axis_MotionApps20.h>
 #include "calcAzimuth.h"
 
 #include "point.h"
-class Sensors;
 
-class SensorData : public Component {
+/**
-    public:
+ * @brief A class to manage all sensors
+ *
+ * Each sensor have separately to be enabled
+ */
+class SensorData : public Component
+{
+public:
     SensorData();
+    ~SensorData();
 
-        void enableGnss(SPIClass* spiPort, uint8_t csPin);
+    void enableNtrip(String host, uint16_t port, String mountPoint, String user, String password);
+
+    /**
+     * @brief Enable the gnss module over spi
+     *
+     * @param spiPort
+     * @param csPin
+     */
+    void enableGnss(SPIClass *spiPort, uint8_t csPin);
+
+    /**
+     * @brief Enable the gnss module over i2c
+     */
     void enableGnss();
+
     void enableRealCompass();
     void enableCalcCompass();
-        void enableGyroskop();
+    void enableGyroscope();
 
-        // Interface Const kram
+    // Interface Const
+    /**
+     * @brief Get the azimuth measured by the compass module
+     *
+     * @return int16_t
+     */
     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; };
+    const UBX_NAV_PVT_data_t *getGnssData() const { return this->gnssData; };
-        const void* const getGyroData() const;
+    NTRIPClientStates getNtripState() const;
 
-        CalcAzimuth* getCalcCompass() const { return this->calcCompass; }
+    /**
-        QMC5883LCompass* getRealCompass() const { return this->realCompass; }
+     * @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
+     * @return QMC5883LCompass*
+     */
+    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
     /**
@@ -59,31 +133,47 @@ class SensorData : public Component {
      */
     static void setOutputStatusPrintPVTdata(bool status);
 
-    private:
+private:
     void run() override;
     void runAsChild() override;
     void initGnss();
+    void updateUbxData();
 
-        QMC5883LCompass* realCompass = nullptr;
+    QMC5883LCompass *realCompass = nullptr;
-        CalcAzimuth* calcCompass = nullptr;
+    CalcAzimuth *calcCompass = nullptr;
-        SFE_UBLOX_GNSS* gnss = nullptr;
+    SFE_UBLOX_GNSS *gnss = nullptr;
+    NTRIPClient *ntripClient = nullptr;
+    MPU6050 *gyroscope = nullptr;
 
-        UBX_NAV_PVT_data_t* gnssData;
+    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);
 
-        static UBX_NAV_PVT_data_t* ubxDataStatic;
+    static UBX_NAV_PVT_data_t *ubxDataStatic;
-
     static uint32_t ubxUpdateTimeStatic;
-
     static bool outputStatusPrintPVTdata;
-        static bool newData;
+
+    static constexpr uint8_t loopDelay = 50;
 };
 
-#endif //SENSOR_DATA_H
+#endif // SENSOR_DATA_H

lib/Speedometer/speedometer.cpp

@@ -11,111 +11,144 @@
  */
 #include "speedometer.h"
 
-Speedometer::Speedometer(uint8_t pin, double diameter, uint16_t steps) {
+Speedometer::Speedometer(uint8_t pin, double diameter, uint16_t steps)
-    this->diameter = diameter;
+    : pulseCounter{new Counter(pin)}, diameter{diameter}, steps{steps}, buf{}
-    this->steps = steps;
+{
-
+    this->pulseCounter->setFilterValue(Speedometer::maxFilterValue); // ignore pulses less than 1000 x 2.5ns
-    this->pulseCounter = new Counter(pin);
-    this->pulseCounter->setFilterValue(1000); // ignore pulses less than 1000 x 2.5ns
 
     this->pulseCounter->clear();
     this->pulseCounter->resume();
 
     Component::loopDelay = Speedometer::loopDelay;
 
-    clearAvgBuf();
+    this->clearAvgBuf();
 }
 
-Speedometer::~Speedometer() {
+Speedometer::~Speedometer()
+{
     delete this->pulseCounter;
 }
 
-void Speedometer::run() {
+void Speedometer::run()
+{
+    static constexpr float minimalSpeed = 0.1;
+
     if (this->calibrationRunning)
+    {
         return;
+    }
 
-    uint32_t time = millis();
+    const uint32_t time = millis();
 
-    uint16_t elapsedTime = time - this->lastMillisCalc;
+    const uint16_t elapsedTime = time - this->lastMillisCalc;
     this->lastMillisCalc = time;
 
-    int16_t pulse = this->pulseCounter->getValue();
+    const double pulse = this->pulseCounter->getValue();
     this->pulseCounter->clear();
     this->pulseCounter->resume();
 
+    const double wheelRevolutionsAbsolute = pulse / this->steps;
+    const double wheelRevolutionsRelativ = wheelRevolutionsAbsolute / (elapsedTime / 1000.0);
 
-    double n = (double)pulse / this->steps;   // Wheel revolutions in absolute time
+    double meterPerSecond = wheelRevolutionsRelativ * (diameter * PI);
-    double u = n / ((double)elapsedTime / 1000);           // Wheel revolutions per second
+    double radPerSecond = wheelRevolutionsRelativ * 2 * PI;
-    double ms = u * (diameter * PI);                      // Speed in m/s
 
-    switch (this->currentDirection) {
+    if (meterPerSecond < minimalSpeed)
-        case Direction::Forward :
+    {
-            this->speed = ms;
+        meterPerSecond = 0;
+        radPerSecond = 0;
+    }
+
+    switch (this->currentDirection)
+    {
+    case Direction::Forward:
+        this->speed = meterPerSecond;
+        this->rad = radPerSecond;
         break;
 
-        case Direction::Backward :
+    case Direction::Backward:
-            this->speed = -ms;
+        this->speed = -meterPerSecond;
+        this->rad = -radPerSecond;
         break;
 
-        case Direction::None :
+    case Direction::None:
         this->speed = 0;
+        this->rad = 0;
         break;
     }
 
     this->addValToBuf(static_cast<int16_t>(this->speed * Speedometer::conversionFactor));
 }
 
-void Speedometer::setDirection(Direction dir) {
+void Speedometer::setDirection(Direction dir)
+{
     if (this->currentDirection == dir)
+    {
         return;
+    }
     this->currentDirection = dir;
     this->clearAvgBuf();
 }
 
-void Speedometer::setEncFilter(uint16_t val) {
+void Speedometer::setEncFilter(uint16_t val)
-    if (val > 1023) 
+{
-        val = 1023;
+    if (val > Speedometer::maxFilterValue)
+    {
+        val = Speedometer::maxFilterValue;
+    }
     this->pulseCounter->setFilterValue(val);
 }
 
-double Speedometer::getAvgSpeed() const {
+double Speedometer::getAvgSpeed() const
-    int16_t avg = this->calcAverage();
+{
-    return (float)avg / Speedometer::conversionFactor;
+    const double avg = this->calcAverage();
+    return avg / Speedometer::conversionFactor;
 }
 
-void Speedometer::calibrationMeasurementStart() {
+void Speedometer::calibrationMeasurementStart()
+{
     std::cout << "Start" << std::endl;
     this->calibrationRunning = true;
     this->pulseCounter->clear();
     this->pulseCounter->resume();
 }
 
-uint16_t Speedometer::calibrationMeasurementStop() {
+uint16_t Speedometer::calibrationMeasurementStop()
+{
     std::cout << "Ende" << std::endl;
     this->calibrationRunning = false;
-    uint16_t res = abs(this->pulseCounter->getValue());
+    const uint16_t res = abs(this->pulseCounter->getValue());
     this->pulseCounter->clear();
     this->pulseCounter->resume();
     std::cout << "Result: " << res << std::endl;
     return res;
 }
 
-void Speedometer::clearAvgBuf() {
+void Speedometer::clearAvgBuf()
+{
     for (uint8_t i = 0; i < bufSize; i++)
+    {
         this->buf[i] = 0;
+    }
 }
 
-void Speedometer::addValToBuf(int16_t val) {
+void Speedometer::addValToBuf(int16_t val)
+{
     this->buf[this->bufPos] = val;
     this->bufPos++;
 
     if (bufPos == bufSize)
+    {
         bufPos = 0;
+    }
 }
 
-int16_t Speedometer::calcAverage() const {
+int16_t Speedometer::calcAverage() const
+{
     int16_t sum = 0;
-    for (int i = 0; i < this->bufSize; i++)
+    for (int i = 0; i < Speedometer::bufSize; i++)
+    {
         sum += this->buf[i];
-    return sum / this->bufSize;
+    }
+    return sum / Speedometer::bufSize;
 }

lib/Speedometer/speedometer.h

@@ -26,8 +26,9 @@
  * The calculated speed is the average of an amount of last measurements.
  *
  */
-class Speedometer : public Component {
+class Speedometer : public Component
-    public:
+{
+public:
     /**
      * @brief Enum to control the direction.
      *
@@ -35,7 +36,8 @@ class Speedometer : public Component {
      * If the Direction is Backward, the internal counter counts down and a negativ speed will be returned.
      * If the Direction is None, no measurement will be taken.
      */
-        enum Direction {
+    enum Direction
+    {
         None,
         Forward,
         Backward
@@ -46,7 +48,7 @@ class Speedometer : public Component {
      *
      * @param pin Pin on the Esp from the encoder.
      * @param diameter Diameter of the wheel in meters.
-         * @param steps Encodersteps for a complete wheel rotation.
+     * @param steps EncoderSteps for a complete wheel rotation.
      */
     Speedometer(uint8_t pin, double diameter, uint16_t steps);
 
@@ -59,13 +61,6 @@ class Speedometer : public Component {
      */
     void setDirection(Direction dir);
 
-        /**
-         * @brief Set the number of last values ​​to be taken into account for the average.
-         * 
-         * @param val length of the array
-         */
-        void setNumOfValForAvg(uint8_t val);
-
     /**
      * @brief Set the Enc Filter to prevent bouncing
      *
@@ -83,11 +78,24 @@ class Speedometer : public Component {
     Direction getDirection() const { return this->currentDirection; }
 
     /**
-         * @brief Get the calculated speed of the Wheel
+     * @brief Get the calculated speed of the wheel
      *
      * @return double speed in m/s
      */
     double getSpeed() const { return this->speed; }
+
+    /**
+     * @brief Get the calculated speed of the wheel
+     *
+     * @return double speed in rad/s
+     */
+    double getSpeedRad() const { return this->rad; };
+
+    /**
+     * @brief Get the calculated average speed of the wheel
+     *
+     * @return double speed in m/s
+     */
     double getAvgSpeed() const;
 
     /**
@@ -108,10 +116,8 @@ class Speedometer : public Component {
      */
     uint16_t calibrationMeasurementStop();
 
-
+private:
-    private:
     void run() override;
-        void init(uint8_t pin, double diameter, uint16_t steps);
     void clearAvgBuf();
     void addValToBuf(int16_t val);
     int16_t calcAverage() const;
@@ -120,21 +126,22 @@ class Speedometer : public Component {
     static constexpr uint8_t bufSize = 5;
     static constexpr uint8_t conversionFactor = 100;
 
-        Counter* pulseCounter;
+    Counter *pulseCounter;
     Direction currentDirection = Direction::None;
 
     bool calibrationRunning = false;
 
     double speed = 0;
+    double rad = 0;
     double diameter;
 
     uint8_t printCounter = 0;
     uint8_t bufPos = 0;
     uint16_t steps;
-
     int16_t buf[Speedometer::bufSize];
-
     uint32_t lastMillisCalc = 0;
+
+    static constexpr uint16_t maxFilterValue = 1023;
 };
 
 #endif // SPEEDOMETER_H

lib/Times/debugTimes.cpp

@@ -1,7 +1,7 @@
 /**
  * @file debugTimes.cpp
  * @author Alexander Klein (alex@kleiax.de)
- * @brief Implemention of the class debugTimes.h.
+ * @brief Implementation of the class debugTimes.h.
  * @version 0.1
  * @date 2021-12-13
  *
@@ -13,33 +13,44 @@
 bool DebugTimes::print = false;
 bool DebugTimes::printWarning = true;
 
-DebugTimes::DebugTimes() {
+DebugTimes::DebugTimes()
-    this->startTime = millis();
+    : startTime{millis()}
-
+{
-    if (DebugTimes::printWarning) {
+    if (DebugTimes::printWarning)
-        std::cout << std::endl << "Warning: DebugTimes is muuted, no times are be shown." << std::endl << std::endl;
+    {
+        std::cout << std::endl
+                  << "Warning: DebugTimes is muted, no times are be shown." << std::endl
+                  << std::endl;
         DebugTimes::printWarning = false;
     }
 }
 
-void DebugTimes::restart() {
+void DebugTimes::restart()
+{
     this->startTime = millis();
 }
 
-uint16_t DebugTimes::stop() {
+const uint16_t DebugTimes::stop()
+{
     return millis() - this->startTime;
 }
 
-uint16_t DebugTimes::stopConsol(const char* name, uint16_t minTime) {
+const uint16_t DebugTimes::stopConsol(const char *name, uint16_t minTime)
-    uint64_t time = millis() - this->startTime;
+{
+    const uint64_t time = millis() - this->startTime;
     if (time > minTime && DebugTimes::print)
+    {
         std::cout << name << " needs " << time << " ms" << std::endl;
+    }
     return time;
 }
 
-void DebugTimes::setConsolOutput(bool enable) {
+void DebugTimes::setConsolOutput(bool enable)
+{
     if (enable == DebugTimes::print)
+    {
         return;
+    }
 
     DebugTimes::print = enable;
     DebugTimes::printWarning = !enable;

lib/Times/debugTimes.h

@@ -28,8 +28,9 @@
  * @warning This class is not very accurate
  * It only give you the time in milliseconds.
  */
-class DebugTimes {
+class DebugTimes
-    public:
+{
+public:
     /**
      * @brief Construct a new Debug Times object
      * Starts to count milliseconds
@@ -47,17 +48,17 @@ class DebugTimes {
      *
      * @return uint16_t elapsed milliseconds
      */
-        uint16_t stop();
+    const uint16_t stop();
 
     /**
      * @brief Print the elapsed time to consol
      *
-         * @param name Functionname to print
+     * @param name FunctionName to print
      * @param minTime A minimum time before printing
      *
      * @return uint16_t elapsed milliseconds
      */
-        uint16_t stopConsol(const char* name, uint16_t minTime = 0);
+    const uint16_t stopConsol(const char *name, uint16_t minTime = 0);
 
     /**
      * @brief Sets if the result should be printed.
@@ -66,11 +67,11 @@ class DebugTimes {
      */
     static void setConsolOutput(bool enable);
 
-    private:
+private:
     uint64_t startTime;
 
     static bool print;
     static bool printWarning;
 };
 
-#endif //DEBUG_TIMES_H
+#endif // DEBUG_TIMES_H

lib/calibrateCompass/calibrateCompass.h

@@ -1,60 +0,0 @@
-/**
- * @file calibrateCompass.h
- * @author Alexander Klein (alex@kleiax.de)
- * @brief 
- * @version 0.1
- * @date 2023-05-23
- * 
- * @copyright Copyright (c) 2023
- * 
- */
-
-#pragma once
-
-#include <QMC5883LCompass.h>
-#include <Preferences.h>
-#include <iostream>
-
-#include "component.h"
-
-class CalibrateCompass : public Component {
-    public:
-        enum State {
-            Ready,
-            Calibrating,
-            Finished
-        };
-
-        struct CallibrationData {
-            int data[3][2];
-        };
-
-        CalibrateCompass(QMC5883LCompass* compass);
-
-        void start();
-        void useData();
-        void removeCalibration();
-        void reset();
-        void saveData();
-        void loadData();
-
-        State getState() const { return this->state; }
-        CallibrationData getCallibrationData() const { return this->data; }
-
-        friend std::ostream& operator<<(std::ostream& os, const CalibrateCompass& caliComp);
-
-    private:
-        void runAsChild() override;
-        void run() override;
-        void checkDataValidity();
-
-        QMC5883LCompass* compass;
-        State state;
-        CallibrationData data;
-
-        void clearData();
-
-        bool dataValid = false;
-        const uint16_t maxTimeWithoutChange = 10000;
-        uint32_t lastChange = 0;
-};

platformio.ini

@@ -15,7 +15,7 @@ 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
 	knolleary/PubSubClient@^2.8
@@ -23,16 +23,18 @@ lib_deps =
 	marcoschwartz/LiquidCrystal_I2C@^1.1.4
 	bblanchon/ArduinoJson@^6.20.0
 	mprograms/QMC5883LCompass@^1.2.0
-	https://git.kleiax.de/PlatformIO-Libs/Menu.git
+	jrowberg/I2Cdevlib-MPU6050@^1.0.0
-	nrf24/RF24@^1.4.5
+; upload_port = COM6
-upload_port = COM6
-; extra_scripts = 
-;    pre:autoVersionIncrement/version_increment_pre.py
-;    post:autoVersionIncrement/version_increment_post.py
 test_ignore = test_desktop
 build_type = debug
 monitor_filters = esp32_exception_decoder
 check_tool = clangtidy
+check_flags =
+	clangtidy: --checks=-*,cert-*,cppcoreguidelines-*,clang-analyzer-*,google-*,hicpp-*,modernize-*,performance-*,portability-*,readability-*,bugprone-*,misc-*,openmp-*,objc-* --fix
+check_src_filters =
+	+<src/*>
+	+<lib/*>
+	+<include/*>
 
 [env:native]
 platform = native
@@ -40,3 +42,21 @@ test_ignore = test_embedded
 
 [platformio]
 description = A Rover who should be drive a route by gps.
+
+
+; Bei Möglichkeit folgendes aus clang tidy entfernen
+; cppcoreguidelines-owning-memory
+; 
+; 
+; 
+; 
+; 
+; 
+; 
+; 
+; 
+; 
+; 
+; 
+; 
+#

src/OutputBuf/outputBuf.cpp

@@ -1,5 +1,5 @@
 /**
- * @file outputBufMqtt.cpp
+ * @file outputBuf.cpp
  * @author Alexander Klein (alex@kleiax.de)
  * @brief Contains the implementation of the class OutputBufMqtt
  * @version 0.1
@@ -11,49 +11,66 @@
 
 #include "OutputBuf/outputBuf.h"
 
-OutputBuf::OutputBuf(DebugMqtt* debugMqtt, BluetoothSerial* serialBT) 
+OutputBuf::OutputBuf(DebugMqtt *debugMqtt, BluetoothSerial *serialBT)
-    : std::streambuf() {
+    : debugMqtt{debugMqtt}, serialBT{serialBT}
-    this->debugMqtt = debugMqtt;
+{
-    this->serialBT = serialBT;
+    if (static_cast<bool>(debugMqtt))
-
+    {
-    if (debugMqtt)
         this->isMqttActive = true;
+    }
 
-    if (serialBT)
+    if (static_cast<bool>(serialBT))
+    {
         this->isMqttActive = true;
+    }
 }
 
-void OutputBuf::activateMqtt(bool status) {
+void OutputBuf::activateMqtt(bool status)
-    if (debugMqtt)
+{
+    if (static_cast<bool>(debugMqtt))
+    {
         this->isMqttActive = status;
+    }
 }
 
-void OutputBuf::activateSerialBT(bool status) {
+void OutputBuf::activateSerialBT(bool status)
-    if (serialBT)
+{
+    if (static_cast<bool>(serialBT))
+    {
         this->isSerialBTActive = status;
+    }
 }
 
-void OutputBuf::setDebugMqtt(DebugMqtt* debugMqtt) {
+void OutputBuf::setDebugMqtt(DebugMqtt *debugMqtt)
+{
     this->debugMqtt = debugMqtt;
     this->isMqttActive = true;
 }
 
-void OutputBuf::setSerialBT(BluetoothSerial* serialBT) {
+void OutputBuf::setSerialBT(BluetoothSerial *serialBT)
+{
     this->serialBT = serialBT;
-    this->isSerialBTActive = true;;
+    this->isSerialBTActive = true;
+    ;
 }
 
-std::streambuf::int_type OutputBuf::overflow(std::streambuf::int_type c) {
+std::streambuf::int_type OutputBuf::overflow(std::streambuf::int_type character)
-    if (c != EOF) {
+{
+    if (character != EOF)
+    {
         // c = std::toupper(static_cast<char>(c), getloc());
 
         if (this->debugMqtt && this->isMqttActive)
-            this->debugMqtt->addCharacter((char) c);
+        {
+            this->debugMqtt->addCharacter(static_cast<char>(character));
+        }
 
         if (this->serialBT && this->isSerialBTActive && this->serialBT->connected())
-            this->serialBT->print((char) c);
+        {
-
+            this->serialBT->print(static_cast<char>(character));
-        Serial.print((char) c);
         }
-    return c;
+
+        Serial.print(static_cast<char>(character));
+    }
+    return character;
 }

src/OutputBuf/outputBuf.h

@@ -1,5 +1,5 @@
 /**
- * @file outputBufMqtt.h
+ * @file outputBuf.h
  * @author Alexander Klein (alex@kleiax.de)
  * @brief Contains a small class that provide a streambuf
  *
@@ -26,26 +26,28 @@
 #include <BluetoothSerial.h>
 
 /**
- * @brief A alternativ streambuf for std::cout
+ * @brief A alternative streambuf for std::cout
  *
  * The streambuf is used to double the std::cout to consol
  * and MQTT.
  *
  */
-class OutputBuf : public std::streambuf {
+class OutputBuf : public std::streambuf
-    public:
+{
+public:
     /**
      * @brief Construct a new Output Buf Mqtt object
      *
      * @param debugMqtt
+     * @param serialBT
      */
-        OutputBuf(DebugMqtt* debugMqtt = nullptr, BluetoothSerial* serialBT = nullptr);
+    OutputBuf(DebugMqtt *debugMqtt = nullptr, BluetoothSerial *serialBT = nullptr);
 
     /**
      * @brief Activate the mqtt output
      *
      * This only works if the DebugMqtt is set by
-         * setDebugMqtt. 
+     * setDebugMqtt().
      *
      * @param status
      */
@@ -55,17 +57,16 @@ class OutputBuf : public std::streambuf {
      * @brief Active the output over the bluetooth serial consol
      *
      * This only works if BluetoothSerial is set by
-         * setSerialBT.
+     * setSerialBT().
      *
      * @param status
      */
     void activateSerialBT(bool status);
 
-        void setDebugMqtt(DebugMqtt* debugMqtt);
+    void setDebugMqtt(DebugMqtt *debugMqtt);
-        void setSerialBT(BluetoothSerial* serialBT);
+    void setSerialBT(BluetoothSerial *serialBT);
 
-
+protected:
-    protected:
     /**
      * @brief
      *
@@ -74,12 +75,12 @@ class OutputBuf : public std::streambuf {
      */
     virtual std::streambuf::int_type overflow(std::streambuf::int_type c);
 
-    private:
+private:
     bool isMqttActive = false;
     bool isSerialBTActive = false;
 
-        DebugMqtt* debugMqtt;
+    DebugMqtt *debugMqtt;
-        BluetoothSerial* serialBT;
+    BluetoothSerial *serialBT;
 };
 
 #endif // OUTPUT_STREAM_MQTT_H

src/SpecialMenus/CalibrateBattery/menuCalibrateBattery.cpp

@@ -0,0 +1,70 @@
+/**
+ * @file menuCalibrateBattery.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief
+ * @version 0.1
+ * @date 2023-09-26
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+
+#include "menuCalibrateBattery.h"
+
+MenuCalibrateBattery::MenuCalibrateBattery(Battery *battery)
+    : battery{battery}
+{
+    this->setUpdateDelay(MenuCalibrateBattery::updateDelay);
+}
+
+void MenuCalibrateBattery::printMenu()
+{
+    if (this->battery->getCalibrationState() == Battery::CalibrationState::None)
+    {
+        this->battery->startCalibration();
+    }
+
+    String lineOne = "";
+    String lineTwo = "";
+
+    switch (this->battery->getCalibrationState())
+    {
+    case Battery::CalibrationState::Waiting:
+        lineOne = "Target Voltage:";
+        lineTwo = "7 + ";
+        lineTwo.concat(this->battery->getCurrentCalibrationVoltage());
+        lineTwo.concat(" / 10");
+        break;
+
+    case Battery::CalibrationState::Reading:
+        lineOne = "Reading...";
+        break;
+
+    case Battery::CalibrationState::Finished:
+        lineOne = "Finished!";
+        break;
+
+    default:
+        break;
+    }
+
+    this->print(lineOne, lineTwo);
+}
+
+void MenuCalibrateBattery::left()
+{
+    this->battery->finishCalibration();
+
+    if (static_cast<bool>(parentMenu))
+    {
+        this->parentMenu->printMenu();
+    }
+}
+
+void MenuCalibrateBattery::right()
+{
+    if (this->battery->getCalibrationState() == Battery::CalibrationState::Waiting)
+    {
+        this->battery->nextVoltageIsReady();
+    }
+}

src/SpecialMenus/CalibrateBattery/menuCalibrateBattery.h

@@ -0,0 +1,50 @@
+/**
+ * @file menuCalibrateBattery.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains a Menu class to calibrate the Battery class
+ * @version 0.1
+ * @date 2023-09-26
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+
+#ifndef MENU_CALIBRATE_BATTERY_H
+#define MENU_CALIBRATE_BATTERY_H
+
+#include "menuControl.h"
+#include "battery.h"
+
+/**
+ * @brief A Menu class to calibrate the Battery class
+ */
+class MenuCalibrateBattery : public MenuControl
+{
+public:
+    /**
+     * @brief Construct a new MenuCalibrateBattery object
+     *
+     * Prepare the calibration.
+     *
+     * @param battery
+     */
+    MenuCalibrateBattery(Battery *battery);
+
+    /**
+     * @brief Prints the instructions for the calibration
+     */
+    void printMenu() override;
+
+    // User Inputs
+    void left() override;
+    void no() override { this->left(); }
+    void right() override;
+    void yes() override { this->right(); }
+
+private:
+    Battery *battery;
+
+    static constexpr uint16_t updateDelay = 400;
+};
+
+#endif // MENU_CALIBRATE_BATTERY_H

src/SpecialMenus/GPS/menuGPS.cpp

@@ -1,132 +0,0 @@
-/**
- * @file menuGPS.cpp
- * @author Alexander Klein (alex@kleiax.de)
- * @brief Contains an implementation of the class MenuGPS
- * @version 0.1
- * @date 2022-01-29
- * 
- * @copyright Copyright (c) 2022
- * 
- */
-
-#include "menuGPS.h"
-
-MenuGPS::MenuGPS(SensorData* sensorData) :
-    MenuInformationSites(10) {
-    this->sensorData = sensorData;
-}
-
-void MenuGPS::printPage() const {
-    const UBX_NAV_PVT_data_t* gpsData = this->sensorData->getGnssData();
-    uint8_t fixType = 0;
-    if (gpsData)
-        fixType = gpsData->fixType;
-
-    String lineOne = "Data isn't valid";
-    String lineTwo = "or no GPS signal";
-
-    switch (this->getCurrentPage()) {
-        case 0:
-            if (fixType) {
-                lineOne = "Sats: ";
-                lineOne.concat(gpsData->numSV);
-                lineTwo = "PDOP: ";
-                lineTwo.concat(gpsData->pDOP);
-            }
-            break;
-        
-        case 1:
-            if (fixType) {
-                lineOne = "Lat: ";
-                lineOne.concat(gpsData->lat);
-                lineTwo = "Lon: ";
-                lineTwo.concat(gpsData->lon);
-            }
-            break;
-
-        case 2:
-            lineOne = "Time: ";
-            lineTwo = "";
-            if (fixType) {
-                if (gpsData->hour < 10)
-                    lineTwo.concat("0");
-                lineTwo.concat(gpsData->hour);
-                lineTwo.concat(":");
-                if (gpsData->min < 10)
-                    lineTwo.concat("0");
-                lineTwo.concat(gpsData->min);
-                lineTwo.concat(":");
-                if (gpsData->sec < 10)
-                    lineTwo.concat("0");
-                lineTwo.concat(gpsData->sec);
-            } else
-                lineTwo = "00:00:00";
-
-            break;
-
-        case 3:
-            lineOne = "Fix type:";
-            if (fixType == 0)
-                lineTwo = "None";
-            else if (fixType == 1)
-                lineTwo = "Dead Reckoning";
-            else if (fixType == 2)
-                lineTwo = "2D";
-            else if (fixType == 3)
-                lineTwo = "3D";
-            else if (fixType == 3)
-                lineTwo = "GNSS + Dead Reck";
-            else if (fixType == 5)
-                lineTwo = "Time Only";
-            else
-                lineTwo = "UNKNOWN";
-            break;
-
-        case 4: {
-            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 5: 
-            lineOne = "Hrizntl Accuracy";
-            if (fixType) {
-                lineTwo = "";
-                lineTwo.concat(gpsData->hAcc);
-            } else
-                lineTwo = "0";
-            break;
-
-        case 6:
-            lineOne = "magDec: ";
-            lineTwo = "magAcc: ";
-            if (fixType) {
-                lineOne.concat(gpsData->magDec);
-                lineTwo.concat(gpsData->magAcc);
-            } else {
-                lineOne.concat("---");
-                lineTwo.concat("---");   
-            }
-            break;
-
-        case 7:
-            lineOne = "Azimuth: ";
-            lineTwo = "";
-            lineTwo.concat(this->sensorData->getRealAzimuth());
-            break;
-
-        default:
-            this->printDefault();
-            return;
-    }
-
-    this->print(lineOne, lineTwo);
-}

src/SpecialMenus/GPS/menuGPS.h

@@ -1,39 +0,0 @@
-/**
- * @file menuGPS.h
- * @author Alexander Klein (alex@kleiax.de)
- * @brief Contains a class to print informations about the GPS object
- * @version 0.1
- * @date 2022-01-29
- * 
- * @copyright Copyright (c) 2022
- * 
- */
-
-#ifndef MENU_GPS_H
-#define MENU_GPS_H
-
-#include <SparkFun_u-blox_GNSS_Arduino_Library.h>
-
-#include "menuInformationSites.h"
-#include "sensorData.h"
-
-/**
- * @brief A class to print informations about the GPS object
- * 
- */
-class MenuGPS : public MenuInformationSites {
-    public:
-        /**
-         * @brief Construct a new Menu GPS object
-         * 
-         * @param gps 
-         */
-        MenuGPS(SensorData* sensorData);
-
-    private:
-        void printPage() const override;
-
-        SensorData* sensorData;
-};
-
-#endif // MENU_GPS_H

src/SpecialMenus/PID/menuPidSettings.cpp

@@ -11,12 +11,13 @@
 
 #include "menuPidSettings.h"
 
-MenuPidSettings::MenuPidSettings(PID* pid) {
+MenuPidSettings::MenuPidSettings(PID *pid) : pid{pid} {}
-    this->pid = pid;
-}
 
-void MenuPidSettings::action(int16_t* values, uint8_t length) {
+void MenuPidSettings::action(int16_t *values, uint8_t length)
+{
     if (length != 3)
+    {
         return;
+    }
     this->pid->SetTunings(values[0], values[1], values[2]);
 }

src/SpecialMenus/PID/menuPidSettings.h

@@ -20,20 +20,20 @@
  * @brief A class to tune the PID settings
  *
  */
-class MenuPidSettings : public MenuIntInputWrapper {
+class MenuPidSettings : public MenuIntInputWrapper
-    public:
+{
+public:
     /**
      * @brief Construct a new Menu Pid Settings object
      *
      * @param pid
      */
-        MenuPidSettings(PID* pid);
+    MenuPidSettings(PID *pid);
 
-        void action(int16_t* values, uint8_t length);
+    void action(int16_t *values, uint8_t length) override;
-
-    private:
-        PID* pid;
 
+private:
+    PID *pid;
 };
 
 #endif // PID_SETTINGS_H

src/SpecialMenus/SensorData/menuSensorData.cpp

@@ -0,0 +1,152 @@
+/**
+ * @file menuSensorData.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief
+ * @version 0.1
+ * @date 2023-09-04
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+
+#include "menuSensorData.h"
+
+MenuSensorData::MenuSensorData(SensorData *sensorData) : MenuInformationSites(7), sensorData{sensorData}
+{
+    this->noEqualLeft = true;
+}
+
+void MenuSensorData::printPage() const
+{
+    const UBX_NAV_PVT_data_t *gpsData = this->sensorData->getGnssData();
+    uint8_t fixType = 0;
+    if (static_cast<bool>(gpsData))
+    {
+        fixType = gpsData->fixType;
+    }
+
+    String lineOne = "";
+    String lineTwo = "";
+
+    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 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:";
+        const Point pos = this->sensorData->getCurrentPos();
+        lineOne.concat(pos.getLatitude());
+        lineTwo.concat(pos.getLongitude());
+    }
+    break;
+
+    case 4:
+        lineOne = "Time: ";
+        lineTwo = "";
+        if (static_cast<bool>(fixType))
+        {
+            if (gpsData->hour < 10)
+            {
+                lineTwo.concat("0");
+            }
+            lineTwo.concat(gpsData->hour);
+            lineTwo.concat(":");
+            if (gpsData->min < 10)
+            {
+                lineTwo.concat("0");
+            }
+            lineTwo.concat(gpsData->min);
+            lineTwo.concat(":");
+            if (gpsData->sec < 10)
+            {
+                lineTwo.concat("0");
+            }
+            lineTwo.concat(gpsData->sec);
+        }
+        else
+        {
+            lineTwo = "00:00:00";
+        }
+
+        break;
+
+    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::pushData)
+        {
+            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))
+        {
+            lineOne.concat(gpsData->hAcc);
+            lineTwo.concat(gpsData->numSV);
+        }
+        else
+        {
+            lineOne.concat("-/-");
+            lineTwo.concat("-/-");
+        }
+        break;
+
+    default:
+        this->printDefault();
+        return;
+    }
+
+    this->print(lineOne, lineTwo);
+}

src/SpecialMenus/SensorData/menuSensorData.h

@@ -0,0 +1,37 @@
+/**
+ * @file menuSensorData.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains a MenuInformationClass to print the SensorData
+ * @version 0.1
+ * @date 2023-09-04
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+
+#ifndef MENU_SENSOR_DATA_H
+#define MENU_SENSOR_DATA_H
+
+#include "menuInformationSites.h"
+#include "sensorData.h"
+
+/**
+ * @brief A MenuInformationClass to print the SensorData
+ */
+class MenuSensorData : public MenuInformationSites
+{
+public:
+    /**
+     * @brief Construct a new MenuSensorData object
+     *
+     * @param SensorData
+     */
+    MenuSensorData(SensorData *SensorData);
+
+private:
+    void printPage() const override;
+
+    SensorData *sensorData;
+};
+
+#endif // MENU_SENSOR_DATA_H

src/SpecialMenus/Speed/menuSpeed.cpp

@@ -0,0 +1,22 @@
+/**
+ * @file menuSpeed.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief
+ * @version 0.1
+ * @date 2023-09-26
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+
+#include "menuSpeed.h"
+
+void MenuSpeed::action(int16_t *values, uint8_t length)
+{
+    if (length != 2)
+    {
+        return;
+    }
+    this->speeds.x = values[0] / 10.0;
+    this->speeds.rot = values[1] / 10.0;
+}

src/SpecialMenus/Speed/menuSpeed.h

@@ -0,0 +1,38 @@
+/**
+ * @file menuSpeed.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains a MenuIntInputWrapper to change speeds
+ * @version 0.1
+ * @date 2023-09-26
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+
+#ifndef MENU_SPEED_H
+#define MENU_SPEED_H
+
+#include <menuIntInput.h>
+#include "moveControl.h"
+
+/**
+ * @brief A MenuIntInputWrapper class to change speeds
+ */
+class MenuSpeed : public MenuIntInputWrapper
+{
+public:
+    MenuSpeed(DrivingSpeeds &speeds) : speeds(speeds){};
+
+    /**
+     * @brief Changes the speeds
+     *
+     * @param values
+     * @param length
+     */
+    void action(int16_t *values, uint8_t length) override;
+
+private:
+    DrivingSpeeds &speeds;
+};
+
+#endif // MENU_SPEED_H

src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp

@@ -11,22 +11,25 @@
 
 #include "menuSysteminformation.h"
 
-MenuSysteminformation::MenuSysteminformation(Battery* mainBattery) 
+MenuSysteminformation::MenuSysteminformation(Battery *mainBattery)
-            : MenuInformationSites(8) {
+    : MenuInformationSites(8), mainBattery{mainBattery}
-    this->mainBattery = mainBattery;
+{
+    this->noEqualLeft = true;
 }
 
-void MenuSysteminformation::printPage() const {
+void MenuSysteminformation::printPage() const
+{
     String lineOne = "";
     String lineTwo = "";
-    switch (this->getCurrentPage()) {
+    switch (this->getCurrentPage())
+    {
     case 0:
         lineOne = "Free Heap:";
         lineTwo.concat(ESP.getFreeHeap());
         break;
 
     case 1:
-            lineOne = "Max alloc heap:";
+        lineOne = "Max alloc Heap:";
         lineTwo.concat(ESP.getMaxAllocHeap());
         break;
 
@@ -71,4 +74,3 @@ void MenuSysteminformation::printPage() const {
 
     this->print(lineOne, lineTwo);
 }
-

src/SpecialMenus/Systeminformation/menuSysteminformation.h

@@ -26,20 +26,20 @@
  *
  * The informations are about the batteries, memory and uptime.
  */
-class MenuSysteminformation : public MenuInformationSites {
+class MenuSysteminformation : public MenuInformationSites
-    public:
+{
+public:
     /**
      * @brief Construct a new Menu Systeminformation object
      *
      * @param mainBattery
-         * @param gamepad 
      */
-        MenuSysteminformation(Battery* mainBattery);
+    MenuSysteminformation(Battery *mainBattery);
 
-    private:
+private:
     void printPage() const override;
 
-        Battery* mainBattery;
+    Battery *mainBattery;
 };
 
 #endif // MENU_SYSTEMINFORMATION_H

src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp

@@ -10,18 +10,21 @@
  */
 #include "menuCalibrateCompass.h"
 
-MenuCalibrateCompass::MenuCalibrateCompass(DriveManager* driveManager) : MenuDriveMode(driveManager) {}
+MenuCalibrateCompass::MenuCalibrateCompass(DriveManager *driveManager) : MenuDriveMode(driveManager) {}
 
-MenuCalibrateCompass::~MenuCalibrateCompass() {
+MenuCalibrateCompass::~MenuCalibrateCompass()
+{
     delete this->caliCompass;
     this->caliCompassMode->setCalibrateCompass();
 }
 
-void MenuCalibrateCompass::printPage() const {
+void MenuCalibrateCompass::printPage() const
+{
     String lineOne = "";
     String lineTwo = "";
 
-    switch (this->getCurrentPage()) {
+    switch (this->getCurrentPage())
+    {
     case 0:
         lineOne = "-Ready to drive-";
         lineTwo = "Compass Mode";
@@ -33,13 +36,14 @@ void MenuCalibrateCompass::printPage() const {
         break;
 
     case 2:
-            switch (this->caliCompass->getState()) {
+        switch (this->caliCompass->getState())
-                case CalibrateCompass::State::Ready :
+        {
+        case CalibrateCompass::State::Ready:
             lineOne = "Start compass";
             lineTwo = "calibration";
             break;
 
-                case CalibrateCompass::State::Calibrating :
+        case CalibrateCompass::State::Calibrating:
             lineOne = "Calibrating...";
             lineTwo = "Move around";
             break;
@@ -81,23 +85,23 @@ void MenuCalibrateCompass::printPage() const {
 
     case 8:
         lineOne = "X min: ";
-            lineOne.concat(this->caliCompass->getCallibrationData().data[0][0]);
+        lineOne.concat(this->caliCompass->getCalibrationData().data[0][0]);
         lineTwo = "X max: ";
-            lineTwo.concat(this->caliCompass->getCallibrationData().data[0][1]);
+        lineTwo.concat(this->caliCompass->getCalibrationData().data[0][1]);
         break;
 
     case 9:
         lineOne = "Y min: ";
-            lineOne.concat(this->caliCompass->getCallibrationData().data[1][0]);
+        lineOne.concat(this->caliCompass->getCalibrationData().data[1][0]);
         lineTwo = "Y max: ";
-            lineTwo.concat(this->caliCompass->getCallibrationData().data[1][1]);
+        lineTwo.concat(this->caliCompass->getCalibrationData().data[1][1]);
         break;
 
     case 10:
         lineOne = "Z min: ";
-            lineOne.concat(this->caliCompass->getCallibrationData().data[2][0]);
+        lineOne.concat(this->caliCompass->getCalibrationData().data[2][0]);
         lineTwo = "Z max: ";
-            lineTwo.concat(this->caliCompass->getCallibrationData().data[2][1]);
+        lineTwo.concat(this->caliCompass->getCalibrationData().data[2][1]);
         break;
 
     default:
@@ -108,20 +112,24 @@ void MenuCalibrateCompass::printPage() const {
     this->print(lineOne, lineTwo);
 }
 
-void MenuCalibrateCompass::init() {
+void MenuCalibrateCompass::init()
+{
     this->firstPrint = false;
-    this->driveManager->changeModus(Modi::CalibrateCompass);
+    this->caliCompassMode = new CalibrateCompassM();
-    this->caliCompassMode = (CalibrateCompassM*) this->driveManager->getDriveModiPtr();
+    this->driveManager->changeModus(this->caliCompassMode);
     this->caliCompass = new CalibrateCompass(this->caliCompassMode->getSensorData()->getRealCompass());
     this->setCountPages(11);
-    this->updateDelay = 500; 
+    MenuDriveMode::updateDelay = MenuCalibrateCompass::updateDelay;
 }
 
-void MenuCalibrateCompass::runCommand() const {
+void MenuCalibrateCompass::runCommand()
-    switch (this->getCurrentPage()) {
+{
+    switch (this->getCurrentPage())
+    {
     case 2:
-            switch (this->caliCompass->getState()) {
+        switch (this->caliCompass->getState())
-                case CalibrateCompass::State::Ready :
+        {
+        case CalibrateCompass::State::Ready:
             this->caliCompassMode->setCalibrateCompass(this->caliCompass);
             this->caliCompass->start();
             break;

src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h

@@ -20,14 +20,15 @@
  * @brief A class to print informations about the class ManualControl
  *
  */
-class MenuCalibrateCompass : public MenuDriveMode {
+class MenuCalibrateCompass : public MenuDriveMode
-    public:
+{
+public:
     /**
      * @brief Construct a new Menu Manual Control object
      *
      * @param driveManager
      */
-        MenuCalibrateCompass(DriveManager* driveManager);
+    MenuCalibrateCompass(DriveManager *driveManager);
     ~MenuCalibrateCompass();
 
     /**
@@ -40,13 +41,14 @@ class MenuCalibrateCompass : public MenuDriveMode {
      */
     void printPage() const override;
 
-    protected:
+private:
     void init() override;
-        void runCommand() const override;
+    void runCommand() override;
 
-    private:
+    CalibrateCompassM *caliCompassMode = nullptr;
-        CalibrateCompassM* caliCompassMode;
+    CalibrateCompass *caliCompass = nullptr;
-        CalibrateCompass* caliCompass;
+
+    static constexpr uint16_t updateDelay = 500;
 };
 
 #endif // MENU_CALIBRATE_COMPASS_H

src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp

@@ -10,18 +10,18 @@
  */
 #include "menuManualDrive.h"
 
-MenuManualControl::MenuManualControl(DriveManager* driveManager) : MenuDriveMode(driveManager) {
+MenuManualControl::MenuManualControl(DriveManager *driveManager) : MenuDriveMode(driveManager)
-
+{
+    this->noEqualLeft = true;
 }
 
-MenuManualControl::~MenuManualControl() {
+void MenuManualControl::printPage() const
-}
+{
-
-void MenuManualControl::printPage() const {
     String lineOne = "";
     String lineTwo = "";
 
-    switch (this->getCurrentPage()) {
+    switch (this->getCurrentPage())
+    {
     case 0:
         lineOne = "-Ready to drive-";
         break;
@@ -29,21 +29,21 @@ void MenuManualControl::printPage() const {
     case 1:
         lineOne = "Input mode:";
         if (this->manualControl->getInputMode() == ManualControl::InputMode::Analog)
+        {
             lineTwo = "Analog";
+        }
         else
+        {
             lineTwo = "Digital";
+        }
         break;
 
     case 2:
-            lineOne = "Speed: ";
+        lineOne = "Speed Menu";
-            lineOne.concat(this->manualControl->getMaxSpeed());
-            lineTwo = "- dec 0.1 inc +";
         break;
 
     case 3:
-            lineOne = "RotSpeed: ";
+        lineOne = "Sensor Menu";
-            lineOne.concat(this->manualControl->getMaxRotation());
-            lineTwo = "- dec 0.1 inc +";
         break;
 
     default:
@@ -54,42 +54,31 @@ void MenuManualControl::printPage() const {
     this->print(lineOne, lineTwo);
 }
 
-void MenuManualControl::init() {
+void MenuManualControl::init()
+{
     this->firstPrint = false;
-    this->driveManager->changeModus(Modi::ManualControl);
+    this->manualControl = new ManualControl();
-    this->manualControl = (ManualControl*) this->driveManager->getDriveModiPtr();
+    this->driveManager->changeModus(this->manualControl);
     this->setCountPages(4);
-    this->updateDelay = 500; 
+    MenuDriveMode::updateDelay = MenuManualControl::updateDelay;
+    this->activateSpeedMenu();
+    MenuDriveMode::init();
 }
 
-void MenuManualControl::runCommand() const {
+void MenuManualControl::runCommand()
-    switch (this->getCurrentPage()) {
+{
+    switch (this->getCurrentPage())
+    {
     case 1:
         this->manualControl->switchInputMode();
         break;
 
     case 2:
-            this->manualControl->increaseMaxSpeed();
+        this->enterSpeedMenu();
         break;
 
     case 3:
-            this->manualControl->increaseMaxRotation();
+        this->enterSensorMenu();
-            break;
-        
-        default:
-            break;
-    }
-}
-
-void MenuManualControl::runCommandNo() const {
-    switch (this->getCurrentPage())
-    {
-    case 2:
-        this->manualControl->decreaseMaxSpeed();
-        break;
-
-    case 3:
-        this->manualControl->decreaseMaxRotation();
         break;
 
     default:

src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.h

@@ -13,21 +13,21 @@
 #define MENU_MANUAL_DRIVE_H
 
 #include "SpecialMenus/driveModi/menuDriveMode.h"
-#include "calibrateCompass.h"
+#include "driveModi/Modi/ManualControl/manualControl.h"
 
 /**
  * @brief A class to print informations about the class ManualControl
  *
  */
-class MenuManualControl : public MenuDriveMode {
+class MenuManualControl : public MenuDriveMode
-    public:
+{
+public:
     /**
      * @brief Construct a new Menu Manual Control object
      *
      * @param driveManager
      */
-        MenuManualControl(DriveManager* driveManager);
+    MenuManualControl(DriveManager *driveManager);
-        ~MenuManualControl();
 
     /**
      * @brief Prints the Information to display and console
@@ -39,14 +39,14 @@ class MenuManualControl : public MenuDriveMode {
      */
     void printPage() const override;
 
-    protected:
+private:
     void init() override;
-        void runCommand() const override;
+    void runCommand() override;
-        void runCommandNo() const override;
 
-    private:
+    ManualControl *manualControl = nullptr;
-        ManualControl* manualControl;
+    CalibrateCompass *caliCompass = nullptr;
-        CalibrateCompass* caliCompass;
+
+    static constexpr uint16_t updateDelay = 500;
 };
 
 #endif // MENU_MANUAL_DRIVE_H

src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp

@@ -10,18 +10,23 @@
  */
 #include "menuTestMode.h"
 
-MenuTestMode::MenuTestMode(DriveManager* driveManager) {
+MenuTestMode::MenuTestMode(DriveManager *driveManager)
-    this->driveManager = driveManager;    
+    : driveManager{driveManager}
+{
 }
 
-MenuTestMode::~MenuTestMode() {
+MenuTestMode::~MenuTestMode()
-    if (isInit) {
+{
+    if (isInit)
+    {
         delete this->mainMenu;
     }
 }
 
-void MenuTestMode::printMenu() {
+void MenuTestMode::printMenu()
-    if (!this->isInit) {
+{
+    if (!this->isInit)
+    {
         this->isInit = true;
         this->init();
     }
@@ -29,76 +34,98 @@ void MenuTestMode::printMenu() {
     this->mainMenu->printMenu();
 }
 
-void MenuTestMode::down() {
+void MenuTestMode::down()
+{
     if (this->checkInput())
+    {
         this->mainMenu->down();
+    }
 }
 
-void MenuTestMode::up() {
+void MenuTestMode::up()
+{
     if (this->checkInput())
+    {
         this->mainMenu->up();
+    }
 }
 
-void MenuTestMode::right() {
+void MenuTestMode::right()
+{
     if (this->checkInput())
+    {
         this->mainMenu->right();
+    }
 }
 
-void MenuTestMode::left() {
+void MenuTestMode::left()
+{
     if (this->mainMenu->isInSubmenu())
         if (this->checkInput())
+        {
             this->mainMenu->left();
-        else {
+        }
+        else
+        {
             this->testMode->abortManeuver();
             this->mainMenu->left();
         }
     else
+    {
         this->parentMenu->printMenu();
+    }
 }
 
-void MenuTestMode::yes() {
+void MenuTestMode::yes()
+{
     if (this->checkInput())
+    {
         this->mainMenu->yes();
+    }
 }
 
-void MenuTestMode::no() {
+void MenuTestMode::no()
+{
     if (this->mainMenu->isInSubmenu())
+    {
         if (this->checkInput())
             this->mainMenu->no();
-        else {
+        else
+        {
             this->testMode->abortManeuver();
             this->mainMenu->no();
         }
+    }
     else
+    {
         this->left();
+    }
 }
 
-void MenuTestMode::init() {
+void MenuTestMode::init()
-    this->driveManager->changeModus(Modi::TestMode);
+{
-    this->testMode = (TestMode*) this->driveManager->getDriveModiPtr();
+    this->testMode = new TestMode();
+    this->driveManager->changeModus(this->testMode);
 
-    testMode->setMaxSpeed(1);
+    auto dummy = []()
-    testMode->setMaxRotation(8);
+    {
-
+        std::cout << "Dummy in Action" << std::endl;
-    auto dummy = []() {
-        std::cout << "Dummy in Action" <<std::endl;
     };
 
     // Create menu
     this->mainMenu = new Menu;
-    Menu* engineMenu = new Menu;
+    auto *engineMenu = new Menu;
-    Menu* lightMenu = new Menu;
+    auto *lightMenu = new Menu;
-    Menu* encoderMenu = new Menu;
+    auto *encoderMenu = new Menu;
     // Sound
     // Ping google
 
-    MenuIntInput* drivingMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::drive));
+    auto *drivingMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::drive));
-    MenuIntInput* engineLeftMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::leftEngine));
+    auto *engineLeftMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::leftEngine));
-    MenuIntInput* engineRightMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::rightEngine));
+    auto *engineRightMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::rightEngine));
-    MenuIntInput* engineBothMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::bothEngine));
+    auto *engineBothMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::bothEngine));
-    SpeedometerTest* encoderLeftMenu = new SpeedometerTest(this->testMode->getSpeedometerLeft());
+    auto *encoderLeftMenu = new SpeedometerTest(this->testMode->getSpeedometerLeft());
-    SpeedometerTest* encoderRightMenu = new SpeedometerTest(this->testMode->getSpeedometerRight());
+    auto *encoderRightMenu = new SpeedometerTest(this->testMode->getSpeedometerRight());
-
 
     // Set menus on LCD
     this->mainMenu->setLcd(this->lcd);
@@ -113,49 +140,53 @@ void MenuTestMode::init() {
     encoderRightMenu->setLcd(this->lcd);
 
     // Other menu config
+    static constexpr uint8_t percentMax = 100;
+    static constexpr uint16_t degreeMax = 360;
+    static constexpr uint8_t secondsMax = 120;
+    static constexpr uint8_t steps = 5;
+
     drivingMenu->setEntry(0, "Centimeter", 100);
-    drivingMenu->setMinMaxSteps(0, -1000, 1000, 25);
+    drivingMenu->setMinMaxSteps(0, -1000, 1000, steps * 5);
     drivingMenu->setEntry(1, "Degree");
-    drivingMenu->setMinMaxSteps(1, -360, 360, 15);
+    drivingMenu->setMinMaxSteps(1, -degreeMax, degreeMax, steps * 3);
     drivingMenu->setPrintParentMenu(false);
     engineLeftMenu->setEntry(0, "Percentage");
-    engineLeftMenu->setMinMaxSteps(0, -100, 100, 5);
+    engineLeftMenu->setMinMaxSteps(0, -percentMax, percentMax, steps);
-    engineLeftMenu->setEntry(1, "Seconds", 5);
+    engineLeftMenu->setEntry(1, "Seconds", steps);
-    engineLeftMenu->setMinMaxSteps(1, 0, 120, 1);
+    engineLeftMenu->setMinMaxSteps(1, 0, secondsMax, 1);
     engineLeftMenu->setPrintParentMenu(false);
     engineRightMenu->setEntry(0, "Percentage");
-    engineRightMenu->setMinMaxSteps(0, -100, 100, 5);
+    engineRightMenu->setMinMaxSteps(0, -percentMax, percentMax, steps);
-    engineRightMenu->setEntry(1, "Seconds", 5);
+    engineRightMenu->setEntry(1, "Seconds", steps);
-    engineRightMenu->setMinMaxSteps(1, 0, 120, 1);
+    engineRightMenu->setMinMaxSteps(1, 0, secondsMax, 1);
     engineRightMenu->setPrintParentMenu(false);
     engineBothMenu->setEntry(0, "Percentage");
-    engineBothMenu->setMinMaxSteps(0, -100, 100, 5);
+    engineBothMenu->setMinMaxSteps(0, -percentMax, percentMax, steps);
-    engineBothMenu->setEntry(1, "Seconds", 5);
+    engineBothMenu->setEntry(1, "Seconds", steps);
-    engineBothMenu->setMinMaxSteps(1, 0, 120, 1);
+    engineBothMenu->setMinMaxSteps(1, 0, secondsMax, 1);
     engineBothMenu->setPrintParentMenu(false);
 
-
     // Entrys for the menus
-    MenuAction* engineAction = new MenuAction("Engine", engineMenu);
+    auto *engineAction = new MenuAction("Engine", engineMenu);
-    MenuAction* engineLeftAction = new MenuAction("Left", engineLeftMenu);
+    auto *engineLeftAction = new MenuAction("Left", engineLeftMenu);
-    MenuAction* engineRightAction = new MenuAction("Right", engineRightMenu);
+    auto *engineRightAction = new MenuAction("Right", engineRightMenu);
-    MenuAction* engineBothAction = new MenuAction("Both", engineBothMenu);
+    auto *engineBothAction = new MenuAction("Both", engineBothMenu);
 
-    MenuAction* drivingAction = new MenuAction("Driving", drivingMenu);
+    auto *drivingAction = new MenuAction("Driving", drivingMenu);
 
-    MenuAction* encoderAction = new MenuAction("Encoder", encoderMenu);
+    auto *encoderAction = new MenuAction("Encoder", encoderMenu);
-    MenuAction* encoderLeftAction = new MenuAction("Left", encoderLeftMenu);
+    auto *encoderLeftAction = new MenuAction("Left", encoderLeftMenu);
-    MenuAction* encoderRightAction = new MenuAction("Right", encoderRightMenu);
+    auto *encoderRightAction = new MenuAction("Right", encoderRightMenu);
 
-    MenuAction* lightAction = new MenuAction("Light", lightMenu);
+    auto *lightAction = new MenuAction("Light", lightMenu);
-    MenuAction* lightFlashAction = new MenuAction("Flash", dummy);
+    auto *lightFlashAction = new MenuAction("Flash", dummy);
-    MenuAction* lightFadeAction = new MenuAction("Fade", dummy);
+    auto *lightFadeAction = new MenuAction("Fade", dummy);
-    MenuAction* lightRedAction = new MenuAction("Red", dummy);
+    auto *lightRedAction = new MenuAction("Red", dummy);
-    MenuAction* lightGreenAction = new MenuAction("Green", dummy);
+    auto *lightGreenAction = new MenuAction("Green", dummy);
-    MenuAction* lightBlueAction = new MenuAction("Blue", dummy);
+    auto *lightBlueAction = new MenuAction("Blue", dummy);
-    MenuAction* lightOffAction = new MenuAction("Off", dummy);    
+    auto *lightOffAction = new MenuAction("Off", dummy);
 
-    // Add entrys to the menus
+    // Add entries to the menus
     this->mainMenu->addEntry(engineAction);
     this->mainMenu->addEntry(drivingAction);
     this->mainMenu->addEntry(encoderAction);
@@ -176,65 +207,85 @@ void MenuTestMode::init() {
     lightMenu->addEntry(lightOffAction);
 }
 
-void MenuTestMode::update() {
+void MenuTestMode::update()
+{
     if (this->maneuverInAction)
-        if (millis() - this->lastUpdateTime > updateDelay) {
+    {
+        if (millis() - this->lastUpdateTime > updateDelay)
+        {
             this->printManeuverTime();
             this->lastUpdateTime = millis();
         }
+    }
 }
 
-bool MenuTestMode::checkInput() {
+bool MenuTestMode::checkInput()
-    if (this->testMode->getBusy()) {
+{
+    if (this->testMode->getBusy())
+    {
         this->printManeuverTime();
         this->maneuverInAction = true;
         return false;
     }
 
     if (this->maneuverInAction)
+    {
         this->maneuverInAction = false;
+    }
 
     return true;
 }
 
-void MenuTestMode::printManeuverTime() {
+void MenuTestMode::printManeuverTime()
+{
     uint8_t maneuverTime = this->testMode->getRemainingManeuverTime();
-    if (maneuverTime) {
+    if (maneuverTime)
+    {
         String lineTwo = "";
         lineTwo.concat(maneuverTime);
         lineTwo.concat(" secs");
         this->print("Maneuver time:", lineTwo);
-    } else {
+    }
+    else
+    {
         this->maneuverInAction = false;
         this->printMenu();
     }
 }
 
-MenuTestModeWrapper::MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionSingle testModeFunction) {
+MenuTestModeWrapper::MenuTestModeWrapper(MenuTestMode *menu, TestModeFunctionSingle testModeFunction)
-    this->menu = menu;
+    : menu{menu},
-    this->testMode = this->menu->getTestMode();
+      testModeFunctionSingle{testModeFunction},
-    this->testModeFunctionSingle = testModeFunction;
+      testMode{menu->getTestMode()}
+{
 }
 
-MenuTestModeWrapper::MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionDouble testModeFunction) {
+MenuTestModeWrapper::MenuTestModeWrapper(MenuTestMode *menu, TestModeFunctionDouble testModeFunction)
-    this->menu = menu;
+    : menu{menu},
-    this->testMode = this->menu->getTestMode();
+      testModeFunctionDouble{testModeFunction},
-    this->testModeFunctionDouble = testModeFunction;
+      testMode{menu->getTestMode()}
+{
 }
 
-void MenuTestModeWrapper::action(int16_t* values, uint8_t length) {
+void MenuTestModeWrapper::action(int16_t *values, uint8_t length)
+{
     // This function calls a member function with a pointer
     // Very helpful site:
     // https://isocpp.org/wiki/faq/pointers-to-members#fnptr-vs-memfnptr-types
 
     bool res = false;
 
-    if (length == 2 && this->testModeFunctionDouble)
+    if (length == 2 && static_cast<bool>(this->testModeFunctionDouble))
+    {
         res = (this->testMode->*this->testModeFunctionDouble)(values[0], values[1]);
-    else if (length == 1 && this->testModeFunctionSingle)
+    }
+    else if (length == 1 && static_cast<bool>(this->testModeFunctionSingle))
+    {
         res = (this->testMode->*this->testModeFunctionSingle)(values[0]);
+    }
 
-    if (res) {
+    if (res)
+    {
         this->menu->printManeuverTime();
         this->menu->maneuverStarted();
     }

src/SpecialMenus/driveModi/TestMode/menuTestMode.h

@@ -22,8 +22,9 @@
  * @brief Interact with the TestMode
  *
  */
-class MenuTestMode : public MenuControl {
+class MenuTestMode : public MenuControl
-    public:
+{
+public:
     /**
      * @brief Construct a new Menu Test Mode object
      *
@@ -62,16 +63,16 @@ class MenuTestMode : public MenuControl {
      */
     void update() override;
 
-        TestMode* getTestMode() const { return this->testMode; }
+    TestMode *getTestMode() const { return this->testMode; }
     void maneuverStarted() { this->maneuverInAction = true; }
 
-    private:
+private:
     void init();
     bool checkInput();
 
-        DriveManager* driveManager;
+    DriveManager *driveManager;
-        TestMode* testMode;
+    TestMode *testMode = nullptr;
-        Menu* mainMenu;
+    Menu *mainMenu = nullptr;
 
     bool isInit = false;
     bool maneuverInAction = false;
@@ -82,16 +83,17 @@ class MenuTestMode : public MenuControl {
 
 typedef bool (TestMode::*TestModeFunctionSingle)(int16_t);
 typedef bool (TestMode::*TestModeFunctionDouble)(int16_t, int16_t);
-class MenuTestModeWrapper : public MenuIntInputWrapper {
+class MenuTestModeWrapper : public MenuIntInputWrapper
-    public:
+{
-        MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionSingle testModeFunction);
+public:
-        MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionDouble testModeFunction);
+    MenuTestModeWrapper(MenuTestMode *menu, TestModeFunctionSingle testModeFunction);
+    MenuTestModeWrapper(MenuTestMode *menu, TestModeFunctionDouble testModeFunction);
 
-        void action(int16_t* values, uint8_t length) override;
+    void action(int16_t *values, uint8_t length) override;
 
-    private:
+private:
-        MenuTestMode* menu;
+    MenuTestMode *menu;
-        TestMode* testMode;
+    TestMode *testMode;
     TestModeFunctionSingle testModeFunctionSingle = nullptr;
     TestModeFunctionDouble testModeFunctionDouble = nullptr;
 };

src/SpecialMenus/driveModi/TestMode/speedometerTest.cpp

@@ -1,5 +1,5 @@
 /**
- * @file menuTest.cpp
+ * @file speedometerTest.cpp
  * @author Alexander Klein (alex@kleiax.de)
  * @brief Contains the implementation of the class SpeedometerTest.
  * @version 0.1
@@ -11,23 +11,26 @@
 
 #include "speedometerTest.h"
 
-SpeedometerTest::SpeedometerTest(Speedometer* speedometer) {
+SpeedometerTest::SpeedometerTest(Speedometer *speedometer)
-    this->speedometer = speedometer;
+    : speedometer{speedometer}
+{
     this->speedometer->setDirection(Speedometer::Forward);
 }
 
-
+void SpeedometerTest::printMenu()
-void SpeedometerTest::printMenu() {
+{
-    switch (this->state) {
+    switch (this->state)
-        case State::Off :
+    {
+    case State::Off:
         this->print("Press Yes (O)", "to start");
         break;
 
-        case State::Running :
+    case State::Running:
         this->print("Running, Yes (O)", "to stop");
         break;
 
-        case State::Finished : {
+    case State::Finished:
+    {
         String res = "";
         res.concat(this->result);
         this->print("Result: ", res);
@@ -40,34 +43,39 @@ void SpeedometerTest::printMenu() {
     }
 }
 
-
+void SpeedometerTest::left()
-void SpeedometerTest::left() {
+{
     this->no();
 }
 
-void SpeedometerTest::no() {
+void SpeedometerTest::no()
+{
     if (this->state == State::Running)
+    {
         speedometer->calibrationMeasurementStop();
+    }
 
     this->state = State::Off;
     this->parentMenu->printMenu();
 }
 
-void SpeedometerTest::yes() {
+void SpeedometerTest::yes()
-    switch (this->state) {
+{
-        case State::Off :
+    switch (this->state)
+    {
+    case State::Off:
         this->state = State::Running;
         this->speedometer->calibrationMeasurementStart();
         this->printMenu();
         break;
 
-        case State::Running :
+    case State::Running:
         this->state = State::Finished;
         this->result = this->speedometer->calibrationMeasurementStop();
         this->printMenu();
         break;
 
-        case State::Finished :
+    case State::Finished:
         this->state = State::Running;
         this->speedometer->calibrationMeasurementStart();
         this->printMenu();

src/SpecialMenus/driveModi/TestMode/speedometerTest.h

@@ -1,5 +1,5 @@
 /**
- * @file menuTest.h
+ * @file speedometerTest.h
  * @author Alexander Klein (alex@kleiax.de)
  * @brief Contains a class to test the wheel encoder.
  * @version 0.1
@@ -18,12 +18,14 @@
  * @brief Tests the wheel encoder.
  *
  */
-class SpeedometerTest : public MenuControl {
+class SpeedometerTest : public MenuControl
-    public:
+{
+public:
     /**
      * @brief The states of the test.
      */
-        enum State {
+    enum State
+    {
         Off,
         Running,
         Finished
@@ -34,7 +36,7 @@ class SpeedometerTest : public MenuControl {
      *
      * @param speedometer
      */
-        SpeedometerTest(Speedometer* speedometer);
+    SpeedometerTest(Speedometer *speedometer);
 
     /**
      * @brief Prints the actual state of the test.
@@ -56,8 +58,8 @@ class SpeedometerTest : public MenuControl {
      */
     void yes() override;
 
-    private:
+private:
-        Speedometer* speedometer;
+    Speedometer *speedometer;
     State state = State::Off;
 
     uint16_t result = 0;

src/SpecialMenus/driveModi/menuDriveMode.cpp

@@ -11,20 +11,161 @@
 
 #include "menuDriveMode.h"
 
-MenuDriveMode::MenuDriveMode(DriveManager* driveManager) {
+MenuDriveMode::MenuDriveMode(DriveManager *driveManager)
-    this->driveManager = driveManager;
+    : driveManager{driveManager}
+{
 }
 
-void MenuDriveMode::left() {
+MenuDriveMode::~MenuDriveMode()
-    this->firstPrint = true;
+{
-    this->configureOnLeave();
+    if (this->selfCreatedMenuSpeed)
-    this->driveManager->changeModus(Modi::Off);
+    {
-    if (parentMenu) {
+        delete this->menuSpeed;
-        this->parentMenu->printMenu();
-        this->leaved = true;
     }
 }
 
-void MenuDriveMode::no() {
+void MenuDriveMode::addSpeedMenu(MenuIntInput *menuSpeed)
-    this->left();
+{
+    if (this->selfCreatedMenuSpeed)
+    {
+        delete this->menuSpeed;
+        this->selfCreatedMenuSpeed = false;
+    }
+
+    this->menuSpeed = menuSpeed;
+}
+
+void MenuDriveMode::left()
+{
+    if (static_cast<bool>(this->activeMenu))
+    {
+        if (static_cast<bool>(this->activeMenu))
+        {
+            this->activeMenu->left();
+        }
+        return;
+    }
+
+    this->firstPrint = true;
+    this->configureOnLeave();
+    delete this->menuSpeed;
+    this->driveManager->changeModus();
+    MenuInformationSites::left();
+}
+
+void MenuDriveMode::right()
+{
+    if (static_cast<bool>(this->activeMenu))
+    {
+        this->activeMenu->right();
+        return;
+    }
+    MenuInformationSites::right();
+}
+
+void MenuDriveMode::up()
+{
+    if (static_cast<bool>(this->activeMenu))
+    {
+        this->activeMenu->up();
+        return;
+    }
+    MenuInformationSites::up();
+}
+
+void MenuDriveMode::down()
+{
+    if (static_cast<bool>(this->activeMenu))
+    {
+        this->activeMenu->down();
+        return;
+    }
+    MenuInformationSites::down();
+}
+
+void MenuDriveMode::yes()
+{
+    if (static_cast<bool>(this->activeMenu))
+    {
+        this->activeMenu->yes();
+        return;
+    }
+    MenuInformationSites::yes();
+}
+
+void MenuDriveMode::no()
+{
+    if (static_cast<bool>(this->activeMenu))
+    {
+        this->activeMenu->no();
+        return;
+    }
+    MenuInformationSites::no();
+}
+
+void MenuDriveMode::prepareReenterMenu()
+{
+    this->activeMenu = nullptr;
+}
+
+void MenuDriveMode::printMenu()
+{
+    if (static_cast<bool>(this->activeMenu))
+    {
+        this->activeMenu->printMenu();
+        return;
+    }
+
+    MenuInformationSites::printMenu();
+}
+
+void MenuDriveMode::init()
+{
+    this->activeMenu = nullptr;
+}
+
+void MenuDriveMode::activateSpeedMenu()
+{
+    if (!this->driveManager->isActive())
+    {
+        return;
+    }
+
+    if (this->selfCreatedMenuSpeed)
+    {
+        delete this->menuSpeed;
+    }
+
+    DrivingSpeeds &speeds = this->driveManager->getDriveModiPtr()->getSpeedsRef();
+    auto *menu = new MenuIntInput(2, new MenuSpeed(speeds));
+
+    menu->setMinMax(5, UINT8_MAX);
+    menu->setEntry(0, "x m/s e-1", driveManager->getDrivingSpeedsRef().x * 10);
+    menu->setEntry(1, "z rad/s e-1", driveManager->getDrivingSpeedsRef().rot * 10);
+
+    this->selfCreatedMenuSpeed = true;
+    this->addSpeedMenu(menu);
+}
+
+void MenuDriveMode::enterSpeedMenu()
+{
+    this->activeMenu = this->menuSpeed;
+    this->enterMenu();
+}
+
+void MenuDriveMode::enterSensorMenu()
+{
+    this->activeMenu = this->menuSensor;
+    this->enterMenu();
+}
+
+void MenuDriveMode::enterMenu()
+{
+    if (!static_cast<bool>(this->activeMenu))
+    {
+        return;
+    }
+
+    this->activeMenu->printMenu();
+    this->activeMenu->setParentMenu(this);
 }

src/SpecialMenus/driveModi/menuDriveMode.h

@@ -13,35 +13,85 @@
 
 #include "menuInformationSites.h"
 #include "driveModi/driveManager.h"
+#include "SpecialMenus/SensorData/menuSensorData.h"
+#include "SpecialMenus/Speed/menuSpeed.h"
 
 /**
  * @brief A base class for Menus about DriveModi
  *
  */
-class MenuDriveMode : public MenuInformationSites {
+class MenuDriveMode : public MenuInformationSites
-    public:
+{
+public:
     /**
      * @brief Construct a new Menu Drive Mode object
      *
      * @param driveManager
      */
-        MenuDriveMode(DriveManager* driveManager);
+    MenuDriveMode(DriveManager *driveManager);
+
+    ~MenuDriveMode();
+
+    void addSensorMenu(MenuSensorData *menuSensor) { this->menuSensor = menuSensor; }
+    void addSpeedMenu(MenuIntInput *menuSpeed);
 
     /**
      * @brief Goes back to the parentMenu.
      */
     void left() override;
-
+    void right() override;
-        /**
+    void up() override;
-         * @brief Calls left.
+    void down() override;
-         */
+    void yes() override;
     void no() override;
 
-    protected:
+    void prepareReenterMenu() override;
-        void configureOnLeave() {}
+    void printMenu() override;
+
+protected:
+    void init() override;
+
+    /**
+     * @brief Prepare leaving the menu
+     *
+     * Can be overwritten to add extra functionality to safely
+     * exit the menu. This function is called before the parentMenu
+     * is printed.
+     */
+    virtual void configureOnLeave() {}
+
+    /**
+     * @brief Creates an own SpeedMenu
+     *
+     * This function uses addSpeedMenu() at the end to that the new menu
+     */
+    void activateSpeedMenu();
+
+    /**
+     * @brief Enter a Menu to edit the speeds
+     *
+     * To enter this menu it has to be set by addSpeedMenu() or activateSpeedMenu()
+     */
+    void enterSpeedMenu();
+
+    /**
+     * @brief Enter a Menu with the SensorData
+     *
+     * To enter this menu it has to be set by addSensorMenu()
+     */
+    void enterSensorMenu();
+
+    DriveManager *driveManager;
 
-        DriveManager* driveManager;
     bool firstPrint = true;
-};
 
+private:
+    void enterMenu();
+
+    MenuControl *activeMenu = nullptr;
+    MenuSensorData *menuSensor = nullptr;
+    MenuIntInput *menuSpeed = nullptr;
+
+    bool selfCreatedMenuSpeed = false;
+};
 #endif // MENU_DRIVE_MODI_H

src/driveModi/Modi/CalibrateCompass/calibrateCompassM.cpp

@@ -11,13 +11,15 @@
 
 #include "calibrateCompassM.h"
 
-CalibrateCompassM::CalibrateCompassM(DriveModiParams params) : ManualControl(params) {}
+void CalibrateCompassM::setCalibrateCompass(CalibrateCompass *caliCompass)
-
+{
-void CalibrateCompassM::setCalibrateCompass(CalibrateCompass *caliCompass) {
+    if (static_cast<bool>(caliCompass))
-    if (caliCompass) {
+    {
         this->caliCompass = caliCompass;
         this->addChildComponent(this->caliCompass);
-    } else if (this->caliCompass) {
+    }
+    else if (static_cast<bool>(this->caliCompass))
+    {
         this->removeChildComponent(this->caliCompass);
         this->caliCompass = caliCompass;
     }

src/driveModi/Modi/CalibrateCompass/calibrateCompassM.h

@@ -1,7 +1,7 @@
 /**
  * @file calibrateCompassM.h
  * @author Alexander Klein (alex@kleiax.de)
- * @brief 
+ * @brief Contains a Menu class to calibrate the compass
  * @version 0.1
  * @date 2023-09-03
  *
@@ -16,14 +16,21 @@
 
 #include "driveModi/Modi/ManualControl/manualControl.h"
 
-class CalibrateCompassM : public ManualControl {
+/**
-    public:
+ * @brief A Menu class to interact with CalibrateCompass class
-        CalibrateCompassM(DriveModiParams params);
+ */
+class CalibrateCompassM : public ManualControl
+{
+public:
+    /**
+     * @brief Set the CalibrateCompass object
+     *
+     * @param caliCompass
+     */
+    void setCalibrateCompass(CalibrateCompass *caliCompass = nullptr);
 
-        void setCalibrateCompass(CalibrateCompass* caliCompass = nullptr);
+private:
-
+    CalibrateCompass *caliCompass = nullptr;
-    private:
-        CalibrateCompass* caliCompass = nullptr;
 };
 
-#endif //CALIBRATE_COMPASS_M_H
+#endif // CALIBRATE_COMPASS_M_H

src/driveModi/Modi/ManualControl/manualControl.cpp

@@ -10,13 +10,15 @@
  */
 #include "manualControl.h"
 
-void ManualControl::run() {    
+void ManualControl::run()
-    switch (this->inputMode) {
+{
-        case InputMode::Analog :
+    switch (this->inputMode)
+    {
+    case InputMode::Analog:
         this->analogControl();
         break;
 
-        case InputMode::Digital :
+    case InputMode::Digital:
         this->digitalControl();
         break;
 
@@ -25,52 +27,66 @@ void ManualControl::run() {
     }
 }
 
-void ManualControl::switchInputMode() {
+void ManualControl::switchInputMode()
+{
     this->inputMode = (this->inputMode == InputMode::Analog) ? InputMode::Digital : InputMode::Analog;
 }
 
-void ManualControl::analogControl() {
+void ManualControl::analogControl()
+{
     // Have to be int16_t to avoid overflow (int8_t = 255 - 127 = -128)
-    int16_t x = this->input->x - 127;
+    const int16_t xAxis = this->input->x - 127;
-    int16_t y = this-> input->y - 127;
+    const int16_t yAxis = this->input->y - 127;
 
-    // static int counter = 0;
+    double value_per_step = this->maxSpeeds.x * 2 / UINT8_MAX;
-    // if (counter % 60 == 0) {
+    this->moveControl->setSpeed(-xAxis * value_per_step);
-    //     std::cout << "Input: x: " << (int) this->input->x << " y: " << (int) this->input->y << std::endl;
-    //     std::cout << "Output: x: " << (int) x << " y: " << (int) y << std::endl;
-    // }
-    // counter++;
 
-    double value_per_step = this->maxForwardSpeed * 2 / UINT8_MAX;
+    value_per_step = this->maxSpeeds.rot * 2 / UINT8_MAX;
-    this->moveControl->setSpeed(-x * value_per_step);
+    this->moveControl->setRotationSpeed(yAxis * value_per_step);
-
-    value_per_step = this->maxRotationSpeed * 2 / UINT8_MAX;
-    this->moveControl->setRotationSpeed(y * value_per_step);
 }
 
-void ManualControl::digitalControl() {
+void ManualControl::digitalControl()
-    int16_t y = this->input->x - 127;
+{
-    int16_t x = this->input->y - 127;
+    static constexpr uint8_t deadzone = 120;
+    const int16_t yAxis = this->input->x - 127;
+    const int16_t xAxis = this->input->y - 127;
 
-    if (y > 120)
+    if (yAxis > deadzone)
-        this->moveControl->setSpeed(-this->maxForwardSpeed);
+    {
-    else if (y < -120)
+        this->moveControl->setSpeed(-this->maxSpeeds.x);
-        this->moveControl->setSpeed(this->maxForwardSpeed);
+    }
+    else if (yAxis < -deadzone)
+    {
+        this->moveControl->setSpeed(this->maxSpeeds.rot);
+    }
     else
+    {
         this->moveControl->setSpeed(0);
+    }
 
-    if (x > 120)
+    if (xAxis > deadzone)
-        this->moveControl->setRotationSpeed(this->maxRotationSpeed);
+    {
-    else if (x < -120)
+        this->moveControl->setRotationSpeed(this->maxSpeeds.rot);
-        this->moveControl->setRotationSpeed(-this->maxRotationSpeed);
+    }
+    else if (xAxis < -deadzone)
+    {
+        this->moveControl->setRotationSpeed(-this->maxSpeeds.rot);
+    }
     else
+    {
         this->moveControl->setRotationSpeed(0);
+    }
 
-    if (this->directionChangeWrapper && (x > 120 || x < -120))
+    if (static_cast<bool>(this->directionChangeWrapper) && (xAxis > deadzone || xAxis < -deadzone))
+    {
         this->lastLoopTurned = true;
-    else if (this->lastLoopTurned) {
+    }
-        if (this->directionChangeWrapper)
+    else if (this->lastLoopTurned)
+    {
+        if (static_cast<bool>(this->directionChangeWrapper))
+        {
             this->directionChangeWrapper->action();
+        }
         this->lastLoopTurned = false;
     }
 }

src/driveModi/Modi/ManualControl/manualControl.h

@@ -13,45 +13,60 @@
 
 #include "driveModi/driveModi.h"
 
-class DirectionChangeWrapper {
+class DirectionChangeWrapper
-    public:
+{
+public:
     virtual void action() = 0;
 };
 
 /**
  * @brief Drive the Rover with a Joystick
  *
- * This class gets the x and y value from the PS3 controller
+ * This class gets the x and y value from the ControlPad
  * and map the values to moveControl
  *
  * @see MoveControl
  */
-class ManualControl : public DriveModi {
+class ManualControl : public DriveModi
-    public:
+{
-        enum class InputMode : uint8_t {
+public:
+    /**
+     * @brief The enum is used to change the interpretation of the joystick data
+     */
+    enum class InputMode : uint8_t
+    {
         Analog,
         Digital
     };
 
-        ManualControl(DriveModiParams params) : DriveModi(params){};
+    /**
-
+     * @brief Change the InputMode to the opposite
+     */
     void switchInputMode();
     void setInputMode(InputMode mode) { this->inputMode = mode; }
     InputMode getInputMode() const { return this->inputMode; }
-        void setDirectionChangeCallback(DirectionChangeWrapper* callback) { this->directionChangeWrapper = callback; }
+
+    /**
+     * @brief Set the DirectionChangeWrapper object
+     *
+     * This callback is used to inform the CalcAzimuth Component about a direction change
+     *
+     * @param callback
+     */
+    void setDirectionChangeCallback(DirectionChangeWrapper *callback) { this->directionChangeWrapper = callback; }
 
     uint16_t getDutycycleLeft() const { return this->moveControl->getDutycycleLeft(); }
     uint16_t getDutycycleRight() const { return this->moveControl->getDutycycleRight(); }
 
-    protected:
+protected:
     void run() override;
 
-    private:
+private:
     void analogControl();
     void digitalControl();
 
     bool lastLoopTurned = false;
-        DirectionChangeWrapper* directionChangeWrapper = nullptr;
+    DirectionChangeWrapper *directionChangeWrapper = nullptr;
     InputMode inputMode = InputMode::Analog;
 };
 

src/driveModi/Modi/TestMode/testMode.cpp

@@ -10,21 +10,19 @@
  */
 #include "testMode.h"
 
-TestMode::TestMode(DriveModiParams params) 
+void TestMode::run()
-        : DriveModi(params) {}
+{
-
+    if (this->maneuver == Maneuver::Turn)
-TestMode::~TestMode() {
+    {
-    this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
-}
-
-void TestMode::run() {
-    if (this->maneuver == Maneuver::Turn) {
         uint16_t delta = abs(this->azimuth - this->getSensorData()->getRealAzimuth());
         if (delta > this->degree)
+        {
             this->abort = true;
         }
+    }
 
-    if (millis() - this->actionStart > this->maneuverTime || this->abort) {
+    if (this->busy && millis() - this->actionStart > this->maneuverTime || this->abort)
+    {
         this->busy = false;
         this->abort = false;
         this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
@@ -32,21 +30,29 @@ void TestMode::run() {
     }
 }
 
-bool TestMode::drive(int16_t cm, int16_t degree) {
+bool TestMode::drive(int16_t cmDistance, int16_t degree)
+{
     if (this->busy)
+    {
         return false;
+    }
 
     this->actionStart = millis();
     this->moveControl->setDrivingStatus(MoveControl::Status::Drive);
 
-    if (cm == 0) {
+    if (cmDistance == 0)
-        //Only left or right
+    {
+        // Only left or right
         this->moveControl->setSpeed(0);
 
         if (degree < 0)
-            this->moveControl->setRotationSpeed(-this->maxRotationSpeed);
+        {
+            this->moveControl->setRotationSpeed(-this->maxSpeeds.rot);
+        }
         else if (degree > 0)
-            this->moveControl->setRotationSpeed(this->maxRotationSpeed);
+        {
+            this->moveControl->setRotationSpeed(this->maxSpeeds.rot);
+        }
 
         this->azimuth = this->getSensorData()->getRealAzimuth();
         this->degree = degree;
@@ -54,29 +60,40 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
         this->maneuverTime = 5 * 1000;
         this->busy = true;
         return true;
-
+    }
-    } else if (degree == 0) {
+    if (degree == 0)
-        //Only forward or backward
+    {
+        // Only forward or backward
         this->moveControl->setRotationSpeed(0);
 
-        if (cm < 0)
+        if (cmDistance < 0)
-            this->moveControl->setSpeed(-this->maxForwardSpeed);
+        {
-        else if (cm > 0)
+            this->moveControl->setSpeed(-this->maxSpeeds.x);
-            this->moveControl->setSpeed(this->maxForwardSpeed);
+        }
+        else if (cmDistance > 0)
+        {
+            this->moveControl->setSpeed(this->maxSpeeds.x);
+        }
 
-        this->maneuverTime = (uint32_t) (((double) abs(cm) / 100.0) / this->maxForwardSpeed) * 1000;
+        this->maneuverTime = static_cast<uint32_t>(((abs(cmDistance) / 100.0) / this->maxSpeeds.x) * 1000);
         this->busy = true;
         this->maneuver = Maneuver::Drive;
         return true;
-    } else {
+    }
-        //forward or backward and left or right
+    else
+    {
+        // forward or backward and left or right
         //  TODO: Calculate roationspeed
-        if (cm < 0)
+        if (cmDistance < 0)
-            this->moveControl->setSpeed(-this->maxForwardSpeed);
+        {
-        else if (cm > 0)
+            this->moveControl->setSpeed(-this->maxSpeeds.x);
-            this->moveControl->setSpeed(this->maxForwardSpeed);
+        }
+        else if (cmDistance > 0)
+        {
+            this->moveControl->setSpeed(this->maxSpeeds.x);
+        }
 
-        this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->maxForwardSpeed) * 1000;
+        this->maneuverTime = static_cast<uint32_t>(((cmDistance / 100.0) / this->maxSpeeds.x) * 1000);
         this->busy = true;
         this->maneuver = Maneuver::Drive;
         return true;
@@ -86,17 +103,22 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
     return false;
 }
 
-bool TestMode::leftEngine(int16_t powerPercentage, int16_t seconds) {
+bool TestMode::leftEngine(int16_t powerPercentage, int16_t seconds)
-    if (this->engineInit(powerPercentage, seconds)) {
+{
+    if (this->engineInit(powerPercentage, seconds))
+    {
         this->moveControl->setRawPowerLeft(powerPercentage);
         this->maneuver = Maneuver::LeftEngine;
+        std::cout << "TestMode::leftEngine" << std::endl;
         return true;
     }
     return false;
 }
 
-bool TestMode::rightEngine(int16_t powerPercentage, int16_t seconds) {
+bool TestMode::rightEngine(int16_t powerPercentage, int16_t seconds)
-    if (this->engineInit(powerPercentage, seconds)) {
+{
+    if (this->engineInit(powerPercentage, seconds))
+    {
         this->moveControl->setRawPowerRight(powerPercentage);
         this->maneuver = Maneuver::RightEngine;
         return true;
@@ -104,8 +126,10 @@ bool TestMode::rightEngine(int16_t powerPercentage, int16_t seconds) {
     return false;
 }
 
-bool TestMode::bothEngine(int16_t powerPercentage, int16_t seconds) {
+bool TestMode::bothEngine(int16_t powerPercentage, int16_t seconds)
-    if (this->engineInit(powerPercentage, seconds)) {
+{
+    if (this->engineInit(powerPercentage, seconds))
+    {
         this->moveControl->setRawPowerLeft(powerPercentage);
         this->moveControl->setRawPowerRight(powerPercentage);
         this->maneuver = Maneuver::BothEngine;
@@ -114,23 +138,34 @@ bool TestMode::bothEngine(int16_t powerPercentage, int16_t seconds) {
     return false;
 }
 
-void TestMode::abortManeuver() {
+void TestMode::abortManeuver()
+{
     this->abort = true;
 }
 
-uint8_t TestMode::getRemainingManeuverTime() const {
+uint8_t TestMode::getRemainingManeuverTime() const
-    if (busy)
+{
-        return (uint8_t) ((this->maneuverTime - (millis() - this->actionStart)) / 1000);
+    if (this->busy)
+    {
+        return static_cast<uint8_t>((this->maneuverTime - (millis() - this->actionStart)) / 1000);
+    }
     return 0;
 }
 
-bool TestMode::engineInit(int16_t powerPercentage, int16_t seconds) {
+bool TestMode::engineInit(int16_t powerPercentage, int16_t seconds)
+{
     if (this->busy)
+    {
         return false;
-    if (powerPercentage >= 100 || powerPercentage <= -100)
+    }
+    if (powerPercentage > TestMode::maxPercentage || powerPercentage < -TestMode::maxPercentage)
+    {
         return false;
+    }
     if (seconds < 0)
+    {
         return false;
+    }
 
     this->actionStart = millis();
     this->moveControl->setDrivingStatus(MoveControl::Status::Raw);

src/driveModi/Modi/TestMode/testMode.h

@@ -16,19 +16,19 @@
 #include "moveControl.h"
 #include "driveModi/driveModi.h"
 
-
 /**
  * @brief Test different functions from the rover.
  *
  * You can test engine, engine controller, light and speed meter.
  */
-class TestMode : public DriveModi {
+class TestMode : public DriveModi
-    public:
+{
+public:
     /**
      * @brief Different states to test the engine
-         * 
      */
-        enum Maneuver {
+    enum Maneuver
+    {
         None,
         LeftEngine,
         RightEngine,
@@ -38,14 +38,14 @@ class TestMode : public DriveModi {
     };
 
     /**
-         * @brief Construct a new Test Mode object
+     * @brief Let the rover drive
      *
-         * @param moveControl
+     * @param cmDistance to drive
+     * @param degree degree to rotate over the hole distance
+     * @return true
+     * @return false
      */
-        TestMode(DriveModiParams params);
+    bool drive(int16_t cmDistance = 0, int16_t degree = 0);
-        ~TestMode();
-
-        bool drive(int16_t cm = 0, int16_t degree = 0);
 
     /**
      * @brief Sets the left Engine to a specific power value.
@@ -96,11 +96,12 @@ class TestMode : public DriveModi {
      * @return false
      */
     bool getBusy() const { return this->busy; }
-        Maneuver getManeuver() const { return this->maneuver; }
-        Speedometer* getSpeedometerLeft() const { return this->moveControl->getSpeedometerLeft(); }
-        Speedometer* getSpeedometerRight() const { return this->moveControl->getSpeedometerRight(); }
 
-    private:
+    Maneuver getManeuver() const { return this->maneuver; }
+    Speedometer *getSpeedometerLeft() const { return this->moveControl->getSpeedometerLeft(); }
+    Speedometer *getSpeedometerRight() const { return this->moveControl->getSpeedometerRight(); }
+
+private:
     void run() override;
     bool engineInit(int16_t powerPercentage, int16_t seconds);
 
@@ -115,6 +116,8 @@ class TestMode : public DriveModi {
     int16_t degree;
     uint32_t maneuverTime = 0;
     uint32_t actionStart = 0;
+
+    static constexpr int8_t maxPercentage = 100;
 };
 
 #endif // TEST_MODE_H

src/driveModi/driveManager.cpp

@@ -11,54 +11,39 @@
 
 #include "driveModi/driveManager.h"
 
-DriveManager::DriveManager(MoveControl *moveControl, const SensorData* sensorData, const ControlPadInput *input) {
+DriveManager::DriveManager(MoveControl *moveControl, const SensorData *sensorData, const ControlPadInput *input)
-    this->driveModiParams.input = input;
+    : driveModiParams{moveControl, input, sensorData}
-    this->driveModiParams.moveControl = moveControl;
+{
-    this->driveModiParams.sensorData = sensorData;
-
     this->init();
 }
 
-DriveManager::DriveManager(DriveModiParams params){
+DriveManager::DriveManager(DriveModiParams params)
-    this->driveModiParams = params;
+    : driveModiParams{params}
-    
+{
     this->init();
 }
 
-void DriveManager::changeModus(Modi modus) {
+void DriveManager::changeModus(DriveModi *modus)
-    this->currentModus = modus;
+{
-
+    if (static_cast<bool>(this->currentModusPtr))
-    if (this->currentModusPtr) {
+    {
         this->removeChildComponent(this->currentModusPtr);
         delete this->currentModusPtr;
-    }
-    
-    switch (modus) {
-        case Modi::Off:
         this->currentModusPtr = nullptr;
-            break;
-
-        case Modi::ManualControl: {
-            this->currentModusPtr = new ManualControl(this->driveModiParams);
-            }
-            break;
-
-        case Modi::TestMode: {
-                this->currentModusPtr = new TestMode(driveModiParams);
-            }
-            break;
-        
-        default:
-            this->currentModusPtr = nullptr;
-            break;
     }
 
-    if (this->currentModusPtr)
+    this->currentModusPtr = modus;
+
+    if (static_cast<bool>(this->currentModusPtr))
+    {
+        this->currentModusPtr->activate(this->driveModiParams);
+        this->currentModusPtr->setSpeeds(this->drivingSpeeds);
         this->addChildComponent(this->currentModusPtr);
+    }
 }
 
-void DriveManager::init() {    
+void DriveManager::init()
+{
     this->addChildComponent(this->driveModiParams.moveControl);
     this->activateOnlyChilds();
 }
-

src/driveModi/driveManager.h

@@ -12,45 +12,35 @@
 #ifndef DRIVE_MANAGER_H
 #define DRIVE_MANAGER_H
 
-#include <Arduino.h>
-#include <SPI.h>
-
 #include "moveControl.h"
 #include "driveModi/driveModi.h"
 #include "config.h"
-#include "networkConfig.h"
-#include "debugTimes.h"
 #include "controlPadInput.h"
 #include "component.h"
 #include "sensorData.h"
 
-// All Drive Modi
-#include "driveModi/Modi/ManualControl/manualControl.h"
-#include "driveModi/Modi/TestMode/testMode.h"
-
 /**
- * @brief An enum to choose the DriveMode
+ * @brief A class to manage and hold the different DriveModi
  *
  */
-enum class Modi {   
+class DriveManager : public Component
-    Off,
+{
-    ManualControl,
+public:
-    CalibrateCompass,
+    /**
-    CaptureRoute,
+     * @brief Construct a new Drive Manager object
-    Autopilot,
+     *
-    TestMode
+     * @param moveControl
-};
+     * @param sensorData
-
+     * @param input
-class DriveManager : public Component {
+     */
-    public:
+    DriveManager(MoveControl *moveControl, const SensorData *sensorData, const ControlPadInput *input);
-        DriveManager(MoveControl *moveControl, const SensorData* sensorData, const ControlPadInput *input);
-        DriveManager(DriveModiParams params);
 
     /**
-         * @brief Destroy the Drive Manager object
+     * @brief Construct a new Drive Manager object
      *
+     * @param params
      */
-        ~DriveManager();
+    DriveManager(DriveModiParams params);
 
     /**
      * @brief Change the DriveModi to a specific value
@@ -60,7 +50,7 @@ class DriveManager : public Component {
      *
      * @param modus
      */
-        void changeModus(Modi modus);
+    void changeModus(DriveModi *modus = nullptr);
 
     /**
      * @brief Get the DriveModi Ptr object
@@ -72,23 +62,18 @@ class DriveManager : public Component {
      * @see getDriveModi
      * @return DriveModi*
      */
-        DriveModi* getDriveModiPtr() const { return this->currentModusPtr; }
+    DriveModi *getDriveModiPtr() const { return this->currentModusPtr; }
+    DrivingSpeeds &getDrivingSpeedsRef() { return this->drivingSpeeds; }
 
-        /**
+    bool isActive() const { return this->currentModusPtr; }
-         * @brief Get the DriveModi enum
-         * 
-         * @return Modi 
-         */
-        Modi getDriveModi() const { return this->currentModus; }
 
-    private:
+private:
-        void run() override {};
+    void run() override{};
     void init();
 
-        Modi currentModus = Modi::Off;
     DriveModi *currentModusPtr = nullptr;
     DriveModiParams driveModiParams;
+    DrivingSpeeds drivingSpeeds = {1, 7};
 };
 
-
 #endif // DRIVE_MANAGER_H

src/driveModi/driveModi.cpp

@@ -1,26 +1,31 @@
 #include "driveModi.h"
 
-DriveModi::DriveModi(MoveControl* moveControl, ControlPadInput* input, SensorData* sensorData) {
+DriveModi::~DriveModi()
+{
+    this->moveControl->setSpeed(0);
+    this->moveControl->setRotationSpeed(0);
+    this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
+}
+
+void DriveModi::activate(MoveControl *moveControl, ControlPadInput *input, SensorData *sensorData)
+{
     this->moveControl = moveControl;
     this->input = input;
     this->sensorData = sensorData;
     this->init();
 }
 
-DriveModi::DriveModi(DriveModiParams params){
+void DriveModi::activate(DriveModiParams params)
+{
     this->moveControl = params.moveControl;
     this->input = params.input;
     this->sensorData = params.sensorData;
     this->init();
 }
 
-DriveModi::~DriveModi() {
+void DriveModi::init()
-    this->moveControl->setSpeed(0);
+{
-    this->moveControl->setRotationSpeed(0);
-    this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
-}
-
-void DriveModi::init() {
     this->moveControl->setDrivingStatus(MoveControl::Status::Drive);
-    this->loopDelay = 40;
+    this->loopDelay = DriveModi::defaultDelay;
+    this->afterActivate();
 }

src/driveModi/driveModi.h

@@ -17,10 +17,14 @@
 #include "controlPadInput.h"
 #include "sensorData.h"
 
-struct DriveModiParams {
+/**
-    MoveControl* moveControl;
+ * @brief The struct contains all objects needed by a DriveMode
-    const ControlPadInput* input;
+ */
-    const SensorData* sensorData;
+struct DriveModiParams
+{
+    MoveControl *moveControl;
+    const ControlPadInput *input;
+    const SensorData *sensorData;
 };
 
 /**
@@ -29,42 +33,48 @@ struct DriveModiParams {
  * This class must be inherited by other classes which want to be
  * act as a DriveModi, because the DriveModi structure uses polymorphism.
  */
-class DriveModi : public Component {
+class DriveModi : public Component
-    public:
+{
-        DriveModi(MoveControl* moveControl, ControlPadInput* input, SensorData* sensorData);
+public:
-        DriveModi(DriveModiParams);
     virtual ~DriveModi();
 
-        const SensorData* getSensorData() const { return this->sensorData; }
+    const SensorData *getSensorData() const { return this->sensorData; }
 
     /**
-         * @brief Set the max speed
+     * @brief This activates a DriveMode
      *
-         * @param maxSpeed in m/s 
+     * This function have to be called to provide the DriveMode
+     * with this objects
+     *
+     * @param moveControl
+     * @param input
+     * @param sensorData
      */
-        void setMaxSpeed(double maxForwardSpeed) { maxForwardSpeed = maxForwardSpeed; }
+    void activate(MoveControl *moveControl, ControlPadInput *input, SensorData *sensorData);
-        void increaseMaxSpeed(double increase = 0.1) { maxForwardSpeed += increase; }
+    void activate(DriveModiParams params);
-        void decreaseMaxSpeed(double increase = 0.1) { maxForwardSpeed -= increase; }
-        double getMaxSpeed() const { return this->maxForwardSpeed; }
 
+    void setSpeeds(DrivingSpeeds speeds) { this->maxSpeeds = speeds; }
+    DrivingSpeeds getSpeeds() const { return this->maxSpeeds; }
+    DrivingSpeeds &getSpeedsRef() { return this->maxSpeeds; }
+
+protected:
     /**
-         * @brief Set the max rotation
+     * @brief Prepare other things
      *
-         * @param maxRotation in rad/s (maybe) 
+     * This function can be overwritten to prepare things
+     * for the specific DriveModi. This function will be
+     * called after activate().
      */
-        void setMaxRotation(double maxRotation) { maxRotationSpeed = maxRotation; }
+    virtual void afterActivate() {}
-        void increaseMaxRotation(double increase = 0.1) { maxRotationSpeed += increase; }
-        void decreaseMaxRotation(double increase = 0.1) { maxRotationSpeed -= increase; }
-        double getMaxRotation() const { return this->maxRotationSpeed; }
 
-    protected:
+    MoveControl *moveControl = nullptr;
-        MoveControl *moveControl;
+    DrivingSpeeds maxSpeeds = {1, 7};
-        const ControlPadInput* input;
+    const ControlPadInput *input = nullptr;
-        const SensorData* sensorData;
+    const SensorData *sensorData = nullptr;
-        double maxForwardSpeed = 1;
-        double maxRotationSpeed = 7;
 
-    private:
+private:
     void init();
+
+    static constexpr uint8_t defaultDelay = 40;
 };
 #endif // DRIVEMODI_H

src/main.cpp

@@ -29,6 +29,7 @@
 #include "LcdWrapper.h"
 #include "moveControl.h"
 #include "network.h"
+#include "networkConfig.h"
 #include "debugMqtt.h"
 #include "battery.h"
 #include "sensorData.h"
@@ -42,73 +43,78 @@
 #include "SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h"
 #include "SpecialMenus/Systeminformation/menuSysteminformation.h"
 #include "SpecialMenus/PID/menuPidSettings.h"
-#include "SpecialMenus/GPS/menuGPS.h"
+#include "SpecialMenus/Route/menuRoute.h"
+#include "SpecialMenus/SensorData/menuSensorData.h"
+#include "SpecialMenus/CalibrateBattery/menuCalibrateBattery.h"
 
 MoveControl moveController;
-DriveManager* driveManager;
+Network *network;
-Menu* main_m;
+DriveManager *driveManager;
-LiquidCrystal_I2C* lcd;
+Menu *main_m;
-LcdWrapper* lcdWrapper;
+LiquidCrystal_I2C *lcd;
-OutputBuf* outputBuf;
+LcdWrapper *lcdWrapper;
-DebugMqtt* debugMqtt = nullptr;
+OutputBuf *outputBuf;
-Battery* mainBattery;
+DebugMqtt *debugMqtt = nullptr;
-SPIClass* spiPort;
+Battery *mainBattery;
-SensorData* sensorData;
+SPIClass *spiPort;
-ControlPad* controlPad;
+SensorData *sensorData;
+ControlPad *controlPad;
 
-bool wifiIsActive;
+constexpr uint16_t displayUpdateDelay = 500;
 
-void i2cScanner(void);
+void i2cScanner();
-void makeMenu(void);
+void makeMenu();
-void restart(void);
+void restart();
-void receiveCallback (const uint8_t * mac, const uint8_t *incomingData, int len); 
+void receiveCallback(const uint8_t *mac, const uint8_t *incomingData, int len);
-void sendCallback (const uint8_t *mac_addr, esp_now_send_status_t status);
+void sendCallback(const uint8_t *mac_addr, esp_now_send_status_t status);
-void lcdWrapperCallback (const char data[][LcdWrapper::totalRows], uint8_t lines, uint8_t rows);
+void lcdWrapperCallback(const char data[][LcdWrapper::totalRows], uint8_t lines, uint8_t rows);
+NetworkAddresses setIPs();
 
-
+void setup()
-void setup() {
+{
-    Serial.begin(115200);
+    Serial.begin(Settings::baudRate);
     // DebugTimes::setConsolOutput(true);
     DebugTimes setupTime;
-    char wifiIndicator = 'X';
-
-    WiFi.mode(WIFI_MODE_STA);
-    Serial.print("MAC Address: ");
-    Serial.println(WiFi.macAddress());
 
     spiPort = new SPIClass(HSPI);
     spiPort->begin(PinNumbers::spiSck, PinNumbers::spiCipo, PinNumbers::spiCopi, PinNumbers::gnssSpiCs);
 
-    mainBattery = new Battery(35);
+    mainBattery = new Battery(PinNumbers::battery);
     controlPad = new ControlPad();
 
-    Wire.begin(21, 19);
+    Wire.begin(PinNumbers::sda, PinNumbers::scl);
-    Wire.setClock(400000);
+    Wire.setClock(Settings::i2cSpeed);
     i2cScanner();
-    lcd = new LiquidCrystal_I2C(0x3F,16,2);
+    lcd = new LiquidCrystal_I2C(0x3F, 16, 2);
     lcd->init();
     lcd->clear();
     lcd->noBacklight();
 
-    Network::setIps();
+    network = new Network(static_cast<const char *>(NetworkConfig::ssid),
-    Network::setupMQTT();
+                          static_cast<const char *>(NetworkConfig::password),
-    wifiIsActive = Network::connectWifi();
+                          setIPs());
-    if (wifiIsActive) {
+    network->activateEspNow(receiveCallback, sendCallback);
-        Network::initMQTT();
+    if (NetworkConfig::mqtt)
-        if (Network::checkMQTT()) {
+    {
-            DebugMqtt::init(Network::getMqttClient(), Loglevel::debug);
+        if (network->activateMqtt(static_cast<const char *>(MqttConfig::user),
-            std::cout << "Activate additional output via MQTT..." << std::endl;
+                                  static_cast<const char *>(MqttConfig::password)))
+        {
+            DebugMqtt::init(network->getMqttClient(), Loglevel::debug);
             debugMqtt = new DebugMqtt("Console");
             outputBuf = new OutputBuf(debugMqtt);
-        } else
+            outputBuf->activateMqtt(true);
+        }
+        else
+        {
             outputBuf = new OutputBuf();
-        wifiIndicator = '-';
+        }
-    } else
+    }
+    else
+    {
         outputBuf = new OutputBuf();
+    }
     std::cout.rdbuf(outputBuf);
 
-    Network::connectEspNow(receiveCallback, sendCallback);
-
     std::cout << "Welcome to Kleiax-Rover" << std::endl;
     std::cout << "Project verion: " << VERSION << std::endl;
     std::cout << "Build timestamp: " << BUILD_TIMESTAMP << std::endl;
@@ -117,10 +123,19 @@ 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());
 
-    outputBuf->activateMqtt(true);
+    char wifiIndicator = 'X';
-
+    if (network->isWifiConnected())
+    {
+        wifiIndicator = '-';
+    }
     lcd->backlight();
     lcd->printf("%c Kleiax-Rover %c", wifiIndicator, wifiIndicator);
     lcd->setCursor(0, 1);
@@ -133,17 +148,9 @@ void setup() {
     setupTime.stopConsol("Setup");
 }
 
-void loop() {
+void loop()
-    if (wifiIsActive) {
+{
-        DebugTimes wifiTime;
+    network->loop();
-        Network::checkWiFi();
-        #ifdef MQTT
-        bool err = Network::checkMQTT();
-        if (!err)
-            std::cout << "loop(): checkMQTT returns false" << std::endl;
-        #endif //MQTT
-        wifiTime.stopConsol("WiFi-Time", 10);
-    }
     sensorData->loop();
     driveManager->loop();
     controlPad->loop();
@@ -151,82 +158,119 @@ void loop() {
     lcdWrapper->loop();
     mainBattery->loop();
 
-    if (mainBattery->isNewValue()) {
+    // new Value ervery 0.5s
+    if (mainBattery->isNewValue())
+    {
         static uint8_t batteryLowCounter = 0;
-        if (mainBattery->isBatteryLow(10))
+        static constexpr uint8_t minVoltage = 10;
+        if (mainBattery->isBatteryLow(minVoltage))
+        {
             batteryLowCounter++;
+        }
         else
+        {
             batteryLowCounter = 0;
+        }
 
-        if (batteryLowCounter >= 10) {        
+        if (batteryLowCounter >= 10)
+        {
             moveController.emergencyStop();
-            uint16_t voltage = (uint16_t) (mainBattery->getBatteryVoltage() * 100);
+            const auto voltage = static_cast<uint16_t>(mainBattery->getBatteryVoltage() * 100);
             lcd->setCursor(0, 0);
             lcd->printf("Low Battery: %u", voltage);
             lcd->setCursor(0, 1);
             lcd->print("Please turn off.");
 
-            while (true);
+            while (true)
+            {
+            }
         }
     }
 }
 
-void i2cScanner() {
+void i2cScanner()
+{
+    constexpr uint8_t checkForLength = 16;
+    constexpr uint8_t maxAdresses = UINT8_MAX / 2;
     std::cout << "\nI2C Scanner" << std::endl;
 
-    byte error, address;
+    byte error = 0;
-    int nDevices;
+    byte address = 0;
+    int nDevices = 0;
     std::cout << "Scanning..." << std::endl;
-    nDevices = 0;
+    for (address = 1; address < maxAdresses; address++)
-    for(address = 1; address < 127; address++ ) {
+    {
         Wire.beginTransmission(address);
         error = Wire.endTransmission();
-        if (error == 0) {
+        if (error == 0)
+        {
             std::cout << "I2C device found at address 0x";
-            if (address<16)
+            if (address < checkForLength)
+            {
                 std::cout << "0";
-            std::cout << std::hex << (int) address << std::dec << std::endl;
+            }
+            std::cout << std::hex << static_cast<int>(address) << std::dec << std::endl;
             // std::cout << (int) address << std::endl;
             nDevices++;
         }
-        else if (error==4) {
+        else if (error == 4)
+        {
             std::cout << "Unknow error at address 0x";
-            if (address<16)
+            if (address < checkForLength)
+            {
                 std::cout << "0";
-            std::cout << std::hex << (int) address << std::dec << std::endl;
+            }
+            std::cout << std::hex << static_cast<int>(address) << std::dec << std::endl;
         }
     }
     if (nDevices == 0)
-        std::cout << "No I2C devices found\n" << std::endl;
+    {
+        std::cout << "No I2C devices found\n"
+                  << std::endl;
+    }
     else
-        std::cout << "done\n"  << std::endl;
+    {
+        std::cout << "done\n"
+                  << std::endl;
+    }
 }
 
-void makeMenu() {
+void makeMenu()
-    auto dummy = []() {
+{
-        std::cout << "Dummy in Action" <<std::endl;
+    auto dummy = []()
+    {
+        std::cout << "Dummy in Action" << std::endl;
     };
 
     // Create Menu
     main_m = new Menu();
     main_m->setLcd(lcdWrapper);
-    Menu* mode_m = new Menu();
+    auto *mode_m = new Menu();
-    Menu* pid_m = new Menu();
+    auto *set_m = new Menu();
-    MenuIntInput* pidl_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(0)));
+    auto *pid_m = new Menu();
-    MenuIntInput* pidr_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(1)));
+    auto *pidl_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(0)));
-    MenuManualControl* man_m = new MenuManualControl(driveManager);
+    auto *pidr_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(1)));
-    MenuTestMode* testM_m = new MenuTestMode(driveManager);
+    auto *speed_m = new MenuIntInput(2, new MenuSpeed(driveManager->getDrivingSpeedsRef()));
-    MenuCalibrateCompass* comp_m = new MenuCalibrateCompass(driveManager);
+    auto *man_m = new MenuManualControl(driveManager);
-    MenuGPS* gps_m = new MenuGPS(sensorData);
+    auto *cap_m = new MenuCaptureRoute(driveManager);
-    MenuSysteminformation* sys_m = new MenuSysteminformation(mainBattery);
+    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);
 
-    gps_m->setUpdateDelay(1000);
+    auto_m->setUpdateDelay(displayUpdateDelay);
-    sys_m->setUpdateDelay(1500);
+    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("GPS", gps_m));
+    main_m->addEntry(new MenuAction("Sensor", sen_m));
-    main_m->addEntry(new MenuAction("PID", pid_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));
 
@@ -234,6 +278,14 @@ void makeMenu() {
     mode_m->addEntry(new MenuAction("Manual Control", man_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));
+
+    // Entry for the setting menu
+    set_m->addEntry(new MenuAction("PID", pid_m));
+    set_m->addEntry(new MenuAction("Speed", speed_m));
+    set_m->addEntry(new MenuAction("Distance", dummy));
+    set_m->addEntry(new MenuAction("WiFi", dummy));
+    set_m->addEntry(new MenuAction("Battery", bat_m));
 
     // Entry for the PID Menu
     pid_m->addEntry(new MenuAction("Left", pidl_m));
@@ -241,42 +293,76 @@ void makeMenu() {
 
     // Other config
     pidl_m->setMinMax(0, UINT8_MAX);
-    pidl_m->setEntry(0, "P-Part", (uint8_t) moveController.getPID(0)->GetKp());
+    pidl_m->setEntry(0, "P-Part", static_cast<uint8_t>(moveController.getPID(0)->GetKp()));
-    pidl_m->setEntry(1, "I-Part", (uint8_t) moveController.getPID(0)->GetKi());
+    pidl_m->setEntry(1, "I-Part", static_cast<uint8_t>(moveController.getPID(0)->GetKi()));
-    pidl_m->setEntry(2, "D-Part", (uint8_t) moveController.getPID(0)->GetKd());
+    pidl_m->setEntry(2, "D-Part", static_cast<uint8_t>(moveController.getPID(0)->GetKd()));
 
     pidr_m->setMinMax(0, UINT8_MAX);
-    pidr_m->setEntry(0, "P-Part", (uint8_t) moveController.getPID(1)->GetKp());
+    pidr_m->setEntry(0, "P-Part", static_cast<uint8_t>(moveController.getPID(1)->GetKp()));
-    pidr_m->setEntry(1, "I-Part", (uint8_t) moveController.getPID(1)->GetKi());
+    pidr_m->setEntry(1, "I-Part", static_cast<uint8_t>(moveController.getPID(1)->GetKi()));
-    pidr_m->setEntry(2, "D-Part", (uint8_t) moveController.getPID(1)->GetKd());
+    pidr_m->setEntry(2, "D-Part", static_cast<uint8_t>(moveController.getPID(1)->GetKd()));
+
+    speed_m->setMinMax(5, UINT8_MAX);
+    speed_m->setEntry(0, "x m/s e-1", driveManager->getDrivingSpeedsRef().x * 10);
+    speed_m->setEntry(1, "z rad/s e-1", driveManager->getDrivingSpeedsRef().rot * 10);
+
+    man_m->addSensorMenu(sen_m);
 
     controlPad->setMenuControl(main_m);
 }
 
-void restart(void) {
+void restart()
+{
     lcdWrapper->clear();
     lcdWrapper->setCursor(0, 0);
     lcdWrapper->print("Rebooting ...");
     ESP.restart();
 }
 
-void receiveCallback (const uint8_t * mac, const uint8_t *incomingData, int len) {
+void receiveCallback(const uint8_t *mac, const uint8_t *incomingData, int len)
+{
     if (len != sizeof(ControlPadInput))
+    {
         return;
+    }
     controlPad->insertData(incomingData);
 }
 
-void sendCallback (const uint8_t *mac_addr, esp_now_send_status_t status) {
+void sendCallback(const uint8_t *mac_addr, esp_now_send_status_t status)
+{
     if (status != ESP_NOW_SEND_SUCCESS)
+    {
         std::cout << "sendCallback - Delivery Fail" << std::endl;
+    }
 }
 
-void lcdWrapperCallback (const char data[][LcdWrapper::totalRows], uint8_t lines, uint8_t rows) {
+void lcdWrapperCallback(const char data[][LcdWrapper::totalRows], uint8_t lines, uint8_t rows)
+{
     if (!controlPad->isControlPadConnected())
+    {
         return;
+    }
 
-    esp_err_t result = esp_now_send(Network::getBroadcastAddress(), (uint8_t *) data, lines * rows);
+    const esp_err_t result = esp_now_send(network->getBroadcastAddress(), reinterpret_cast<const uint8_t *>(data), lines * rows);
 
     if (result != ESP_OK)
+    {
         Serial.println("Error sending the data");
+    }
+}
+
+NetworkAddresses setIPs()
+{
+    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)
+    {
+        adresses.mqttServer.fromString(static_cast<const char *>(MqttConfig::server));
+        adresses.mqttPort = MqttConfig::port;
+    }
+
+    return adresses;
 }

src/moveControl.cpp

@@ -10,69 +10,70 @@
  */
 #include "moveControl.h"
 
-MoveControl::MoveControl() {
+MoveControl::MoveControl()
-    this->loopDelay = 20;
+    : Component(MoveControl::loopDelay),
-    this->left_motor = new MotorControl();
+      leftMotor{new MotorControl()},
-    this->right_motor = new MotorControl();
+      rightMotor{new MotorControl()},
-    this->left_speedometer = new Speedometer(PinNumbers::LeftMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps);
+      leftSpeedometer{new Speedometer(PinNumbers::LeftMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps)},
-    this->right_speedometer = new Speedometer(PinNumbers::RightMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps);
+      rightSpeedometer{new Speedometer(PinNumbers::RightMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps)},
-
+      leftPid{new PID(&this->wheelspeedLeft,
-    this->left_pid = new PID(   &this->wheelspeed_left,
+                      &this->leftPidOut,
-                                &this->left_pid_out,
+                      &this->wheelspeedLeftTarget,
-                                &this->wheelspeed_left_target,
                       Settings::Pid::Left::P,
                       Settings::Pid::Left::I,
                       Settings::Pid::Left::D,
-                                DIRECT);
+                      DIRECT)},
-    this->right_pid = new PID(  &this->wheelspeed_right,
+      rightPid{new PID(&this->wheelspeedRight,
-                                &this->right_pid_out,
+                       &this->rightPidOut,
-                                &this->wheelspeed_right_target,
+                       &this->wheelspeedRightTarget,
                        Settings::Pid::Right::P,
                        Settings::Pid::Right::I,
                        Settings::Pid::Right::D,
-                                DIRECT);
+                       DIRECT)}
+{
+    this->leftPid->SetOutputLimits(Settings::Pid::outMin, Settings::Pid::outMax);
+    this->leftPid->SetSampleTime(Settings::Pid::sampleTime);
+    this->leftPid->SetMode(AUTOMATIC);
 
-    this->left_pid->SetOutputLimits(Settings::Pid::outMin, Settings::Pid::outMax);
+    this->rightPid->SetOutputLimits(Settings::Pid::outMin, Settings::Pid::outMax);
-    this->left_pid->SetSampleTime(Settings::Pid::sampleTime);
+    this->rightPid->SetSampleTime(Settings::Pid::sampleTime);
-    this->left_pid->SetMode(AUTOMATIC);
+    this->rightPid->SetMode(AUTOMATIC);
 
-    this->right_pid->SetOutputLimits(Settings::Pid::outMin, Settings::Pid::outMax);
+    this->leftMotor->init(PinNumbers::LeftMotor::pwm, PinNumbers::LeftMotor::pmwChannel, PinNumbers::LeftMotor::dir1, PinNumbers::LeftMotor::dir2);
-    this->right_pid->SetSampleTime(Settings::Pid::sampleTime);
+    this->rightMotor->init(PinNumbers::RightMotor::pwm, PinNumbers::RightMotor::pmwChannel, PinNumbers::RightMotor::dir1, PinNumbers::RightMotor::dir2);
-    this->right_pid->SetMode(AUTOMATIC); 
 
-    this->left_motor->init(PinNumbers::LeftMotor::pwm, PinNumbers::LeftMotor::pmwChannel, PinNumbers::LeftMotor::dir1, PinNumbers::LeftMotor::dir2);
+    this->addChildComponent(this->leftMotor);
-    this->right_motor->init(PinNumbers::RightMotor::pwm, PinNumbers::RightMotor::pmwChannel, PinNumbers::RightMotor::dir1, PinNumbers::RightMotor::dir2);
+    this->addChildComponent(this->rightMotor);
-
+    this->addChildComponent(this->leftSpeedometer);
-    this->addChildComponent(this->left_motor);
+    this->addChildComponent(this->rightSpeedometer);
-    this->addChildComponent(this->right_motor); 
-    this->addChildComponent(this->left_speedometer);
-    this->addChildComponent(this->right_speedometer);                             
 }
 
-MoveControl::~MoveControl() {
+MoveControl::~MoveControl()
-    this->left_motor->emergencyStop();
+{
-    this->right_motor->emergencyStop();
+    this->leftMotor->emergencyStop();
-
+    this->rightMotor->emergencyStop();
-    delete this->left_motor;
-    delete this->right_motor;
-    delete this->left_speedometer;
-    delete this->right_speedometer;
-    delete this->left_pid;
-    delete this->right_pid;
 
+    delete this->leftMotor;
+    delete this->rightMotor;
+    delete this->leftSpeedometer;
+    delete this->rightSpeedometer;
+    delete this->leftPid;
+    delete this->rightPid;
 }
 
-void MoveControl::run() {
+void MoveControl::run()
+{
     this->updateCurrentWheelSpeed();
     this->calcTargetWheelSpeed();
 
-    this->right_pid->Compute();
+    this->rightPid->Compute();
-    this->left_pid->Compute();
+    this->leftPid->Compute();
 
     this->regulateMotors();
 }
 
-void MoveControl::setDrivingStatus(Status status) {
+void MoveControl::setDrivingStatus(Status status)
+{
     this->setSpeed(0);
     this->setRotationSpeed(0);
     this->setRawPowerLeft(0);
@@ -80,11 +81,15 @@ void MoveControl::setDrivingStatus(Status status) {
     this->driving_status = status;
     // switch (this->driving_status) {
     //     case Status::Stop :
-    //         Serial.println("New drivingState = Stop in MoveControl::setDrivingStatus");
+    //         std::cout << "New drivingState = Stop in MoveControl::setDrivingStatus" << std::endl;
     //         break;
 
     //     case Status::Drive :
-    //         Serial.println("New drivingState = Drive in MoveControl::setDrivingStatus");
+    //         std::cout << "New drivingState = Drive in MoveControl::setDrivingStatus" << std::endl;
+    //         break;
+
+    //     case Status::Raw :
+    //         std::cout << "New drivingState = Raw in MoveControl::setDrivingStatus" << std::endl;
     //         break;
 
     //     default:
@@ -93,9 +98,10 @@ void MoveControl::setDrivingStatus(Status status) {
     // }
 }
 
-void MoveControl::emergencyStop() {
+void MoveControl::emergencyStop()
-    this->left_motor->emergencyStop();
+{
-    this->right_motor->emergencyStop();
+    this->leftMotor->emergencyStop();
+    this->rightMotor->emergencyStop();
     this->setSpeed(0);
     this->setRotationSpeed(0);
     this->setRawPowerLeft(0);
@@ -103,97 +109,132 @@ void MoveControl::emergencyStop() {
     this->driving_status = Status::Stop;
 }
 
-void MoveControl::setSpeed(double speed) {
+void MoveControl::setSpeed(double speed)
-    if (speed < 0.2 && speed > -0.2) {
+{
-        this->x_speed = 0;
+    if (speed < MoveControl::minSpeed && speed > -MoveControl::minSpeed)
+    {
+        this->drivingSpeeds.x = 0;
         return;
     }
 
-    this->x_speed = speed;
+    this->drivingSpeeds.x = speed;
 }
 
-void MoveControl::setRotationSpeed(double speed) {
+void MoveControl::setRotationSpeed(double speed)
-    if (speed < 0.1 && speed > -0.1){
+{
-        this->rotation_speed = 0;
+    if (speed < MoveControl::minSpeed && speed > -MoveControl::minSpeed)
+    {
+        this->drivingSpeeds.rot = 0;
         return;
     }
 
-    this->rotation_speed = speed;
+    this->drivingSpeeds.rot = speed;
 }
 
-void MoveControl::setRawPowerLeft(int16_t power) {
+void MoveControl::setSpeeds(DrivingSpeeds drivingSpeeds)
-    if (power <= 100 && power >= -100)
+{
-        this->rawPowerLeft = power;
+    this->setSpeed(drivingSpeeds.x);
+    this->setRotationSpeed(drivingSpeeds.rot);
 }
 
-void MoveControl::setRawPowerRight(int16_t power) {
+void MoveControl::setRawPowerLeft(int16_t power)
-    if (power <= 100 && power >= -100)
+{
-        this->rawPowerRight = power;
+    if (power <= MoveControl::maxPercentage && power >= -MoveControl::maxPercentage)
+    {
+        this->rawPowerLeft = static_cast<int8_t>(power);
+    }
 }
 
-void MoveControl::setPidTunings(uint8_t side, double p, double i, double d) {
+void MoveControl::setRawPowerRight(int16_t power)
-    PID* selectedPID = nullptr;
+{
+    if (power <= MoveControl::maxPercentage && power >= -MoveControl::maxPercentage)
+    {
+        this->rawPowerRight = static_cast<int8_t>(power);
+    }
+}
+
+void MoveControl::setPidTunings(uint8_t side, double pPart, double iPart, double dPart)
+{
+    PID *selectedPID = nullptr;
     if (side == 0)
-        selectedPID = this->left_pid;
+    {
+        selectedPID = this->leftPid;
+    }
     else if (side == 1)
-        selectedPID = this->right_pid;
+    {
+        selectedPID = this->rightPid;
+    }
 
-    selectedPID->SetTunings(p, i, d);
+    selectedPID->SetTunings(pPart, iPart, dPart);
 }
 
-PID* MoveControl::getPID(uint8_t side) const {
+PID *MoveControl::getPID(uint8_t side) const
+{
     if (side == 0)
-        return this->left_pid;
+    {
-    else if (side == 1)
+        return this->leftPid;
-        return this->right_pid;
+    }
-    else
+    if (side == 1)
+    {
+        return this->rightPid;
+    }
     return nullptr;
 }
 
-void MoveControl::setSpeedometerDirection(Speedometer *speedometer, double value) {
+void MoveControl::setSpeedometerDirection(Speedometer *speedometer, double value)
+{
     if (value > 0)
+    {
         speedometer->setDirection(Speedometer::Direction::Forward);
+    }
     else if (value < 0)
+    {
         speedometer->setDirection(Speedometer::Direction::Backward);
+    }
     else
+    {
         speedometer->setDirection(Speedometer::Direction::None);
+    }
 }
 
-void MoveControl::calcTargetWheelSpeed() {
+void MoveControl::calcTargetWheelSpeed()
+{
     /* original formula:
        (1 / r) / 1  b \ / x \ = / Xl \
                \ 1 -b / \ T /   \ Xr /  */
 
     // (1 / r) * 1
-    constexpr double A = 15.82278481;
+    constexpr double A1r1 = 1.0 / (Settings::wheelDiameter / 2);
     // (1 / r) * b
-    constexpr double B = 2.096518987;
+    constexpr double B1rb = (1.0 / (Settings::wheelDiameter / 2)) * (Settings::wheelDistance / 2);
 
-    this->wheelspeed_right_target = (A * this->x_speed + B * this->rotation_speed) * (Settings::wheelDiameter / 2);
+    this->wheelspeedRightTarget = (A1r1 * this->drivingSpeeds.x + B1rb * this->drivingSpeeds.rot);
-    this->wheelspeed_left_target  = (A * this->x_speed + (-B) * this->rotation_speed) * (Settings::wheelDiameter / 2);
+    this->wheelspeedLeftTarget = (A1r1 * this->drivingSpeeds.x + (-B1rb) * this->drivingSpeeds.rot);
 }
 
-void MoveControl::regulateMotors() {
+void MoveControl::regulateMotors()
-    switch (this->driving_status) {
+{
-        case Status::Stop :
+    switch (this->driving_status)
-            this->left_motor->setTargetPower(0);
+    {
-            this->right_motor->setTargetPower(0);
+    case Status::Stop:
-            this->setSpeedometerDirection(this->left_speedometer, 0);
+        this->leftMotor->setTargetPower(0);
-            this->setSpeedometerDirection(this->right_speedometer, 0);
+        this->rightMotor->setTargetPower(0);
+        this->setSpeedometerDirection(this->leftSpeedometer, 0);
+        this->setSpeedometerDirection(this->rightSpeedometer, 0);
         break;
 
-        case Status::Drive :
+    case Status::Drive:
-            this->left_motor->setTargetPower( (int8_t) this->left_pid_out);
+        this->leftMotor->setTargetPower(static_cast<int8_t>(this->leftPidOut));
-            this->right_motor->setTargetPower( (int8_t) this->right_pid_out);
+        this->rightMotor->setTargetPower(static_cast<int8_t>(this->rightPidOut));
-            this->setSpeedometerDirection(this->left_speedometer, this->left_motor->getPower());
+        this->setSpeedometerDirection(this->leftSpeedometer, this->leftMotor->getPower());
-            this->setSpeedometerDirection(this->right_speedometer, this->right_motor->getPower());
+        this->setSpeedometerDirection(this->rightSpeedometer, this->rightMotor->getPower());
         break;
 
-        case Status::Raw :
+    case Status::Raw:
-            this->left_motor->setTargetPower(this->rawPowerLeft);
+        this->leftMotor->setTargetPower(this->rawPowerLeft);
-            this->right_motor->setTargetPower(this->rawPowerRight);
+        this->rightMotor->setTargetPower(this->rawPowerRight);
-            this->setSpeedometerDirection(this->left_speedometer, this->rawPowerLeft);
+        this->setSpeedometerDirection(this->leftSpeedometer, this->rawPowerLeft);
-            this->setSpeedometerDirection(this->right_speedometer, this->rawPowerRight);
+        this->setSpeedometerDirection(this->rightSpeedometer, this->rawPowerRight);
         break;
 
     default:
@@ -202,7 +243,8 @@ void MoveControl::regulateMotors() {
     }
 }
 
-void MoveControl::updateCurrentWheelSpeed() {
+void MoveControl::updateCurrentWheelSpeed()
-    this->wheelspeed_left = this->left_speedometer->getSpeed();
+{
-    this->wheelspeed_right = this->right_speedometer->getSpeed();
+    this->wheelspeedLeft = this->leftSpeedometer->getSpeedRad();
+    this->wheelspeedRight = this->rightSpeedometer->getSpeedRad();
 }

src/network.cpp

@@ -1,174 +0,0 @@
-/**
- * @file network.cpp
- * @author Alexander Klein (alex@kleiax.de)
- * @brief Contains the implementation of the ststic class Network
- * @version 0.1
- * @date 2021-12-14
- * 
- * @copyright Copyright (c) 2021
- * 
- */
-
-#include "network.h"
-
-IPAddress Network::local_IP;
-IPAddress Network::gateway;
-IPAddress Network::subnet;
-IPAddress Network::mqtt_server;
-IPAddress Network::dnsServer;
-
-WiFiClient Network::wifi_client;
-PubSubClient* Network::mqtt_client;
-
-esp_now_peer_info_t Network::peerInfo;
-uint8_t Network::broadcastAddress[6] = ESP_NOW_CONTROLER_MAC;
-
-void Network::setIps() {
-    local_IP.fromString(WLAN_IP);
-    gateway.fromString(WLAN_GATEWAY);
-    subnet.fromString(WLAN_SUBNETMASK);
-    mqtt_server.fromString(MQTT_SERVER);
-    dnsServer.fromString(WLAN_DNS_SERVER);
-}
-
-void Network::setupMQTT() {
-    Network::mqtt_client = new PubSubClient(wifi_client);
-    Network::mqtt_client->setServer(MQTT_SERVER, MQTT_PORT);
-    Network::mqtt_client->setSocketTimeout(1);
-}
-
-bool Network::connectMQTT() {
-    #ifndef MQTT
-        return false;
-    #endif
-
-    // Create a random client ID
-    String clientId = "ESP32Rover-";
-    clientId += String(random(0xffff), HEX);
-
-    #ifdef MQTT_AUTH
-        if (mqtt_client->connect(clientId.c_str(), MQTT_USER, MQTT_PASSWORD)) {
-    #endif //MQTT_AUTH
-
-    #ifndef MQTT_AUTH
-        if (mqtt_client->connect(clientId.c_str())) {
-    #endif //MQTT_AUTH
-
-        // Once connected, publish an announcement...
-        mqtt_client->publish("Rover/Info", "Connected to Mqtt-Broker");
-    }
-    return mqtt_client->connected();
-}
-bool Network::connectWifi() {
-   // Configures static IP address
-   
-    if (!WiFi.mode(WIFI_AP_STA))
-        std::cout << "Network::connectWiFi failed WiFi.mode" << std::endl;
-
-    if (!WiFi.config(local_IP, gateway, subnet, dnsServer))
-        std::cout << "STA Failed to configure" << std::endl;
-
-    // Connect to Wi-Fi network with SSID and password
-    std::cout << "Connecting to " << WLAN_SSID << std::endl;
-    WiFi.begin(WLAN_SSID, WLAN_PASSWORD);
-    uint8_t timeout = WLAN_CONNECT_TIMEOUT;
-    while (WiFi.status() != WL_CONNECTED) {
-        delay(WLAN_CONNECT_LOOP_TIME);
-        std::cout << "." << std::flush;
-        timeout--;
-        if (timeout == 0) {
-            std::cout << std::endl;
-            std::cout << "WiFi NOT connected." << std::endl;
-            return false;
-        }
-    }
-
-    // Print local IP address and start web server
-    std::cout << std::endl;
-    std::cout << "WiFi connected." << std::endl;
-    std::cout << "IP address: " << std::endl;
-    std::cout << WLAN_IP << std::endl;
-    // std::cout << "WiFi MAC Address: " << WiFi.macAddress() << std::endl << std::endl;
-    return true;
-}
-
-bool Network::connectEspNow(recieveCallbackPtr reci, sendCallbackPtr send) {
-    if (esp_now_init() != ESP_OK) {
-        std::cout << "Network::connectEspNow - Error initializing ESP-NOW" << std::endl;
-        return false;
-    }
-
-    esp_now_register_send_cb(send);
-
-    memcpy(peerInfo.peer_addr, broadcastAddress, 6);
-    peerInfo.channel = 0;  
-    peerInfo.encrypt = false;
-
-    if (esp_now_add_peer(&peerInfo) != ESP_OK) {
-        std::cout << "Network::connectEspNow - Failed to add peer" << std::endl;
-        return false;
-    }
-
-    esp_now_register_recv_cb(reci);
-    return true;
-}
-
-uint8_t Network::getCurrentChannel() {
-    uint8_t channel;
-    wifi_second_chan_t secondChannel;
-    if (esp_wifi_get_channel(&channel, &secondChannel) != ESP_OK) {
-        std::cout << "Network::getCurrentChannel - Error!" << std::endl;
-        return -1;
-    }
-    // std::cout << "Network::getCurrentChannel - Current WiFi channel: " 
-    //           << (int) channel << " second channel: " << (int) secondChannel 
-    //           << std::endl;
-    return channel;
-}
-
-bool Network::checkMQTT() {
-    static uint64_t lastReconnectAttempt = 0;
-    if (!mqtt_client->connected()) {
-        long now = millis();
-        if (now - lastReconnectAttempt > MQTT_TIME_RECONNECT) {
-            lastReconnectAttempt = now;
-            // Attempt to reconnect
-            if (connectMQTT())
-                return true;
-        }
-        return false;
-    }
-    mqtt_client->loop();
-    return true;
-}
-
-bool Network::initMQTT() {
-    if (connectMQTT())
-        return true;
-    return false;
-}
-
-int32_t Network::getWiFiChannel(const char *ssid) {
-    if (int32_t n = WiFi.scanNetworks())
-        for (uint8_t i=0; i<n; i++)
-            if (!strcmp(ssid, WiFi.SSID(i).c_str()))
-                return WiFi.channel(i);
-    return 0;
-}
-
-void Network::checkWiFi() {
-    static uint64_t previousMillis = 0;
-    static uint16_t delay = 5000;
-    uint64_t currentMillis = millis();
-    if ((WiFi.status() != WL_CONNECTED) && (currentMillis - previousMillis >= delay)) {
-        std::cout << "Reconnecting to WiFi..." << std::endl;
-        WiFi.disconnect();
-        WiFi.reconnect();
-        previousMillis = currentMillis;
-    }
-
-}
-
-PubSubClient* Network::getMqttClient() {
-    return mqtt_client;
-}