Untitled

mail@pastecode.io avatar
unknown
c_cpp
2 years ago
4.0 kB
2
Indexable
Never
/*
    Fonksiyon isimlerini değiştirdikten sonra kod içerisinde isimleri değiştirmemişiz
    bunların değiştirilmesi gerekiyor.
*/

void MPU6050_ReadAccelData(MPU6050_TypeDef *mpu6050)
{

  uint8_t x_accel_high = i2c_read8RegisterByte(MPU6050_I2C_ADDR, MPU6050_ACCEL_XOUT_H);
  uint8_t x_accel_low = i2c_read8RegisterByte(MPU6050_I2C_ADDR, MPU6050_ACCEL_XOUT_L);

  uint8_t y_accel_high = i2c_read8RegisterByte(MPU6050_I2C_ADDR, MPU6050_ACCEL_YOUT_H);
  uint8_t y_accel_low = i2c_read8RegisterByte(MPU6050_I2C_ADDR, MPU6050_ACCEL_YOUT_L);

  uint8_t z_accel_high = i2c_read8RegisterByte(MPU6050_I2C_ADDR, MPU6050_ACCEL_ZOUT_H);
  uint8_t z_accel_low = i2c_read8RegisterByte(MPU6050_I2C_ADDR, MPU6050_ACCEL_ZOUT_L);

  mpu6050->accel_register_val[0] = (int16_t)(x_accel_high << 8 | x_accel_low);
  mpu6050->accel_register_val[1] = (int16_t)(y_accel_high << 8 | y_accel_low);
  mpu6050->accel_register_val[2] = (int16_t)(z_accel_high << 8 | z_accel_low);

  mpu6050->accel_data[0] = (float)(mpu6050->accel_register_val[0] / MPU6050_ACCEL_2G_SENS);
  mpu6050->accel_data[1] = (float)(mpu6050->accel_register_val[1] / MPU6050_ACCEL_2G_SENS);
  mpu6050->accel_data[2] = (float)(mpu6050->accel_register_val[2] / MPU6050_ACCEL_2G_SENS);
  
  mpu6050->accel_data[0] *= EARTH_GRAVITY;
  mpu6050->accel_data[1] *= EARTH_GRAVITY;
  mpu6050->accel_data[2] *= EARTH_GRAVITY;

  float accel_square = sqrt(pow(mpu6050->accel_data[0], 2) + pow(mpu6050->accel_data[1], 2) + pow(mpu6050->accel_data[2], 2));

  mpu6050->accel_data[0] = asin(mpu6050->accel_data[0] / accel_square) * SENSORS_RADS_TO_DPS;
  mpu6050->accel_data[1] = asin(mpu6050->accel_data[1] / accel_square) * SENSORS_RADS_TO_DPS;
  mpu6050->accel_data[2] = 0;
    
  /*    
    Kaibrasyon verilerin okunduktan sonra yanlış veriden çıkarttık 
    ve yanlış yerde çağırdık. Doğrusu bu şekilde olmalı çünkü bizim
    kalibrasyon değerlerinde topladığımız değer açı değerleriydi 
    register değerleri değil. Burada çağırmamızın sebebi ise açı değerleri
    27,28,29. satırlarda elde ediliyor o sebeple burada çağırdık. Kısacası
    kodu bu şekilde güncellemen gerekiyor. 
  */
  mpu6050->accel_data[0] -= mpu6050->accel_cal_val[0];
  mpu6050->accel_data[1] -= mpu6050->accel_cal_val[1];
}

void MPU6050_ReadGyroData(MPU6050_TypeDef *mpu6050)
{
  mpu6050->previous_time = mpu6050->current_time;
  mpu6050->current_time = millis();
  mpu6050->elapsed_time = (float)(mpu6050->current_time - mpu6050->previous_time);
  mpu6050->elapsed_time /= 1000;

  uint8_t x_gyro_high = i2c_read8RegisterByte(MPU6050_I2C_ADDR, MPU6050_GYRO_XOUT_H);
  uint8_t x_gyro_low = i2c_read8RegisterByte(MPU6050_I2C_ADDR, MPU6050_GYRO_XOUT_L);

  uint8_t y_gyro_high = i2c_read8RegisterByte(MPU6050_I2C_ADDR, MPU6050_GYRO_YOUT_H);
  uint8_t y_gyro_low = i2c_read8RegisterByte(MPU6050_I2C_ADDR, MPU6050_GYRO_YOUT_L);

  uint8_t z_gyro_high = i2c_read8RegisterByte(MPU6050_I2C_ADDR, MPU6050_GYRO_ZOUT_H);
  uint8_t z_gyro_low = i2c_read8RegisterByte(MPU6050_I2C_ADDR, MPU6050_GYRO_ZOUT_L);

  mpu6050->gyro_register_val[0] = (int16_t)(x_gyro_high << 8 | x_gyro_low);
  mpu6050->gyro_register_val[1] = (int16_t)(y_gyro_high << 8 | y_gyro_low);
  mpu6050->gyro_register_val[2] = (int16_t)(z_gyro_high << 8 | z_gyro_low);

  float period = 1 / mpu6050->elapsed_time;

  mpu6050->gyro_data[0] = (float)(((mpu6050->gyro_register_val[0] / MPU6050_GYRO_250_SENS) / period));
  mpu6050->gyro_data[1] = (float)(((mpu6050->gyro_register_val[1] / MPU6050_GYRO_250_SENS) / period));
  mpu6050->gyro_data[2] = (float)(((mpu6050->gyro_register_val[2] / MPU6050_GYRO_250_SENS) / period));
    
  /*    
    Accelerometer için yazılan her şey burada da geçerli. 
  */
  mpu6050->gyro_data[0] -= mpu6050->gyro_cal_val[0];
  mpu6050->gyro_data[1] -= mpu6050->gyro_cal_val[1];
  mpu6050->gyro_data[2] -= mpu6050->gyro_cal_val[2];
}