3-х осевой гироскоп и акселерометр GY-521 (MPU 6050): описание, подключение, схема, характеристики | ВИКИ

3-х осевой гироскоп и акселерометр GY-521 (MPU 6050): описание, подключение, схема, характеристики | ВИКИ Вертолеты

3чтение показаний датчика тока acs712с помощью arduino

В скетче будем постоянно читать значение с порта A0 и выводить в монитор последовательных данных. Напомню, АЦП у разных плат Arduino имеет различную разрядность, обычно 10 или 12 бит. Подробнее здесь.

Это означает, что с аналогового порта могут приходить значения от 0 до 210 = 1024 для 10-разрядного АЦП. Будем считать, что у нас датчик тока, диапазон измерений которого от -5 А до 5 А, а чувствительность 185 мВ/А.

Если на 1 А приходится 185 мВ, это соответствует примерно 38 единицам измерения АЦП: 185·1024/5000 = 37.888, (1) где 5000 – это максимальное значение напряжения, которое способен измерить АЦП Arduino, в милливольтах.

На выходе OUT датчика ACS712 при отсутствии измеряемого тока должна быть половина напряжения питания, т.е. 2.5 В. Так как вся шкала АЦП лежит в диапазоне от 0 до 1024, то при отсутствии измеряемого тока мы должны считывать с аналогового порта Arduino число 512.

Это начало шкалы отсчёта. Обозначим его value_zero. Отклонение тока value_adc от нулевого уровня в большую или меньшую сторону и будет показывать силу тока. Следовательно, чтобы посчитать в амперах значение тока с датчика ACS712, необходимо разницу нулевого уровня и измеренного значения с аналогового порта A0 поделить на 38.

Пояснение принципа вычисления силы тока
Пояснение принципа вычисления силы тока

На практике значение на аналоговом выводе A0 не будет равняться точно 512. Поэтому, чтобы определить начало отсчёта, добавим в скетч примитивную калибровку. Калибровка будет заключаться в том, что некоторое количество раз прочитаем значение с аналогового порта A0 при отсутствии тока на датчике ACS712, и усредним его. Естественно, нагрузка на время калибровки должна быть выключена, чтобы ток не протекал через датчик.

Скетч для измерения постоянного тока датчиком ACS712 (разворачивается)
const int acs712_pin = A0;

int zero; // уровень нуля, относительно которого измеряется ток, обычно VCC/2

void setup() {
  Serial.begin(9600);
  calibrate();
}

// определим нуль шкалы (до включения нагрузки)
void calibrate(){
  zero = 0;
  int repeats = 10;
  for (int i=0; i<repeats; i  ){
    zero  = analogRead(acs712_pin);
    delay(100);
  }
  zero /= repeats; // берём среднее арифметическое
  Serial.print("Zero=");
  Serial.println(zero);
}

void loop() {
  int sensorValue = analogRead(acs712_pin); // читаем значение с АЦП и выводим в монитор
  Serial.print(sensorValue); 
  Serial.print(" = ");
  int c = getCurrent(sensorValue); // преобразуем в значение тока и выводим в монитор
  Serial.print(c); 
  Serial.println(" mA");
  delay(100);
}

// рассчитывает ток в мА по значению с АЦП
int getCurrent(int adc) {
  int delta = zero - adc; // отклонение от нуля шкалы
  float scale = 37.888; // сколько единиц АЦП приходится на 1 ампер, по формуле (1)
  int current = (int)delta*1000/scale; // считаем ток в мА и округляем до целых, по формуле (2)
  return current;
}

Загрузим скетч и плавно начнём поднимать напряжение и ток на нагрузке. Какое-то время подождём, а затем начнём уменьшать ток. В результате получим примерно такую картинку:

Вывод тока датчика ACS712 в монитор последовательного порта и его график
Вывод тока датчика ACS712 в монитор последовательного порта и его график

Как видно, аналоговый сигнал постоянно «прыгает». Чтобы этого избежать, следует добавить в скетч сглаживание. Для этого будем проводить подряд несколько измерений, а затем брать среднее арифметическое от них в качестве действительного значения. Заодно совместим начальную калибровку, т.к. она выполняется точно так же. Вот как изменится в результате скетч:

Скетч для измерения постоянного тока датчиком ACS712 со сглаживанием
const int acs712_pin = A0;

int zero; // уровень нуля, относительно которого измеряется ток, обычно VCC/2

void setup() {
  Serial.begin(9600);
  zero = getSmoothedValue(); // определим нуль шкалы (до включения нагрузки)
  Serial.print("Zero=");
  Serial.println(zero);
}

// получает сглаженное значение с АЦП Arduino
int getSmoothedValue(){
  int value;
  int repeats = 10;
  for (int i=0; i<repeats; i  ){ // измеряем значение несколько раз
    value  = analogRead(acs712_pin); // суммируем измеренные значения
    delay(1);
  }
  value /= repeats; // и берём среднее арифметическое
  return value;
}

void loop() {
  int sensorValue = getSmoothedValue(); // читаем значение с АЦП и выводим в монитор
  Serial.print(sensorValue); 
  Serial.print(" = ");
  int c = getCurrent(sensorValue); // преобразуем в значение тока и выводим в монитор
  Serial.print(c); 
  Serial.println(" mA");
  delay(100);
}

// рассчитывает ток в мА по значению с АЦП
int getCurrent(int adc) {
  int delta = zero - adc; // отклонение от нуля шкалы
  float scale = 37.888; // сколько единиц АЦП приходится на 1 ампер
  int current = (int)delta*1000/scale; // считаем ток в мА
  return current;
}

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

В результате выполнения данного скетча картинка получается гораздо более приятная:

Сглаженный график тока, измеренного датчиком ACS712
Сглаженный график тока, измеренного датчиком ACS712

Тот же самый принцип заложен в библиотеки для Arduino, которые оперируют с датчиком тока ACS712. Например, вот эта библиотека Troyka Current.

По результату эксперимента получается, что датчик ACS712 очень простой, но при этом довольно не точный. Гораздо точнее датчик тока, который мы рассмотрим в следующем разделе.

4управление 7-сегментным индикатором с помощью драйвера cd4511 и arduino

При подключении двоичного декодера будем руководствоваться следующей таблицей:

Вывод CD4511НазначениеПримечание
A0…A3Входы двоичного преобразователяСоответствуют разрядам двоичного числа.
a…gВыходы на сегменты индикатораПодключаются через токоограничивающие резисторы к соответствующим сегментам светодиодного индикатора.
Lamp Test#Тест индикатора (включает все сегменты)Подключим к питанию, не использовать его.
Blanking#Очистка индикатора (отключает все сегменты)Подключим к питанию, чтобы не использовать его.
Latch Enabled#Выход активенБудет подключен к земле, чтобы выход был всегда активен.
VDDПитание микросхемы и индикатораОт 3 до 15 В.
GNDЗемляОбщая у CD4511, Arduino, 7-сегментного индикатора.

