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

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

2описание оптопары на микросхеме tlp281-4

Рассмотрим работу оптопары на примере микросхемы TLP281, а точнее её разновидности TLP281-4. Микросхема TLP281-4 имеет 4 канала. То есть у неё есть 4 управляющих ножки и 4 выходных ножки, к которым подключается полезная нагрузка.

Будем использовать для работы модуль HW-399. Выглядит он так, как показано на иллюстрации ниже. Рядом приведена его схема.

Внешний вид модуля HW-399 с микросхемой TLP281-4 и её схема
Внешний вид модуля HW-399 с микросхемой TLP281-4 и её схема

Здесь выводы IN1…IN4 – это управляющие входные сигналы от микроконтроллера, например, Arduino, или другого управляющего элемента. На них можно подавать напряжение от 3,3 до 5 вольт. Выводы OUT1…OUT4 – выходы. Ножки HVCC и HGND – питание и земля управляемой части электрической схемы. На ножку питания HVCC можно подавать напряжение до 24 вольт.

Выводы IN1…IN4 соответствуют анодам светодиодов модуля, которые и являются источниками светового сигнала для фотокатодов модуля, которые являются электронными ключами OUT1…OUT4.

Для демонстрации работы оптопары давайте соберём схему, показанную на следующем рисунке. Здесь управлять будем одним каналом IN1 модуля HW-399 с помощью Arduino. К выходу OUT1 модуля подключим светодиод, питание на который будем подавать с отдельного источника питания (хотя можно и с самого Arduino, в данном случае это не принципиально). Подключать светодиод необходимо через токоограничивающий резистор, разумеется.

Схема подключения модуля HW-399 с микросхемой TLP284-1 к Arduino
Схема подключения модуля HW-399 с микросхемой TLP284-1 к Arduino

Как только мы соберём схему и подадим питание на внешнюю цепь (ножка HVCC), светодиод загорится. Это из-за того, что на управляющий пин IN1 ещё не подан управляющий сигнал. При отсутствии напряжения логической единицы на входе IN1 (допустим, он просто «висит» в воздухе или подключён к земле)

Подключение оптопары TLP284-1 к Arduino
Подключение оптопары TLP284-1 к Arduino

Давайте загрузим в Arduino стандартный скетч из примеров – Blink. Этот скетч каждую секунду меняет логический уровень на 13-ой ножке Arduino. Таким образом, мы наглядно увидим, как работает управление оптопарой.

Подключение оптопары TLP284-1 к Arduino
Подключение оптопары TLP284-1 к Arduino

Когда на 13-ом выводе Arduino высокий логический уровень – загорается встроенный светодиод платы Arduino, и отправляется управляющий сигнал на вход IN1 модуля. На выходе OUT1 появляется высокий уровень, и светодиод, подключённый к модулю, гаснет, т.к. нулевая разность потенциалов, и ток не может протекать через светодиод.

Когда на 13-ой ножке Arduino низкий уровень, то встроенный светодиод гаснет, и управляющий сигнал переключается также в низкий уровень. Из-за этого между выходом OUT1 и питанием HVCC модуля возникает разность потенциалов, и подключённый к микросхеме TLP281 светодиод загорается. Таким образом эти два светодиода будут загораться как бы в противофазе.

Осциллограмма при работе оптопары в скетче Blink
Осциллограмма при работе оптопары в скетче Blink

На приведённой осциллограмме голубой график – управляющий сигнал с пина 13 платы Arduino. А фиолетовый график – напряжение на светодиоде на 1-ом выходе модуля HW-399.

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

Мы можем подключить индикатор прямо к выводам Arduino. Для этого придётся задействовать сразу 7 ножек (или 8, если нужна десятичная точка). Обратим внимание, что индикатор 3361AS не имеет токоограничивающих резисторов. Необходимо обеспечить наличие сопротивления номиналом около 180…220 Ом на каждый вывод индикатора (т.к. питание подаём 5 В от Arduino).

Электрическая схема 7-сегментного индикатора 3361AS
Электрическая схема 7-сегментного индикатора 3361AS

Расположение выводов индикатора показано на иллюстрации:

