 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();

      // 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 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;
  }
  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
    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;
  }
  
  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);
    n_accel_range_old = n_accel_range;
  }
}

#define Ki 0.0f

int16_t a1, a2, a3, g1, g2, g3, m1, m2, m3;     // raw data arrays reading
uint16_t last_output_ms = 0;  // used to control display output rate
uint16_t delt_t_ms = 0; // used to control display output rate
uint8_t BaroRate;     // read rate for magnetometer data

float pitch, yaw, roll;
float deltat_s = 0.0f;        // integration interval for both filter schemes
uint64_t lastUpdate_us = 0; // used to calculate integration interval
uint64_t time_boot_us = 0;        // used to calculate integration interval

float ax_g, ay_g, az_g, gx_degps, gy_degps, gz_degps, mx, my, mz; // variables to hold latest sensor data values 
