 
 void sendDataToPC() {
   if (deviceConnected) {
-    updateSensorsData();
     DataPacket dataPackage;
     dataPackage.accelerometerX = myIMU.ax;
     dataPackage.accelerometerY = myIMU.ay;

    while (1);
  }
  // Set accelerometers low pass filter at 5Hz
  I2CwriteByte(MPU9250_ADDRESS, 29, 0x06);
  // Set gyroscope low pass filter at 5Hz
  I2CwriteByte(MPU9250_ADDRESS, 26, 0x06);
  // Configure gyroscope range
  I2CwriteByte(MPU9250_ADDRESS, 27, GYRO_FULL_SCALE_250_DPS);
  // Configure accelerometers range

               myIMU.magbias[1];
    myIMU.mz = (float)myIMU.magCount[2] * myIMU.mRes * myIMU.magCalibration[2] -
               myIMU.magbias[2];
    myIMU.updateTime();
    MahonyQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az, myIMU.gx * DEG_TO_RAD,
                           myIMU.gy * DEG_TO_RAD, myIMU.gz * DEG_TO_RAD, myIMU.my,
                           myIMU.mx, myIMU.mz, myIMU.deltat);
    myIMU.yaw   = atan2(2.0f * (*(getQ() + 1) * *(getQ() + 2) + *getQ()
                                * *(getQ() + 3)), *getQ() * *getQ() + * (getQ() + 1)
                        * *(getQ() + 1) - * (getQ() + 2) * *(getQ() + 2) - * (getQ() + 3)
                        * *(getQ() + 3));
    myIMU.pitch = -asin(2.0f * (*(getQ() + 1) * *(getQ() + 3) - *getQ()
                                * *(getQ() + 2)));
    myIMU.roll  = atan2(2.0f * (*getQ() * *(getQ() + 1) + * (getQ() + 2)
                                * *(getQ() + 3)), *getQ() * *getQ() - * (getQ() + 1)
                        * *(getQ() + 1) - * (getQ() + 2) * *(getQ() + 2) + * (getQ() + 3)
                        * *(getQ() + 3));
    myIMU.pitch *= RAD_TO_DEG;
    myIMU.yaw   *= RAD_TO_DEG;

    // Declination of SparkFun Electronics (40°05'26.6"N 105°11'05.9"W) is
    //   8° 30' E  ± 0° 21' (or 8.5°) on 2016-07-19
    // Latitude: 50° 26' 53" N
    // Longitude:  30° 30' 8" E
    // Date  Declination
    // 2017-02-15  7° 31' E  ± 0° 22'  changing by  0° 7' E per year
    // - http://www.ngdc.noaa.gov/geomag-web/#declination
    myIMU.yaw  -= 0.7;
    myIMU.roll *= RAD_TO_DEG;
  }
}