Размеры корпуса и расположение выводов 7-сегментного индикатора 3361AS
Размеры корпуса и расположение выводов 7-сегментного индикатора 3361AS

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

Вывод индикатора 3361ASНазначениеВывод Arduino
1Сегмент ED6
2Сегмент DD5
3DPD9
4Сегмент CD4
5Сегмент GD8
7Сегмент BD3
8Выбор 3-го разряда5V
9Выбор 2-го разряда5V
10Сегмент FD7
11Сегмент AD2
12Выбор 1-го разрядаGND

Напишем скетч, который последовательно выводит числа от 0 до 9 на первом разряде индикатора.

Скетч управления индикатором 3361AS (разворачивается)
const int A = 2;
const int B = 3;
const int C = 4;
const int D = 5;
const int E = 6;
const int F = 7;
const int G = 8;
const int DP = 9;

void setup() {
  pinMode(A, OUTPUT);
  pinMode(B, OUTPUT);
  pinMode(C, OUTPUT);
  pinMode(D, OUTPUT);
  pinMode(E, OUTPUT);
  pinMode(F, OUTPUT);
  pinMode(G, OUTPUT);
  pinMode(DP, OUTPUT);
}

void loop() {
  for (int i=0; i<=9; i  ){
    printNumber(i);
    delay(1000);
  }
}

// зажигает на 7-сегментном индикаторе заданную цифру
void printNumber(int num){
  int numbers[10][8] = { // многомерный массив, в котором описаны состояния сегментов A…G и DP для цифр от 0 до 9
    {1,1,1,1,1,1,0,0}, // 0 
    {0,1,1,0,0,0,0,0}, // 1 
    {1,1,0,1,1,0,1,0}, // 2 
    {1,1,1,1,0,0,1,0}, // 3 
    {0,1,1,0,0,1,1,0}, // 4 
    {1,0,1,1,0,1,1,0}, // 5 
    {1,0,1,1,1,1,1,0}, // 6 
    {1,1,1,0,0,0,0,0}, // 7 
    {1,1,1,1,1,1,1,0}, // 8 
    {1,1,1,1,0,1,1,0}  // 9 
  };
  lightSegments(numbers[num]);
}

// зажигает заданные сегменты
void lightSegments(int segments[]){
  digitalWrite(A, segments[0]);
  digitalWrite(B, segments[1]);
  digitalWrite(C, segments[2]);
  digitalWrite(D, segments[3]);
  digitalWrite(E, segments[4]);
  digitalWrite(F, segments[5]);
  digitalWrite(G, segments[6]);
  digitalWrite(DP, segments[7]);
}

Небольшое пояснение по поводу массива numbers[] в функции printNumber(). Отображение нуля на семисегментном индикаторе Этот массив состоит из 10-ти подмассивов, каждый из которых определяет одну цифру от 0 до 9. В свою очередь подмассивы состоят из 8-ми элементов, которые задают состояния сегментов от A до G и DP. Например, первый подмассив описан как {1,1,1,1,1,1,0,0} и он отвечает за вывод на индикатор нуля. Это означает, что сегменты A,B,C,D,E,F должны гореть, а сегменты G и DP – нет.

[1|1|1|1|1|1|0|0 ]
[A|B|C|D|E|F|G|DP]

В результате получаем примерно следующее:

Управление 7-сегментным индикатором с помощью Arduino Nano
Управление 7-сегментным индикатором с помощью Arduino Nano

И вот так в динамике:

Это самый простой способ управления сегментным индикатором, но, как мы видим, он задействует почти все цифровые ножки Arduino. Особенно если мы решим использовать все разряды индикатора. Тогда кроме ножек для управления сегментами придётся дополнительно использовать столько выводов, сколько разрядов у индикатора.

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

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

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

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

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

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

Выводы двоично-десятичного преобразователя 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. Но чисто физически это невозможно. Однако можно добиться иллюзии постоянного свечения всех разрядов светодиодного индикатора. Для этого придётся быстро переключаться между разрядами, постоянно обновляя показание каждого разряда.

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

