 uint8_t system_id = 100;
 uint8_t component_id = 200;
 uint32_t time_boot_ms;
-uint32_t last_mag_read_ms;
+uint32_t last_baro_read_ms;
 
 //MS5611
 MS561101BA baro = MS561101BA();

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

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

      // 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!
    }
}
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()
{
  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;
    }
  }
  
  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
    accelGyroMag.setFullScaleGyroRange(n_gyro_range); // set gyro range to 250 degrees/sec
    n_gyro_range_old = n_gyro_range;
  }
}
void automaticAccelRangeSelection()
{
  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
    accelGyroMag.setFullScaleAccelRange(n_accel_range);
    n_accel_range_old = n_accel_range;
  }
}
