implement gyroskop

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() {