Рейтинг@Mail.ru

Как сделать электронный компас на датчике MPU-9250 и Arduino

7 комментарии Arduino
Print Friendly, PDF & Email
Сделаем электронный компас на датчике MPU-9250 (или MPU-9255). В качестве микроконтроллера будем использовать Arduino Nano (Atmega 328). Показания компаса с точностью до 1 градуса будем выводить на трёхразрядный светодиодный 7-сегментный индикатор. Добавим компасу возможность отслеживать азимут по нажатию на кнопку. А при отклонении от азимута будем издавать звуковой сигнал.

Разобьём всю задачу на более мелкие подзадачи. Так сказать, проведём декомпозицию. И по шагам опишем весь процесс создания электронного компаса. Для проекта нам понадобятся:

1Подключение датчика MPU-9250к Arduino

Датчик MPU-9250 – это по сути несколько датчиков, расположенных на одном чипе. Так, он реализует функции 3-осевого гироскопа, 3-осевого акселерометра и 3-осевого магнитометра. В данном проекте мы будем использовать только магнитометр. Остальные датчики можно отключить в целях уменьшения потребляемого тока, т.к. будем делать портативное устройство, питающееся от батареи «Крона».

Внешний вид модуля с датчиком MPU-9250
Внешний вид модуля с датчиком MPU-9250

Модуль имеет 10 выводов. Вот их назначение:

ВыводНазначение вывода модуля с MPU-9250 (MPU-9255)
VCCВнешнее питание 3.3 В.
GNDОбщий.
SCLЛиния тактовых импульсов I2C и SPI.
SDAЛиния данных для I2C или SPI.
EDAЛиния данных при подключении внешних датчиков по шине I2C.
ECLЛиния тактов при подключении внешних датчиков по шине I2C.
AD0Для выставления адреса I2C в режиме I2C. В режиме SPI это линия данных от датчика.
INTЛиния прерываний. Срабатывание настраивается при конфигурировании датчика MPU-9250.
NCSВ режиме SPI – линия выбора ведомого (chip select). В режиме I2C не соединяется ни с чем.
FSYNCЗависит от конфигурации.

Прежде чем подключать датчик MPU-9250 к Arduino, проверим его работоспособность с помощью моей любимой платы с микросхемой FT2232H. Для самого простого теста прочитаем содержимое регистра датчика, в котором содержится постоянное значение. Таким регистром может служить, например, регистр, в котором хранится идентификатор магнитометра, равный 0x48. Подключаться будем по интерфейсу I2C (для сокращения числа проводников). Как и большинство датчиков, MPU-9250 является ведомым на I2C шине. Схема подключения предельно проста: питание +3.3 В подаётся на вывод VCC, земля – GND, тактовая частота приходит на вывод SCL с ножки ADBUS0 микросхемы FT2232, линия данных SDA подключается одновременно к выводам ADBUS1 и ADBUS2 микросхемы FT2232.

Подключение датчика MPU-9250 по I2C в качестве ведомого к микросхеме FT2232H
Подключение датчика MPU-9250 по I2C в качестве ведомого к микросхеме FT2232H

Теперь, когда всё подключено, запускаем в режиме I2C программу SPI via FTDI, многократно описанную нами ранее. Оставляем настройки по умолчанию и сканируем шину I2C. Мы увидим, что программа обнаружила на шине одно устройство по адресу 0x68. Если прочитать из него 127 байтов (именно столько регистров имеет датчик MPU9250, техническое описание можно скачать в приложении к статье), то увидим следующее:

Чтение всех регистров датчика MPU-9250 по I2C с помощью FT2232
Чтение всех регистров датчика MPU-9250 по I2C с помощью FT2232

Здесь нет показаний магнитометра. Магнитометр (он называется AK8963) – это отдельное устройство на кристалле, которое не активно при подаче питания на датчик MPU-9250. Его нужно активировать явно. Для этого необходимо в регистр под номером 0x37 (INT_PIN_CFG) записать значение 0x02. Для этого в программе в поле записи укажем команду "37 02", как на рисунке, и нажмём кнопку «Записать». Полное описание регистра приводится далее.

Включение магнитометра датчика MPU-9250 через регистр I2C_SLV0_ADDR
Включение магнитометра датчика MPU-9250 через регистр I2C_SLV0_ADDR

Карта регистров магнитометра AK8963 представляет собой довольно короткую таблицу, состоящую всего из 13-ти байтов:

Карта регистров магнитометра MPU-9250
Карта регистров магнитометра MPU-9250
Чтение регистров магнитометра MPU-9250 по I2C
Чтение регистров магнитометра MPU-9250 по I2C

Как видно, по адресу 0x00 размещается постоянный Device ID, который должен быть равен 0x48. В нашем случае так и есть. Это хороший признак. Значит, магнитометр как минимум отвечает осмысленные данные, а мы можем их читать.

Схема подключений датчика MPU-9250 к Arduino остаётся предельно простой:

Вывод датчика MPU-9250Вывод Arduino
SCLA5
SDAA4
VCC+3.3V
GNDGND

Для того чтобы использовать датчик MPU-9250 в режиме магнитометра, следует придерживаться такой последовательности действий:

  • настроить регистр управления питанием PWR_MGMT_1 по адресу 0x6B;
    Структура регистра PWR_MGMT_1 датчика MPU-9255
    Структура регистра PWR_MGMT_1 датчика MPU-9255
  • настроить пользовательский регистр контроля USER_CTRL по адресу 0x6A чтобы отключить мастер шины I2C (не будем его использовать);
    Структура регистра USER_CTRL датчика MPU-9255
    Структура регистра USER_CTRL датчика MPU-9255
  • настроить регистр конфигурации прерываний INT_PIN_CFG по адресу 0x37 чтобы включить магнитометр;
    Структура регистра INT_PIN_CFG датчика MPU-9255
    Структура регистра INT_PIN_CFG датчика MPU-9255
  • настроить регистр управления магнитометра CNTL1 по адресу 0x0A на непрерывное проведение измерений.
    Структура регистра CNTL1 магнитометра AK8963
    Структура регистра CNTL1 магнитометра AK8963

Реализуем это в следующем коде.

Скетч для чтения показаний датчика MPU-9255 (разворачивается)
#include <Wire.h>