Желательно также подключить керамический конденсатор ёмкостью примерно 1 мкФ между землёй и питанием микросхемы CD4511.

Подключение 7-сегментного индикатора к Arduino с двоичным декодером CD4511B
Подключение 7-сегментного индикатора к Arduino с двоичным декодером CD4511B

Теперь напишем простой скетч, чтобы проверить работоспособность 7-сегментного индикатора 3361AS-1 в связке с двоично-десятичным декодером, а также получить опыт работы с ними. Данный скетч будет поочерёдно перебирать числа от 0 до 9, перемещаясь по циклу от одного разряда индикатора к следующему.

Скетч для управления 7-сегментным индикатором (светится 1 разряд) (разворачивается)
// выводы Arduino для управления двоичным кодом на входе декодера CD4511:
const byte D_0 = 11;
const byte D_1 = 10;
const byte D_2 = 9;
const byte D_3 = 8;

// выводы Arduino для выбора десятичного разряда индикатора:
const byte B_0 = 7;
const byte B_1 = 6;
const byte B_2 = 5;

void setup() {
  pinMode(D_0, OUTPUT);
  pinMode(D_1, OUTPUT);
  pinMode(D_2, OUTPUT);
  pinMode(D_3, OUTPUT);
  pinMode(B_0, OUTPUT);
  pinMode(B_1, OUTPUT);
  pinMode(B_2, OUTPUT);
}

void loop() {
  for (int i=0; i<3; i  ){ // перебираем разряды с 0 по 2-ой
    setDigit(i);
    for (int n=0; n<10; n  ){ // перебираем числа от 0 до 9
      printNumber(n);
      delay(200);
    }    
  }
}

// выбирает разряд десятичного числа на счётчике:
void setDigit(byte b){
  switch (b) {
    case 0:
      digitalWrite(B_0, LOW);
      digitalWrite(B_1, HIGH);
      digitalWrite(B_2, HIGH);
      break;
    case 1:
      digitalWrite(B_0, HIGH);
      digitalWrite(B_1, LOW);
      digitalWrite(B_2, HIGH);
      break;
    case 2:
      digitalWrite(B_0, HIGH);
      digitalWrite(B_1, HIGH);
      digitalWrite(B_2, LOW);
      break;
  }
}

// зажигает заданную цифру 7-сегментного индикатора
void printNumber(byte n){
  switch(n){
    case 0:
      digitalWrite(D_0, LOW);
      digitalWrite(D_1, LOW);
      digitalWrite(D_2, LOW);
      digitalWrite(D_3, LOW);
      break;
    case 1:
      digitalWrite(D_0, HIGH);
      digitalWrite(D_1, LOW);
      digitalWrite(D_2, LOW);
      digitalWrite(D_3, LOW);
      break;
    case 2:
      digitalWrite(D_0, LOW);
      digitalWrite(D_1, HIGH);
      digitalWrite(D_2, LOW);
      digitalWrite(D_3, LOW);
      break;
    case 3:
      digitalWrite(D_0, HIGH);
      digitalWrite(D_1, HIGH);
      digitalWrite(D_2, LOW);
      digitalWrite(D_3, LOW);
      break;
    case 4:
      digitalWrite(D_0, LOW);
      digitalWrite(D_1, LOW);
      digitalWrite(D_2, HIGH);
      digitalWrite(D_3, LOW);
      break;
    case 5:
      digitalWrite(D_0, HIGH);
      digitalWrite(D_1, LOW);
      digitalWrite(D_2, HIGH);
      digitalWrite(D_3, LOW);
      break;
    case 6:
      digitalWrite(D_0, LOW);
      digitalWrite(D_1, HIGH);
      digitalWrite(D_2, HIGH);
      digitalWrite(D_3, LOW);
      break;
    case 7:
      digitalWrite(D_0, HIGH);
      digitalWrite(D_1, HIGH);
      digitalWrite(D_2, HIGH);
      digitalWrite(D_3, LOW);
      break;
    case 8:
      digitalWrite(D_0, LOW);
      digitalWrite(D_1, LOW);
      digitalWrite(D_2, LOW);
      digitalWrite(D_3, HIGH);
      break;
    case 9:
      digitalWrite(D_0, HIGH);
      digitalWrite(D_1, LOW);
      digitalWrite(D_2, LOW);
      digitalWrite(D_3, HIGH);
      break;
  }
}

Загрузим скетч в Arduino и посмотрим результат.

В один момент времени светится только один разряд индикатора. Как же задействовать одновременно сразу три разряда индикатора? Это будет немного сложнее. Сложность заключается в том, что нам одновременно нужно управлять тремя разрядами десятичного число, используя только один преобразователь CD4511.

Но чисто физически это невозможно. Однако можно добиться иллюзии постоянного свечения всех разрядов светодиодного индикатора. Для этого придётся быстро переключаться между разрядами, постоянно обновляя показание каждого разряда. Мы будем поочерёдно активировать каждый из разрядов индикатора 3361AS, выставлять на нём с помощью двоичного преобразователя CD4511 нужную цифру, а затем переключаться на следующий разряд.

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

Также перепишем функцию setNumber() отправки двоичного кода на вход микросхемы преобразователя CD4511. Вместо использования оператора switch, используем массив массивов.

Скетч для управления трёхразрядным 7-сегментным индикатором (разворачивается)
// Выводы Arduino для управления двоичным конвертером CD4511:
const byte bit0 = 11;
const byte bit1 = 10;
const byte bit2 = 9;
const byte bit3 = 8;

// Выводы Arduino для выбора десятичных разрядов индикатора 3361AS:
const byte B_0 = 5;
const byte B_1 = 6;
const byte B_2 = 7;

#define seconds() (millis()/1000) // макрос определения секунд, прошедших с начала работы скетча

void setup()
{
  pinMode(bit0, OUTPUT);
  pinMode(bit1, OUTPUT);
  pinMode(bit2, OUTPUT);
  pinMode(bit3, OUTPUT);
  
  pinMode(B_0, OUTPUT);
  pinMode(B_1, OUTPUT);
  pinMode(B_2, OUTPUT);

  digitalWrite(B_0, HIGH);
  digitalWrite(B_1, HIGH);
  digitalWrite(B_2, HIGH);
}

void loop()
{
  // Каждую секунду увеличиваем показания индикатора на 1:
  int sec = seconds();
  for (int i=0; i<1000; i  ) {
    while (sec == seconds()) {
      printNumber(i);
    }
    sec = seconds();
  }
}

