Датчик MPU-6050

Программирование STM32: Работа с датчиком MPU6050

Коллеги, добрый день!

Сегодня, в рамках задачи по написанию автопилота под микроконтроллер STM32, мы постараемся детально, как водится — в примерах, разобрать работу с датчиком MPU-6050 по шине I2C. Текстовое описание будет очень сжатым, если нужно более подробное раскрытие материала, прошу заглянуть на наш канал, а точнее на данный видео-урок. Не забываем подписываться и ставить лайки))

Итак, начнем!

Для начала нам нужно настроить микроконтроллер для работы с шиной I2C, это делается в программе STM32CubeMX.

Это было самое сложное, далее осталось самое простое — программирование)

Пишем 2 функции для отправки данных по шине …


/*Отправка данных по шине  I2C*/
void I2C_WriteBuffer(uint8_t I2C_ADDRESS, uint8_t *aTxBuffer, uint8_t TXBUFFERSIZE) {
    while(HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)I2C_ADDRESS<<1, (uint8_t*)aTxBuffer, (uint16_t)TXBUFFERSIZE, (uint32_t)1000)!= HAL_OK){
        if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF){
            _Error_Handler(__FILE__, aTxBuffer[0]);
        }

    }

      while (HAL_I2C_GetState(&hi2c1) != HAL_I2C_STATE_READY){}
}

…и для приема данных от модуля MPU-6050


/*Прием данных по шине  I2C*/
void I2C_ReadBuffer(uint8_t I2C_ADDRESS, uint8_t RegAddr, uint8_t *aRxBuffer, uint8_t RXBUFFERSIZE){

    I2C_WriteBuffer(I2C_ADDRESS, &RegAddr, 1);

    while(HAL_I2C_Master_Receive(&hi2c1, (uint16_t)I2C_ADDRESS<<1, aRxBuffer, (uint16_t)RXBUFFERSIZE, (uint32_t)1000) != HAL_OK){
        if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF){
            _Error_Handler(__FILE__, __LINE__);
        }
    }

    while (HAL_I2C_GetState(&hi2c1) != HAL_I2C_STATE_READY){}
}

Теперь наша отладочная плата(я использую STM32VLDISCOVERY) может спокойно общаться с любым устройством, использующим шину I2C. Дальнейший код будет заточен только под датчик GY-251, но адаптировать его под любой другой, мне кажется, не составит большого труда!(если нужна будет помощь, обращайтесь через форму обратной связи)

Теперь мы можем настроить наш модуль на нужные параметры, мы устанавливаем гироскоп на ±500°/с и акселерометр на ±8g. Делается это следующим кодом.


/*Инициализация и конфигурироване модуля*/
void MPU6050_Init(void){
    
    uint8_t buffer[7];

    // включение/побудка модуля
    buffer[0] = MPU6050_RA_PWR_MGMT_1;
    buffer[1] = 0x00;
    I2C_WriteBuffer(MPU6050_ADDRESS_AD0_LOW,buffer,2);

    // конфиг гироскопа на ±500°/с
    buffer[0] = MPU6050_RA_GYRO_CONFIG;
    buffer[1] = 0x8;
    I2C_WriteBuffer(MPU6050_ADDRESS_AD0_LOW,buffer,2);

    // конфиг акселерометра на ±8g
    buffer[0] = MPU6050_RA_ACCEL_CONFIG;
    buffer[1] = 0x10;
    I2C_WriteBuffer(MPU6050_ADDRESS_AD0_LOW,buffer,2);
}

Итак, следующий шаг — это калибровка датчика гироскопа, приведение его к нулевому положению.


void MPU6050_Calibrate(void){
    
  int16_t mpu6050data[6];
  uint16_t iNumCM = 1000;
  for (int i = 0; i < iNumCM ; i ++){                  
    MPU6050_GetAllData(mpu6050data);                                            
    fGX_Cal += mpu6050data[3];
    fGY_Cal += mpu6050data[4];                                       
    fGZ_Cal += mpu6050data[5];                                        
    HAL_Delay(3); // 3 сек на калибровку                                                    
  }
  fGX_Cal /= iNumCM;                                                  
  fGY_Cal /= iNumCM;                                                  
  fGZ_Cal /= iNumCM; 
   isinitialized = 1;
}