Скетч для управления трёхразрядным 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);
}
Управление трёхразрядным семисегментным индикатором с помощью преобразователя CD4511 и Arduino
Управление трёхразрядным семисегментным индикатором с помощью преобразователя CD4511 и Arduino

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

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.

Смотрите про коптеры:  Обсуждение (Mavic Air 2S) - Страница 3 - Mavic Air / Air 2 / Air 2S - Dji-Club
Пояснение принципа вычисления силы тока
Пояснение принципа вычисления силы тока

На практике значение на аналоговом выводе 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изучение драйвера max7219с помощью ft2232h

Но прежде чем подключать модули с MAX7219 к Arduino, попробуем поизучать их с помощью отладочной платы с микросхемой FT2232H. Она позволяет обмениваться по SPI с устройствами и не требует программирования. Все настройки делаются в программе SPI via FTDI, и для быстрого знакомства с новым устройством это очень удобно.

Для начала переведём все драйверы MAX7219 в цепочке в режим теста. Для этого необходимо в каждое из устройств в нашей цепочке передать команду 0F 01. В драйвере MAX7219 имеется сдвиговый регистр, и нам нужно «протолкнуть» команду по всем регистрам в цепочке. Чтобы команда прошла по всей цепочке, необходимо повторить её выдачу 8 раз (по числу драйверов в цепочке).

Настройки программы для перевода в режим теста 8-ми драйверов MAX7219
Настройки программы для перевода в режим теста 8-ми драйверов MAX7219

Почему команда именно такая? Смторим в документацию и видим, что адрес регистра Display Test – 0x0F. А чтобы перейти в режим теста, нужно в регистр Display Test записать 1 (см. таблицу 10 из технического описания драйвера MAX7219). В режиме теста должны загореться все светодиоды, подключённые к драйверу.

Цепочка из драйверов MAX7219 в режиме теста
Цепочка из драйверов MAX7219 в режиме теста

Чтобы выйти из режима теста, следует записать 8 раз команду 0F 00. Уже догадались, почему такая команда? Выход из тестового режима осуществляется записью нуля в регистр 0x0F.

Кстати, если в поле количества раз поставить 8, то можно записать команду за одно нажатие на кнопку «Записать».

После включения драйвер MAX7219 переходит в режим выключения, за который отвечает регистр Shutdown (0xXC). В этом режиме единственные команды, которые мы можем послать – это переход в режим теста и выход из него. Чтобы перевести устройство в рабочий режим, нужно в регистр 0x0C записать “1” (см. таблицу 3 технического описания):

Настройки программы для перевода MAX7219 в рабочий режим
Настройки программы для перевода MAX7219 в рабочий режим

Теперь устройство готово к приёму всех команд.

Драйвер MAX7219 может работать в режиме декодирования или без него. Причём можно включать и отключать режим декодирования для каждого из разрядов дисплея (или столбцов LED матрицы) индивидуально. Давайте отключим режим декодирования. Для этого нужно послать команду 09 00 восемь раз (см.

Соответствие битов D0..D7 сегментам в режиме без декодирования
Соответствие битов D0..D7 сегментам в режиме без декодирования

Режим с декодированием используется, когда драйвер MAX7219 подключён к 7-сегментному индикатору. Этот режим позволяет передавать драйверу число, и он сам зажигает нужные сегменты дисплея, чтобы отобразить это число на дисплее. Но так как в нашем случае к драйверу подключена LED панель, то режим с декодированием нам не подходит.

Давайте для примера зажгём в первой строчке всех панелей убывающее число светодиодов: на первой панели в первой строчке будет гореть 8 светодиодов, на второй – 7, и так далее. Для этого мы должны так же послать 8 команд, но теперь они будут разные. За запись в первую строчку отвечает регистр Digit 0 (0x01).

    01 01 (00000001)
    01 03 (00000011)
    01 07 (00000111)
    01 0F (00001111)
    01 1F (00011111)
    01 3F (00111111)
    01 7F (01111111)
    01 FF (11111111)

Первое число – номер регистра (Digit 0), второе число – то, что мы записываем в регистр. В скобках указано двоичное представление того, что записываем в регистр. Единицы будут горящими светодиодами на LED панели, нули – потухшими.

Горит первая строка с убывающим числом светодиодов
Горит первая строка с убывающим числом светодиодов

Теперь давайте изменим яркость свечения LED панели. Для этого необходимо в регистр Intensity (0x0A) записать число от 0 до 15 (0x0F), где “0” соответствует минимальной яркости, а 0x0F – максимальной (см. таблицу 7 технического описания).

Максимальная и минимальная яркости: управление яркостью драйвером MAX7219
Максимальная и минимальная яркости: управление яркостью драйвером MAX7219

Теперь мы знаем, как управлять драйвером MAX7219 и уже почти готовы подключить его к Arduino. Осталось только освоить ещё одну деталь нашего проекта: часы реального времени DS1302. О том, как это сделать, подробно описано в этой статье.

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 нужную цифру, а затем переключаться на следующий разряд.

Смотрите про коптеры:  30 крутых трюков Siri, которые стоит попробовать в iOS и macOS в 2019 году • Оки Доки

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

Также перепишем функцию 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);
}

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