// Выводит 3-разрядное число на 7-сегментный индикатор.
void printNumber(int n) {
  setDigit(B_0, n/100); // выводим сотни десятичного числа
  setDigit(B_1, n/10 ); // выводим десятки
  setDigit(B_2, n/1  ); // выводим единицы
}

// Выводит заданное число на заданный разряд индикатора.
void setDigit(byte digit, int value) {
  digitalWrite(digit, LOW); // выбираем разряд индикатора 3361AS-1
  setNumber(value); // выводим на этот разряд число
  delay(4);
  digitalWrite(digit, HIGH); // снимаем выбор разряда индикатора
}

// Выставляет двоичный код на входе преобразователя CD4511
void setNumber(int n) {
  static const struct number {
    byte b3;
    byte b2;
    byte b1;
    byte b0;
  }
  
  numbers[] = {
    {0, 0, 0, 0}, // 0 
    {0, 0, 0, 1}, // 1 
    {0, 0, 1, 0}, // 2 
    {0, 0, 1, 1}, // 3 
    {0, 1, 0, 0}, // 4 
    {0, 1, 0, 1}, // 5 
    {0, 1, 1, 0}, // 6 
    {0, 1, 1, 1}, // 7 
    {1, 0, 0, 0}, // 8 
    {1, 0, 0, 1}, // 9 
  };

  digitalWrite(bit0, numbers[n].b0);
  digitalWrite(bit1, numbers[n].b1);
  digitalWrite(bit2, numbers[n].b2);
  digitalWrite(bit3, numbers[n].b3);
}

Получится вот такая картина.

Смотрите про коптеры:  Инструкция и руководство пользования для Квадрокоптер Syma X25Pro дополнительная батарея в комплекте белый - скачать, читать в интернет магазине DNS.
Управление трёхразрядным семисегментным индикатором с помощью преобразователя CD4511 и Arduino
Управление трёхразрядным семисегментным индикатором с помощью преобразователя CD4511 и Arduino

В динамике это выглядит так. Тут как раз временами видны мерцания сегментов светодиодного индикатора.

Можно попробовать поиграть значением задержек в функции setDigit(). Если сделать задержки меньше, то мерцание станет меньше заметно. Но начнут сильнее засвечиваться соседние сегменты на выбранном разряде индикатора. Тут необходимо выбрать какое-то компромиссное решение.

6как читать данные сдатчика тока и напряжения ina219

Если посмотреть на обмен данными по шине I2C, который происходит при работе данного скетча (с помощью логического анализатора, конечно), то увидим следующее.

Осциллограмма чтения регистров датчика INA219
Осциллограмма чтения регистров датчика INA219

Чтобы понять, что здесь происходит, необходимо познакомиться с картой регистров датчика INA219. Датчик содержит всего 6 регистров. Все регистры 16-разрядные.

Карта регистров датчика тока и напряжения INA219
Адрес регистраНазвание регистраНазначение регистраТип
0x00ConfigurationСброс всех регистров, настройка диапазона измерений, усиления PGA, разрешения АЦП и фильтрации.Чтение/Запись
0x01Shunt voltageХранит измеренное значение напряжения на шунтирующем резисторе 0,1 Ом.Чтение
0x02Bus voltageХранит измеренное значение напряжения шины.Чтение
0x03PowerХранит измеренное значение мощности.Чтение
0x04CurrentСодержит значение силы тока, протекающего через шунтирующий резистор.Чтение
0x05CalibrationКалибровочный регистр. Задаёт диапазон измерений и позволяет осуществлять калибровку системы.Чтение/Запись

Для обмена с модулем воспользуемся отладочной платой с микросхемой FT2232H и программой SPI via FTDI. Это будет проще, чем использовать Arduino, т.к. для внесения изменений в целях эксперимента не придётся каждый раз перепрограммировать ПЗУ, а можно будет вносить изменения в передаваемые команды «на лету».

Чтение регистров датчика тока INA219 с помощью FT2232H
Чтение регистров датчика тока INA219 с помощью FT2232H

Запустим программу SPI via FTDI, выберем в меню «Устройство» интерфейс I2C. Подключимся к порту A. Просканируем устройства на шине I2C. Программа найдёт устройство по адресу 64 (0x40), если конечно вы не меняли адрес перемычками A0 и A1.

Чтение регистров датчика тока INA219 с помощью FT2232H и программы "SPI via FTDI"
Чтение регистров датчика тока INA219 с помощью FT2232H и программы “SPI via FTDI”

Как вы уже наверное догадались, команда “0” означает адрес регистра, из которого мы хотим прочитать данные. А число 0x399F – это данные в нулевом регистре (регистр конфигурации). И это соответствует документации, т.к. после включения и загрузки микросхема INA219 имеет именно такую конфигурацию по умолчанию. Вот какую структуру имеет регистр конфигурации.

Структура конфигурационного регистра датчика тока INA219
Структура конфигурационного регистра датчика тока INA219

В регистре конфигурации датчика INA219 присутствуют следующие части:

  • RST (reset) – сброс;
  • BRNG (bus voltage range) – диапазон измерения шины;
  • BADC (bus ADC resolution/averaging) – разрешающая способность АЦП шины;
  • SADC (shunt ADC resolution/averaging) – разрешающая способность АЦП шунта;
  • MODE – режим;
  • PG – усиление и диапазон PGA.

0x399F в двоичном виде это “001_11_0011_0011_111”. Следовательно, значения по умолчанию после включения такие.

  • BRNG равен “1”, что означает диапазон измерений 32 вольта FSR;
  • PG равно “11”: задаёт диапазон ±320 мВ и коэффициент усиления 8;
  • BADC, SADC равны “0011”: максимальная разрешающая способность АЦП – 12 бит;
  • MODE, равное “111”, означает непрерывный режим работы, включены и шунт, и шина.

Для чтения других регистров необходимо сначала так же записать их адрес в поле «Чтение» «Команда», а затем прочитать 2 байта. Или можно записать номер регистра в поле «Запись» «Команда», а затем просто читать (не указывая адрес регистра в команде чтения).

К сожалению, последовательного чтения всех регистров микросхемы INA219 «за один проход» не предусмотрено.

Вернёмся к нашей осциллограмме. Мы видим на ней 6 циклов чтения (каждый начинается с зелёной точки ● и заканчивается тёмно-красной ●). Сначала читаем регистр с напряжением шунта Vшунт. (адрес 0x01), который хранит значение 0x1957.