const byte MPU9255_I2C_ADDR = 0x68; // I2C адрес датчика MPU-9255
const byte MPU9255_MAG_I2C_ADDR = 0x0C; // I2C адрес магнитометра

// Регистры датчика MPU-9255:
const byte USER_CTRL = 0x6A; // { - | FIFO_EN | I2C_MST_EN | I2C_IF_DIS | - | FIFO_RST | I2C_MST_RST | SIG_COND_RST }
const byte PWR_MGMT_1 = 0x6B; // { H_RESET | SLEEP | CYCLE | GYRO_STANDBY | PD_PTAT | CLKSEL[2:0] }
const byte INT_PIN_CFG = 0x37; // { ACTL | OPEN | LATCH_INT_EN | INT_ANYRD_2CLEAR | ACTL_FSYNC | FSYNC_INT_MODE_EN | BYPASS_EN | - }
const byte CNTL1 = 0x0A; // { 0 | 0 | 0 | BIT | MODE3 | MODE2 | MODE1 | MODE0 }

void setup()
{
    Serial.begin(115200);
    Wire.begin();
    Write(PWR_MGMT_1, 0x8); // Отключаем гироскоп 
    Write(USER_CTRL, 0);  // Отключаем I2C мастер 
    Write(INT_PIN_CFG, 0x02); // включаем режим bypass
    WriteMag(CNTL1, 0x12); // задаём непрерывный режим измерений с точностью 16 разрядов
    int mag_id = ReadMag(00); // читаем идентификатор датчика MPU-9255
    Serial.print("Magnetometer ID=");
    Serial.println(mag_id, HEX);
}

void loop()
{
    int xh = ReadMag(0x04); // читаем показания оси X, старший байт
    int xl = ReadMag(0x03); // читаем показания оси X, младший байт
    int yh = ReadMag(0x06); // аналогично для осей Y и Z
    int yl = ReadMag(0x05);
    int zh = ReadMag(0x08);
    int zl = ReadMag(0x07);
    int status = ReadMag(0x09); // сообщаем датчику, что прочитали значения, и можно продолжать измерения
    
    // Собираем значения:
    int x = word(xh, xl); // или так: (xh << 8) | (xl & 0xFF); 
    int y = word(yh, yl); // или так: (yh << 8) | (yl & 0xFF);
    int z = word(zh, zl); // или так: (zh << 8) | (zl & 0xFF);
    
    // Выводим показания в мкТл:
    Serial.print("x=");
    Serial.print(GetMagneticFlux(x));
    Serial.print(", y=");
    Serial.print(GetMagneticFlux(y));
    Serial.print(", z=");
    Serial.print(GetMagneticFlux(z));
    Serial.println(" uT");
    delay(200);
}

// Читает по шине I2C из заданного регистра датчика MPU-9255
byte Read(int reg)
{
    Wire.beginTransmission(MPU9255_I2C_ADDR); // начинаем обмен по I2C
    Wire.write(reg);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU9255_I2C_ADDR, 1, false); // запрашиваем 1 байт от MPU-9255
    byte val = Wire.read();
    Wire.endTransmission(true);
    return val;
}

// Записывает по шине I2C в заданный регистр датчика MPU-9255
void Write(int reg, int data)
{
    Wire.beginTransmission(MPU9255_I2C_ADDR); // начинаем обмен по I2C
    Wire.write(reg);
    Wire.write(data);
    Wire.endTransmission(true);
}

// Читает значение регистра магнитометра
byte ReadMag(int reg)
{
    Wire.beginTransmission(MPU9255_MAG_I2C_ADDR);
    Wire.write(reg);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU9255_MAG_I2C_ADDR, 1, false); // запрашиваем у магнитометра 1 байт
    byte val = Wire.read();
    Wire.endTransmission(true);
    return val;
}

// Записывает значение в регистр магнитометра
void WriteMag(int reg, int data)
{
    Wire.beginTransmission(MPU9255_MAG_I2C_ADDR);
    Wire.write(reg);
    Wire.write(data);
    Wire.endTransmission(true);
}

// Преобразует показания с датчика MPU-9255 в значения магнитного потока в микротеслах
float GetMagneticFlux(int value){
    return (value * 0.15);
}

Теперь остаётся перевести показания индукции магнитного поля в азимут. Для этого воспользуемся следующей формулой:

float GetAzimuth(int x, int y) {
  float azimuth = atan2(x, y) * 180 / PI;
  return azimuth;
}

Будем двигаться к следующей задаче.

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

В качестве индикатора для вывода показаний компаса будем использовать семисегментный индикатор 3361AS-1. Он построен по принципу индикатора с общим катодом.

Светодиодный индикатор с общим катодом – это тип индикатора, состоящий из нескольких светодиодов в одном корпусе, у которых общая земля, а питание на каждый светодиод подаётся отдельно.

Напомню, что 7-сегментным индикатор называется из-за того, что он состоит из 7-ми светодиодов, которые расположены в форме цифры "8". Зажигая определённые сегменты, можно изображать разные цифры. Это похоже на цифры индекса на почтовом конверте: закрашивая определённые участки, мы пишем разные индексы. Зачастую дополнительно к 7-ми сегментам, индикатор содержит десятичную точку. Также индикатор может иметь несколько цифр – разрядов. Сегменты индикатора обозначаются латинскими буквами от A до G, как на рисунке.

Обозначение сегментов индикатора латинскими буквами
Обозначение сегментов индикатора латинскими буквами

Воспользуемся популярным способом управления 7-сегментным индикатором с помощью драйвера CD4511. Это микросхема двоично-десятичного преобразователя, который переводит двоичный код числа в напряжение на соответствующих цифре сегментах индикатора. Такой преобразователь использует всего 4 ножки Arduino. Например, если необходимо отобразить на индикаторе десятичное число "7", следует выставить на входе преобразователя двоичное "0111". Микросхема CD4511 выполняется в разных типах корпусов. Назначение выводов в исполнении с 16-тью ножками, такое:

Выводы двоично-десятичного преобразователя CD4511
Выводы двоично-десятичного преобразователя CD4511

Отечественными аналогами данного преобразователя являются микросхемы серий ИД1…ИД7.

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

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

Индикатор 3361AS не имеет токоограничительных резисторов, поэтому необходимо озаботиться этим самому, подключая индикатор. При напряжении питания 5 В сопротивление на каждый сегмент должно быть около 200 Ом.

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

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

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

