Kütüphanesiz Roll, Pitch ve Yaw Hesabı

Programlama Kodları

//AÇIKLAMA
//Bu uygulamada jiroskop ve ivmeölçerden alınan verilerle Roll, Pitch ve Yaw hesabı yapılır.
//İlk başta "IMU_olcum_hatalari()" fonksiyonu ile jiroskop ve ivmeölçerden alınan 200'er adet
//verinin ortalaması alınarak Ofset(hata payı) değerleri bulunur ve okunan değerlerlerden
//çıkarılarak gerçek değerler bulunur.

#include <Wire.h>

const int modul_adres = 0x68;
float GyroX, GyroY, GyroZ;  //jiroskop değerleri için değişkenler
float AccX, AccY, AccZ;     //ivmeölçer değerleri için değişkenler
float gyroAngleX, gyroAngleY, gyroAngleZ; //jiroskop açı değişkenleri
float accAngleX, accAngleY; //ivmeölçer açı değişkenleri
float GyroErrorX, GyroErrorY, GyroErrorZ; //jiroskop Ofset(hata payı) değişkenleri
float AccErrorX, AccErrorY;  //ivmeölçer Ofset(hata payı) değişkenleri
float roll, pitch, yaw;
float simdiki_zaman, gecmis_zaman, gecen_zaman;
int i = 0;

void setup()
{
  Serial.begin(9600);
  Wire.begin(); //I2C seri iletişimini başlat

  //Modül resetleniyor
  Wire.beginTransmission(modul_adres);  //0x68 adresli modül ile iletişime geç
  Wire.write(0x6B); //0x6B adresli register'a git.
  Wire.write(0x00); //Register içeriğini sıfırla
  Wire.endTransmission();

  //Jiroskop ölçek ayarının varsayılan değeri olan ±250deg/s'yi kullanacağız
  //İvmeölçer ölçek ayarının varsayılan değeri olan ±2g'yi kullanacağız.

  //Dijital Alçak Geçiş Filtresinin (DLPF) Band Genişiliği belirleniyor
  Wire.beginTransmission(modul_adres); //0x68 slave adresli modül ile iletişime başla.
  Wire.write(0x1A);   //0x1A adresli register’a git.
  Wire.write(0x02);   //Register içeriğini 00000010 yap. İvmeölçer band genişliği 94 Hz ve jiroskop bant genişliği 98 Hz olsun.
  Wire.endTransmission();

  //Ölçüm hataları hesaplanıyor
  IMU_olcum_hatalari();

  delay(3000);
}

void loop()
{
  //_______________İvmeölçer Hesaplamaları______________
  //İvmeölçer değerleri okunuyor
  Wire.beginTransmission(modul_adres);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(modul_adres, 6, true);
  AccX = (Wire.read() << 8 | Wire.read()) / 16384.0;
  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0;
  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0;

  //İvmeölçerden okunan değerlerle açılar hesaplanıyor ve Ofsetler(hata payları) çıkarılıyor
  accAngleX = (atan2(AccY, sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - (-1.02);
  accAngleY = (atan2(-1 * AccX, sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) - (-6.98);

  //__________________Jiroskop Hesaplamaları________________
  //zaman hesapları yapılıyor. jiroskop açı hesabı için kullanılacak
  gecmis_zaman = simdiki_zaman;
  simdiki_zaman = millis(); //Arduino çalışmaya başladığından itibaren ms olarak geçen süre
  gecen_zaman = (simdiki_zaman - gecmis_zaman) / 1000; //geçen süre hesaplanıp sn'ye çevrildi

  //Jiroskop değerleri okunuyor
  Wire.beginTransmission(modul_adres);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(modul_adres, 6, true);
  GyroX = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;

  //Jiroskoptan okunan değerlerden Ofsetler(hata payları) çıkarılıyor
  GyroX = GyroX - (-0.18);
  GyroY = GyroY - (-0.60);
  GyroZ = GyroZ - (-0.32);

  //Jiroskop açıları hesaplanıyor
  gyroAngleX = gyroAngleX + GyroX * gecen_zaman;
  gyroAngleY = gyroAngleY + GyroY * gecen_zaman;
  gyroAngleZ = gyroAngleZ + GyroZ * gecen_zaman;

  //_________________Tamamlayıcı Filtre (Complementary Filter___________
  //Tamamlayıcı filtre ile Roll, Pitch, Yaw hesaplanıyor
  roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
  pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;
  yaw = gyroAngleZ;

  //________________________Seri Port Ekranına Yazdırma___________________
  Serial.print("Roll = ");  Serial.print(roll);   Serial.print(" | ");
  Serial.print("Pitch = "); Serial.print(pitch);  Serial.print(" | ");
  Serial.print("Yaw = ");   Serial.println(yaw);

  delay(200);
}

void IMU_olcum_hatalari()
{
  //__________________İvmeölçer Ölçüm Hatası_______________
  //İvmeölçerden ard arda 200 okuma yapılıp ortalaması alınıyor
  while (i < 200)
  {
    Wire.beginTransmission(modul_adres);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(modul_adres, 6, true);
    AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;

    AccErrorX  = AccErrorX + (atan2(AccY, sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI);
    AccErrorY  = AccErrorY + (atan2(-1 * AccX, sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI);

    i++;
  }
  AccErrorX = AccErrorX / 200;
  AccErrorY = AccErrorY / 200;
  i = 0;

  //__________________Jiroskop Ölçüm Hatası_______________
  //Jiroskoptan ard arda 200 okuma yapılıp ortalaması alınıyor
  while (i < 200)
  {
    Wire.beginTransmission(modul_adres);
    Wire.write(0x43);
    Wire.endTransmission(false);
    Wire.requestFrom(modul_adres, 6, true);
    GyroX = (Wire.read() << 8 | Wire.read()) / 131.0;
    GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
    GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;

    GyroErrorX = GyroErrorX + GyroX;
    GyroErrorY = GyroErrorY + GyroY;
    GyroErrorZ = GyroErrorZ + GyroZ;

    i++;
  }

  GyroErrorX = GyroErrorX / 200;
  GyroErrorY = GyroErrorY / 200;
  GyroErrorZ = GyroErrorZ / 200;

  //________________________Seri Port Ekranına Yazdırma___________________
  Serial.print("AccErrorX = ");   Serial.println(AccErrorX);
  Serial.print("AccErrorY = ");   Serial.println(AccErrorY);
  Serial.print("GyroErrorX = ");  Serial.println(GyroErrorX);
  Serial.print("GyroErrorY = ");  Serial.println(GyroErrorY);
  Serial.print("GyroErrorZ = ");  Serial.println(GyroErrorZ);
}

Devre Şeması