Arduino.ru

Решил далее замахнуться на более сложнныи модуль MPU 9250/6500 ( GY-6500.GY-9250).. Полазил в интернете, попросил, дали код 

#include <Wire.h>

#define    MPU9250_ADDRESS            0x68
#define    MAG_ADDRESS                0xC
#define AK8963_ADDRESS                0xC
#define AK8963_ST1                    0x02

#define    GYRO_FULL_SCALE_250_DPS    0x00
#define    GYRO_FULL_SCALE_500_DPS    0x08
#define    GYRO_FULL_SCALE_1000_DPS   0x10
#define    GYRO_FULL_SCALE_2000_DPS   0x18

#define    ACC_FULL_SCALE_2_G        0x00
#define    ACC_FULL_SCALE_4_G        0x08
#define    ACC_FULL_SCALE_8_G        0x10
#define    ACC_FULL_SCALE_16_G       0x18
#define    Pi                        3,1415926

#define WHO_AM_I_MPU9250             0x75


enum Mscale {
  MFS_14BITS = 0, // 0.6 mG per LSB
  MFS_16BITS      // 0.15 mG per LSB
};

uint8_t Mmode = 0x06;
uint8_t Mscale = MFS_16BITS;
float aRes, gRes, mRes;


float X ;
float Y ;
float Z ;



void getMres() {
  switch (Mscale)
  {
  // Possible magnetometer scales (and their register bit settings) are:
  // 14 bit resolution (0) and 16 bit resolution (1)
    case MFS_14BITS:
          mRes = 10.*4912./8190.; // Proper scale to return milliGauss
          break;
    case MFS_16BITS:
          mRes = 10.*4912./32760.0; // Proper scale to return milliGauss
          break;
  }
}

// This function read Nbytes bytes from I2C device at address Address.
// Put read bytes starting at register Register in the Data array.
void I2Cread(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data)
{
  // Set register address
  Wire.beginTransmission(Address);
  Wire.write(Register);
  Wire.endTransmission(false);

  // Read Nbytes
  Wire.requestFrom(Address, Nbytes);
  uint8_t index=0;
  while (Wire.available())
    Data[index++]=Wire.read();
}


// Write a byte (Data) in device (Address) at register (Register)
void I2CwriteByte(uint8_t Address, uint8_t Register, uint8_t Data)
{
  // Set register address
  Wire.beginTransmission(Address);
  Wire.write(Register);
  Wire.write(Data);
  Wire.endTransmission();
}


float xv, yv, zv;

//calibrated_values[3] is the global array where the calibrated data will be placed
//calibrated_values[3]: [0]=Xc, [1]=Yc, [2]=Zc
float calibrated_values[3]; 
//transformation(float uncalibrated_values[3]) is the function of the magnetometer data correction
//uncalibrated_values[3] is the array of the non calibrated magnetometer data
//uncalibrated_values[3]: [0]=Xnc, [1]=Ync, [2]=Znc
void transformation(float uncalibrated_values[3])  
{
  //calibration_matrix[3][3] is the transformation matrix
  //replace M11, M12,..,M33 with your transformation matrix data
  double calibration_matrix[3][3] =
  {
    {0.677010, 0.025999, 6.147725},// Матрица пока не верная но уже работает!
    {0.025999, 0.860586, 0.093041},
    {-0.010483, 0.093041, 0.649263}
  };
  //bias[3] is the bias
  //replace Bx, By, Bz with your bias data
  double bias[3] =
  {
    209.429645,
    836.883938,
    795.251581
  };
  //calculation
  for (int i=0; i<3; ++i) uncalibrated_values[i] = uncalibrated_values[i] - bias[i];
  float result[3] = {0, 0, 0};
  for (int i=0; i<3; ++i)
    for (int j=0; j<3; ++j)
      result[i] += calibration_matrix[i][j] * uncalibrated_values[j];
  for (int i=0; i<3; ++i) calibrated_values[i] = result[i];
}

//vector_length_stabilasation() - is the function of the magnetometer vector length stabilasation (stabilisation of the sphere radius)
float scaler;
boolean scaler_flag = false;
float normal_vector_length;
void vector_length_stabilasation(){
  //calculate the normal vector length
  if (scaler_flag == false)
  {
    getHeading();
    normal_vector_length = sqrt(calibrated_values[0]*calibrated_values[0] + calibrated_values[1]*calibrated_values[1] + calibrated_values[2]*calibrated_values[2]);
    scaler_flag = true;
  }
  //calculate the current scaler
  scaler = normal_vector_length/sqrt(calibrated_values[0]*calibrated_values[0] + calibrated_values[1]*calibrated_values[1] + calibrated_values[2]*calibrated_values[2]);
  //apply the current scaler to the calibrated coordinates (global array calibrated_values)
  calibrated_values[0] = calibrated_values[0]*scaler;
  calibrated_values[1] = calibrated_values[1]*scaler;
  calibrated_values[2] = calibrated_values[2]*scaler;
}

// Initializations
void setup()
{
  // Arduino initializations
  Wire.begin();
  Serial.begin(115200);

  // Configure gyroscope range
  I2CwriteByte(MPU9250_ADDRESS,27,GYRO_FULL_SCALE_2000_DPS);
  // Configure accelerometers range
  I2CwriteByte(MPU9250_ADDRESS,28,ACC_FULL_SCALE_16_G);
  // Set by pass mode for the magnetometers
  I2CwriteByte(MPU9250_ADDRESS,0x37,0x02);

  // Request first magnetometer single measurement
  I2CwriteByte(MAG_ADDRESS,0x0A,Mscale << 4 | Mmode );//Mscale << 4 | Mmode

}