Скетч для управления трёхразрядным 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%10].b0);
  digitalWrite(bit1, numbers[n%10].b1);
  digitalWrite(bit2, numbers[n%10].b2);
  digitalWrite(bit3, numbers[n%10].b3);
}
Управление трёхразрядным семисегментным индикатором с помощью преобразователя CD4511 и Arduino
Управление трёхразрядным семисегментным индикатором с помощью преобразователя CD4511 и Arduino

Итак, теперь мы умеем выводить трёхзначные числа на 7-сегментный индикатор, что нам понадобится для отображения азимута.

3Подключение динамика / пьезоизлучателяк Arduino

Для оповещения об отклонении от азимута, как было решено, будем использовать звуковой пьезоизлучатель. Мы уже обсуждали в отдельной статье, как подключить пьезоизлучатель к Arduino. Поэтому останавливаться подробно здесь не будем. Напомню ключевые моменты.

Схема подключения излучателя к Arduino очень простая: объединяем земли, а в цепь питания ставим резистор сопротивлением около 100 Ом (для защиты порта Ardunio).

Схема подключения пьезоизлучателя к Arduino
Схема подключения пьезоизлучателя к Arduino

Для Arduino есть специальные функции tone() и noTone(), которые используются для извлечения звука заданной частоты:

    tone(piezoPin, freq); // подача звука с частотой freq (Гц)
    tone(piezoPin, freq, duration); // подача звука с частотой freq (Гц) длительностью duration (мс)
    noTone(piezoPin); // остановка звука

Здесь piezoPin – номер вывода Arduino, к которой подключён звуковой извещатель. Давайте изменим в предыдущем скетче функцию loop() таким образом (изменения выделены жирным):

void loop()
{
  // Каждую секунду увеличиваем показания индикатора на 1:
  int sec = seconds();
  for (int i=0; i<1000; i++) {
    while (sec == seconds()) {
      print_number(i);
      if (sec % 60 == 0){ // каждые 60 секунд
        tone(piezoPin, 1000, 500); // издавать звуковой сигнал
      }
    }
    sec = seconds();
  }
}

И конечно же, не забудем объявить в начале скетча piezoPin и задать ему режим работы OUTPUT. Теперь каждую минуту излучатель будет подавать звуковой сигнал продолжительностью 500 мс и частотой 1000 Гц.

4Собираем всё вместе:компас на Arduino

Схема нашего устройства будет такой (нарисована в DipTrace Schematic):

Схема электронного компаса на MPU-9255 и Arduino
Схема электронного компаса на MPU-9255 и Arduino

Здесь ARD1 – это Arduino Nano, CD4511 – драйвер управления 7-сегментным дисплеем 3361AS, MPU-9255 – собственно, сам модуль с магнитным датчиком, SW1 – кнопка для запуска и останова отслеживания азимута, BUZ – звуковой извещатель, а PWR – клемма для подачи внешнего питания от батареи «Крона» на устройство.

Монтаж компаса будем производить на печатной плате, которую «разведём» в программе DipTrace PCB Layout.

Печатная плата для электронного компаса на Arduino
Печатная плата для электронного компаса на Arduino

Закажем печатную плату здесь. На этом предприятии делают всё быстро и качественно. Например, изготовление данной печатной платы заняло около суток от момента заказа до отправки. Единственный минус – придётся долго ждать доставки из Китая (2-4 недели).

Печатная плата для электронного компаса на MPU-9255 и Arduino
Печатная плата для электронного компаса на MPU-9255 и Arduino

Распаяем элементы на плате.

Пайка радиоэлементов на плате электронного компаса MPU-9250
Пайка радиоэлементов на плате электронного компаса MPU-9250

После распайки компонентов плата электронного компаса будет выглядеть так:

Печатная плата для электронного компаса с распаянными элементами
Печатная плата для электронного компаса с распаянными элементами

Останется только придумать какой-то корпус для платы с компасом.

Сборка платы с датчиком MPU-9250 в корпус
Сборка платы с датчиком MPU-9250 в корпус

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

Электронный компас в процессе отладки
Электронный компас в процессе отладки

Во-вторых, изначальный скетч определения азимута выводит довольно приблизительные и нестабильные измерения. Поэтому в части работы компаса всё было переделано. Я взял за основу скетч, представленный в этой статье. Он отличается тем, что используются показания акселерометра для коррекции наклона датчика, а также вводятся дополнительные коррекции, связанные с индивидуальными особенностями датчика (в частности, чувствительность ASAX, ASAY, ASAZ). Для нормальной работы этого скетча необходимо сделать следующее.

  1. Определить магнитное наклонение географического места, в котором вы находитесь. Сделать это можно, например, на сайте magnetic-declination.com. Значение магнитного наклонения необходимо присвоить переменной Declination скетча.
  2. Скорректировать остаточный угол наклона акселерометра MPU-9255. Для этого следует положить датчик на ровную горизонтальную поверхность и добиться того, чтобы в выводе монитора последовательного порта значения accel_pitch и accel_roll были максимально близкими к нулю. Это достигается в несколько итераций, последовательными приближениями. Начните с нулевых значений.
  3. Ввести калибровочные коэффициенты. Для этого необходимо задать Record_data = true. Скомпилировать и загрузить в плату скетч. Медленно вращать компас во всех направлениях, пока в мониторе порта не появятся значения коэффициентов. Скопировать из монитора порта эти значения в соответствующие поля в скетче (они там обозначены). Вернуть Record_data = false, скомпилировать и загрузить скетч в Arduino с новыми коэффициентами.

Для всего описанного ниже переменная DEBUG в скетче должна быть true. Более подробные пояснения приводятся в самом скетче.

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

Скетч для магнитного компаса на датчике MPU-9250/MPU-9255 (разворачивается)
#include <ButtonDebounce.h>
#include <Wire.h>

#define DEBUG true // для вывода отладочных сообщений

ButtonDebounce button(2, 50); // кнопка

#define  Piezo  4   // пищалка
#define  Fsync  3   // вывод FSYNC датчика MPU9250/MPU9255
#define  Intr   12  // вывод прерывания датчика MPU9250

// Выводы Arduino для управления двоичным конвертером CD4511:
#define  Bit0  11
#define  Bit1  10
#define  Bit2  9
#define  Bit3  8

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

