       // stabilization control of a fast-moving robot or quadcopter. Compare to the update rate of 200 Hz
       // produced by the on-board Digital Motion Processor of Invensense's MPU6050 6 DoF and MPU9150 9DoF sensors.
       // The 3.3 V 8 MHz Pro Mini is doing pretty well!
-  
-      count = millis();
     }
+}
+void getBaroData()
+{
+    temperature = baro.getTemperature(MS561101BA_OSR_4096);
+    press = baro.getPressure(MS561101BA_OSR_4096);
+    pushAvg(press);
+    press = getAvg(movavg_buff, MOVAVG_SIZE);
+    altitude = getAltitude(press, temperature);
 }
 void automaticGyroRangeSelection()
 {
-  if ((fabs(gx_degps)<0.2*GYRO_RANGE_DEGPS)&&(fabs(gy_degps)<0.2*GYRO_RANGE_DEGPS)&&(fabs(gz_degps)<0.2*GYRO_RANGE_DEGPS))
-  {
-    n_gyro_range = 0;
+  for (int i =0; i<=2; i++)
+  {  
+    if (max(max(fabs(gx_degps),fabs(gy_degps)),fabs(gz_degps)) <0.2*GYRO_RANGE_DEGPS*pow(2, i))
+    {
+      n_gyro_range = i;
+    }
+    else
+    {
+      n_gyro_range = i+1;
+    }
   }
-  else   if ((fabs(gx_degps)<0.2*GYRO_RANGE_DEGPS*2)&&(fabs(gy_degps)<0.2*GYRO_RANGE_DEGPS*2)&&(fabs(gz_degps)<0.2*GYRO_RANGE_DEGPS*2))
-  {
-    n_gyro_range = 1;
-  }
-  else   if ((fabs(gx_degps)<0.2*GYRO_RANGE_DEGPS*4)&&(fabs(gy_degps)<0.2*GYRO_RANGE_DEGPS*4)&&(fabs(gz_degps)<0.2*GYRO_RANGE_DEGPS*4))
-  {
-    n_gyro_range = 2;
-  }
-  else
-  {
-    n_gyro_range = 3;
-  }
-  
   
   if (n_gyro_range!=n_gyro_range_old)
   {
     // Full-scale range of the gyro sensors:
     // 0 = +/- 250 degrees/sec, 1 = +/- 500 degrees/sec, 2 = +/- 1000 degrees/sec, 3 = +/- 2000 degrees/sec
-    mpu.setFullScaleGyroRange(n_gyro_range); // set gyro range to 250 degrees/sec
+    accelGyroMag.setFullScaleGyroRange(n_gyro_range); // set gyro range to 250 degrees/sec
     n_gyro_range_old = n_gyro_range;
   }
 }
 void automaticAccelRangeSelection()
 {
-  if ((fabs(ax_g)<0.2*ACCEL_RANGE_G)&&(fabs(ay_g)<0.2*ACCEL_RANGE_G)&&(fabs(az_g)<0.2*ACCEL_RANGE_G))
-  {
-    n_accel_range = 0;
-  }
-  else   if ((fabs(ax_g)<0.2*ACCEL_RANGE_G*2)&&(fabs(ay_g)<0.2*ACCEL_RANGE_G*2)&&(fabs(az_g)<0.2*ACCEL_RANGE_G*2))
-  {
-    n_accel_range = 1;
-  }
-  else   if ((fabs(ax_g)<0.2*ACCEL_RANGE_G*4)&&(fabs(ay_g)<0.2*ACCEL_RANGE_G*4)&&(fabs(az_g)<0.2*ACCEL_RANGE_G*4))
-  {
-    n_accel_range = 2;
-  }
-  else
-  {
-    n_accel_range = 3;
+  for (int i =0; i<=2; i++)
+  {  
+    if (max(max(fabs(ax_g),fabs(ay_g)),fabs(az_g)) <0.2*ACCEL_RANGE_G*pow(2, i))
+    {
+      n_accel_range = i;
+    }
+    else
+    {
+      n_accel_range = i+1;
+    }
   }
   
   if (n_accel_range!=n_accel_range_old)
   {
   // Full-scale accelerometer range.
 // The full-scale range of the accelerometer: 0 = +/- 2g, 1 = +/- 4g, 2 = +/- 8g, 3 = +/- 16g
-    mpu.setFullScaleAccelRange(n_accel_range);
+    accelGyroMag.setFullScaleAccelRange(n_accel_range);
     n_accel_range_old = n_accel_range;
   }
 }

 * 5        | 10Hz      | 13.8ms | 10Hz      | 13.4ms | 1kHz
 * 6        | 5Hz       | 19.0ms | 5Hz       | 18.6ms | 1kHz
 */
  mpu.setDLPFMode(4); // set bandwidth of both gyro and accelerometer to ~20 Hz

// Full-scale range of the gyro sensors:
// 0 = +/- 250 degrees/sec, 1 = +/- 500 degrees/sec, 2 = +/- 1000 degrees/sec, 3 = +/- 2000 degrees/sec
  mpu.setFullScaleGyroRange(0); // set gyro range to 250 degrees/sec

// Full-scale accelerometer range.
// The full-scale range of the accelerometer: 0 = +/- 2g, 1 = +/- 4g, 2 = +/- 8g, 3 = +/- 16g
  mpu.setFullScaleAccelRange(0); // set accelerometer to 2 g range

  mpu.setIntDataReadyEnabled(true); // enable data ready interrupt
  
  // Suppose that the CSB pin is connected to GND.
  // You'll have to check this on your breakout schematics

            qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;

            // Integrate to yield quaternion
            q1 += qDot1 * deltat_s;
            q2 += qDot2 * deltat_s;
            q3 += qDot3 * deltat_s;
            q4 += qDot4 * deltat_s;
            norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
            norm = 1.0f/norm;
            q[0] = q1 * norm;