long int cpt=0;
// Main loop, read and display data
void loop()
{

  // _______________
  // ::: Counter :::

  // Display data counter
  //Serial.print (cpt++,DEC);
  //Serial.print ("\t");

  // ____________________________________
  // :::  accelerometer and gyroscope :::

  // Read accelerometer and gyroscope
  uint8_t Buf[14];
  I2Cread(MPU9250_ADDRESS,0x3B,14,Buf);


  // Create 16 bits values from 8 bits data

  // Accelerometer
  int16_t ax=-(Buf[0]<<8 | Buf[1]);
  int16_t ay=-(Buf[2]<<8 | Buf[3]);
  int16_t az=Buf[4]<<8 | Buf[5];

  // Gyroscope
  int16_t gx=-(Buf[8]<<8 | Buf[9]);
  int16_t gy=-(Buf[10]<<8 | Buf[11]);
  int16_t gz=Buf[12]<<8 | Buf[13];

    // Display values

   //Accelerometer
  //Serial.print ("Значения акселерометра");
  //Serial.print ("\t");
  Serial.print (ax,DEC);
  Serial.print (",");
  Serial.print (ay,DEC);
  Serial.print (",");
  Serial.print (az,DEC);
  Serial.print (",");

   //Gyroscope
  //Serial.print ("Значения гироскопа");
  //Serial.print ("\t");
  Serial.print (gx,DEC);
  Serial.print (",");
  Serial.print (gy,DEC);
  Serial.print (",");
  Serial.print (gz,DEC);
  Serial.print (",");


  // _____________________
  // :::  Magnetometer :::



   //Read register Status 1 and wait for the DRDY: Data Ready

  //uint8_t ST1;
  //do
  //{
   // I2Cread(MAG_ADDRESS,0x02,1,&ST1);
  //}
     // while (!(ST1&0x01));

   

  if(readByte(MAG_ADDRESS, 0x02)& 0x01); {
  // Read magnetometer data
  uint8_t Mag[7];
  //readBytes(AK8963_ADDRESS, 0x03, 7, &Mag[0]);

  I2Cread(MAG_ADDRESS,0x03,7,&Mag[0]);

  uint8_t c = Mag[6];

if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
   
 
  // Create 16 bits values from 8 bits data

  // Magnetometer
  int16_t mx=-(Mag[3]<<8 | Mag[2]);
  int16_t my=-(Mag[1]<<8 | Mag[0]);
  int16_t mz=-(Mag[5]<<8 | Mag[4]);





  X = mx;
  Y = my;
  Z = mz;





//float values_from_magnetometer[3];

// getHeading();
// values_from_magnetometer[0] = xv;
  //values_from_magnetometer[1] = yv;
  //values_from_magnetometer[2] = zv;
  //transformation(values_from_magnetometer);

  //vector_length_stabilasation();

  //Serial.flush();
  //Serial.print(calibrated_values[0]);
  //Serial.print(",");
// Serial.print(calibrated_values[1]);
  //Serial.print(",");
  //Serial.print(calibrated_values[2]);
// Serial.println();

// delay(100);



  // Magnetometer
  //Serial.print ("Значения магнитометра");
  //Serial.print ("\t");
  Serial.print (X ,DEC);
  Serial.print (",");
  Serial.print (Y ,DEC);
  Serial.print (",");
  Serial.print (Z ,DEC);
  Serial.print (",");
  //Serial.print ( Mscale << 4 | Mmode ,DEC);
  //Serial.print ("\t");
  Serial.println("");

// Serial.print (mx,DEC);
  //Serial.print ("\t");
  //Serial.println (my ,DEC);
  //Serial.println ("\t");
  //Serial.print (mz,DEC);
  //Serial.print ("\t");
  }

  }


  // End of line
  //Serial.println("");
delay(100);  
}

uint8_t readByte(uint8_t address, uint8_t subAddress)
{
  uint8_t data; // `data` will store the register data 
  Wire.beginTransmission(address);         // Initialize the Tx buffer
  Wire.write(subAddress);                  // Put slave register address in Tx buffer
  Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive
  Wire.requestFrom(address, (uint8_t) 1);  // Read one byte from slave register address
  data = Wire.read();                      // Fill Rx buffer with result
  return data;                             // Return data read from slave register
}

void getHeading()
{

  xv = X;
  yv = Y;
  zv = Z;
}

 Начал некоторыи анализ. Произвел записи по всем осям.Сделал 6 деиствии вокруг каждои (XYZ) оси повернул модуль, туда/обратно на 360 град. с паузами (состояния покоя) Построил графики...

Почему то вокруг оси z показания от других осей отличается? Их почти нет... (кроме акселерометра)

Как определить какие ограничения в показателях у гироскопа и магнитометра? (-2200/+2200 и -500/+500, соответственно)?

Видна ли зависимость "стратегическая"? показании магнитометра от наклонов вдоль осей?

Какие переменные в скетче (вышеуказанном) - еще без учета коррекционнои матрицы?



 



Или все же нужно для начало использовать отильтрованно/калиброванные данные с гироскопа и акселерометра (фильтр Калмана и Mэджика) и только потом сравнивать полученные углы (крен рыскание тангаж) с данными магнитометра?

Есть несколько видео, где вроде используется для определения крена рыскания тангажа (а я так понимаю углов наклона вокруг осеи) все три датчика (аксель, гироск, магнитом). Где бы раздабыть этот код на MPU 9250? (тогда эти углы можно будет сравнивать с "девственными" (не обработанными) показаниями магнитометра. Единственное, наверное нужна какая ни будь калибровка в строну совпадения осеи всех трех чипов....