bool TrackingOn         = false;  // признак включения/выключения режима отслеживания
float CurrentAzimuth    = 0;      // текущий азимут
float TrackingAzimuth   = 0;      // азимут, который необходимо отслеживать
float AzmThreshold      = 3;      // максимальное отклонение от азимута отслеживания, градусы, +/-
int Counter             = 0;      // счётчик измерений для сглаживания азимута
const int SmoothingBy   = 5;     // по скольким элементам сглаживать показания азимута
float AzimuthArray[SmoothingBy];  // массив азимутов, по которым проводится усреднение

// Регистры датчика MPU9250:
#define MPU9255_I2C_ADDR  0x68  // I2C адрес датчика MPU-9255
#define PWR_MGMT_1        0x6B  // регистр управления питанием (H_RESET|SLEEP|CYCLE|GYRO_STANDBY|PD_PTAT|CLKSEL[2:0])
#define ACCEL_CONFIG      0x1C  // регистр конфигурации акселерометра (ax_st_en|ay_st_en|az_st_en|ACCEL_FS_SEL[1:0]|-[2:0])
#define GYRO_CONFIG       0x1B  // регистр конфигурации гироскопа (XGYRO_Ct_en|YGYRO_Ct_en|ZGYRO_Ct_en|GYRO_FS_SEL[1:0]|-|FCHOICE_B[1:0])
#define USER_CTRL         0x6A  // USER_CTRL[5]=I2C_MST_EN
#define INT_PIN_CFG       0x37  // INT_PIN_CFG[1]=BYPASS_EN

// Гироскоп
bool    Gyro_synchronised = false; // флаг синхронизации гироскопа

#define Frequency 125           // интервал сэмплирования = 8 мс
#define Gyro_Sensitivity 65.5   // чувствительность гироскопа (см. datasheet Gyro_Sensitivity)

int     Gyro_x, Gyro_y, Gyro_z;
long    Gyro_x_cal, Gyro_y_cal, Gyro_z_cal;
float   Gyro_pitch, Gyro_roll, Gyro_yaw;
float   Gyro_pitch_output, Gyro_roll_output;

// Акселерометр
long    Accel_x, Accel_y, Accel_z;

// Магнитометр
#define AK8963_I2C_ADDR 0x0C               // I2C адрес магнитометра AK8963
#define CNTL1 0x0A                         // CNTL[3:0] - режим работы магнитометра
#define AK8963_status_reg_1 0x02           // ST1[0] - готовность данных DRDY
#define AK8963_data_ready_mask 0b00000001  // маска готовности данных
#define AK8963_overflow_mask 0b00001000    // маска признака переполнения магнитометра
#define AK8963_data_start 0x03             // начальный адрес данных по X,Y,Z
#define AK8963_ASA 0x10                    // коррекция чувствительности (ASA) по осям X,Y,Z (fuze ROM)

float   Declination = +11.532; // магнитное склонение в градусах. УКАЖИТЕ ЗДЕСЬ СВОЁ ЗНАЧЕНИЕ.

int     Mag_x, Mag_y, Mag_z;  // "сырые" данные магнитометра
float   Mag_x_dampened, Mag_y_dampened, Mag_z_dampened; // со сглаженными флуктуациями
float   Mag_x_hor, Mag_y_hor;
float   Mag_pitch, Mag_roll;

// Калибровочные данные:
/*
     При первом использовании для учёта аппаратных и программных искажений необходимо откалибровать компас.
     Для этого:
     1) Задать (Record_data = true).
     2) Открыть монитор последовательного порта.
     3) Медленно вращать компас во всех направлениях.
     4) Скопировать значения из монитора порта в поля ниже.
     5) Задать (Record_data = false), скомпилировать и загрузить скетч в Arduino.
*/
bool    Record_data = false; // ЗАДАЙТЕ TRUE ДЛЯ КАЛИБРОВКИ.
int     Mag_x_offset = 162, Mag_y_offset = 69,  Mag_z_offset = 92;  // аппаратные отклонения (hard-iron)
float   Mag_x_scale = 0.72, Mag_y_scale = 0.79, Mag_z_scale = 2.96; // программные поправочные коэффициенты (soft-iron)
float   ASAX = 1.20,        ASAY = 1.20,        ASAZ = 1.16;        /* значения коэффициентов чувствительности (A)sahi (S)ensitivity (A)djustment, 
                                                                     * записанные в однократно программируемое ПЗУ (fuse ROM) */
// Макросы:
#define Sensor_to_deg   1/(Gyro_Sensitivity*Frequency)  // макрос преобразования показаний датчика в градусы
#define Sensor_to_rad   Sensor_to_deg*DEG_TO_RAD        // макрос преобразования показаний датчика в радианы
#define Loop_time       1000000/Frequency               // макрос вычисления времени цикла (мкс)

long    Loop_start;  // время начала цикла измерения, мкс
long    Loop_start_time;

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);

  pinMode(Intr, INPUT);
  pinMode(Fsync, OUTPUT);
  digitalWrite(Fsync, LOW);

  pinMode(Piezo, OUTPUT);
  
  button.setCallback(buttonChanged); // назначаем обработчик нажатия кнопки
  
  Wire.begin();          // инициализация шины I2C
  Wire.setClock(400000); // с заданной скоростью
  delay(100);
#if DEBUG
  Serial.begin(115200);
#endif

  int mag_id = ReadMag(0x00, 1); // читаем идентификатор датчика MPU-9255
#if DEBUG
  Serial.print("Magnetometer ID = ");
  Serial.println(mag_id, HEX);
#endif
  if (mag_id != 0x48) { // если идентификатор датчика неверный (датчик отсутствует или неисправен)
    do
    {
      buzz(1, 1000); // бесконечный цикл звукового оповещения о неготовности датчика
    }
    while (true);
  }

  configure_magnetometer(); // конфигурируем магнитометр

  // Калибруем магнитометр (по необходимости):
  if (Record_data) 
  {
    calibrate_magnetometer();
  }
  
  config_mpu9255(); // конфигурируем датчик MPU-9250/9255
  calibrate_gyro(); // калибруем гироскоп
  
  Loop_start_time = micros(); // запоминаем время начала цикла измерения для контроля скорости обновления данных гироскопа
}