Далее читаем значение регистра напряжения шины Vшины (0x02), в котором значение 0x19BA. Далее читаем регистр калибровки Cal (0x05) со значением 0x1000. Потом регистр тока шунта Iшунт. (0x04), в котором значение 0x1959.

Вывод монитора порта в момент снятия осциллограммы с датчика INA219
Вывод монитора порта в момент снятия осциллограммы с датчика INA219

Рассмотрим, как привести данные в регистрах «в человеческий вид». Нам интересны не все значения, а только напряжения и ток. Плюс регистр калибровки, который играет роль поправочного коэффициента.

Акселерометр и гироскоп mpu6050

Прежде чем приступить к рассмотрению модуля гироскопа и акселерометра, думаю, будет не лишним коротко разобраться что это такое. Гироскоп представляет собой устройство, реагирующее на изменение углов ориентации контролируемого тела. В классическом представлении это какой-то инерционный предмет, который быстро вращается на подвесах.

Как результат вращающийся предмет всегда будет сохранять свое направление, а по положению подвесов можно определить угол отклонения. На самом же деле электронные гироскопы построены по другой схеме и устроены немного сложнее (вращающийся волчок впихнуть в микросхему было бы не просто).

Акселерометр – это устройство, которое измеряет проекцию кажущегося ускорения, то есть разницы между истинным ускорением объекта и гравитационным ускорением. На простом примере такая система представляет собой некоторую массу, закрепленную на подвесе, обладающим упругостью (пружина для хорошего примера).

Так вот если такую систему повернуть под каким-то углом, или бросить, или предать линейное ускорение, то упругий подвес отреагирует на движение под действием массы и отклонится и вот по этому отклонению определяется ускорение.

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

Микросхема MPU6050 содержит на борту как акселерометр, так и гироскоп, а помимо этого еще и температурный сенсор. MPU6050 является главным элементом модуля GY-531.

Помимо этой микросхемы на плате модуля расположена необходимая обвязка MPU6050, в том числе подтягивающие резисторы интерфейса I2C, а также стабилизатор напряжения на 3,3 вольта с малым падением напряжения (при питании уже в 3,3 вольта на выходе стабилизатора будет 3 ровно вольта) с фильтрующими конденсаторами.

Ну и бонусом на плате распаян SMD светодиод с ограничивающим резистором как индикатор питающего напряжения. Размер платы модуля GY-521 10 х 20 мм. 

Схема модуля представлена ниже (номиналы могут немного отличаться в разных версиях модуля):

Характеристики MPU6050:

  • напряжения питания 2,375 – 3,46 вольт 
  • потребляемый ток до 4 мА
  • интерфейс передачи данных – I2C
  • максимальная скорость I2C – 400 кГц
  • вход для других датчиков I2C
  • внутренний генератор на 8 МГц (вне модуля возможность подключить внешний кварцевый резонатор на 32,768 кГц или 19,2 МГц)

Нужно отметить возможность MPU6050 работать в мастер режиме I2C для AUX выводов, к которым можно подключить еще один внешний датчик (например магнитометр). Честно говоря, я не понимаю для чего это вообще нужно, если проще подключать дополнительные датчики к общей шине I2C микроконтроллера.

Функции MPU6050:

  • трех осевой MEMS гироскоп с 16 битным АЦП
  • трех осевой MEMS акселерометр с 16 битным АЦП
  • Digital Motion Processor (DMP)
  • slave I2C для подключения к микроконтроллеру
  • master I2C для подключения к микросхеме дополнительного датчика
  • регистры данных датчиков
  • FIFO
  • прерывания
  • температурный сенсор
  • самопроверка гироскопа и акселерометра
  • регистр идентификации устройства

Внешний вид модуля GY-521:

В комплекте идут штыревые соединения угловые и прямые. Припаян был прямой штыревой разъем.

Данные измерений датчиков можно считывать как из регистров хранения, так и пользоваться функциями FIFO.

Имеется отдельный регистр под названием Who am I, значение, записанное в этом регистре постоянно и его можно только считать, можно использовать как идентификатор устройства, значение в регистре 104 или 0х68.

Отдельным выводом является выход прерываний, который настраивается регистрами настройки под определенные события.

Датчики гироскопа и акселерометра изготовлены как MEMS (микроэлектромеханическая система) – внешнее воздействие на датчик сначала изменяет состояние механической части, затем изменение состояния механической части приводит к изменению сигнала электрической части.

Смотрите про коптеры:  Как устранить основные неисправности квадрокоптеров?

Одним словом в одном корпусе собрана не только электроника, но и механика. В микросхеме MPU6050 содержится сразу два MEMS датчика, производитель утверждает, что их взаимное воздействие друг на друга сведено к минимуму. Ну что же, совсем не плохо за цену готового модуля порядка 2 уе.

Между прочим эти модули можно приобрести на торговых площадках aliexpress или ebay.

Разберемся как можно использовать датчики акселерометра и гироскопа. Температурный датчик трогать даже не будем – данные о температуре прочитали, перевели в человеческие значения и наслаждаемся. Гироскоп выдает значения мгновенной угловой скорости с разрешением, заданным в настройках, например 2000 градусов в секунду.

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

Далее с помощью этих данных можно получить ориентацию объекта в пространстве. Для этого нужно получить мгновенное значение угловой скорости и умножить его на промежуток времени между опросами датчика гироскопа.

Пример разрешение 2000 градусов в секунду, промежуток между опросами датчика 0,1 секунда, значение мгновенной скорости 300, значит 300*0,1=30 – за это время ось гироскопа была повернута на 30 градусов. Далее каждое полученное значение нужно сложить с предыдущим.

Если ось двигалась в одном направлении – значение 30 градусов, если в другом, то -30, таким образом, при возвращении датчика в исходное положение всегда (в идеале) будет 0, при отклонении от исходного положения, при выполнении вышеописанных действий, получим угол отклонения. Обрабатывая углы трех осей гироскопа можно получить ориентацию объекта в пространстве.

Таким образом, при интегрировании состояния угла положения, также интегрируется и погрешность – при длительном использовании можно получить уже абсолютно неправильные значения. Поэтому часто гироскоп используют в паре с акселерометром, образуя в простом варианте альфа-бета фильтр или комплементарный фильтр.

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

Помимо этого акселерометр измеряет линейные ускорения, то есть ориентация объекта может искажаться при движении датчика в линейных направлениях. Также с помощью акселерометра можно определять движение объекта или его столкновение.

Например детектировать падение объекта или толчок о преграду, чтобы обходить это.

Данные от акселерометра получаем всегда достаточно точные, то есть нуль всегда остается нулем ни при каких воздействиях (имеется ввиду не зависит ни от времени, ни от характера воздействия), однако недостаток кроется в том, что данные идут шумом в некотором диапазоне данных, то есть до десятых долей градуса точно измерять угол не получится.

