implement gyroskop

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