implement gyroskop

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