Если датчик приобрели, можно переходить к рассмотрению внутренностей модуля, а именно главного элемента – микросхемы MPU6050. Информация хранится в регистрах микросхемы, которых более 100 (!). И вот тут то и кроется огромный подводный камень.

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

Ну что же, придется экспериментально определять влияния значений, записанных в некоторые регистры.

Работа с arduino и mpu6050 | alexgyver

Для достижения максимальной точности измерений нужно откалибровать акселерометр и гироскоп. Калибровка акселерометра позволяет выставить “ноль” для вектора силы тяжести, а калибровка гироскопа уменьшает его “дрифт”, то есть статическое отклонение в режиме покоя. Идеально откалиброванный и лежащий горизонтально датчик должен показывать ускорение ~16384 по оси Z и нули по всем остальным осям ускорения и угловой скорости. Но это фантастика =)

Максимально правильно использовать калибровку в проекте нужно так: калибровка по запросу (кнопка, меню, и т.д.), затем запись калибровочных значений в EEPROM. При запуске – чтение и настройка оффсетов. Да, можно замерить значения по всем 6 осям в покое, сохранить их в переменные, а затем вычитать из свежих прочитанных в процессе работы. Такой способ работает для каких-то базовых операций с датчиком (определение перегрузок, тряски, наличия вращения, и т.д.). Для использования MPU6050 с целью максимально точного определения углов поворота платы такой вариант к сожалению не подходит: калибровать нужно рекурсивно.

Рассмотрим несколько примеров калибровки, первый – из библиотеки. Калибрует долго, но максимально точно. При малых вибрациях и движениях датчика в процессе калибровки (даже от громкого звука) калибровка может не закончиться. Второй вариант – мой упрощённый алгоритм калибровки, калибрует быстро, без возможности зависнуть при тряске, но даёт менее точный результат. Я делюсь примерами, в свой проект их нужно будет переносить вручную и аккуратно =)

Калибровка из примера библиотеки

// положи датчик горизонтально, надписями вверх
// ДАЖЕ НЕ ДЫШИ НА НЕГО
// отправь любой символ в сериал, чтобы начать калибровку
// жди результатов
// --------------------- НАСТРОЙКИ ----------------------
const int buffersize = 70;     // количество итераций калибровки
const int acel_deadzone = 10;  // точность калибровки акселерометра (по умолчанию 8)
const int gyro_deadzone = 6;   // точность калибровки гироскопа (по умолчанию 2)
// --------------------- НАСТРОЙКИ ----------------------
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"
MPU6050 mpu(0x68);
int16_t ax, ay, az, gx, gy, gz;
int mean_ax, mean_ay, mean_az, mean_gx, mean_gy, mean_gz, state = 0;
int ax_offset, ay_offset, az_offset, gx_offset, gy_offset, gz_offset;

///////////////////////////////////   SETUP   ////////////////////////////////////
void setup() {
  Wire.begin();
  Serial.begin(9600);
  mpu.initialize();
  // ждём очистки сериал
  while (Serial.available() && Serial.read()); // чистим
  while (!Serial.available()) {
    Serial.println(F("Send any character to start sketch.n"));
    delay(1500);
  }
  while (Serial.available() && Serial.read()); // чистим ещё на всякий
  Serial.println("nMPU6050 Calibration Sketch");
  delay(2000);
  Serial.println("nYour MPU6050 should be placed in horizontal position, with package letters facing up. nDon't touch it until you see a finish message.n");
  delay(3000);
  // проверка соединения
  Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
  delay(1000);
  // сбросить оффсеты
  mpu.setXAccelOffset(0);
  mpu.setYAccelOffset(0);
  mpu.setZAccelOffset(0);
  mpu.setXGyroOffset(0);
  mpu.setYGyroOffset(0);
  mpu.setZGyroOffset(0);
}

///////////////////////////////////   LOOP   ////////////////////////////////////
void loop() {
  if (state == 0) {
    Serial.println("nReading sensors for first time...");
    meansensors();
    state  ;
    delay(1000);
  }
  if (state == 1) {
    Serial.println("nCalculating offsets...");
    calibration();
    state  ;
    delay(1000);
  }
  if (state == 2) {
    meansensors();
    Serial.println("nFINISHED!");
    Serial.print("nSensor readings with offsets:t");
    Serial.print(mean_ax);
    Serial.print("t");
    Serial.print(mean_ay);
    Serial.print("t");
    Serial.print(mean_az);
    Serial.print("t");
    Serial.print(mean_gx);
    Serial.print("t");
    Serial.print(mean_gy);
    Serial.print("t");
    Serial.println(mean_gz);
    Serial.print("Your offsets:t");
    Serial.print(ax_offset);
    Serial.print(", ");
    Serial.print(ay_offset);
    Serial.print(", ");
    Serial.print(az_offset);
    Serial.print(", ");
    Serial.print(gx_offset);
    Serial.print(", ");
    Serial.print(gy_offset);
    Serial.print(", ");
    Serial.println(gz_offset);
    Serial.println("nData is printed as: acelX acelY acelZ giroX giroY giroZ");
    Serial.println("Check that your sensor readings are close to 0 0 16384 0 0 0");
    Serial.println("If calibration was succesful write down your offsets so you can set them in your projects using something similar to mpu.setXAccelOffset(youroffset)");
    while (1);
  }
}
///////////////////////////////////   FUNCTIONS   ////////////////////////////////////
void meansensors() {
  long i = 0, buff_ax = 0, buff_ay = 0, buff_az = 0, buff_gx = 0, buff_gy = 0, buff_gz = 0;
  while (i < (buffersize   101)) { // read raw accel/gyro measurements from device mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); if (i > 100 && i <= (buffersize   100)) { //First 100 measures are discarded
      buff_ax = buff_ax   ax;
      buff_ay = buff_ay   ay;
      buff_az = buff_az   az;
      buff_gx = buff_gx   gx;
      buff_gy = buff_gy   gy;
      buff_gz = buff_gz   gz;
    }
    if (i == (buffersize   100)) {
      mean_ax = buff_ax / buffersize;
      mean_ay = buff_ay / buffersize;
      mean_az = buff_az / buffersize;
      mean_gx = buff_gx / buffersize;
      mean_gy = buff_gy / buffersize;
      mean_gz = buff_gz / buffersize;
    }
    i  ;
    delay(2);
  }
}

