MPU6050 - Hilfe bei Winkelbestimmung

#include <Wire.h>

int gyro_x, gyro_y, gyro_z;

int angle_pitch_buffer, angle_roll_buffer;

int temp;

int count = 0;

long gyro_x_cal, gyro_y_cal, gyro_z_cal;

long acc_x, acc_y, acc_z, acc_total_vector;

long loop_timer;

long acc_x_average, acc_y_average, acc_z_average;

long gyro_x_offset = 0;

long gyro_y_offset = 0;

long gyro_z_offset = 0;

long acc_x_offset = 0; // 10

long acc_y_offset = 0; // 180

long acc_z_offset = 0; // -225

boolean set_gyro_angles;

boolean toggle;

float angle_roll_acc, angle_pitch_acc;

float angle_pitch, angle_roll;

float angle_pitch_output, angle_roll_output;

void setup() 

{

  Wire.begin();                                                        //Start I2C as master

  Serial.begin(115200);

  setup_mpu_6050_registers();                                          //Setup the registers of the MPU-6050 

  Serial.println("Calibrating gyro");

  for(int cal_int = 0; cal_int < 500 ; cal_int ++)

  {                                                                    //Read the raw acc and gyro data from the MPU-6050 for 500 times

    read_mpu_6050_data();                                             

    gyro_x_cal += gyro_x;                                              //Add the gyro x offset to the gyro_x_cal variable

    gyro_y_cal += gyro_y;                                              //Add the gyro y offset to the gyro_y_cal variable

    gyro_z_cal += gyro_z;                                              //Add the gyro z offset to the gyro_z_cal variable

    delay(3);                                                          //Delay 3us to have 250Hz for-loop

  }

  pinMode(8, OUTPUT);

  pinMode(9, OUTPUT);

  pinMode(10, OUTPUT);

  // divide by 500 to get avarage offset

  gyro_x_cal /= 500;                                                 

  gyro_y_cal /= 500;                                                 

  gyro_z_cal /= 500;                                                 

  Serial.begin(115200);

  loop_timer = micros();                                               //Reset the loop timer

}

void loop(){

  read_mpu_6050_data();   

  //Subtract the offset values from the raw gyro values

  gyro_x -= gyro_x_cal;                                                

  gyro_y -= gyro_y_cal;                                                

  gyro_z -= gyro_z_cal;

  acc_x += acc_x_offset;

  acc_y += acc_y_offset; 

  acc_z += acc_z_offset;                                               

  //Gyro angle calculations . Note 0.0000611 = 1 / (250Hz x 65.5)

  angle_pitch += gyro_x * 0.0000611;                                   //Calculate the traveled pitch angle and add this to the angle_pitch variable

  angle_roll += gyro_y * 0.0000611;                                    //Calculate the traveled roll angle and add this to the angle_roll variable

  //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians

  angle_pitch += angle_roll * sin(gyro_z * 0.000001066);               //If the IMU has yawed transfer the roll angle to the pitch angel

  angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);               //If the IMU has yawed transfer the pitch angle to the roll angel

  //Accelerometer angle calculations

  acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z));  //Calculate the total accelerometer vector

  //57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians

  angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296;       //Calculate the pitch angle

  angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296;       //Calculate the roll angle

  angle_pitch_acc -= -2.04;                                            //Accelerometer calibration value for pitch

  angle_roll_acc -= 0.38;                                              //Accelerometer calibration value for roll

  if(set_gyro_angles)

  {                                                                    //If the IMU is already started

    angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004;     //Correct the drift of the gyro pitch angle with the accelerometer pitch angle

    angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;        //Correct the drift of the gyro roll angle with the accelerometer roll angle

  }

  else

  {                                                                    //At first start

    angle_pitch = angle_pitch_acc;                                     //Set the gyro pitch angle equal to the accelerometer pitch angle 

    angle_roll = angle_roll_acc;                                       //Set the gyro roll angle equal to the accelerometer roll angle 

    set_gyro_angles = true;                                            //Set the IMU started flag

  }

  //To dampen the pitch and roll angles a complementary filter is used

  angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1;   //Take 90% of the output pitch value and add 10% of the raw pitch value

  angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1;      //Take 90% of the output roll value and add 10% of the raw roll value

  acc_x_average += acc_x;

  acc_y_average += acc_y;

  acc_z_average += acc_z;

  count++;

  if(count == 100)

  {  

  Serial.print(acc_x_average/100);

  Serial.print("\t");

  Serial.print(acc_y_average/100);

  Serial.print("\t");

  Serial.print(acc_z_average/100);

  Serial.print("\t\t");

  Serial.print(angle_pitch_output);

  Serial.print("\t");

  Serial.println(angle_roll_output);

  count = 0;

  acc_x_average = 0;

  acc_y_average = 0;

  acc_z_average = 0;

  }

  // half frequency output

  toggle = !toggle; if(!toggle) { PORTB |= (1 << PB2); } else { PORTB &= ~(1 << PB2); }

  if(angle_pitch_output <= 90.1 && angle_pitch_output >= 89.9) { digitalWrite(8, HIGH); } else { digitalWrite(8, LOW); }

  if(angle_roll_output <= 90.1 && angle_roll_output >= 89.9) { digitalWrite(9, HIGH); } else { digitalWrite(9, LOW); }

  while(micros() - loop_timer < 4000);                                 //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop

  loop_timer = micros();//Reset the loop timer 

}

void setup_mpu_6050_registers()

{

  //Activate the MPU-6050

  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050

  Wire.write(0x6B);                                                    //Send the requested starting register

  Wire.write(0x00);                                                    //Set the requested starting register

  Wire.endTransmission();                                             

  //Configure the accelerometer (+/-8g)

  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050

  Wire.write(0x1C);                                                    //Send the requested starting register

  Wire.write(0x10);                                                    //Set the requested starting register

  Wire.endTransmission();                                             

  //Configure the gyro (500dps full scale)

  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050

  Wire.write(0x1B);                                                    //Send the requested starting register

  Wire.write(0x08);                                                    //Set the requested starting register

  Wire.endTransmission();                                             

}

void read_mpu_6050_data()

{                                                                       //Subroutine for reading the raw gyro and accelerometer data

  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050

  Wire.write(0x3B);                                                    //Send the requested starting register

  Wire.endTransmission();                                              //End the transmission

  Wire.requestFrom(0x68,14);                                           //Request 14 bytes from the MPU-6050

  while(Wire.available() < 14);                                        //Wait until all the bytes are received

  acc_x = Wire.read()<<8|Wire.read();                                  

  acc_y = Wire.read()<<8|Wire.read();                                  

  acc_z = Wire.read()<<8|Wire.read();                                  

  temp = Wire.read()<<8|Wire.read();                                   

  gyro_x = Wire.read()<<8|Wire.read();                                 

  gyro_y = Wire.read()<<8|Wire.read();                                 

  gyro_z = Wire.read()<<8|Wire.read();                                 

}