void loop()
{
  button.update(); // определяем состояние кнопки  
  
  ////////////////////////////////////////////
  //        ВЫЧИСЛЕНИЕ УГЛА НАКЛОНА         //
  ////////////////////////////////////////////

  /* Ориентация MPU-9250; ось X смотрит на север */
  read_mpu_9250_data();   // читаем "сырые" данные акселерометра и гироскопа MPU-6050
  Gyro_x -= Gyro_x_cal;   // вычитаем смещение X
  Gyro_y -= Gyro_y_cal;   // вычитаем смещение Y
  Gyro_z -= Gyro_z_cal;   // вычитаем смещение Z

  // Вычисление углов
  /*
    Pitch - тангаж, наклон вперёд/назад; roll - крен, наклон вправо/влево. 
    Далее я не буду расшифровывать эти термины и буду называть их pitch и roll.
    Подстройка знаков показаний X,Y,Z гироскопа:
    Pitch (Nose - up) = +ve reading 
    Roll (Right - wing down) = +ve reading
    Yaw (Clock - wise rotation)  = +ve reading
  */
  // Суммируем с "сырыми" показаниями гироскопа:
  Gyro_pitch  += -Gyro_y * Sensor_to_deg;  // по оси Y
  Gyro_roll   +=  Gyro_x * Sensor_to_deg;  // по оси X
  Gyro_yaw    += -Gyro_z * Sensor_to_deg;  // по оси Z

  // Компенсация угла крена гироскопа:
  Gyro_pitch += Gyro_roll * sin(Gyro_z * Sensor_to_rad); 
  Gyro_roll -= Gyro_pitch * sin(Gyro_z * Sensor_to_rad); 

  // Вычисление угла наклона акселерометра:
  long accel_total_vector = sqrt((Accel_x * Accel_x) + (Accel_y * Accel_y) + (Accel_z * Accel_z)); // полный вектор
  float accel_pitch = asin((float)Accel_x / accel_total_vector) * RAD_TO_DEG; // угол крена pitch
  float accel_roll = asin((float)Accel_y / accel_total_vector) * RAD_TO_DEG;  // угол крена roll

  /* 
   *  Учёт остаточных отклонений показаний акселерометра:
   *  1) поместите акселерометр на ровную горизонтальную поверхность;     
   *  2) подберите эти 2 значения, пока углы крена pitch и roll не станут "0".
  */
  accel_pitch -= 1.0f;  // корректировка угла крена (pitch) - ЗАДАЙТЕ СВОЁ ЗНАЧЕНИЕ
  accel_roll  -= 1.0f;  // корректировка угла крена (roll) - ЗАДАЙТЕ СВОЁ ЗНАЧЕНИЕ

  // Коррекция показаний гироскопа:
  if (Gyro_synchronised)  // если гироскоп и акселерометр синхронизированы
  {
    // коррекция крена гироскопа:
    Gyro_pitch = 0.9996 * Gyro_pitch + 0.0004 * accel_pitch; // угол pitch
    Gyro_roll  = 0.9996 * Gyro_roll  + 0.0004 * accel_roll;  // угол roll
  }
  else // иначе синхронизируем гироскоп и акселерометр
  {
    // задаём равными углы отклонения pitch и roll гироскопа и акселерометра:
    Gyro_pitch = accel_pitch;
    Gyro_roll = accel_roll;
    Gyro_synchronised = true; // устанавливаем флаг синхронизации
  }

  // Учёт флуктуаций:
  Gyro_pitch_output = 0.9 * Gyro_pitch_output + 0.1 * Gyro_pitch; // берём 90% от выходного значения угла pitch и добавляем 10% "сырого" угла pitch
  Gyro_roll_output  = 0.9 * Gyro_roll_output  + 0.1 * Gyro_roll;  // берём 90% от выходного значения угла roll и добавляем 10% "сырого" угла roll
 
  ////////////////////////////////////////////
  //    ВЫЧИСЛЕНИЕ ПОКАЗАНИЙ МАГНИТОМЕТРА   //
  ////////////////////////////////////////////

  read_magnetometer(); // читаем показания магнитометра

  // Коррекция угла наклона и знака азимута:
  /*
     Оси X и Y гироскопа MPU-9250 и магнитометра AK8963 ориентированы под углом 90 градусов друг к другу,
     поэтому Mag_pitch совпадает с Gyro_roll, а Mag_roll совпадает с Gyro_pitch.
     Ось Z датчика MPU-9520 и ось Z магнитометра AK8963 направлены в противоположных направлениях, 
     поэтому для компенсации знак Mag_pitch должен быть взят с противоположным знаком.
  */
  Mag_pitch = -Gyro_roll_output  * DEG_TO_RAD;
  Mag_roll  =  Gyro_pitch_output * DEG_TO_RAD;

  // Применяем стандартные формулы наклона:
  Mag_x_hor = Mag_x * cos(Mag_pitch) + Mag_y * sin(Mag_roll) * sin(Mag_pitch) - Mag_z * cos(Mag_roll) * sin(Mag_pitch);
  Mag_y_hor = Mag_y * cos(Mag_roll)  + Mag_z * sin(Mag_roll);
  
  // Учёт флуктуаций:
  Mag_x_dampened = 0.9 * Mag_x_dampened + 0.1 * Mag_x_hor;
  Mag_y_dampened = 0.9 * Mag_y_dampened + 0.1 * Mag_y_hor;

  // Вычисляем азимут. Азимут - в направлении оси X магнитометра:
  float Heading = atan2(Mag_x_dampened, Mag_y_dampened) * RAD_TO_DEG; // направление на магнитный полюс
  /*  Для вычисления направления на географический полюс необходимо учесть магнитное склонение.
   *  Магнитное склонение Declination для своего географического местоположения 
   *  можно найти, например, на сайте http://www.magnetic-declination.com/ 
   */
  Heading += Declination; // направление на географический северный полюс
    
  /*   
   * Магнитное отклонение положительно, когда магнитный полюс расположен    
   * на востоке от географического севера, и отрицательно - когда на западе.    
   * Для перевода в систему 0-360, необходимо скорректировать значение.
  */
  // Перевод азимута в систему 0..360 градусов:
  if (Heading >= 360.0) Heading -= 360.0;
  if (Heading < 0.0) Heading += 360.0;
  
  // Сглаживаем показания азимута:
  if (Counter < SmoothingBy)
  {
    AzimuthArray[Counter] = Heading; // заполняем массив азимутов
    Counter +=1;
  }
  else // когда массив заполнен, вычисляем среднее арифметическое значение азимута
  {    
    CurrentAzimuth = 0;
    for (int i=0; i<SmoothingBy; i++)
    {
      CurrentAzimuth += AzimuthArray[i];
      // если значения около "0", то может быть ошибка усреднения и заниженный результат;
      // но ситуация довольно редкая, поэтому оставим пока без обработки.
    }
    CurrentAzimuth /= SmoothingBy; // считаем среднее арифметическое
    Counter = 0; // сбрасываем счётчик
  }

// Выводим азимут:
#if DEBUG 
  Serial.print((String)CurrentAzimuth + "\t");
  Serial.print((String)accel_pitch + "\t"); // ...и углы наклона pitch и roll акселерометра
  Serial.println((String)accel_roll);
#endif
  printNumber(round(CurrentAzimuth)); // выводим азимут на 7-сегментный дисплей
  
  if (TrackingOn) // если включено отслеживание...
  {
    // ...проверяем, что не отклонились от заданного азимута больше допустимого:
    float delta = abs(CurrentAzimuth - TrackingAzimuth);
    if (delta > AzmThreshold) // при превышении допуска 
    {
      buzz(1, 1000); // издаём звуковой сигнал 
      digitalWrite(LED_BUILTIN, HIGH); // и зажигаем светодиод статуса
    }
    else // если азимут в допуске
    {
      digitalWrite(LED_BUILTIN, LOW); // гасим светодиод статуса
    }
  }
  
  // Контроль за циклом измерений
  while ((micros() - Loop_start_time) < 8000); // ждём окончания цикла
  Loop_start_time = micros(); // запоминаем время начала нового цикла измерений
}

