Сверхточный электронный компас и датчик положения / Хабр

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

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

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

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

// выводы Arduino для управления двоичным кодом на входе декодера CD4511:
const byte D_0 = 11;
const byte D_1 = 10;
const byte D_2 = 9;
const byte D_3 = 8;

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

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

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

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

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

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

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

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

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

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

// Выводы 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);
}

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

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

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

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

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

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

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

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

#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 довольно точно совпадают с показаниями амперметра.

Mpu 9250 и arduino

#include <Wire.h>
#include <TimerOne.h>
 
#define MPU9250_ADDRESS 0x68
#define MAG_ADDRESS 0x0C
 
#define GYRO_FULL_SCALE_250_DPS 0x00 
#define GYRO_FULL_SCALE_500_DPS 0x08
#define GYRO_FULL_SCALE_1000_DPS 0x10
#define GYRO_FULL_SCALE_2000_DPS 0x18
 
#define ACC_FULL_SCALE_2_G 0x00 
#define ACC_FULL_SCALE_4_G 0x08
#define ACC_FULL_SCALE_8_G 0x10
#define ACC_FULL_SCALE_16_G 0x18
 
// This function read Nbytes bytes from I2C device at address Address. 
// Put read bytes starting at register Register in the Data array. 
void I2Cread(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data)
    {
      // Set register address
      Wire.beginTransmission(Address);
      Wire.write(Register);
      Wire.endTransmission();
 
      // Read Nbytes
      Wire.requestFrom(Address, Nbytes); 
      uint8_t index=0;
      while (Wire.available())
      Data[index  ]=Wire.read();
    }
 
// Write a byte (Data) in device (Address) at register (Register)
void I2CwriteByte(uint8_t Address, uint8_t Register, uint8_t Data)
    {
      // Set register address
      Wire.beginTransmission(Address);
      Wire.write(Register);
      Wire.write(Data);
      Wire.endTransmission();
    }
 
// Initial time
long int ti;
volatile bool intFlag=false;
 
// Initializations
void setup()
    {
      // Arduino initializations
      Wire.begin();
      Serial.begin(115200);
 
      // Set accelerometers low pass filter at 5Hz
      I2CwriteByte(MPU9250_ADDRESS,29,0x06);
      // Set gyroscope low pass filter at 5Hz
      I2CwriteByte(MPU9250_ADDRESS,26,0x06);
 
      // Configure gyroscope range
      I2CwriteByte(MPU9250_ADDRESS,27,GYRO_FULL_SCALE_1000_DPS);
      // Configure accelerometers range
      I2CwriteByte(MPU9250_ADDRESS,28,ACC_FULL_SCALE_4_G);
      // Set by pass mode for the magnetometers
      I2CwriteByte(MPU9250_ADDRESS,0x37,0x02);
 
      // Request continuous magnetometer measurements in 16 bits
      I2CwriteByte(MAG_ADDRESS,0x0A,0x16);
 
      pinMode(13, OUTPUT);
      Timer1.initialize(10000); // initialize timer1, and set a 1/2 second period
      Timer1.attachInterrupt(callback); // attaches callback() as a timer overflow interrupt
 
      // Store initial time
      ti=millis();
    } 
 
// Counter
long int cpt=0;
 
void callback()
    { 
      intFlag=true;
      digitalWrite(13, digitalRead(13) ^ 1);
    }
 
// Main loop, read and display data
void loop()
    {
      while (!intFlag);
      intFlag=false;
 
      // Display time
      Serial.print (millis()-ti,DEC);
      Serial.print ("t");
 
      // _______________
      // ::: Counter :::
 
      // Display data counter
      // Serial.print (cpt  ,DEC);
      // Serial.print ("t");
 
      // ____________________________________
      // ::: accelerometer and gyroscope :::
 
      // Read accelerometer and gyroscope
      uint8_t Buf[14];
      I2Cread(MPU9250_ADDRESS,0x3B,14,Buf);
 
      // Create 16 bits values from 8 bits data
 
      // Accelerometer
      int16_t ax=-(Buf[0]<<8 | Buf[1]);
      int16_t ay=-(Buf[2]<<8 | Buf[3]);
      int16_t az=Buf[4]<<8 | Buf[5];
 
      // Gyroscope
      int16_t gx=-(Buf[8]<<8 | Buf[9]);
      int16_t gy=-(Buf[10]<<8 | Buf[11]);
      int16_t gz=Buf[12]<<8 | Buf[13];
 
      // Display values
 
     // Accelerometer
     Serial.print (ax,DEC); 
     Serial.print ("t");
     Serial.print (ay,DEC);
     Serial.print ("t");
     Serial.print (az,DEC); 
     Serial.print ("t");
 
     // Gyroscope
     Serial.print (gx,DEC); 
     Serial.print ("t");
     Serial.print (gy,DEC);
     Serial.print ("t");
     Serial.print (gz,DEC); 
     Serial.print ("t");
 
     // _____________________
     // ::: Magnetometer :::
 
     // Read register Status 1 and wait for the DRDY: Data Ready
 
     uint8_t ST1;
     do
         {
           I2Cread(MAG_ADDRESS,0x02,1,&ST1);
         }
     while (!(ST1&0x01));
 
     // Read magnetometer data 
     uint8_t Mag[7]; 
     I2Cread(MAG_ADDRESS,0x03,7,Mag);
 
     // Create 16 bits values from 8 bits data
 
     // Magnetometer
     int16_t mx=-(Mag[3]<<8 | Mag[2]);
     int16_t my=-(Mag[1]<<8 | Mag[0]);
     int16_t mz=-(Mag[5]<<8 | Mag[4]);
 
     // Magnetometer
     Serial.print (mx 200,DEC); 
     Serial.print ("t");
     Serial.print (my-70,DEC);
     Serial.print ("t");
     Serial.print (mz-700,DEC); 
     Serial.print ("t");
 
     // End of line
     Serial.println("");
     // delay(100); 
   }

Автоматическая калибровка магнитометра

Это одна из самых простых, но в то же время важных участков кода программы. Функция magcalMPU9250(float * dest1, float * dest2) производит калибровку магнитометра в то время когда вы перемещаете его по фигуре восьмерки (цифры “8”). Она сохраняет максимальные и минимальные значения, а затем на их основе вычисляет среднее.

Получение “реальных” значений

Наконец, на завершающем этапе обработки данных, мы получим значения параметров yaw, pitch и roll из кватернионов.

Фильтрация данных

Поскольку исходные данные (raw data) содержат достаточно большое количество шума, мы будем использовать различные фильтры (Madgwick/Mahony/Kalman) чтобы преобразовать их в кватернионы (Quaternions).

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

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

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

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