 
 uint32_t count = 0;  // used to control display output rate
 uint32_t count2 = 0;  // used to control display output rate
+uint32_t count3 = 5000;  // used to control display output rate
 uint32_t delt_t = 0; // used to control display output rate
 float pitch, yaw, roll, heading;
 float deltat = 0.0f;        // integration interval for both filter schemes

  float gxm = dof.calcGyro(GX.mean()) - gbias[0];
  float gym = dof.calcGyro(GY.mean()) - gbias[1];
  float gzm = dof.calcGyro(GZ.mean()) - gbias[2];
  MadgwickQuaternionUpdate(axm, aym, azm, gxm*PI/180.0f, gym*PI/180.0f, gzm*PI/180.0f, mx, my, mz);
   //MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);
 //MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);

    // Serial print and/or display at 0.5 s rate independent of data rates

    
    
    //Serial.print((int)1000*az); Serial.print("\t"); // Raw
    /*Serial.print(1000*(dof.calcAccel(AX.mean()) - abias[0])); Serial.print("\t");
    Serial.print(1000*(dof.calcAccel(AY.mean()) - abias[1])); Serial.print("\t");
    Serial.print(1000*(dof.calcAccel(AZ.mean()) - abias[2])); Serial.print("\t");*/
    
    Serial.print(1000*ax); Serial.print("\t");
    Serial.print(1000*ay); Serial.print("\t");
    Serial.print(1000*az); Serial.print("\t");
    
    //printGforce();
    