//  Конфигурирование магнитометра.
void configure_magnetometer()
{
  /*     
   *  Для доступа к магнитометру AK8963 шина I2C датчика MPU-9250
   *  должна быть переведена в режим pass-through. Для этого необходимо:
   *  - отключить мастер I2C;
   *  - включить режим bypass.
  */
  // Отключаем мастер шины I2C датчика MPU9250:
  Wire.beginTransmission(MPU9255_I2C_ADDR);
  Wire.write(USER_CTRL);
  Wire.write(0x00);  // отключаем мастер интерфейса I2C
  Wire.endTransmission();

  // Активируем режим bypass:
  Wire.beginTransmission(MPU9255_I2C_ADDR);
  Wire.write(INT_PIN_CFG);
  Wire.write(0x02);  // включаем режим bypass
  Wire.endTransmission();

  /*  Доступ к коэффициентам чувствительности AK8963.
   *  Заводские значения коэффициентов чувствительности для осей X,Y,Z хранятся в однократно программируемом ПЗУ (fuse ROM).
   *  Для доступа к ним мы должны изменить режим работы магнитометра AK9863.
   */
  Wire.beginTransmission(AK8963_I2C_ADDR);
  Wire.write(CNTL1);      // CNTL1[3:0] - биты управления режимом
  Wire.write(0b00011111); // задаём режим: выход 16-битный; доступ к ПЗУ
  Wire.endTransmission();  
  delay(100); // ожидание смены режима

  // Читаем заводские установки чувствительности по X,Y,Z из ПЗУ магнитометра:
  Wire.beginTransmission(AK8963_I2C_ADDR); 
  Wire.write(AK8963_ASA);
  Wire.endTransmission();
  
  Wire.requestFrom(AK8963_I2C_ADDR, 3); // запрашиваем 3 байта данных (регистры ASAX, ASAY, ASAZ)
  while (Wire.available() < 3); // ждём данные  
  // Коррекция коэффициентов чувствительности:
  ASAX = (Wire.read() - 128) * 0.5 / 128 + 1; // по X
  ASAY = (Wire.read() - 128) * 0.5 / 128 + 1; // по Y
  ASAZ = (Wire.read() - 128) * 0.5 / 128 + 1; // по Z
  /* Это получается из формулы на странице 53 документа "MPU-9250, Register Map and Decriptions":
      Hadj = H*(((ASA-128)*0.5)/128) + 1, где
      H    - считанное из регистра значение;
      ASA  - значение чувствительности по оси;
      Hadj - измеренное скорректированное значение.
  */

  // Выключаем магнитометр AK8963 пока идёт смена режима:
  Wire.beginTransmission(AK8963_I2C_ADDR);
  Wire.write(CNTL1);
  Wire.write(0);  // выключаем магнитометр
  Wire.endTransmission();
  delay(100); // ожидание смены режима

  // Задаём режим 2:
  Wire.beginTransmission(AK8963_I2C_ADDR);
  Wire.write(CNTL1);
  Wire.write(0b00010110); // выход 16-битный, измерения непрерывные с частотой 100 Гц
  Wire.endTransmission();
  delay(100); // ожидание смены режима
}