void calibration() {
  ax_offset = -mean_ax / 8;
  ay_offset = -mean_ay / 8;
  az_offset = (16384 - mean_az) / 8;
  gx_offset = -mean_gx / 4;
  gy_offset = -mean_gy / 4;
  gz_offset = -mean_gz / 4;
  while (1) {
    int ready = 0;
    mpu.setXAccelOffset(ax_offset);
    mpu.setYAccelOffset(ay_offset);
    mpu.setZAccelOffset(az_offset);
    mpu.setXGyroOffset(gx_offset);
    mpu.setYGyroOffset(gy_offset);
    mpu.setZGyroOffset(gz_offset);
    meansensors();
    Serial.println("...");
    if (abs(mean_ax) <= acel_deadzone) ready  ;
    else ax_offset = ax_offset - mean_ax / acel_deadzone;
    if (abs(mean_ay) <= acel_deadzone) ready  ;
    else ay_offset = ay_offset - mean_ay / acel_deadzone;
    if (abs(16384 - mean_az) <= acel_deadzone) ready  ;
    else az_offset = az_offset   (16384 - mean_az) / acel_deadzone;
    if (abs(mean_gx) <= gyro_deadzone) ready  ;
    else gx_offset = gx_offset - mean_gx / (gyro_deadzone   1);
    if (abs(mean_gy) <= gyro_deadzone) ready  ;
    else gy_offset = gy_offset - mean_gy / (gyro_deadzone   1);
    if (abs(mean_gz) <= gyro_deadzone) ready  ;
    else gz_offset = gz_offset - mean_gz / (gyro_deadzone   1);
    if (ready == 6) break;
  }
}

Мой вариант калибровки

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 mpu;
#define BUFFER_SIZE 100
int16_t ax, ay, az;
int16_t gx, gy, gz;

void setup() {
  Wire.begin();
  Serial.begin(9600);
  mpu.initialize();
  mpu.setXAccelOffset(0);
  mpu.setYAccelOffset(0);
  mpu.setZAccelOffset(0);
  mpu.setXGyroOffset(0);
  mpu.setYGyroOffset(0);
  mpu.setZGyroOffset(0);
  Serial.println(F("Send any character to start sketch"));
  delay(100);
  while (1) {                     //входим в бесконечный цикл
    if (Serial.available() > 0) { //если нажата любая кнопка
      Serial.read();              //прочитать (чтобы не висел в буфере)
      break;                      //выйти из цикла
    }
  }
  delay(1000);
}

void loop() {
  // выводим начальные значения
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  Serial.print(ax); Serial.print(" ");
  Serial.print(ay); Serial.print(" ");
  Serial.print(az); Serial.print(" ");
  Serial.print(gx); Serial.print(" ");
  Serial.print(gy); Serial.print(" ");
  Serial.println(gz);
  calibration();
  // выводим значения после калибровки
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  Serial.print(ax); Serial.print(" ");
  Serial.print(ay); Serial.print(" ");
  Serial.print(az); Serial.print(" ");
  Serial.print(gx); Serial.print(" ");
  Serial.print(gy); Serial.print(" ");
  Serial.println(gz);
  delay(20);
  while (1);
}

// ======= ФУНКЦИЯ КАЛИБРОВКИ =======
void calibration() {
  long offsets[6];
  long offsetsOld[6];
  int16_t mpuGet[6];
  // используем стандартную точность
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
  // обнуляем оффсеты
  mpu.setXAccelOffset(0);
  mpu.setYAccelOffset(0);
  mpu.setZAccelOffset(0);
  mpu.setXGyroOffset(0);
  mpu.setYGyroOffset(0);
  mpu.setZGyroOffset(0);
  delay(10);
  Serial.println("Calibration start. It will take about 5 seconds");
  for (byte n = 0; n < 10; n  ) {     // 10 итераций калибровки
    for (byte j = 0; j < 6; j  ) {    // обнуляем калибровочный массив
      offsets[j] = 0;
    }
    for (byte i = 0; i < 100   BUFFER_SIZE; i  ) { // делаем BUFFER_SIZE измерений для усреднения mpu.getMotion6(&mpuGet[0], &mpuGet[1], &mpuGet[2], &mpuGet[3], &mpuGet[4], &mpuGet[5]); if (i >= 99) {                         // пропускаем первые 99 измерений
        for (byte j = 0; j < 6; j  ) {
          offsets[j]  = (long)mpuGet[j];   // записываем в калибровочный массив
        }
      }
    }
    for (byte i = 0; i < 6; i  ) {
      offsets[i] = offsetsOld[i] - ((long)offsets[i] / BUFFER_SIZE); // учитываем предыдущую калибровку
      if (i == 2) offsets[i]  = 16384;                               // если ось Z, калибруем в 16384
      offsetsOld[i] = offsets[i];
    }
    // ставим новые оффсеты
    mpu.setXAccelOffset(offsets[0] / 8);
    mpu.setYAccelOffset(offsets[1] / 8);
    mpu.setZAccelOffset(offsets[2] / 8);
    mpu.setXGyroOffset(offsets[3] / 4);
    mpu.setYGyroOffset(offsets[4] / 4);
    mpu.setZGyroOffset(offsets[5] / 4);
    delay(2);
  }
  /*
    // выводим в порт
    Serial.println("Calibration end. Your offsets:");
    Serial.println("accX accY accZ gyrX gyrY gyrZ");
    Serial.print(mpu.getXAccelOffset()); Serial.print(", ");
    Serial.print(mpu.getYAccelOffset()); Serial.print(", ");
    Serial.print(mpu.getZAccelOffset()); Serial.print(", ");
    Serial.print(mpu.getXGyroOffset()); Serial.print(", ");
    Serial.print(mpu.getYGyroOffset()); Serial.print(", ");
    Serial.print(mpu.getZGyroOffset()); Serial.println(" ");
    Serial.println(" ");
  */
}

Мой вариант калибровки запись в EEPROM

// калибровка и запись в EEPROM
// при запуске настраиваем оффсеты
// послать символ в сериал - начать повторную калибровку
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "EEPROMex.h"
MPU6050 mpu;
#define BUFFER_SIZE 100
#define START_BYTE 1010
int16_t ax, ay, az;
int16_t gx, gy, gz;

void setup() {
  Wire.begin();
  Serial.begin(9600);
  mpu.initialize();
  //----------------- ВСПОМИНАЕМ ОФФСЕТЫ ------------------------
  int offsets[6];
  for (byte i = 0; i < 6; i  ) { // обнуляем калибровочный массив // по умолч оффсеты хранятся в ячейках 1010, 1012, 1014, 1016, 1018, 1020 offsets[i] = EEPROM.readInt(START_BYTE   i * 2); } // ставим оффсеты из памяти mpu.setXAccelOffset(offsets[0]); mpu.setYAccelOffset(offsets[1]); mpu.setZAccelOffset(offsets[2]); mpu.setXGyroOffset(offsets[3]); mpu.setYGyroOffset(offsets[4]); mpu.setZGyroOffset(offsets[5]); //----------------- ВСПОМИНАЕМ ОФФСЕТЫ ------------------------ Serial.println(F("Send any character to start sketch")); delay(100); while (1) { //входим в бесконечный цикл if (Serial.available() > 0) {     //если нажата любая кнопка
      Serial.read();                  //прочитать (чтобы не висел в буфере)
      break;                          //выйти из цикла
    }
  }
  delay(1000);
}