Управление трёхразрядным семисегментным индикатором с помощью преобразователя 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

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

7подключение трёхканального датчика тока и напряжения ina3221 к arduino

Датчик тока INA3221 практически идентичен датчику INA219. Основное отличие состоит в том, что он имеет 3 измерительных канала вместо одного. Показания с них можно снимать независимо друг от друга. Будем использовать вот такую небольшую плату с датчиком:

Плата с датчиком INA3221
Плата с датчиком INA3221

Подключается данный модуль к Arduino всего 4-мя проводами: два для питания, и ещё два – шина I2C.

Вывод модуля INA3221Вывод ArduinoНазначение
SDAA4Данные шины I2C
SCLA5Импульсы синхронизации шины I2C
VS 3.3VПитание
GNDGNDОбщий
Подключение датчика INA3221 к Arduino Nano
Подключение датчика INA3221 к Arduino Nano

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

Вывод модуля INA3221Назначение
TCЦифровой выход оповещения о сбое таймингов (timing control alert).
WARЦифровой выход оповещения о сбоях измерений (warning).
CRIЦифровой выход оповещения о критических сбоях (critical).
PVЦифровой выход оповещения о валидности питающего напряжения (power valid).
VPUАналоговый вход подтягивающего напряжения для смещения выходных цепей определения валидности питания.
POWАналоговый вход питания измеряемой нагрузки.
CH1, CH2, CH3Порты для подключения измеряемых цепей.

Используем библиотеку для работы с датчиком INA3221. Поместим файлы с расширениями *.cpp и *.h в одну директорию, в ней же создадим файл с расширением *.ino и следующим содержимым:

Скетч для чтения показаний датчика INA3221
#include "Wire.h"
#include "SDL_Arduino_INA3221.h"

SDL_Arduino_INA3221 ina3221; // создаём экземпляр класса датчика

// Три канала измерения датчика INA3221
#define CHANNEL_1 1
#define CHANNEL_2 2
#define CHANNEL_3 3

void setup(void)
{
  Serial.begin(115200);
  Serial.println("Arduino INA3221 test");
  ina3221.begin();

  Serial.print("ID=0x");
  int id = ina3221.getManufID();
  Serial.println(id, HEX);

  Serial.println("Measuring voltage and current with ina3221 ...");
}

