implement gyroskop

doc/TODO allgemein.txt

@@ -10,7 +10,9 @@ Do later:
     -> 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)
@@ -27,21 +29,18 @@ Do later:
     -> Menü für Einstellungen
         - PID
         - Geschwindigkeiten
+        - 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)
 
 
 Do now:
 Code:
     Doxygen Kommentare aktualisieren
-    Automat in MoveControl weil jetzt in Arbeit beschrieben
+    Add Gyroskop Sensor
-    Liste mit Betriebsmodi, automatisch in Menü einfügen
-    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ü
 
 Latex:
     Genaue Beschreibung von PulseCounter HardwareUnit wenn möglich

lib/Battery/battery.cpp

@@ -15,7 +15,7 @@ Battery::Battery(uint8_t pin, uint32_t r1, uint32_t r2) {
     this->pin = pin;
     this->r1 = r1;
     this->r2 = r2;
-    this->batteryVoltageFactor = (double) (this->r1 + this->r2)) / (double) this->r2
+    this->batteryVoltageFactor = (double) (this->r1 + this->r2) / (double) this->r2;
     this->initBuffer();
     this->loopDelay = 100;
 }
@@ -72,7 +72,7 @@ double Battery::calculateInputVoltage() {
 
 void Battery::calculateBatteryVoltage() {
     if (this->r1 && this->r2) {
-        this->batteryVoltage = (this->calculateInputVoltage() * this->batteryVoltageFactor;
+        this->batteryVoltage = this->calculateInputVoltage() * this->batteryVoltageFactor;
         return;
     }
 

lib/Sensors/sensorData.cpp

@@ -70,7 +70,32 @@ void SensorData::enableCalcCompass() {
 }
 
 void SensorData::enableGyroskop() {
+    this->gyroskop->initialize();
+    if (!this->gyroskop->testConnection()) {
+        std::cout << "SensorData::enableGyroskop: Gyroskop is not conntected. Freeze!" << std::endl;
+        while (true);
+    }
+
+    uint8_t deviceStatus = this->gyroskop->dmpInitialize();
+
+    this->gyroskop->setXGyroOffset(220);
+    this->gyroskop->setYGyroOffset(76);
+    this->gyroskop->setZGyroOffset(-85);
+    this->gyroskop->setZAccelOffset(1788);
     
+    if (deviceStatus == 0) {
+        this->gyroskop->CalibrateAccel(6);
+        this->gyroskop->CalibrateGyro(6);
+        this->gyroskop->PrintActiveOffsets();
+        this->gyroskop->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::enableGyroskop: DMP Initialization failed (code" << (int) deviceStatus <<"). Freeze!" << std::endl;
+        while (true);
+    }
 }
 
 CalcAzimuth::State SensorData::getCalcAzimuthState() const {
@@ -141,6 +166,12 @@ void SensorData::setOutputStatusPrintPVTdata(bool status) {
 void SensorData::run() {
     this->realCompass->read();
     this->realAzimuth = this->realCompass->getAzimuth();
+
+    if (this->gyroskop->dmpGetCurrentFIFOPacket(this->gyroBuffer)) {
+        this->gyroskop->dmpGetQuaternion(&this->quaternion, this->gyroBuffer);
+        this->gyroskop->dmpGetGravity(&this->gravity, &this->quaternion);
+        this->gyroskop->dmpGetYawPitchRoll(this->yawPitchRoll, &this->quaternion, &this->gravity);
+    }
 }
 
 void SensorData::runAsChild() {

lib/Sensors/sensorData.h

@@ -13,6 +13,7 @@
 #define SENSOR_DATA_H
 
 #include <SPI.h>
+#include <I2Cdev.h>
 #include <iostream>
 
 #include "component.h"
@@ -20,7 +21,7 @@
 
 #include <SparkFun_u-blox_GNSS_Arduino_Library.h>
 #include <QMC5883LCompass.h>
-// Gyroskop
+#include <MPU6050_6Axis_MotionApps20.h>
 #include "calcAzimuth.h"
 #include "ntripClient.h"
 
@@ -46,11 +47,12 @@ class SensorData : public Component {
 
         Point getCurrentPos() const { return this->currentPosition; }
         const UBX_NAV_PVT_data_t* getGnssData() const { return this->gnssData; };
-        const void* const getGyroData() const;
+        const float* getGyroData() const { return this->yawPitchRoll; }
 
         CalcAzimuth* getCalcCompass() const { return this->calcCompass; }
         QMC5883LCompass* getRealCompass() const { return this->realCompass; }
         NTRIPClient* getNtripClient() const { return this->ntripClient; }
+        MPU6050* getGyroskop() const { return this->gyroskop; }
 
         // static
         /**
@@ -72,9 +74,12 @@ class SensorData : public Component {
         CalcAzimuth* calcCompass = nullptr;
         SFE_UBLOX_GNSS* gnss = nullptr;
         NTRIPClient* ntripClient = nullptr;
+        MPU6050* gyroskop = nullptr;
 
         UBX_NAV_PVT_data_t* gnssData;
         Point currentPosition;
+        Quaternion quaternion;
+        VectorFloat gravity;
 
         char* host;
         char* mountPoint;
@@ -83,18 +88,19 @@ class SensorData : public Component {
         
         bool isNtripInit = false;
 
+        uint8_t gyroBuffer[64];
         uint16_t port;
         int16_t realAzimuth = INT16_MAX;
         int16_t calcAzimuth = INT16_MAX;
 
+        float yawPitchRoll[3];
+
         // static
         static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
         static void savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
 
         static UBX_NAV_PVT_data_t* ubxDataStatic;
-
         static uint32_t ubxUpdateTimeStatic;
-
         static bool outputStatusPrintPVTdata;
         static bool newData;
 };

platformio.ini

@@ -25,10 +25,8 @@ lib_deps =
 	mprograms/QMC5883LCompass@^1.2.0
 	https://git.kleiax.de/PlatformIO-Libs/Menu.git
 	nrf24/RF24@^1.4.5
+	jrowberg/I2Cdevlib-MPU6050@^1.0.0
 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
@@ -37,6 +35,7 @@ check_tool = clangtidy
 [env:native]
 platform = native
 test_ignore = test_embedded
+lib_deps = jrowberg/I2Cdevlib-MPU6050@^1.0.0
 
 [platformio]
 description = A Rover who should be drive a route by gps.

src/SpecialMenus/SensorData/menuSensorData.cpp

@@ -0,0 +1,45 @@
+/**
+ * @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(4) {
+        this->sensorData = sensorData;
+}
+
+void MenuSensorData::printPage() const {
+    String lineOne = "";
+    String lineTwo = "";
+
+    switch (this->getCurrentPage()) {
+        case 0:
+            lineOne = "Yaw:";
+            lineTwo.concat(this->sensorData->getGyroData()[0]);
+            break;
+
+        case 1:
+            lineOne = "Pitch:";
+            lineTwo.concat(this->sensorData->getGyroData()[1]);
+            break;
+
+        case 2:
+            lineOne = "Roll:";
+            lineTwo.concat(this->sensorData->getGyroData()[2]);
+            break;
+
+        default:
+            this->printDefault();
+            return;
+    }
+
+    this->print(lineOne, lineTwo);
+}

src/SpecialMenus/SensorData/menuSensorData.h

@@ -0,0 +1,28 @@
+/**
+ * @file menuSensorData.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @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"
+
+class MenuSensorData : public MenuInformationSites {
+    public:
+        MenuSensorData(SensorData* SensorData);
+
+    private:
+        void printPage() const override;
+
+        SensorData* sensorData;
+};
+
+#endif //MENU_SENSOR_DATA_H

src/driveModi/Modi/ManualControl/manualControl.cpp

@@ -32,7 +32,7 @@ void ManualControl::switchInputMode() {
 void ManualControl::analogControl() {
     // Have to be int16_t to avoid overflow (int8_t = 255 - 127 = -128)
     int16_t x = this->input->x - 127;
-    int16_t y = this-> input->y - 127;
+    int16_t y = this->input->y - 127;
 
     // static int counter = 0;
     // if (counter % 60 == 0) {

src/main.cpp

@@ -46,6 +46,7 @@
 #include "SpecialMenus/PID/menuPidSettings.h"
 #include "SpecialMenus/Route/menuRoute.h"
 #include "SpecialMenus/GPS/menuGPS.h"
+#include "SpecialMenus/SensorData/menuSensorData.h"
 
 MoveControl moveController;
 DriveManager* driveManager;
@@ -225,15 +226,18 @@ void makeMenu() {
     MenuCalibrateCompass* comp_m = new MenuCalibrateCompass(driveManager);
     MenuGPS* gps_m = new MenuGPS(sensorData);
     MenuSysteminformation* sys_m = new MenuSysteminformation(mainBattery);
+    MenuSensorData* sen_m = new MenuSensorData(sensorData);
     //TODO: Wie bekommt jeder die dumme Route?
     MenuRoute* rout_m = new MenuRoute(new Route());
 
     auto_m->setUpdateDelay(1000);
     gps_m->setUpdateDelay(1000);
     sys_m->setUpdateDelay(1500);
+    sen_m->setUpdateDelay(500);
 
     // Entry for the main menu
     main_m->addEntry(new MenuAction("Mode", mode_m));
+    main_m->addEntry(new MenuAction("Sensor", sen_m));
     main_m->addEntry(new MenuAction("GPS", gps_m));
     main_m->addEntry(new MenuAction("Route", rout_m));
     main_m->addEntry(new MenuAction("PID", pid_m));