void loop() {
  calibration();
  Serial.println("Current offsets:");
  Serial.println("accX accY accZ gyrX gyrY gyrZ");
  Serial.print(mpu.getXAccelOffset()); Serial.print(", ");
  Serial.print(mpu.getYAccelOffset()); Serial.print(", ");
  Serial.print(mpu.getZAccelOffset()); Serial.print(", ");
  Serial.print(mpu.getXGyroOffset()); Serial.print(", ");
  Serial.print(mpu.getYGyroOffset()); Serial.print(", ");
  Serial.print(mpu.getZGyroOffset()); Serial.println(" ");
  Serial.println(" ");
  // выводим значения после калибровки
  Serial.println("Readings:");
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  Serial.print(ax); Serial.print(" ");
  Serial.print(ay); Serial.print(" ");
  Serial.print(az); Serial.print(" ");
  Serial.print(gx); Serial.print(" ");
  Serial.print(gy); Serial.print(" ");
  Serial.println(gz);
  delay(20);
  while (1);
}

// =======  ФУНКЦИЯ КАЛИБРОВКИ И ЗАПИСИ В ЕЕПРОМ =======
void calibration() {
  long offsets[6];
  long offsetsOld[6];
  int16_t mpuGet[6];
  // используем стандартную точность
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
  // обнуляем оффсеты
  mpu.setXAccelOffset(0);
  mpu.setYAccelOffset(0);
  mpu.setZAccelOffset(0);
  mpu.setXGyroOffset(0);
  mpu.setYGyroOffset(0);
  mpu.setZGyroOffset(0);
  delay(5);
  for (byte n = 0; n < 10; n  ) {     // 10 итераций калибровки
    for (byte j = 0; j < 6; j  ) {    // обнуляем калибровочный массив
      offsets[j] = 0;
    }
    for (byte i = 0; i < 100   BUFFER_SIZE; i  ) { // делаем BUFFER_SIZE измерений для усреднения mpu.getMotion6(&mpuGet[0], &mpuGet[1], &mpuGet[2], &mpuGet[3], &mpuGet[4], &mpuGet[5]); if (i >= 99) {                         // пропускаем первые 99 измерений
        for (byte j = 0; j < 6; j  ) {
          offsets[j]  = (long)mpuGet[j];   // записываем в калибровочный массив
        }
      }
    }
    for (byte i = 0; i < 6; i  ) {
      offsets[i] = offsetsOld[i] - ((long)offsets[i] / BUFFER_SIZE); // учитываем предыдущую калибровку
      if (i == 2) offsets[i]  = 16384;                               // если ось Z, калибруем в 16384
      offsetsOld[i] = offsets[i];
    }
    // ставим новые оффсеты
    mpu.setXAccelOffset(offsets[0] / 8);
    mpu.setYAccelOffset(offsets[1] / 8);
    mpu.setZAccelOffset(offsets[2] / 8);
    mpu.setXGyroOffset(offsets[3] / 4);
    mpu.setYGyroOffset(offsets[4] / 4);
    mpu.setZGyroOffset(offsets[5] / 4);
    delay(2);
  }

  // запись в память
  for (byte i = 0; i < 6; i  ) {    // обнуляем калибровочный массив
    // оффсеты хранятся в ячейках 1010, 1012, 1014, 1016, 1018, 1020
    if (i < 3)
      EEPROM.updateInt(START_BYTE   i * 2, offsets[i] / 8);
    else
      EEPROM.updateInt(START_BYTE   i * 2, offsets[i] / 4);
  }
}

Также в библиотеке есть готовые функции для калибровки акселя и гиро, они принимают количество итераций калибровки (до 15):

mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);

Функции блокирующие, выполняются и автоматически выставляют оффсеты, т.е. датчик сразу калибруется. Для чтения оффсетов (например для записи в EEPROM) можно воспользоваться тем же способом, что и раньше:

mpu.getXAccelOffset();
mpu.getYAccelOffset();
mpu.getZAccelOffset();
mpu.getXGyroOffset();
mpu.getYGyroOffset();
mpu.getZGyroOffset();

Калибровка компаса и вывод значений осей в монитор порта.

Подключим сенсор через I2C.

Смотрите про коптеры:  Изитроник опель как правильно эксплуатировать советы

При старте происходит калибровка компаса, а далее в монитор порта выводятся значения каждой оси для акселерометра, гироскопа и магнетометра.