void loop(void)
{
  Serial.println("---------------------------------------------");
  Serial.println("Channel:tt(1)t(2)t(3)t"); // "t" - это символ табуляции

  // Вывод напряжений по трём каналам:
  Serial.print("Bus voltage, V: t");
  float busvoltage1 = ina3221.getBusVoltage_V(CHANNEL_1);
  float busvoltage2 = ina3221.getBusVoltage_V(CHANNEL_2);
  float busvoltage3 = ina3221.getBusVoltage_V(CHANNEL_3);
  Serial.print(busvoltage1); Serial.print("t");
  Serial.print(busvoltage2); Serial.print("t");
  Serial.print(busvoltage3); Serial.println("t");

  // Вывод напряжений на шунте по трём каналам:
  Serial.print("Shunt voltage, mV: t");
  float shuntvoltage1 = ina3221.getShuntVoltage_mV(CHANNEL_1);
  float shuntvoltage2 = ina3221.getShuntVoltage_mV(CHANNEL_2);
  float shuntvoltage3 = ina3221.getShuntVoltage_mV(CHANNEL_3);
  Serial.print(shuntvoltage1); Serial.print("t");
  Serial.print(shuntvoltage2); Serial.print("t");
  Serial.print(shuntvoltage3); Serial.println("t");

  // Вывод напряжений нагрузки по трём каналам:
  Serial.print("Load voltage, V: t");
  float loadvoltage1 = busvoltage1   (shuntvoltage1 / 1000);
  float loadvoltage2 = busvoltage2   (shuntvoltage2 / 1000);
  float loadvoltage3 = busvoltage3   (shuntvoltage3 / 1000);
  Serial.print(loadvoltage1); Serial.print("t");
  Serial.print(loadvoltage2); Serial.print("t");
  Serial.print(loadvoltage3); Serial.println("t");

  // Вывод тока по трём каналам:
  Serial.print("Current, mA: tt");
  float current_mA1 = ina3221.getCurrent_mA(CHANNEL_1);
  float current_mA2 = ina3221.getCurrent_mA(CHANNEL_2);
  float current_mA3 = ina3221.getCurrent_mA(CHANNEL_3);
  Serial.print(current_mA1); Serial.print("t");
  Serial.print(current_mA2); Serial.print("t");
  Serial.print(current_mA3); Serial.println("t");

  delay(2000);
}

Загрузим данный скетч в память Arduino. Перед тем как подключать нагрузку, необходимо подать с источника питания напряжение на контакты POW и GND, расположенные с одного из краёв модуля. Это напряжение будет подаваться на нагрузку и оно в данном модуле общее для всех трёх измерительных каналов. Допустимый диапазон напряжений от 0 до 26 вольт. Я сейчас подам 5 В.

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

Теперь можно подключать нагрузку. Давайте нагрузим выходы модуля и посмотрим, что будет выводиться в монитор последовательного порта. Я подключу на канал 1 два параллельных резистора номиналом 4,3 кОм, что в сумме даст сопротивление 2,15 кОм. А на канал 3 – один резистор 4,3 кОм.

Датчик тока INA3221 с нагрузкой
Датчик тока INA3221 с нагрузкой

В мониторе последовательного порта видно, как меняются показания датчика INA3221 при изменении нагрузки. На иллюстрации для примера показаны три состояния: показания датчика без нагрузки, с нагрузкой на одном канале и с разной нагрузкой на двух каналах.

Показания датчика тока INA3221 в мониторе COM-порта
Показания датчика тока INA3221 в мониторе COM-порта

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

Показания датчика тока INA3221 в сравнении с амперметром
Показания датчика тока INA3221 в сравнении с амперметром

Filtering

Because the raw data contains a lot of noise we use certain filters on the output of the sensors to convert them to Quaternions (Madgwick/Mahony/Kalman):