Давайте подведем промежуточный итог:

  • наш МК настроен на общение по шине I2C
  • мы умеем писать и читать данные любого модуля по его адресу на I2C шине
  • мы конфигурировали модуль MPU-6050 под наши нужды
  • провели калибровку гироскопа модуля Gy-251

 

Что нам осталось?

Сущий пустяк — считать все данные с модуля и произвести интегрирование данных гироскопа.
Сначала считываем данные с гироскопа и акселерометра(опционально можно и температуру считывать) во внутреннюю память микроконтроллера.


void MPU6050_GetAllData(int16_t *Data){
  
  uint8_t accelbuffer[14];

  // с 0x3B 14 следующих регистров содержат данные измерения модуля
  I2C_ReadBuffer(MPU6050_ADDRESS_AD0_LOW,MPU6050_RA_ACCEL_XOUT_H,accelbuffer,14);

  /* Registers 59 to 64 – Accelerometer Measurements */
  for (int i = 0; i< 3; i++)
      Data[i] = ((int16_t) ((uint16_t) accelbuffer[2 * i] << 8) + accelbuffer[2 * i + 1]);
  
  /* Registers 65 and 66 – Temperature Measurement */
  //пока пропускаем Temperature in degrees C = (TEMP_OUT Register Value as a signed quantity)/340 + 36.53

  /* Registers 67 to 72 – Gyroscope Measurements */
  for (int i = 4; i < 7; i++)
      Data[i - 1] = ((int16_t) ((uint16_t) accelbuffer[2 * i] << 8) + accelbuffer[2 * i + 1]);

}

 

Теперь самое вкусное!

Интегрирование данных — как много боли в этом словосочетании!)
Мы должны, строго в определенные отрезки времени(их еще называют dt), принимать данные гироскопа, переводить их в градусы в секунду! Да-да, гироскоп присылает скорость изменения своего положения) После этого, градусы в секунды мы переводим в градусы(поэтому нужен dt) и складываем с накопленной суммой градусов по определенной оси. Звучит страшно, но в коде это совсем нестрашно

/*функция вызывается 1000 раз в секунду*/
void HAL_SYSTICK_Callback(void){
  
    if(isinitialized && ++uiTicksCNT <= 20){ // 50 раз в секунду
      int16_t mpu6050data[6];
      // перемещаем в массив mpu6050data все данные с датчика
      MPU6050_GetAllData(mpu6050data);

      // РАБОТА С ГИРОСКОПОМ
      //как перевести Юниты в реальные градусы развернуто на примере Roll
      float Roll = mpu6050data[4] - fGY_Cal; // относительно "нуля"
      Roll = Roll/65.5/50;
      MPU6050_Data.aRoll += Roll;

      //сокращенная запись для Pitch
      MPU6050_Data.aPitch += (mpu6050data[3] - fGX_Cal)/65.5/50;

      
      float Yaw = (mpu6050data[5] - fGZ_Cal)/65.5/50;
      MPU6050_Data.aYaw += Yaw;
      //учитываем параметр по Z если по нему есть движение
      if(Yaw > 0.01){//TODO: сравнение с дельтой
        float _Y = sin(Yaw * 3.1415/180);
        MPU6050_Data.aPitch += MPU6050_Data.aRoll  * _Y;               
        MPU6050_Data.aRoll -= MPU6050_Data.aPitch * _Y; 
      }

      uiTicksCNT = 0;
    }
    
}

Заключение

Теперь наша тестовая плата(у вас может быть любая на STM32) знает все о подключенном гироскопе! Достаточно ли этого? Разумеется, нет)

Гироскоп имеет свойство «плавать» — это называется «дрейф нуля». В следущей серии мы узнаем как взять данные с акселерометра, как их интерполировать на полученные данные, какие бываю фильтры и мы сделаем этот модуль!)))

За более подробной информацией о математике, прошу посетить видео-материал по теме.

Скачать актуальный исходный код проекта
 

Добавить комментарий

Ваш e-mail не будет опубликован. Обязательные поля помечены *