// Калибровка магнитометра.
void calibrate_magnetometer()
{
#if DEBUG
  Serial.println("Slowly rotate compass ");
#endif
  // Максимумы:
  int mag_x_min = 0x7FFF;
  int mag_y_min = 0x7FFF;
  int mag_z_min = 0x7FFF;
  int mag_x_max = 0x8000;
  int mag_y_max = 0x8000;
  int mag_z_max = 0x8000;

  // Запоминаем min/max показания компаса по осям X,Y,Z:
  for (int cnt=0; cnt<16000; cnt++) // по 16000 значениям
  {
    Loop_start = micros(); // запоминаем время начала цикла

    Wire.beginTransmission(AK8963_I2C_ADDR);
    Wire.write(AK8963_status_reg_1); // регистр статуса ST1
    Wire.endTransmission();
    
    Wire.requestFrom(AK8963_I2C_ADDR, 1); // читаем 1 байт из регистра статуса ST1
    while (Wire.available() < 1);
    if (Wire.read() & AK8963_data_ready_mask) // проверяем бит готовности данных (бит 0 регистра - DRDY)
    {
      // Читаем данные по осям X,Y,Z:
      Wire.requestFrom(AK8963_I2C_ADDR, 7);
      while (Wire.available() < 7);
      // Комбинируем LSB,MSB, применяем коррекцию ASA по X,Y,Z:
      int mag_x = (Wire.read() | Wire.read() << 8) * ASAX;
      int mag_y = (Wire.read() | Wire.read() << 8) * ASAY;
      int mag_z = (Wire.read() | Wire.read() << 8) * ASAZ;
      int status2 = Wire.read(); // читаем из регистра ST2; тем самым сообщаем, что закончили чтение данных

      // Проверка прочитанных данных:
      if (!(status2 & AK8963_overflow_mask)) // проверка флага переполнения HOFL в ST2[3]
      {
        // Определяем max/min значения:
        mag_x_min = min(mag_x, mag_x_min);
        mag_x_max = max(mag_x, mag_x_max);
        mag_y_min = min(mag_y, mag_y_min);
        mag_y_max = max(mag_y, mag_y_max);
        mag_z_min = min(mag_z, mag_z_min);
        mag_z_max = max(mag_z, mag_z_max);
      }
    }
    delay(4); // время между чтениями магнитометра
  }

  // Вычисляем аппаратные поправки (hard-iron) как средние значения:
  Mag_x_offset = (mag_x_max + mag_x_min) / 2;
  Mag_y_offset = (mag_y_max + mag_y_min) / 2;
  Mag_z_offset = (mag_z_max + mag_z_min) / 2;

  // Вычисляем программные коэффициенты шкалы (soft-iron):
  float chord_x = ((float)(mag_x_max - mag_x_min)) / 2;
  float chord_y = ((float)(mag_y_max - mag_y_min)) / 2;
  float chord_z = ((float)(mag_z_max - mag_z_min)) / 2;

  float chord_average = (chord_x + chord_y + chord_z) / 3; // вычисляем среднее по всем трём осям

  // Вычисляем коэффициенты шкалы:
  Mag_x_scale = chord_average / chord_x;
  Mag_y_scale = chord_average / chord_y;
  Mag_z_scale = chord_average / chord_z;

  // В режиме первичной калибровки выводит данные магнитометра в последовательный порт:
  if (Record_data)
  {
#if DEBUG
    // Выводим пределы:
    Serial.print("XYZ Max/Min: ");
    Serial.print((String)mag_x_min + "\t");
    Serial.print((String)mag_x_max + "\t");
    Serial.print((String)mag_y_min + "\t");
    Serial.print((String)mag_y_max + "\t");
    Serial.print((String)mag_z_min + "\t");
    Serial.println((String)mag_z_max);

    // Выводим аппаратные отклонения:
    Serial.print("Hard-iron: ");
    Serial.print((String)Mag_x_offset + "\t");
    Serial.print((String)Mag_y_offset + "\t");
    Serial.println(Mag_z_offset);

    // Выводим программные коэффициенты:
    Serial.print("Soft-iron: ");
    Serial.print((String)Mag_x_scale + "\t");
    Serial.print((String)Mag_y_scale + "\t");
    Serial.println(Mag_z_scale);

    // Выводим значения чувствительности из ПЗУ магнитометра:
    Serial.print("ASA: ");
    Serial.print((String)ASAX + "\t");
    Serial.print((String)ASAY + "\t");
    Serial.println(ASAZ);
#endif
    while (true); // дальше не идём (входим бесконечный цикл)
  }
}

// Читает показания магнитометра.
void read_magnetometer()
{
  Wire.beginTransmission(AK8963_I2C_ADDR); 
  Wire.write(AK8963_status_reg_1); // указатель на бит статуса ST1[0]
  Wire.endTransmission();
  
  Wire.requestFrom(AK8963_I2C_ADDR, 1); // запрашиваем 1 байт данных
  while (Wire.available() < 1); // ждём данные
  if (Wire.read() & AK8963_data_ready_mask) // проверяем бит готовности данных
  {
    // Читаем данные по осям:
    Wire.requestFrom(AK8963_I2C_ADDR, 7); // запрашиваем 7 байтов данных
    while (Wire.available() < 7); // ждём данные
    
    // Объединяем LSB и MSB, применяем коррекцию ASA по X,Y,Z:
    int mag_x = (Wire.read() | Wire.read() << 8) * ASAX;
    int mag_y = (Wire.read() | Wire.read() << 8) * ASAY;
    int mag_z = (Wire.read() | Wire.read() << 8) * ASAZ;
    int status2 = Wire.read(); // читаем из регистра ST2; тем самым сообщаем, что закончили чтение данных

    // Проверяем данные:
    if (!(status2 & AK8963_overflow_mask)) // проверяем флаг HOFL в ST2[3]
    {
      Mag_x = (mag_x - Mag_x_offset) * Mag_x_scale;
      Mag_y = (mag_y - Mag_y_offset) * Mag_y_scale;
      Mag_z = (mag_z - Mag_z_offset) * Mag_z_scale;
    }
  }
}

//  Конфигурирование датчика MPU-9250/9255.
void config_mpu9255()
{
  // Активируем MPU9255:
  Wire.beginTransmission(MPU9255_I2C_ADDR);
  Wire.write(PWR_MGMT_1);  // в регистр управления питанием записываем, что...
  Wire.write(0x00);  // ...будем использовать внутренний источник тактового сигнала 20 МГц
  Wire.endTransmission();

  // Конфигурируем акселерометр:
  Wire.beginTransmission(MPU9255_I2C_ADDR);
  Wire.write(ACCEL_CONFIG);
  Wire.write(0x10);  // задаём предел шкалы измерений (+/-8g)
  Wire.endTransmission();

  // Конфигурируем гироскоп:
  Wire.beginTransmission(MPU9255_I2C_ADDR);
  Wire.write(GYRO_CONFIG);
  Wire.write(0x08);  // выбираем шкалу 500dps (DPS – degrees per second, градусы в секунду)
  Wire.endTransmission();
}

// Калибровка гироскопа.
void calibrate_gyro()
{
  digitalWrite(LED_BUILTIN, HIGH); // покажем, что начали калибровку, включив светодиод
  buzz(3, 1000);
#if DEBUG
  Serial.println("Calibrating gyro");
#endif

  for (int cnt=0; cnt<2000; cnt++) // усредним показания гироскопа по 2000 значениям
  {
    Loop_start = micros();
    read_mpu_9250_data();  // читаем "сырые" данные акселерометра и гироскопа MPU-6050
    Gyro_x_cal += Gyro_x;  // добавляем отклонение по X
    Gyro_y_cal += Gyro_y;  // добавляем отклонение по Y
    Gyro_z_cal += Gyro_z;  // добавляем отклонение по Z
    while (micros() - Loop_start < Loop_time);  // ждём истечения периода времени Loop_time
  }
  // Вычисляем среднее отклонение по X,Y,Z:
  Gyro_x_cal /= 2000;
  Gyro_y_cal /= 2000;
  Gyro_z_cal /= 2000;
  
#if DEBUG
  Serial.println("Calibrating gyro done");
#endif
  digitalWrite(LED_BUILTIN, LOW); // гасим светодиод
  buzz(3, 1000);
}

