         Serial.print("\t");
         Serial.println(rr[2]);
         */
+        
+        // read raw heading measurements from device
+        mag.getHeading(&mx16, &my16, &mz16);
+        mx16-=60;
+        my16-=261;
+        mz16+=334;
         mx = mx16;
         my = my16;
         mz = mz16;
-        /*
-        Serial.print("m\t");
+        
+        /*Serial.print("m\t");
         Serial.print(mx);
         Serial.print("\t");
         Serial.print(my);
         Serial.print("\t");
-        Serial.println(mz);
-        */
-        // blink LED to indicate activity
-        blinkState = !blinkState;
-        digitalWrite(LED_PIN, blinkState);
+        Serial.println(mz);*/
+        
+        Now = micros();
+        deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update
+        lastUpdate = Now;
+        MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180*250.0f/32768.0*100, gy*PI/180.0*250.0f/32768.0*100, gz*PI/180.0*250.0f/32768.0*100,  mx,  my,  mz);
+        
+
     }
 }
 void sendData()

        // .
        // .
        // .
        if (millis() > last_time + UPDATE_RATE)
         { 
               Now = micros();
               deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update
               lastUpdate = Now;
               MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180*250.0f/32768.0, gy*PI/180.0*250.0f/32768.0, gz*PI/180.0*250.0f/32768.0,  mx,  my,  mz);
               sendData();
               last_time = millis();
         }

    mpu.setI2CMasterModeEnabled(0);
    mpu.setI2CBypassEnabled(1);
}
// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================
