   dT = ( (double) accel_t_gyro.value.temperature + 12412.0) / 340.0;
   Serial.print(dT, 3);
   Serial.print(F(" degrees Celsius"));
-  Serial.println(F(""));*/
+  Serial.println(F(""));
  
  
   // Print the raw gyro values.

    flag=6;
    else
      flag++;
  //delay(5000);           // wait 5 seconds for next scan
}



    int16_t x_accel;
    int16_t y_accel;
    int16_t z_accel;
    int16_t x_gyro;
    int16_t y_gyro;
    int16_t z_gyro;
  } value;
};

int read_gyro_accel_vals(uint8_t* accel_t_gyro_ptr) {
  // Read the raw values.
  // Read 14 bytes at once, 
  // containing acceleration, temperature and gyro.
  // With the default settings of the MPU-6050,
  // there is no filter enabled, and the values
  // are not very stable.  Returns the error value
  
  accel_t_gyro_union* accel_t_gyro = (accel_t_gyro_union *) accel_t_gyro_ptr;
   
  int error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) accel_t_gyro, sizeof(*accel_t_gyro));

  // Swap all high and low bytes.
  // After this, the registers values are swapped, 
  // so the structure name like x_accel_l does no 
  // longer contain the lower byte.
  uint8_t swap;
  #define SWAP(x,y) swap = x; x = y; y = swap

  SWAP ((*accel_t_gyro).reg.x_accel_h, (*accel_t_gyro).reg.x_accel_l);
  SWAP ((*accel_t_gyro).reg.y_accel_h, (*accel_t_gyro).reg.y_accel_l);
  SWAP ((*accel_t_gyro).reg.z_accel_h, (*accel_t_gyro).reg.z_accel_l);
  SWAP ((*accel_t_gyro).reg.x_gyro_h, (*accel_t_gyro).reg.x_gyro_l);
  SWAP ((*accel_t_gyro).reg.y_gyro_h, (*accel_t_gyro).reg.y_gyro_l);
  SWAP ((*accel_t_gyro).reg.z_gyro_h, (*accel_t_gyro).reg.z_gyro_l);

  return error;
}

unsigned long last_read_time[5];
float         last_x_angle[5];  // These are the filtered angles
float         last_y_angle[5];
float         last_z_angle[5];  
float         last_gyro_x_angle[5];  // Store the gyro angles to compare drift
float         last_gyro_y_angle[5];
float         last_gyro_z_angle[5];


void set_last_read_angle_data(unsigned long time, float x, float y, float z, float x_gyro, float y_gyro, float z_gyro, int a) {
  last_read_time[a] = time;
  last_x_angle[a] = x;
  last_y_angle[a] = y;
  last_z_angle[a] = z;
  last_gyro_x_angle[a] = x_gyro;
  last_gyro_y_angle[a] = y_gyro;
  last_gyro_z_angle[a] = z_gyro;
}

inline unsigned long get_last_time(int a) {return last_read_time[a];}
inline float get_last_x_angle(int a) {return last_x_angle[a];}
inline float get_last_y_angle(int a) {return last_y_angle[a];}
inline float get_last_z_angle(int a) {return last_z_angle[a];}
inline float get_last_gyro_x_angle(int a) {return last_gyro_x_angle[a];}
inline float get_last_gyro_y_angle(int a) {return last_gyro_y_angle[a];}
inline float get_last_gyro_z_angle(int a) {return last_gyro_z_angle[a];}

float    base_x_accel[5];
float    base_y_accel[5];
float    base_z_accel[5];

float    base_x_gyro[5];
float    base_y_gyro[5];
float    base_z_gyro[5];


const int MPU_addr=0x69;  // I2C address of the MPU-6050