void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
{
float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
float norm;
float hx, hy, _2bx, _2bz;
float s1, s2, s3, s4;
float qDot1, qDot2, qDot3, qDot4;
// Auxiliary variables to avoid repeated arithmetic
float _2q1mx;
float _2q1my;
float _2q1mz;
float _2q2mx;
float _4bx;
float _4bz;
float _2q1 = 2.0f * q1;
float _2q2 = 2.0f * q2;
float _2q3 = 2.0f * q3;
float _2q4 = 2.0f * q4;
float _2q1q3 = 2.0f * q1 * q3;
float _2q3q4 = 2.0f * q3 * q4;
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;
// Normalise accelerometer measurement
norm = sqrt(ax * ax ay * ay az * az);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f/norm;
ax *= norm;
ay *= norm;
az *= norm;
// Normalise magnetometer measurement
norm = sqrt(mx * mx my * my mz * mz);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f/norm;
mx *= norm;
my *= norm;
mz *= norm;
// Reference direction of Earth’s magnetic field
_2q1mx = 2.0f * q1 * mx;
_2q1my = 2.0f * q1 * my;
_2q1mz = 2.0f * q1 * mz;
_2q2mx = 2.0f * q2 * mx;
hx = mx * q1q1 – _2q1my * q4 _2q1mz * q3 mx * q2q2 _2q2 * my * q3 _2q2 * mz * q4 – mx * q3q3 – mx * q4q4;
hy = _2q1mx * q4 my * q1q1 – _2q1mz * q2 _2q2mx * q3 – my * q2q2 my * q3q3 _2q3 * mz * q4 – my * q4q4;
_2bx = sqrt(hx * hx hy * hy);
_2bz = -_2q1mx * q3 _2q1my * q2 mz * q1q1 _2q2mx * q4 – mz * q2q2 _2q3 * my * q4 – mz * q3q3 mz * q4q4;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
// Gradient decent algorithm corrective step
s1 = -_2q3 * (2.0f * q2q4 – _2q1q3 – ax) _2q2 * (2.0f * q1q2 _2q3q4 – ay) – _2bz * q3 * (_2bx * (0.5f – q3q3 – q4q4) _2bz * (q2q4 – q1q3) – mx) (-_2bx * q4 _2bz * q2) * (_2bx * (q2q3 – q1q4) _2bz * (q1q2 q3q4) – my) _2bx * q3 * (_2bx * (q1q3 q2q4) _2bz * (0.5f – q2q2 – q3q3) – mz);
s2 = _2q4 * (2.0f * q2q4 – _2q1q3 – ax) _2q1 * (2.0f * q1q2 _2q3q4 – ay) – 4.0f * q2 * (1.0f – 2.0f * q2q2 – 2.0f * q3q3 – az) _2bz * q4 * (_2bx * (0.5f – q3q3 – q4q4) _2bz * (q2q4 – q1q3) – mx) (_2bx * q3 _2bz * q1) * (_2bx * (q2q3 – q1q4) _2bz * (q1q2 q3q4) – my) (_2bx * q4 – _4bz * q2) * (_2bx * (q1q3 q2q4) _2bz * (0.5f – q2q2 – q3q3) – mz);
s3 = -_2q1 * (2.0f * q2q4 – _2q1q3 – ax) _2q4 * (2.0f * q1q2 _2q3q4 – ay) – 4.0f * q3 * (1.0f – 2.0f * q2q2 – 2.0f * q3q3 – az) (-_4bx * q3 – _2bz * q1) * (_2bx * (0.5f – q3q3 – q4q4) _2bz * (q2q4 – q1q3) – mx) (_2bx * q2 _2bz * q4) * (_2bx * (q2q3 – q1q4) _2bz * (q1q2 q3q4) – my) (_2bx * q1 – _4bz * q3) * (_2bx * (q1q3 q2q4) _2bz * (0.5f – q2q2 – q3q3) – mz);
s4 = _2q2 * (2.0f * q2q4 – _2q1q3 – ax) _2q3 * (2.0f * q1q2 _2q3q4 – ay) (-_4bx * q4 _2bz * q2) * (_2bx * (0.5f – q3q3 – q4q4) _2bz * (q2q4 – q1q3) – mx) (-_2bx * q1 _2bz * q3) * (_2bx * (q2q3 – q1q4) _2bz * (q1q2 q3q4) – my) _2bx * q2 * (_2bx * (q1q3 q2q4) _2bz * (0.5f – q2q2 – q3q3) – mz);
norm = sqrt(s1 * s1 s2 * s2 s3 * s3 s4 * s4); // normalise step magnitude
norm = 1.0f/norm;
s1 *= norm;
s2 *= norm;
s3 *= norm;
s4 *= norm;
// Compute rate of change of quaternion
qDot1 = 0.5f * (-q2 * gx – q3 * gy – q4 * gz) – beta * s1;
qDot2 = 0.5f * (q1 * gx q3 * gz – q4 * gy) – beta * s2;
qDot3 = 0.5f * (q1 * gy – q2 * gz q4 * gx) – beta * s3;
qDot4 = 0.5f * (q1 * gz q2 * gy – q3 * gx) – beta * s4;
// Integrate to yield quaternion
q1 = qDot1 * deltat;
q2 = qDot2 * deltat;
q3 = qDot3 * deltat;
q4 = qDot4 * deltat;
norm = sqrt(q1 * q1 q2 * q2 q3 * q3 q4 * q4); // normalise quaternion
norm = 1.0f/norm;
q[0] = q1 * norm;
q[1] = q2 * norm;
q[2] = q3 * norm;
q[3] = q4 * norm;
}
// Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and
// measured ones.
void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
{
float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
float norm;
float hx, hy, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
float pa, pb, pc;
// Auxiliary variables to avoid repeated arithmetic
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;
// Normalise accelerometer measurement
norm = sqrt(ax * ax ay * ay az * az);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f / norm; // use reciprocal for division
ax *= norm;
ay *= norm;
az *= norm;
// Normalise magnetometer measurement
norm = sqrt(mx * mx my * my mz * mz);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f / norm; // use reciprocal for division
mx *= norm;
my *= norm;
mz *= norm;
// Reference direction of Earth’s magnetic field
hx = 2.0f * mx * (0.5f – q3q3 – q4q4) 2.0f * my * (q2q3 – q1q4) 2.0f * mz * (q2q4 q1q3);
hy = 2.0f * mx * (q2q3 q1q4) 2.0f * my * (0.5f – q2q2 – q4q4) 2.0f * mz * (q3q4 – q1q2);
bx = sqrt((hx * hx) (hy * hy));
bz = 2.0f * mx * (q2q4 – q1q3) 2.0f * my * (q3q4 q1q2) 2.0f * mz * (0.5f – q2q2 – q3q3);
// Estimated direction of gravity and magnetic field
vx = 2.0f * (q2q4 – q1q3);
vy = 2.0f * (q1q2 q3q4);
vz = q1q1 – q2q2 – q3q3 q4q4;
wx = 2.0f * bx * (0.5f – q3q3 – q4q4) 2.0f * bz * (q2q4 – q1q3);
wy = 2.0f * bx * (q2q3 – q1q4) 2.0f * bz * (q1q2 q3q4);
wz = 2.0f * bx * (q1q3 q2q4) 2.0f * bz * (0.5f – q2q2 – q3q3);
// Error is cross product between estimated direction and measured direction of gravity
ex = (ay * vz – az * vy) (my * wz – mz * wy);
ey = (az * vx – ax * vz) (mz * wx – mx * wz);
ez = (ax * vy – ay * vx) (mx * wy – my * wx);
if (Ki > 0.0f)
{
eInt[0] = ex; // accumulate integral error
eInt[1] = ey;
eInt[2] = ez;
}
else
{
eInt[0] = 0.0f; // prevent integral wind up
eInt[1] = 0.0f;
eInt[2] = 0.0f;
}
// Apply feedback terms
gx = gx Kp * ex Ki * eInt[0];
gy = gy Kp * ey Ki * eInt[1];
gz = gz Kp * ez Ki * eInt[2];
// Integrate rate of change of quaternion
pa = q2;
pb = q3;
pc = q4;
q1 = q1 (-q2 * gx – q3 * gy – q4 * gz) * (0.5f * deltat);
q2 = pa (q1 * gx pb * gz – pc * gy) * (0.5f * deltat);
q3 = pb (q1 * gy – pa * gz pc * gx) * (0.5f * deltat);
q4 = pc (q1 * gz pa * gy – pb * gx) * (0.5f * deltat);
// Normalise quaternion
norm = sqrt(q1 * q1 q2 * q2 q3 * q3 q4 * q4);
norm = 1.0f / norm;
q[0] = q1 * norm;
q[1] = q2 * norm;
q[2] = q3 * norm;
q[3] = q4 * norm;
}

