Проблема с пониманием кода (Arduino и MPU-6050)

Всем доброго дня
Совсем недавно начал работать с платой MPU-6050. Нашёл на просторах интернета работающую программы для чтения значений. Она работает, но хочется разобраться как. Конкретно не ясна работа функции read_mpu_6050_data(). Помогите пожалуйста разобраться)

void read_mpu_6050_data(){

Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68,14);
while(Wire.available() < 14);
acc_x = Wire.read()<<8|Wire.read();
acc_y = Wire.read()<<8|Wire.read();
acc_z = Wire.read()<<8|Wire.read();
temperature = 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();

}

 

Что не ясно?

Функция работает напрямую с I2C шиной. Устройство на шине с адресом 0x68.
Остальное нужно смотреть в описании MPU-6050.

 

Код (C++):

oid read_mpu_6050_data(){

Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68,14); // Прочитать 14 байт
while(Wire.available() < 14); // Ждем окончания чтения
acc_x = Wire.read()<<8|Wire.read(); // Объеденяем два байта в одно двухбайтное значение
acc_y = Wire.read()<<8|Wire.read();
acc_z = Wire.read()<<8|Wire.read();
temperature = 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();

}

 

"Объеденяем два байта в одно двухбайтное значение" Вот эта часть и не ясна. Зачем это делать ? И можно ли это сделать как-то по-другому ?

 

Двухбайтное число приходит двумя кусками. Мы его собираем

Как работает часть:

Код (C++):

Wire.read()<<8

Допустим получен байт
0x58

Код (C++):

0x58<<8 = 0x5800

Вертикальную палочку можно рассматривать как сложение
Допустим второй байт 0x9A

Код (C++):

0x5800 | 0x9A = 0x589A

Можно переписать так

Код (C++):

acc_x = Wire.read()*256 + Wire.read();

Но я не думаю, что так понятнее.

 

Огромное спасибо, теперь разобрался. А почему датчик не может отправить двухбайтное число сразу, не деля его на две части ?

 

Датчик отправляет 16 бит данных, после приема они складываются в буфер с побайтовой организацией памяти, как наиболее универсальной. Wire.read() возвращает данные из приемного буфера поэлементно, т.е. побайтово. Конечно, можно понаписать всяких Wire.readInt16(), Wire.readFloat() и т.п., чтобы не собирать многобайтные типы вручную, но это влияет только на удобство чтения кода. Внутри программы и то и другое просто копирует байты из одного места в другое.

 

Спасибо. Теперь и с этим разобрался. Да и в datasheet'е наконец то нашел нужную строчку)))
ADC Word Length Output in two’s complement format 16 bits.

 

Появятся ещё вопросы - задам

 

И вот снова проблемы...
На этот раз вот в этом кусочке:

//Place the MPU-6050 spirit level and note the values in the following two lines for calibration
angle_pitch_acc -= 0.0; //Accelerometer calibration value for pitch
angle_roll_acc -= 0.0; //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
}

Здесь не очень понятна часть после else

//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

А назначение этих двух строк и вовсе тайна для меня. Переменные с приставкой output встречаются только в этих двух строках. Получается это взаимодействие с переменной оставшейся с прошлого цикла ?

Заранее спасибо за помощь и простите, если вопросы глупые - я не так давно в этом варюсь)

 

А это программа полностью. Не знаю, поможет это или нет)

Код (C++):

//Include LCD and I2C library
#include <LiquidCrystal_I2C.h>
#include <Wire.h>

//Declaring some global variables
int gyro_x, gyro_y, gyro_z;
long acc_x, acc_y, acc_z, acc_total_vector;
int temperature;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
long loop_timer;
int lcd_loop_counter;
float angle_pitch, angle_roll;
int angle_pitch_buffer, angle_roll_buffer;
boolean set_gyro_angles;
float angle_roll_acc, angle_pitch_acc;
float angle_pitch_output, angle_roll_output;

//Initialize the LCD library
LiquidCrystal_I2C lcd(0x27,16,2);


void setup() {
  Wire.begin();                                                      
  //Serial.begin(57600);
  pinMode(13, OUTPUT);

  setup_mpu_6050_registers();                                          //Setup the registers of the MPU-6050 (500dfs / +/-8g) and start the gyro

  digitalWrite(13, HIGH);                

  lcd.begin();      
  lcd.backlight();    
  lcd.clear();

  lcd.setCursor(0,0);      
  lcd.print("  MPU-6050 IMU");      
  lcd.setCursor(0,1);      
  lcd.print("     V1.0");  

  delay(1500);    
  lcd.clear();  

  lcd.setCursor(0,0);  
  lcd.print("Calibrating gyro");
  lcd.setCursor(0,1);
  for (int cal_int = 0; cal_int < 2000 ; cal_int ++){                  //Run this code 2000 times
    if(cal_int % 125 == 0)lcd.print(".");                              
    read_mpu_6050_data();                                              //Read the raw acc and gyro data from the MPU-6050
    gyro_x_cal += gyro_x;                                              //Add the gyro x-axis offset to the gyro_x_cal variable
    gyro_y_cal += gyro_y;                                              //Add the gyro y-axis offset to the gyro_y_cal variable
    gyro_z_cal += gyro_z;                                              //Add the gyro z-axis offset to the gyro_z_cal variable
    delay(3);                                                          //Delay 3us to simulate the 250Hz program loop
  }
  gyro_x_cal /= 2000;                                                  //Divide the gyro_x_cal variable by 2000 to get the avarage offset
  gyro_y_cal /= 2000;                                                  //Divide the gyro_y_cal variable by 2000 to get the avarage offset
  gyro_z_cal /= 2000;                                                  //Divide the gyro_z_cal variable by 2000 to get the avarage offset

  lcd.clear();
  lcd.setCursor(0,0);  
  lcd.print("Pitch:");  
  lcd.setCursor(0,1);
  lcd.print("Roll :");

  digitalWrite(13, LOW);      
  loop_timer = micros();  
}