#include <Wire.h>
#include <I2Cdev.h>
#include <MPU9250.h>
// По умолчанию адрес устройства на шине I2C - 0x68
MPU9250 accelgyro;
I2Cdev   I2C_M;
uint8_t buffer_m[6];
int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t   mx, my, mz;
float heading;
float tiltheading;
float Axyz[3];
float Gxyz[3];
float Mxyz[3];
// время выполнения предварительной калибровки
#define sample_num_mdate  5000
volatile float mx_sample[3];
volatile float my_sample[3];
volatile float mz_sample[3];
static float mx_centre = 0;
static float my_centre = 0;
static float mz_centre = 0;
volatile int mx_max = 0;
volatile int my_max = 0;
volatile int mz_max = 0;
volatile int mx_min = 0;
volatile int my_min = 0;
volatile int mz_min = 0;
void setup()
{
    //подключаемся к шине I2C (I2Cdev не может сделать это самостоятельно)
    Wire.begin();
    // инициализация подключения в Мониторе порта
    // ( 38400бод выбрано потому, что стабильная работа наблюдается и при 8MHz и при 16Mhz, поэтому
    // в дальнейшем выставляйте скорость согласно ваших требований)
    Serial.begin(38400);
    // Инициализация устройства
    Serial.println("Initializing I2C devices...");
    accelgyro.initialize();
    // Подтверждение подключения
    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection() ? "MPU9250 connection successful" : "MPU9250 connection failed");
    delay(1000);
    Serial.println("     ");
    // Предварительная калибровка магнитометра
    Mxyz_init_calibrated ();
}
void loop()
{
    getAccel_Data();             // Получение значений Акселерометра
    getGyro_Data();              // Получение значений Гироскопа
    getCompassDate_calibrated(); // В этой функции происходит калибровка магнитометра
    getHeading();                // после чего мы получаем откалиброванные значения углов поворота
    getTiltHeading();            // и наклона
    Serial.println("calibration parameter: ");
    Serial.print(mx_centre);
    Serial.print("         ");
    Serial.print(my_centre);
    Serial.print("         ");
    Serial.println(mz_centre);
    Serial.println("     ");
    Serial.println("Acceleration(g) of X,Y,Z:");
    Serial.print(Axyz[0]);
    Serial.print(",");
    Serial.print(Axyz[1]);
    Serial.print(",");
    Serial.println(Axyz[2]);
    Serial.println("Gyro(degress/s) of X,Y,Z:");
    Serial.print(Gxyz[0]);
    Serial.print(",");
    Serial.print(Gxyz[1]);
    Serial.print(",");
    Serial.println(Gxyz[2]);
    Serial.println("Compass Value of X,Y,Z:");
    Serial.print(Mxyz[0]);
    Serial.print(",");
    Serial.print(Mxyz[1]);
    Serial.print(",");
    Serial.println(Mxyz[2]);
    Serial.println("The clockwise angle between the magnetic north and X-Axis:"); // "Угол поворота"
    Serial.print(heading);
    Serial.println(" ");
    Serial.println("The clockwise angle between the magnetic north and the projection of the positive X-Axis in the horizontal plane:"); // "Угол наклона"
    Serial.println(tiltheading);
    Serial.println("   ");
    Serial.println();
    delay(1000);
}
void getHeading(void)
{
    heading = 180 * atan2(Mxyz[1], Mxyz[0]) / PI;
    if (heading < 0) heading  = 360;
}
void getTiltHeading(void)
{
    float pitch = asin(-Axyz[0]);
    float roll = asin(Axyz[1] / cos(pitch));
    float xh = Mxyz[0] * cos(pitch)   Mxyz[2] * sin(pitch);
    float yh = Mxyz[0] * sin(roll) * sin(pitch)   Mxyz[1] * cos(roll) - Mxyz[2] * sin(roll) * cos(pitch);
    float zh = -Mxyz[0] * cos(roll) * sin(pitch)   Mxyz[1] * sin(roll)   Mxyz[2] * cos(roll) * cos(pitch);
    tiltheading = 180 * atan2(yh, xh) / PI;
    if (yh < 0)    tiltheading  = 360;
}
void Mxyz_init_calibrated ()
{
    Serial.println(F("Before using 9DOF,we need to calibrate the compass first. It will takes about 1 minute."));  // Перед использованием сенсора необходимо произвести калибровку компаса. Это займёт около минуты.
    Serial.print("  ");
    Serial.println(F("During  calibrating, you should rotate and turn the 9DOF all the time within 1 minute."));   // На протяжении всего времени калибровки Вам необходимо вращать сенсор во все стороны.
    Serial.print("  ");
    Serial.println(F("If you are ready, please sent a command data 'ready' to start sample and calibrate."));      // Если Вы готовы, для начала калибровки отправьте в Мониторе Порта "ready". 
    while (!Serial.find("ready"));
    Serial.println("  ");
    Serial.println("ready");
    Serial.println("Sample starting......");
    Serial.println("waiting ......");
    get_calibration_Data ();
    Serial.println("     ");
    Serial.println("compass calibration parameter ");
    Serial.print(mx_centre);
    Serial.print("     ");
    Serial.print(my_centre);
    Serial.print("     ");
    Serial.println(mz_centre);
    Serial.println("    ");
}
void get_calibration_Data ()
{
    for (int i = 0; i < sample_num_mdate; i  )
    {
        get_one_sample_date_mxyz();
        /*
        Serial.print(mx_sample[2]);
        Serial.print(" ");
        Serial.print(my_sample[2]);                            // здесь Вы можете увидеть полученные "сырые" значения.
        Serial.print(" ");
        Serial.println(mz_sample[2]);
        */
        if (mx_sample[2] >= mx_sample[1])mx_sample[1] = mx_sample[2];
        if (my_sample[2] >= my_sample[1])my_sample[1] = my_sample[2]; // Поиск максимального значения
        if (mz_sample[2] >= mz_sample[1])mz_sample[1] = mz_sample[2];
        if (mx_sample[2] <= mx_sample[0])mx_sample[0] = mx_sample[2];
        if (my_sample[2] <= my_sample[0])my_sample[0] = my_sample[2]; // Поиск минимального значения
        if (mz_sample[2] <= mz_sample[0])mz_sample[0] = mz_sample[2];
    }
    mx_max = mx_sample[1];
    my_max = my_sample[1];
    mz_max = mz_sample[1];
    mx_min = mx_sample[0];
    my_min = my_sample[0];
    mz_min = mz_sample[0];
    mx_centre = (mx_max   mx_min) / 2;
    my_centre = (my_max   my_min) / 2;
    mz_centre = (mz_max   mz_min) / 2;
}
void get_one_sample_date_mxyz()
{
    getCompass_Data();
    mx_sample[2] = Mxyz[0];
    my_sample[2] = Mxyz[1];
    mz_sample[2] = Mxyz[2];
}
void getAccel_Data(void)
{
    accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
    Axyz[0] = (double) ax / 16384;
    Axyz[1] = (double) ay / 16384;
    Axyz[2] = (double) az / 16384;
}
void getGyro_Data(void)
{
    accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
    Gxyz[0] = (double) gx * 250 / 32768;
    Gxyz[1] = (double) gy * 250 / 32768;
    Gxyz[2] = (double) gz * 250 / 32768;
}
void getCompass_Data(void)
{
    I2C_M.writeByte(MPU9150_RA_MAG_ADDRESS, 0x0A, 0x01); // активируем магнетометр
    delay(10);
    I2C_M.readBytes(MPU9150_RA_MAG_ADDRESS, MPU9150_RA_MAG_XOUT_L, 6, buffer_m);
    mx = ((int16_t)(buffer_m[1]) << 8) | buffer_m[0] ;
    my = ((int16_t)(buffer_m[3]) << 8) | buffer_m[2] ;
    mz = ((int16_t)(buffer_m[5]) << 8) | buffer_m[4] ;
    Mxyz[0] = (double) mx * 1200 / 4096;
    Mxyz[1] = (double) my * 1200 / 4096;
    Mxyz[2] = (double) mz * 1200 / 4096;
}
void getCompassDate_calibrated ()
{
    getCompass_Data();
    Mxyz[0] = Mxyz[0] - mx_centre;
    Mxyz[1] = Mxyz[1] - my_centre;
    Mxyz[2] = Mxyz[2] - mz_centre;
}

Оцените статью
Добавить комментарий

Adblock
detector