Getting readings from mpu9250 with arduino  mega 2560

We have lots of libraries for MPU9250. One of the popular one is given by kriswiner: MPU-9250 Arduino Library by Kriswiner

Смотрите про коптеры:  Робот собака Дюк Silverlit 88557: Обзор, минусы и плюсы и игрушки, где купить отзывы

Once you save the library to your Arduino folder you are ready to go. Open the example MPU9250BasicAHRS.ino. Also have this  setup ready:

MPU9250 Breakout ——— Arduino

  • VIN ———————- 5V
  • SDA ———————– SDA (Pin 20)
  • SCL ———————– SCL (Pin 21)
  • GND ———————- GND

These wires shouldn’t be very long because I2C connections don’t work well for long wires.

Now clean up the MPU9250BasicAHRS code. It has LCD code in it, but we don’t need it, so remove unnecessary lines. Also I have added a part of auto calibration code. Here is the modified code without unnecessary code and added auto calibration: Github.

Now upload the code to your Arduino and make the connections shown above. Open the Serial Terminal and change the baud rate to 115200. You should see this output:

MPU9250
9-DOF 16-bit
motion sensor
60 ug LSB
MPU9250 I AM 71 I should be 71
MPU9250 is online…
x-axis self test: acceleration trim within : 0.8% of factory value
y-axis self test: acceleration trim within : -1.9% of factory value
z-axis self test: acceleration trim within : 1.8% of factory value
x-axis self test: gyration trim within : -0.2% of factory value
y-axis self test: gyration trim within : 0.3% of factory value
z-axis self test: gyration trim within : 0.6% of factory value
MPU9250 bias
x y z
254913-660mg1.1-0.11.2o/s
MPU9250 initialized for active data mode….
AK8963 I AM 48 I should be 48
AK8963 initialized for active data mode….
Mag Calibration: Wave device in a figure eight until done!