void loop(){

  read_mpu_6050_data();                                                //Read the raw acc and gyro data from the MPU-6050

  gyro_x -= gyro_x_cal;                                                //Subtract the offset calibration value from the raw gyro_x value
  gyro_y -= gyro_y_cal;                                                //Subtract the offset calibration value from the raw gyro_y value
  gyro_z -= gyro_z_cal;                                                //Subtract the offset calibration value from the raw gyro_z value

  //Gyro angle calculations
  //0.0000611 = 1 / (250Hz / 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

  //Place the MPU-6050 spirit level and note the values in the following two lines for calibration
  angle_pitch_acc -= 0.0;                                              //Accelerometer calibration value for pitch
  angle_roll_acc -= 0.0;                                               //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

  write_LCD();                                                         //Write the roll and pitch values to the LCD display

  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 read_mpu_6050_data(){  
  Wire.beginTransmission(0x68);  
  Wire.write(0x3B);
  Wire.endTransmission();    
  Wire.requestFrom(0x68,14);  
  while(Wire.available() < 14);
  acc_x = Wire.read()<<8|Wire.read();  
  acc_y = Wire.read()<<8|Wire.read();  
  acc_z = Wire.read()<<8|Wire.read();  
  temperature = 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();

}

void write_LCD(){                                                      //Subroutine for writing the LCD
  //To get a 250Hz program loop (4us) it's only possible to write one character per loop
  //Writing multiple characters is taking to much time
  if(lcd_loop_counter == 14)lcd_loop_counter = 0;                      //Reset the counter after 14 characters
  lcd_loop_counter ++;                                                 //Increase the counter
  if(lcd_loop_counter == 1){
    angle_pitch_buffer = angle_pitch_output * 10;                      //Buffer the pitch angle because it will change
    lcd.setCursor(6,0);                                                //Set the LCD cursor to position to position 0,0
  }
  if(lcd_loop_counter == 2){
    if(angle_pitch_buffer < 0)lcd.print("-");                          //Print - if value is negative
    else lcd.print("+");                                               //Print + if value is negative
  }
  if(lcd_loop_counter == 3)lcd.print(abs(angle_pitch_buffer)/1000);    //Print first number
  if(lcd_loop_counter == 4)lcd.print((abs(angle_pitch_buffer)/100)%10);//Print second number
  if(lcd_loop_counter == 5)lcd.print((abs(angle_pitch_buffer)/10)%10); //Print third number
  if(lcd_loop_counter == 6)lcd.print(".");                             //Print decimal point
  if(lcd_loop_counter == 7)lcd.print(abs(angle_pitch_buffer)%10);      //Print decimal number

  if(lcd_loop_counter == 8){
    angle_roll_buffer = angle_roll_output * 10;
    lcd.setCursor(6,1);
  }
  if(lcd_loop_counter == 9){
    if(angle_roll_buffer < 0)lcd.print("-");                           //Print - if value is negative
    else lcd.print("+");                                               //Print + if value is negative
  }
  if(lcd_loop_counter == 10)lcd.print(abs(angle_roll_buffer)/1000);    //Print first number
  if(lcd_loop_counter == 11)lcd.print((abs(angle_roll_buffer)/100)%10);//Print second number
  if(lcd_loop_counter == 12)lcd.print((abs(angle_roll_buffer)/10)%10); //Print third number
  if(lcd_loop_counter == 13)lcd.print(".");                            //Print decimal point
  if(lcd_loop_counter == 14)lcd.print(abs(angle_roll_buffer)%10);      //Print decimal number
}

void setup_mpu_6050_registers(){
  //Activate the MPU-6050
  Wire.beginTransmission(0x68);                                        
  Wire.write(0x6B);                                                    
  Wire.write(0x00);                                                  
  Wire.endTransmission();                                              
  //Configure the accelerometer (+/-8g)
  Wire.beginTransmission(0x68);                                      
  Wire.write(0x1C);                                                    
  Wire.write(0x10);                                                    
  Wire.endTransmission();                                              
  //Configure the gyro (500dps full scale)
  Wire.beginTransmission(0x68);                                        
  Wire.write(0x1B);                                                  
  Wire.write(0x08);                                                    
  Wire.endTransmission();                                              
}

 

По виду кода, это инклинометр на основе гироскопа и акселерометра, комплементарный фильтр короче Почитай тут http://www.poprobot.ru/theory/complementary_filter.

 

Что это такое и каков принцип работы комплиментарного фильтра я знаю, мне непонятны только те вопросы, которые я задал до программы...