// Читает "сырые" данные с MPU-9250/9255.
void read_mpu_9250_data()
{
  Wire.beginTransmission(MPU9255_I2C_ADDR);
  Wire.write(0x3B); // адрес регистра начала данных (ACCEL_XOUT_H)
  Wire.endTransmission();

  Wire.requestFrom(MPU9255_I2C_ADDR, 14); // запрашиваем 14 байтов у MPU-6050
  while (Wire.available() < 14);          // ждём данные
  // Показания акселерометра по X,Y,Z:
  Accel_x = Wire.read() << 8 | Wire.read();
  Accel_y = Wire.read() << 8 | Wire.read();
  Accel_z = Wire.read() << 8 | Wire.read();
  int temp = Wire.read() << 8 | Wire.read();  /* данные о температуре   
   *  (не используем, но прочитать нужно, чтобы продвинуться на 2 регистра вперёд)
   */
  // Показания гироскопа по X,Y,Z:
  Gyro_x = Wire.read() << 8 | Wire.read();
  Gyro_y = Wire.read() << 8 | Wire.read();
  Gyro_z = Wire.read() << 8 | Wire.read();
}

// Читает num число байтов, начиная с заданного регистра магнитометра.
byte ReadMag(int reg, int num)
{
    Wire.beginTransmission(AK8963_I2C_ADDR);
    Wire.write(reg);
    Wire.endTransmission(false);
    
    Wire.requestFrom(AK8963_I2C_ADDR, num, false); // запрашиваем у магнитометра num байтов
    byte val = Wire.read();
    Wire.endTransmission(true);
    return val;
}

// ПЬЕЗО ********************************************************

// Издаёт звуковой сигнал заданное число раз с заданной частотой звука.
void buzz(int num, int freq)
{
  for (int i=0; i<num-1; i++) 
  {
    tone(Piezo, freq, 50);
    delay(100);
  }
  tone(Piezo, freq, 50);
}

// КНОПКА *******************************************************

// Обработчик нажатия кнопки.
// Если кнопка нажата, то включает слежение за азимутом.
// Повторное нажатие выключает отслеживание азимута.
void buttonChanged(int state)
{
  if (state)
  {
    TrackingOn = !TrackingOn;
    if (TrackingOn)
    {
      TrackingAzimuth = CurrentAzimuth; // запоминаем азимут, который нужно отслеживать
      buzz(1, 500); // сигнализируем о начале отслеживания азимута
    }
    else
    {
      digitalWrite(LED_BUILTIN, LOW); // гасим светодиод
      buzz(2, 500); // сигнализируем о завершении отслеживания азимута
    }
#if DEBUG
      Serial.print("Tracking=");
      Serial.println(TrackingOn);
#endif
  }
}

// 7-СЕГМЕНТНЫЙ ДИСПЛЕЙ *****************************************

// Выводит 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(5);
  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 
  }; // массив структур типа number

  digitalWrite(Bit0, numbers[n%10].b0);
  digitalWrite(Bit1, numbers[n%10].b1);
  digitalWrite(Bit2, numbers[n%10].b2);
  digitalWrite(Bit3, numbers[n%10].b3);
}
Последнее изменениеВторник, 09 Январь 2024 19:57 Прочитано 22260 раз

Поблагодарить автора:

Поделиться

Print Friendly, PDF & Email

7 комментарии

  • Dima
    Dima 08.05.2020 11:09 Комментировать

    А как сделать так, чтобы работал гироскоп, акселерометр и компас?

  • aave1
    aave1 09.05.2020 15:08 Комментировать

    Dima, это слишком общий вопрос. На такой вопрос - общий ответ: следовать документации на датчик MPU-9250. Он имеет большие возможности и огромное число настраиваемых параметров. Краткий ответ: использовать готовые библиотеки для Arduino, они есть. Просто мне интереснее в данном случае разобраться, как взаимодействовать с датчиком MPU-9250 самому, без "посредников".

  • неАдмин
    неАдмин 28.03.2021 16:53 Комментировать

    Откуда брали адреса регистров и значения? Можно ли их вычислить не имея datasheet? Есть микросхема магнитометр + акселерометр с i2c. Знаю только куда провода цеплять.

  • aave1
    aave1 28.03.2021 17:41 Комментировать

    неАдмин, адреса регистров можно узнать только из документации на модуль.

  • Дмитрий
    Дмитрий 28.08.2021 19:55 Комментировать

    // Применяем стандартные формулы наклона:
    Mag_x_hor = Mag_x * cos(Mag_pitch) + Mag_y * sin(Mag_roll) * sin(Mag_pitch) - Mag_z * cos(Mag_roll) * sin(Mag_pitch);
    Mag_y_hor = Mag_y * cos(Mag_roll) + Mag_z * sin(Mag_roll);

    // Компенсация наклона:
    Mag_x_hor = Mag_x;
    Mag_y_hor = Mag_y;

    Что-то не особо понятна логика данного участка кода. Сначала производятся вычисления углов наклона с сохранением их в переменные Mag_x_hor и Mag_y_hor, а потом эти же переменные перезаписываются другими значениями. Здесь ошибка?

  • aave1
    aave1 04.09.2021 21:16 Комментировать

    Дмитрий, вы правы. В финальном варианте скетча этого нет, а тут забыл убрать. Недоглядел. Спасибо.

  • match
    match 25.12.2021 11:29 Комментировать

    Спасибо за статью!
    пытаюсь разобраться, но возникает много вопросов, например
    float ASAX = 1.20, ASAY = 1.20, ASAZ = 1.16; /* значения коэффициентов чувствительности (A)sahi (S)ensitivity (A)djustment,
    * записанные в однократно программируемое ПЗУ (fuse ROM) */
    Почему, зачем вы пытаетесь преобразовать во float значения из восьмибитного регистра?

Оставить комментарий