STM32 MPU6050 HMC5983

Программирование STM32: Работа с магнитометром HMC5983

Коллеги, продолжаем работу над автопилотом.

Сегодня перед нами стоит задача считать, и интерпретировать показания магнитометра(в моем случае HMC5983, но код будет работать и с HMC5883), еще, с помощью данных от акселерометра, нужно нивелировать «дрейф нуля» гироскопа, применив для этого Комплиментарный фильтр. За более подробной информацией прошу на очередной выпуск нашего блокбастера на YouTube.

Итак, продолжаем!

Проблема первая: взять и интерпретировать данные акселерометра!

Хотя «взять данные» — мы это уже сделали на прошлом уроке, нужна интерпретация. Посему, обратимся к картинке:

Как видно из картинки, чтоб получить крен и тангаж из данных акселерометра достаточно двух строк кода:

// РАБОТА С АКСЕЛЕРОМЕТРОМ
// тангаж...
float pitch4macc = atan2(mpu6050data[0] ,( sqrt(SQR(mpu6050data[1]) + SQR(mpu6050data[2]) ) ) )* R2DEG;

// ...и крен
float roll4macc = atan2 (mpu6050data[1] ,( sqrt ( SQR(mpu6050data[0]) + SQR(mpu6050data[2]) ) ) )* R2DEG;

Иными словами, никакого интегрирования! Класс? Конечно! Но, как водится, дьявол в деталях) Любая вибрация, например от моторов, которую поймает акселерометр, будет выдавать такой шум на выходе, что пользоваться этими данными (Roll и Pitch) не представляется возможным. Беда, но не совсем. Во-первых, говорят от шума можно данные почистить(я не знаю как, но если вы знаете расскажите))). И самое главное, если использовать данные акселерометра вкупе с гироскопом — то, тоже так говорят, все будет летать!

Так давайте проверим это!)

Комплементарный фильтр

На просторах Интернета много объяснений данному фильтру, мы его не будем доказывать, просто возьмем формулу:

A = (1-K)*Ag + K*Ac

Из которой следует, что каждый угол следует представлять как сумму интегрированного угла гироскопа и мгновенного угла акселерометра, умноженных на некие коэффициенты. К — коэффициент комплементарного фильтра, который подбирается индивидуально для каждого случая. Главное чтоб шумы акселерометра не вносили сильного влияния на результат и нуль гироскопа не уплывал.

На языке C это будет выглядеть вот так:

MPU6050_Data.aPitch = MPU6050_Data.aPitch * (1-MPU6050_KOEF_COMPL) + pitch4macc * MPU6050_KOEF_COMPL;
MPU6050_Data.aRoll = MPU6050_Data.aRoll * (1-MPU6050_KOEF_COMPL) + roll4macc * MPU6050_KOEF_COMPL;

Ура! Мы закончили с модулем MPU6050… Но осталась еще одна проблема, как же узнать куда мы летим, где брать рыскание? Выдохнули и поехали дальше)

Часть следующая — работаем с магнитометром HMC5983)

Первым делом нам нужно настроить наш магнитометр согласно даташиту(стр. 13-17) и нашим хотелкам.

/*Все настройки находятся в трех регистрах.*/
void HMC5983_Init(void){
  uint8_t buffer[7];
  buffer [0] = HMC5983_Config1;
  buffer [1] = 0b01111000;
  I2C_WriteBuffer(HMCAddr,buffer,2);

  buffer [0] = HMC5983_Config2;
  buffer [1] = 0b10000000;
  I2C_WriteBuffer(HMCAddr,buffer,2);

  buffer [0] = HMC5983_Mode;
  buffer [1] = 0b00000000;
  I2C_WriteBuffer(HMCAddr,buffer,2);
}

Итак, после данного действия мы имеем на борту полностью настроенный и готовый к использованию датчик склонения магнитного поля Земли.

Осталось дело за малым — взять и интерпретировать данные с модуля HMC5983!

А данные, согласно все тому же даташиту, лежат начиная с третьего регистра и занимают еще 6 байт, по 2 байта на каждую из осей, итого по 16 бит…ну как и в прошлый раз)

Сразу обратимся к коду:

void HMC5983_GetAllData() {
  uint8_t buffer[6];

  // с 0x03 6 следующих регистров содержат данные измерения модуля
  I2C_ReadBuffer(HMCAddr,HMC5983_XRegister,buffer,6);
  HMC5983_Data.rawX = ((int16_t)(((uint16_t)buffer[0]) << 8) + buffer[1]);
  HMC5983_Data.rawY = ((int16_t)(((uint16_t)buffer[4]) << 8) + buffer[5]);
  HMC5983_Data.rawZ = ((int16_t)(((uint16_t)buffer[2]) << 8) + buffer[3]);
  // скалируем данные(если это нужно)
  HMC5983_Data.rawX *= 2.27f;
  HMC5983_Data.rawY *= 2.27f;
  HMC5983_Data.rawZ *= 2.27f; 
}

Ну и все! ВСЕ!

Заключение

Теперь у нас есть все данные и положении нашего дрона, нужные, для того чтоб решить задачу его стабилизации в пространстве.

Далее, мы будем учиться управлять БК двигателями из STM32, чтоб при помощи нечеткой логики попытаться сделать нечеткий регулятор.

Скачать актуальный исходный код проекта можно совершенно бесплатно, при желании можно чуточку помочь проекту добраться до конечной точки)

Спасибо за внимание, остаемся на связи!

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

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