If you see this:

MPU9250
9-DOF 16-bit
motion sensor
60 ug LSB
MPU9250 I AM FF I should be 71
Could not connect to MPU9250: 0xFF

This means there is definitely a wiring problem (or in the worst case Mpu/arduino fault) try to rectify it before proceeding.

If everything goes well and you see “MPU is online” and “Mag Calibration: Wave device in a figure eight until done!” then everything is working and you should wave your mpu in figure eights until it finishes the auto calibration. After a while you should be getting yaw, pitch, and roll output as below:

Mag Calibration: Wave device in a figure eight until done!
Mag Calibration done!
X-Axis sensitivity adjustment value 1.19
Y-Axis sensitivity adjustment value 1.19
Z-Axis sensitivity adjustment value 1.15
AK8963
ASAX
1.19
ASAY
1.19
ASAZ
1.15
Yaw, Pitch, Roll: 11.34, 28.62, 50.03
Yaw, Pitch, Roll: 20.47, 25.15, 52.88
Yaw, Pitch, Roll: 26.94, 19.02, 52.70
Yaw, Pitch, Roll: 28.22, 15.02, 50.15
Yaw, Pitch, Roll: 27.10, 13.94, 44.68
Yaw, Pitch, Roll: 23.11, 13.69, 37.51
Yaw, Pitch, Roll: 14.29, 13.22, 27.61
Yaw, Pitch, Roll: 357.03, 8.21, 16.72
Yaw, Pitch, Roll: 342.29, 0.69, 9.19
Yaw, Pitch, Roll: 328.42, -4.80, 3.16
Yaw, Pitch, Roll: 317.19, -10.51, -0.58
Yaw, Pitch, Roll: 311.88, -16.57, -3.64
Yaw, Pitch, Roll: 327.71, -23.45, -16.82
Yaw, Pitch, Roll: 325.74, -22.02, -23.51
Yaw, Pitch, Roll: 325.99, -28.17, -26.95
Yaw, Pitch, Roll: 324.57, -24.96, -23.21
Yaw, Pitch, Roll: 320.01, -26.42, -22.25
Yaw, Pitch, Roll: 322.50, -26.04, -26.62
Yaw, Pitch, Roll: 322.85, -23.43, -29.17
Yaw, Pitch, Roll: 323.46, -19.20, -31.48

Awesome! Now you have the data, and you can play with real time motion!😀

Оцените статью
Радиокоптер.ру
Добавить комментарий

Adblock
detector