Гироскоп GY-521 (на основе MPU-6050)

AndrIvanoff
Offline
Зарегистрирован: 11.10.2013

Доброго времени суток уважаемые пользователи, не подскажите как работает вот этот приборGY-521 на MPU-6050

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

ПС: большая просьба если вы решили подкинут мне данную ссылку
http://playground.arduino.cc/Main/MPU-6050 и предложить мне попробовать "скетч" с этой ссылки, то не объясните ли мне что за значения там получаются и как их конвертировать в (нормальные) уг. скорости и лин. ускорения

MPU-6050
Read accel, temp and gyro, error = 0
accel x,y,z: 172, -440, 15100
temperature: 29.588 degrees Celsius
gyro x,y,z : 363, 200, -769, 

 

carduino.ru
Offline
Зарегистрирован: 06.12.2011

Есть пример использования MPU-6050 на роботе

В сообщении 86 чел. выложил код для ардуины

AndrIvanoff
Offline
Зарегистрирован: 11.10.2013

Спасибо большое, очень хороший пример, я хоть и не разобрался в нём до конца, но он натолкнул меня на "правильную" мысль... теперь осталось узнать как задаются режимы измерений гироскопа (в документации написано что их 4: ±250, ±500, ±1000, and ±2000°/sec)

Elagu.s
Offline
Зарегистрирован: 04.10.2013

тоже хочу разобраться с микросхемой MPU6050

Прочитал на форуме

Цитата:
http://cyber-place.ru/showthread.php?t=404&page=9

Подключил акселерометр-гироскоп и магнитометр. С магнитометром пока не пробовал.
При отладке заметил, что в состоянии покоя значения акселерометра плавают.
А значения гироскопа стабильны. Поэтому решил для коррекции движения прямо
использовать показания оси Z. Скорректировал программу объезда препятствий.
Робот стал ездить прямо.
Библиотеку для работы с акселерометром-гироскопом взял из интернета.
Только добавил настройку фильтра низких частот до 10 Гц + децимацию.
С гироскопом работаю по прерываниям для стабильности показаний.
Коррекцию выполняю каждую 1/10 секунды.

Коррекция чего (акселерометра или гироскопа) и на основании чего? В чем природа возникающей ошибки, которую нужно корректировать ?

carduino.ru
Offline
Зарегистрирован: 06.12.2011

Человек пишет про коррекцию  прямолинейного движения робота 

Ошибка, это когда правое колесо вращается быстрей левого и робота начинает уводить в сторону. Купил MPU-6050 у нас и все у него заработало

 

KBSH
Offline
Зарегистрирован: 21.05.2014

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

baryshnikov
Offline
Зарегистрирован: 09.08.2014

А есть у кого-нибудь код для этого гироскопа gy-521 + выравнивание двигателей с пояснениями на русском? Теория по обработке данных с гироскопа+акселерометра понятна благодаря вот этой статье:http://masters.donntu.edu.ua/2013/fkita/perebeinos/diss/index.htm#p6 особенно про комплементарный фильтр. 
Есть пока только алгоритм: посылаем данные с gy-521 на ардуино, обрабатываем их комплементарным фильтром, если данные по оси Х у нас не равны начальным значениям Х в минусовую зону, то включается двигатель, с помощью PIN9 на Ардуино, а если данные не равны начальным значениям по оси Х в плюсовую зону, то включается PIN10. (Вчера автор пояснил статьи на которую я сслыку дал, что неплохо бы  пи или пид регулятор использовать, для достижения нужных оборотов у двигателей, но это уже следующий разговор)  

виталий_36
Offline
Зарегистрирован: 23.01.2014

Здраствуйте, нужна помошь по этому девайсу

Я умею считатывать данные с GY 521 в виде 2 байт по 8 бит на каждое измерение затем следующая на каждую скорость, ускорение и температуру процедура: 

ax = (H<<8)+L; - это теперь 16 битное слово сдержащее угловую  скорость  по координате х

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

 

baryshnikov
Offline
Зарегистрирован: 09.08.2014

А скетч свой не скините?  Интересно глянуть. Помочь пока не могу, только начал осваивать.

Motto
Offline
Зарегистрирован: 05.06.2014

Есть какой нибудь простой способ определить с помощью этого акселерометра начало движения и остановку?

Или не стоит браться?

Больно уж много проблем.

 

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

ГПС модуль дорого, да и для такой задачи как из пушки по воробьям.

jeka_tm
jeka_tm аватар
Offline
Зарегистрирован: 19.05.2013

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

если определить правильно уровень вибраций для движущейся и стоячей машины то в принципе реально. условия написать правильно и все, с учетом вибраций и времени

Logik
Offline
Зарегистрирован: 05.08.2014

Сегодня доковырял сабж. Работает как ожидалось. Гироскоп из неё хреновенький, азимут уходит дето со скоростью 0,01-0,1град/сек. Остальные углы наверно также, но они корректируются по гравитации. Для коррекции азимута нужен магнитометр. К стати кто знает какой датчик магнитного поля можно подключать напрямую? Два дня ковыряния дали такие впечатления: Штука сложная со стороны матобработки первичных данных с датчиков. ПДФки можете даже не качать, безтолковые и не нужные, всё уже написано до нас, ищем и отбираем работоспособные библиотеки. 

Принципиально есть два подхода - получаем сырые данные с датчиков по I2C  и сами обрабатываем жуткой матиматикой типа фильтра калмена или кватернионами, возможно и фильтры попроще сгодятся, ищите  MPU6050_raw.ino и прикручивайте математику к нему сами. Второй подход - используем DMP, это спецпроцессор внутри датчика уже спрятан. Он сам общитает c первичными датчиками, общитывает их и выдает по I2C более гладкие данные, их тоже можна обработать, например выделить чистое ускорение, без гравитации, и это уже намного проще. Проблема в том, что код в DMP надо самим залить при инициализации, его код - не опенсорс 8) Не пугаемся, гуглим MPU6050_6Axis_MotionApps20.h или аналоги и MPU6050.ino. Там все есть, и код и инит. Откомпилировано это все занимает в раене 20кбайт. Еще в любом случае нужны либы на I2C и MPU6050.h В сети их много, тока с ошибками. Найденая либка I2C делала все, даже DMP заливала корректно, все работало, попробовал откалибровать - попа. Оказалось word не правильно передает, и при записи офсета тока старший байт пишется 8х. В общем ищите в сети, отлаживайте, писать самому не надо. Еще MPU6050_raw.ino работает без прерываний а MPU6050.ino через прерывания, хоть и странноватенько. Только взводится флажек старта в обработчике, а прием из основного цикла. 

Про ДХО - это мысль! Можна даже на двигатель ставить датчик и ловить скопом все - вибрацию, ускорения и повороты. Если есть чтото - едем и светим. К тому же в интерфейсах мелькала настройка параметров для детектирования движения. И даже вроде как можно прерывание генерить по такому случаю. Тогда работа сильно упростится. Может попробую завтра.

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

carduino.ru
Offline
Зарегистрирован: 06.12.2011

Поделитесь рабочими либами

Logik
Offline
Зарегистрирован: 05.08.2014

Да пожалуста - https://cloud.mail.ru/public/6f54703223ff%2FMPU6050.ZIP. Как уже писал, либы не мои, только I2Cшную поковырял малость. Вопрос по ходу. Цеплять такое - http://www.dx.com/p/three-axis-magnetic-field-electronic-compass-sensor-module-for-arduino-148734#.VCSDHMYz_nM к сабжу никто не пробовал?

carduino.ru
Offline
Зарегистрирован: 06.12.2011

Спасибо. Попробую сегодня

А то мне попадаются библиотеки, у них небольшой дреф по оси Z 

Logik
Offline
Зарегистрирован: 05.08.2014

Так с осью Z у всех всегда плохо. (Если не про линейное ускорение говорить) В общем и с другими осями на самом деле не лучше, но там дрейф нуля можно компенсировать по данным о векторе силы тяжести.  Пошаманив матиматикой дрейф нуля сводится на нет. А по Z так не получится, нужен еще какой вектор не паралельный оси Z. Как вариант - магнитное поле земли. Собственно потому и спрашивал выше про датчик магнитного.

carduino.ru
Offline
Зарегистрирован: 06.12.2011

Понял, спасибо

UPD выложили здесь: http://carduino.ru/product_info.php?products_id=1954

robomaker
Offline
Зарегистрирован: 24.02.2015

Привет! не сочтите за некропостера, но у меня проблема с тем же самым модулем GY-521. подключился я к нему вроде правильно, адрес 0х68, прочитал его ячейки, всё вроде как в даташите, смог записать, тоже всё норм, отправил 0х00 в адрес 0х6В слип офф, но у меня по прежнему нули в ячейках с 0х3В до 0х48. и почему то по адресу 0х3А значение 0х01, хотя прерывания не включал. и еще, сразу после прошивки в ячейках с 0х3В до 0х48 вроде какие то значения появляются, но не меняются, а залипают, а при снятии и подаче питания всегда там нули, пока снова не прошьёшь. подозреваю брак гироскопа, но вдруг нет? помогите если кто может

Ares_ekb
Offline
Зарегистрирован: 09.01.2015

С осью Z вряд ли что-то получится. А для других показаний отлично работает фильтр Калмана! Я находил где-то пример скетча, с фильтром вообще идеально.

robomaker, а arduino видит этот датчик? Эта утилитка его находит? http://playground.arduino.cc/Main/I2cScanner

robomaker
Offline
Зарегистрирован: 24.02.2015

Так я и говорю, с датчиком удалось связаться как на чтение, так и на запись, просто как то не корректно сам датчик сообщает искомые 14 байт гиро+температуру+аксель. они либо нули, либо рандомные значения застывшие. при этом я могу менять и проверять регистр who i am и повер менеждмент 1, который я ставлю в 0. я не ардуиной пользуюсь, а просто атмегой32, но сути это не меняет.

robomaker
Offline
Зарегистрирован: 24.02.2015

всё, я разобрался, горелые микросхемы вся партия попалась

Cruetly
Offline
Зарегистрирован: 08.02.2015

Подскажите,есть ли у кого опыт управления какой-либо нагрузкой,в зависимости от угла? Ну например в том же роботе который на колесах балансирует.Я вот подрубил GY-521 нашел библиотеки и скетч вот отсюда http://arduinoprojects.ru/2014/10/%D0%BF%D0%BE%D0%B4%D0%BA%D0%BB%D1%8E%D1%87%D0%B5%D0%BD%D0%B8%D0%B5-%D0%B3%D0%B8%D1%80%D0%BE%D1%81%D0%BA%D0%BE%D0%BF%D0%B0-gy-521-mpu-6050-%D0%BA-arduio/

как мне использовать нагрузку? Пробовал так:если угол больше 180 например,то плавно загорается диод,и чем ближе к 270,тем ярче,и нааборот. Не работает,виснет,начинает вроде работать потом виснет всё.

Пробовал скетч с оф сайта ардуино http://playground.arduino.cc/Main/MPU-6050#info,там тоже ничего не работает ,какие-то глюки ,цикл виснет и всё.

Может как-то надо по другому?Нигде что-то не найду ничего по этому поводу.

Logik
Offline
Зарегистрирован: 05.08.2014

цеплял сервопривод, при повороте модуля он тоже поворачивался, каких либо особенностей нет.

Скай
Offline
Зарегистрирован: 05.02.2015

Добрый день!

Ребят, возникла проблема с GY-521.
Arduino DUE плата
Подключаю вот так:
VCC-5v
GND-GND
SCL-SCL(21)
SDA-SDA(20)

Загружаю скетч, который описан на http://playground.arduino.cc/Main/MPU-6050 и уже обсуждался в самом первом сообщении
На выходе имеется постоянный повтор данных, которые ни фига не меняются, как платку не крути. 
Кто-нибудь в курсе, как это пофиксить?

И вот ещё вопрос:

Вот тут подключается сабж, всё работает.
http://arduinoprojects.ru/2014/10/подключение-гироскопа-gy-521-mpu-6050-к-arduio/
Что за значения показываются на экране?
Это метры в секунду^2, линейные скорости, что это за цифры?

EwigeDreamer
Offline
Зарегистрирован: 18.10.2015

AndrIvanoff пишет:

Спасибо большое, очень хороший пример, я хоть и не разобрался в нём до конца, но он натолкнул меня на "правильную" мысль... теперь осталось узнать как задаются режимы измерений гироскопа (в документации написано что их 4: ±250, ±500, ±1000, and ±2000°/sec)

мне тоже очень хочется узнать ответ на этот вопрос. люди!! помогите пожалуйста!!! 250 град/с ну ооочень мало. если в руке вертеть - показания зашкаливают(((

Valera19701
Valera19701 аватар
Offline
Зарегистрирован: 18.10.2015

учите физику, например юлу:)

Jeka_M
Jeka_M аватар
Онлайн
Зарегистрирован: 06.07.2014

FS_SEL=0 ±250 º/s
FS_SEL=1 ±500 º/s
FS_SEL=2 ±1000 º/s
FS_SEL=3 ±2000 º/s

http://www.seeedstudio.com/wiki/images/b/b1/MPU6050.pdf

http://www6.in.tum.de/pub/Main/TeachingWs2015SeminarAuonomousFahren/RM-MPU-6000A.pdf

 

i2c_writeReg(MPU6050_ADDRESS, 0x1B, 0x18); //GYRO_CONFIG   -- FS_SEL = 3: Full scale set to 2000 deg/sec

https://code.google.com/p/multiwii/source/browse/branches/Modular_MWC/MPU6050_module/MPU6050.ino?r=791

EwigeDreamer
Offline
Зарегистрирован: 18.10.2015

мм, т.е. я должен послать команду на сам гироскоп..

попробовал написать эту команду в сетапе - он выдал ошибку про MPU6050_ADDRESS, что нет такого.

вставил вместо MPU6050_ADDRESS собственно I2c-адрес гировскопа - 0x68

в итоге другая ошибка:

MPU6050_raw_modoutput.ino: In function 'void setup()':
MPU6050_raw_modoutput:118: error: 'i2c_writeReg' was not declared in this scope
'i2c_writeReg' was not declared in this scope

что я делаю не так? и как, допустим, мне указать FS_SEL=2 или 1, а не 3?

использую скетч примера MPU6050_raw

EwigeDreamer
Offline
Зарегистрирован: 18.10.2015

я отсюда увидел, что чтобы изменить конфигурацию, нужно записать в гироскопе по адресу 0x1B значение 0x18, что в двоичном виде выглядит как 00011000 (где единицы, там собственно и FS_SEL сидит... так вот. если взять средние единицы, то я думаю, что будет следующая картина:

000 00 000 = 0x00 = (FS_SEL=0) = ±250 º/s
000 01 000 = 0x08 = (FS_SEL=1) = ±500 º/s
000 10 000 = 0x10 = (FS_SEL=2) = ±1000 º/s
000 11 000 = 0x18 = (FS_SEL=3) = ±2000 º/s

я правильно понимаю?

тогда на пол-вопроса я ответил сам.. вот только как ошибку исправить?(((

на это

i2c_writeReg(0x68, 0x1B, 0x18)

компилятор ругается..

#include <Wire.h>

не помогает

если что, код вот:



// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class
// 10/7/2011 by Jeff Rowberg <jeff@rowberg.net>
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
//      2013-05-08 - added multiple output formats
//                 - added seamless Fastwire support
//      2011-10-07 - initial release

/* ============================================
I2Cdev device library code is placed under the MIT license
Copyright (c) 2011 Jeff Rowberg

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
===============================================
*/

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050.h"

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif


// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
//MPU6050 accelgyro(0x69); // <-- use for AD0 high

int16_t ax, ay, az;
int16_t gx, gy, gz;



// uncomment "OUTPUT_READABLE_ACCELGYRO" if you want to see a tab-separated
// list of the accel X/Y/Z and then gyro X/Y/Z values in decimal. Easy to read,
// not so easy to parse, and slow(er) over UART.
#define OUTPUT_READABLE_ACCELGYRO

// uncomment "OUTPUT_BINARY_ACCELGYRO" to send all 6 axes of data as 16-bit
// binary, one right after the other. This is very fast (as fast as possible
// without compression or data loss), and easy to parse, but impossible to read
// for a human.
//#define OUTPUT_BINARY_ACCELGYRO


#define LED_PIN 13
bool blinkState = false;

void setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    // initialize serial communication
    // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
    // it's really up to you depending on your project)
    Serial.begin(38400);

    // initialize device
    Serial.println("Initializing I2C devices...");
    accelgyro.initialize();

    // verify connection
    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

    // use the code below to change accel/gyro offset values
    /*
    Serial.println("Updating internal sensor offsets...");
    // -76	-2359	1688	0	0	0
    Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -76
    Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -2359
    Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 1688
    Serial.print(accelgyro.getXGyroOffset()); Serial.print("\t"); // 0
    Serial.print(accelgyro.getYGyroOffset()); Serial.print("\t"); // 0
    Serial.print(accelgyro.getZGyroOffset()); Serial.print("\t"); // 0
    Serial.print("\n");
    accelgyro.setXGyroOffset(220);
    accelgyro.setYGyroOffset(76);
    accelgyro.setZGyroOffset(-85);
    Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -76
    Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -2359
    Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 1688
    Serial.print(accelgyro.getXGyroOffset()); Serial.print("\t"); // 0
    Serial.print(accelgyro.getYGyroOffset()); Serial.print("\t"); // 0
    Serial.print(accelgyro.getZGyroOffset()); Serial.print("\t"); // 0
    Serial.print("\n");
    */

    // configure Arduino LED for
    pinMode(LED_PIN, OUTPUT);
}

void loop() {
    // read raw accel/gyro measurements from device
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    // these methods (and a few others) are also available
    //accelgyro.getAcceleration(&ax, &ay, &az);
    //accelgyro.getRotation(&gx, &gy, &gz);

    #ifdef OUTPUT_READABLE_ACCELGYRO
        // display tab-separated accel/gyro x/y/z values
        Serial.print("Axel XYZ:\t");
        Serial.print(ax); Serial.print("\t");
        Serial.print(ay); Serial.print("\t");
        Serial.print(az); Serial.print("\t\t");
        Serial.print("Gyro XYZ:\t");
        Serial.print(gx); Serial.print("\t");
        Serial.print(gy); Serial.print("\t");
        Serial.println(gz);
    #endif

    #ifdef OUTPUT_BINARY_ACCELGYRO
        Serial.write((uint8_t)(ax >> 8)); Serial.write((uint8_t)(ax & 0xFF));
        Serial.write((uint8_t)(ay >> 8)); Serial.write((uint8_t)(ay & 0xFF));
        Serial.write((uint8_t)(az >> 8)); Serial.write((uint8_t)(az & 0xFF));
        Serial.write((uint8_t)(gx >> 8)); Serial.write((uint8_t)(gx & 0xFF));
        Serial.write((uint8_t)(gy >> 8)); Serial.write((uint8_t)(gy & 0xFF));
        Serial.write((uint8_t)(gz >> 8)); Serial.write((uint8_t)(gz & 0xFF));
    #endif

    // blink LED to indicate activity
    blinkState = !blinkState;
    digitalWrite(LED_PIN, blinkState);
}

 

Logik
Offline
Зарегистрирован: 05.08.2014

accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);

Все уже написано до нас :(

EwigeDreamer
Offline
Зарегистрирован: 18.10.2015

Спасибо огромное) эта команда работает!) А можно подробнее, где вы взяли эту команду? Ткните меня носом, где эти команды описаны, буду очень признателен)

Logik
Offline
Зарегистрирован: 05.08.2014

EwigeDreamer пишет:
А можно подробнее, где вы взяли эту команду? Ткните меня носом, где эти команды описаны, буду очень признателен)

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

В общем рассуждаем так: оно гдето есть! мы оптимисты, ищем не чтото очень оригинальное а способ сконфигурировать MPU6050. Такие штуки должны быть на 99%. Логично его искать в либке соответствующей. Все либы у меня лежат d:\arduino-1.0.6\libraries\..  И  MPU6050 там же. Любая либа содержит файл *.h с именем совпадающим с именем либы. Т.е. искать будем в d:\arduino-1.0.6\libraries\MPU6050\MPU6050.h. Признаки по которому ищем это текст "GYRO_GONFIG", его части "GYRO", "GONFIG" , ну и на худой конец - "0x1B". Дальше любимым способом поиска - я тоталкомандером, поиском в файле ищем упоминание нужного. Ожидаем чтото начинающееся на "set" "ini" "load".  И внимательно смотрим на найденное. Мне попалось MPU6050_GYRO_FS_250. А значение 250 мне встречалось у вас, и рядом лежит требуемое MPU6050_GYRO_FS_2000! Ок. Осталось найти куда его пихать. Опять поиск по файлам, ищем где встречается"MPU6050_GYRO_FS_" но уже по всему каталогу MPU6050, находим в mpu6050.cpp такое

void MPU6050::initialize() {

    setClockSource(MPU6050_CLOCK_PLL_XGYRO);
    setFullScaleGyroRange(MPU6050_GYRO_FS_250);
    setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
    setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
}
Это инициализация и установка MPU6050_GYRO_FS_250. Копипастим нужное в форум, поправляем 250 на 2000 и дописываем чего нибудь от себя :)))
 
Подход универсальный, но руку набить надо. Причем путей поиска может быть много, главное результат.
EwigeDreamer
Offline
Зарегистрирован: 18.10.2015

слишком подробно, но огромное спасибо)

Ravor
Offline
Зарегистрирован: 22.02.2016

всем привет , порыля по инету нашёл как минимум 4 скетча на этот гироскоп , но вот в чём прикол , сначало просто проверил модуль этим скетчем.

#include <Wire.h>
#include "Kalman.h"
#include <Servo.h> 
Servo myservoX; 
Servo myservoY; 
Kalman kalmanX;
Kalman kalmanY;
uint8_t IMUAddress = 0x68;
/* IMU Data */
int16_t accX;
int16_t accY;
int16_t accZ;
int16_t tempRaw;
int16_t gyroX;
int16_t gyroY;
int16_t gyroZ;
double accXangle; // Angle calculate using the accelerometer
double accYangle;
double temp;
double gyroXangle = 180; // Angle calculate using the gyro
double gyroYangle = 180;
double compAngleX = 180; // Calculate the angle using a Kalman filter
double compAngleY = 180;
double kalAngleX; // Calculate the angle using a Kalman filter
double kalAngleY;
uint32_t timer;
void setup() {
  Wire.begin();
  Serial.begin(9600);
myservoX.attach(8);  
myservoY.attach(9);
  i2cWrite(0x6B,0x00); // Disable sleep mode      
  kalmanX.setAngle(180); // Set starting angle
  kalmanY.setAngle(180);
  timer = micros();
}
void loop() {
  /* Update all the values */
  uint8_t* data = i2cRead(0x3B,14);
  accX = ((data[0] << 8) | data[1]);
  accY = ((data[2] << 8) | data[3]);
  accZ = ((data[4] << 8) | data[5]);
  tempRaw = ((data[6] << 8) | data[7]);
  gyroX = ((data[8] << 8) | data[9]);
  gyroY = ((data[10] << 8) | data[11]);
  gyroZ = ((data[12] << 8) | data[13]);
  /* Calculate the angls based on the different sensors and algorithm */
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
  accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;  
  double gyroXrate = (double)gyroX/131.0;
  double gyroYrate = -((double)gyroY/131.0);
  gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
  gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);
  kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter
  kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
  timer = micros();
Serial.println();
    Serial.print("X:");
    Serial.print(kalAngleX,0);
    Serial.print(" ");
    Serial.print("Y:");
    Serial.print(kalAngleY,0);
    Serial.println(" ");
 myservoX.write((int)kalAngleX-90);
 myservoY.write((int)kalAngleY-90);
  // The accelerometer's maximum samples rate is 1kHz
}
void i2cWrite(uint8_t registerAddress, uint8_t data){
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.write(data);
  Wire.endTransmission(); // Send stop
}
uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes) {
  uint8_t data[nbytes];
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.endTransmission(false); // Don't release the bus
  Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading
  for(uint8_t i = 0; i < nbytes; i++)
    data [i]= Wire.read();
  return data;
}

Всё было прекрасно , скетч работал , ардуина получала данные , но я использовал его просто для проверки , спустя месяца 3 , наконец решил использовать его , но скетч уже не работал , в мониторе порта выяснилось  , что данные посылаются от балды.
Нашул другой светч , и данные снова посыпались , всё было хорошо , но пока я с ним разбирался , он перестал работать. (к не счастью посеял исходник).
Решил загрузить пример калмана для проверки и появилась вот эта ошибка , exit status 1

'i2cWrite' was not declared in this scope , при компеляции.

А теперь кульминация , вторый скетч перестал работать и появилась эта ошибка , а если нахожу скетч без ошибки , то он не работает вообще либо работает как 1 скетч.
Может кто обьяснить , как от этого избавится ?
Предложение выкинуть гироскоп временно не расматриваю.
AlexAxel
Offline
Зарегистрирован: 26.01.2017

Чтобы не плодить новую тему

Имеется сабжевый гироскоп, подключенный к Pro Mini 3.3V 8 MHz (пробовал также 5V 16 mHz). 

GND-GND

VCC-VCC

SDA-A4

SCL-A5

Код:

#include<Wire.h>
int i=0;
const int MPU_addr=0x68;
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
void setup()
{ Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true); 
pinMode (13,OUTPUT);
digitalWrite(13,LOW);
Serial.begin(9600); }
void loop()
{ Wire.beginTransmission(MPU_addr);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers
AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
i=AcZ/100;
if (i<150)
{
Serial.println(" EMERGENCY!!!! ");
Serial.println(AcZ/100);
Serial.println(AcY/100);
Serial.println(AcX/100); 
digitalWrite(13,HIGH);
delay (500);
digitalWrite(13,LOW);
delay(500);
}
}

смысл в чем - в горизонтальном положении i равно 150-152, при малейшем отклонении на любую ось значение i уменьшается, при этом выдается соответствующее сообщение в Serial,+ мыргает ламочка, и как только гироскоп возвращается в горизонталь - ламочка тухнет. Так вот - это все идеально работает, когда ардуина подключена к USB - все просто изюмительно. Но если подключить ардуину к батарейке (+ на RAW и - на GND) - то гироскоп не отрабатывает (сразу начинает моргать лампочкой, как будто значение i меньше 150. Подключенный "на горячую" разъем USB показывает, что значение i=0). При этом если стартовать через USB, потом подключить батарею, и потом отключить USB - то все работает корректно. В чем может быть затык?

Внешнее питание - 3,7В, 4.2 В, 8.4 В, 9 В.  - результат одинаков. 

AlexAxel
Offline
Зарегистрирован: 26.01.2017

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

harbor
Offline
Зарегистрирован: 31.05.2016

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

модуль еще не купил. насколько точно гироскоп может уловить изменение ? 

и может ли акселерометр фиксировать вибрацию от ударов ?

и заранее чтобы не молотить горы материала, какую бибилиотеку проще взять за основу?

AlexAxel
Offline
Зарегистрирован: 26.01.2017

я настраивал на фиксацию любого изменения положения относительно горизонта. Шкала чувствительности достаточно гибкая, на доли градусов реагирует. Могу скинуть скетч в ЛС с описанием чего оно и как делает

harbor
Offline
Зарегистрирован: 31.05.2016

AlexAxel, скиньте плз

AlexAxel
Offline
Зарегистрирован: 26.01.2017

2 harbor

недоступна отправка ЛС, видимо, не заслужил пока ))

короче - код в моем сообщении выше можно брать за основу

суть в чем: у датчика есть параметры, которые постоянно отслеживают положение датчика относительно горизона (те, которые начинаются на Ас). Если правильно горизонтировать датчик - то будет иметься постоянное значение по всем трем осям, для оси Z - это значение 0 при горизонтировании датчика "мордой вниз (когда пины для подключения торчат вверх)", и 15600 - при горизонтировании "мордой вверх". При изменении положения относительно горизонта в любом направлении достаточно учитывать изменения , которые принимает значение параметра AcZ. Изменения параметров АсХ и АсY интересны только в том случае, когда надо отследить наклон в определенную сторону. У меня значение параметра датчика делится на 100 перед обработкой (т.е. я оперирую значениями от 0 до 156, мне подобной точности достаточно), и датчик я располагаю "мордой вверх", т.е. горизонтальное значение - 156. И если его наклоняют так, что AcZ падает ниже 150 - то загорается ламочка. это примерно 4-5 градусов отклонение

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

Параметры Gy* лучше не использовать, т.к. они в состоянии покоя равны 0, изменяются в момент наклона, а потом снова на 0 возвращаются. Т.к. домкрат слишком плавная штука, то изменения положения датчиком вряд ли учуются. Зато сильный порыв ветра может качнуть машину, и будет сработка

Петрович
Offline
Зарегистрирован: 17.03.2016

AlexAxel пишет:

короче - код в моем сообщении выше можно брать за основу

Прощу прощения у ТС за то, что влез в его тему, но вопрос также по GY521.

Уважаемый AlexAxel, Ваш код работает, только чуть попробовал увеличить чувствтельность - все нравится. Вот вопрос в следующем: как к данному коду "прикрутить" обнуление горизонта при включении питания устройства. Другими словами, не всегда получается установить объект (автомобиль) в горизонтальной плоскости. Нужно заставить датчик считать свое положение в момент загрузки программы в МК и уже это положение использовать как "0". Это возможно? Если да - то прошу помочь мне с этим, т.к. у меня не хватает ума на "борьбу" с данным девайсом.

whoim
Offline
Зарегистрирован: 03.11.2011

Реально ли использовать модуль для работы только в двух осях - крен и тангаж? В общем, относительно линии горизонта, вертикально и вокруг горизонтальной линии модуля, идущей в горизонт?
Второй вопрос, нет готовых примеров с выводом в привычные градусы?)
Спасибо

whoim
Offline
Зарегистрирован: 03.11.2011

Реально ли использовать модуль для работы только в двух осях - крен и тангаж? В общем, относительно линии горизонта, вертикально и вокруг горизонтальной линии модуля, идущей в горизонт?
Второй вопрос, нет готовых примеров с выводом в привычные градусы?)
Спасибо

stepan_sotnikov
Offline
Зарегистрирован: 06.04.2019
В целях освоения среды и языка написал программку работы с Акселерометром, гироскопом и магнитометром. Программа в целом рабочая. И работает как должна. Но при сборки ругается, не понинимаю как устранить это.
C:\...compas_1.2.1.ino: In function 'boolean readMag(int16_t*, int16_t*, int16_t*)':
C:\...compas_1.2.1.ino:83:35: warning: ISO C++ says that these are ambiguous, even though the worst conversion for the first is better than the worst conversion for the second:Wire.requestFrom(Adres_for_Mg, 7);
In file included from C:\...compas_1.2.1.ino:15:0:
C:\...src/Wire.h:64:13: note: candidate 1: uint8_t TwoWire::requestFrom(int, int)
     uint8_t requestFrom(int, int);
 
             ^
C:\...src/Wire.h:61:13: note: candidate 2: uint8_t TwoWire::requestFrom(uint8_t, uint8_t)
     uint8_t requestFrom(uint8_t, uint8_t);
 
             ^
C:\...compas_1.2.1.ino: In function 'boolean readAccGir(int16_t*, int16_t*, int16_t*, int16_t*, int16_t*, int16_t*, int16_t*)':
 
C:\...compas_1.2.1\compas_1.2.1.ino:99:44: warning: ISO C++ says that these are ambiguous, even though the worst conversion for the first is better than the worst conversion for the second:
   Wire.requestFrom(Adres_for_AcGy, 14, true);
 
                                            ^
In file included from C:\...compas_1.2.1\compas_1.2.1.ino:15:0:
 
C:\...src/Wire.h:65:13: note: candidate 1: uint8_t TwoWire::requestFrom(int, int, int)
     uint8_t requestFrom(int, int, int);
 
             ^
C:\...src/Wire.h:62:13: note: candidate 2: uint8_t TwoWire::requestFrom(uint8_t, uint8_t, uint8_t)
     uint8_t requestFrom(uint8_t, uint8_t, uint8_t);
 
             ^

/*Учебная программа. Работа с магнитометром и гироскопом акселерометром (MPU6050; QMC5883)
Для корректировки данных с магнитометра используется матрица трансформации.
Коэффициенты получены обработкой сырых данных в Magneto ver. 1.2
Для гироскопа  коэффициенты дрейфа рассчитываются программно в setup() {}
функцией Corect_fo_Ac() на основании 1000 измерений. 
Для данных магнитометра по осям X;Y применен фильтр Калмана.
Для корректировки данных с акселерометра применен комплементарный фильтр с опорой на данные с акселерометра.
В результате работы программы получаем данные отклонения в градусах от направления на магнитный полюс, и от горизонта, где 0 горизонт.

Скетч использует 8824 байт (27%) памяти устройства. Всего доступно 32256 байт.
Глобальные переменные используют 522 байт (25%) динамической памяти, оставляя 1526 байт для локальных переменных. Максимум: 2048 байт.
*/

#include <Wire.h>
//#include <Servo.h>
// Servo myservo;

//настройка Акселерометра гироскопа
const uint8_t Adres_for_AcGy = 0x68;  // MPU6050
const uint8_t Reg_of_AcGy_PWR_MGMT = 0x6B;  //регистр управления питание
const uint8_t RD_of_AcGy = 0x3B;  //  0x3B (ACCEL_XOUT_H)

//настройка магнитометра
const uint8_t Adres_for_Mg = 0x0D;  // QMC5883
const uint8_t Reg_of_Mg_Set_Reset = 0x0B;  //регистр управления настройками
const uint8_t REG_CONTROL_of_Mg = 0x09;
const uint8_t RD_of_Mg = 0x00;

const float TO_Degrees = 57.2958;
const float TO_Radians = 0.01745;
//const float Pi = 3.14159;

//поправки дрейфа акс.
static int16_t Corect_fo_Ac_X;  //=144;
static int16_t Corect_fo_Ac_Y;  //=-145;
static int16_t Corect_fo_Ac_Z;  //=251;

//переменные для функции calculate
static int16_t MGx, MGy, MGz, AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
static float deltaTimer = 0.0;
static float Gy_roll_X = 0;
static float Gy_roll_Y = 0;
static float Gy_roll_Z = 0;
static uint32_t timer;
static float azimuth;

//коэффициент перехода от сырых данных гироскопа,
const float Transform_Gy_roll_to_Deg = 0.00763;  // 1/131
const float Gy_roll_camp_fo_Z = 0.01;            // коэф камп
const float Gy_roll_camp_fo_XY = 0.01;  //кохф камп для осей X Y


void initMagnetometr() {  //функция иниц. магн.
  Wire.begin();
  Wire.beginTransmission(Adres_for_Mg);
  Wire.write(Reg_of_Mg_Set_Reset);
  Wire.write(0b00000001);  //
  Wire.endTransmission(true);
  //------------------------------------
  Wire.beginTransmission(Adres_for_Mg);
  Wire.write(REG_CONTROL_of_Mg);
  Wire.write(0b00011101);  // Mode_Continuous,ODR_200Hz,RNG_8G,OSR_512
  Wire.endTransmission(true);
}

void initAccelgyro() {  //функция иниц. магн.
  Wire.begin();
  Wire.beginTransmission(Adres_for_AcGy);
  Wire.write(Reg_of_AcGy_PWR_MGMT);
  Wire.write(0b00000000);
  Wire.endTransmission(true);
}

//чтение данных маг.
boolean readMag(int16_t* MGx, int16_t* MGy, int16_t* MGz) { 
  Wire.beginTransmission(Adres_for_Mg);
  Wire.write(RD_of_Mg);
  int err = Wire.endTransmission();
  if (err) {
    return false;
  }
  Wire.requestFrom(Adres_for_Mg, 7);
  *MGx = (Wire.read() | Wire.read() << 8);
  *MGy = (Wire.read() | Wire.read() << 8);
  *MGz = (Wire.read() | Wire.read() << 8);
  return true;
}

boolean readAccGir(int16_t* AcX, int16_t* AcY, int16_t* AcZ, int16_t* Tmp,
                   int16_t* GyX, int16_t* GyY, int16_t* GyZ) {
  Wire.beginTransmission(Adres_for_AcGy);
  Wire.write(RD_of_AcGy);
  Wire.endTransmission(false);
  int err = Wire.endTransmission();
  if (err) {
    return false;
  }
  Wire.requestFrom(Adres_for_AcGy, 14, true);

  *AcX = Wire.read() << 8 | Wire.read();
  *AcY = Wire.read() << 8 | Wire.read();
  *AcZ = Wire.read() << 8 | Wire.read();
  *Tmp = Wire.read() << 8 | Wire.read();
  *GyY = Wire.read() << 8 | Wire.read();//зависит от взаиморасположение датчиков
  *GyX = Wire.read() << 8 | Wire.read(); 
  *GyZ = Wire.read() << 8 | Wire.read();
  return true;
}
     
void Corect_fo_Ac(int16_t* Corect_fo_Ac_X, int16_t* Corect_fo_Ac_Y,
                  int16_t* Corect_fo_Ac_Z) {
  long f_fo_GyX = 0;
  long f_fo_GyY = 0;
  long f_fo_GyZ = 0;
  for (int i = 0; i < 1000; i++) {
    //читаем сырые данные из гироакселерометра
    readAccGir(&AcX, &AcY, &AcZ, &Tmp, &GyX, &GyY, &GyZ); 
    f_fo_GyX = f_fo_GyX + GyX;
    f_fo_GyY = f_fo_GyY + GyY;
    f_fo_GyZ = f_fo_GyZ + GyZ;
  }

  *Corect_fo_Ac_X = (int16_t)(-f_fo_GyX / 1000);
  *Corect_fo_Ac_Y = (int16_t)(-f_fo_GyY / 1000);
  *Corect_fo_Ac_Z = (int16_t)(-f_fo_GyZ / 1000);
}

//функция калибровки данных магнитметра
 void calibrate(int16_t* MGx, int16_t* MGy, int16_t* MGz) {  
  float calibr_matrix[3][3] = {{ 1.117512, -0.032663,  0.003151},
                               {-0.032663, 1.095694, -0.034088},
                               { 0.003151, -0.034088,  1.102701}};
  float CombBias[3] = {       -241.661816, -373.203215, 41.251631};
  float val[3];
  val[0] = *MGx;
  val[1] = *MGy;
  val[2] = *MGz;
  for (uint8_t i = 0; i < 3; ++i) {  //умножения матриц
    val[i] = val[i] - CombBias[i];
  }
  float result[3] = {0, 0, 0};
  for (uint8_t i = 0; i < 3; ++i) {
    for (uint8_t j = 0; j < 3; ++j) {
      result[i] += calibr_matrix[i][j] * val[j];
    }
  }
  for (uint8_t i = 0; i < 3; ++i) {
    val[i] = result[i];
  }
  *MGx = (int16_t)val[0];
  *MGy = (int16_t)val[1];
  *MGz = (int16_t)val[2];
}

// переменные для Калмана
// скорость реакции на изменение (подбирается вручную) для 2 фильтров
float varProcess = 0.1;  
float varVoltX = 7.80255;  //среднее отклонение вычисляем из опыта
float PcX = 0.0, GX = 0.0, PX = 1.0, XpX = 0.0, ZpX = 0.0, XeX = 0.0;
// переменные для Калмана X
// Функция фильтрации
int16_t filterKalmanX(int16_t valX) {// Функция фильтрации
  PcX = PX + varProcess;
  GX = PcX / (PcX + varVoltX);
  PX = (1 - GX) * PcX;
  XpX = XeX;
  ZpX = XpX;
  XeX = GX * ((float)valX - ZpX) + XpX;  
  return (int16_t)(XeX);
}

// переменные для Калмана Y
float varVoltY = 13.29522;  // среднее отклонение вычисляем из опыта
float PcY = 0.0, GY = 0.0, PY = 1.0, XpY = 0.0, ZpY = 0.0, XeY = 0.0;
int16_t filterKalmanY(int16_t valY) {  // Функция фильтрации
  PcY = PY + varProcess;
  GY = PcY / (PcY + varVoltY);
  PY = (1 - GY) * PcY;
  XpY = XeY;
  ZpY = XpY;
  XeY = GY * ((float)valY - ZpY) + XpY;
  return (int16_t)(XeY);
}

void calculate() {
  //трансформируем данные магнитометра согласно калибр. матри.
  calibrate(&MGx, &MGy, &MGz);  
  //фильтруем данные маг.
  MGx = filterKalmanX(MGx);
  MGy = filterKalmanY(MGy);
  //вычисляем углы наклона по акс.
  float Ac_roll_X = atan(((float)(AcX)) / ((float)(AcZ))) * TO_Degrees;
  float Ac_roll_Y = atan(((float)(AcY)) / ((float)(AcZ))) * TO_Degrees;
  //вычисляем углы по гиро.
  Gy_roll_X -= (float)(GyX + Corect_fo_Ac_X) * deltaTimer *
               Transform_Gy_roll_to_Deg;  //здесь внимательно следить за +/-
  Gy_roll_Y += (float)(GyY + Corect_fo_Ac_Y) * deltaTimer *
               Transform_Gy_roll_to_Deg;  //зависит от взаиморасположения датчиков
  Gy_roll_Z -= (float)(GyZ + Corect_fo_Ac_Z) * deltaTimer * 
               Transform_Gy_roll_to_Deg;
  //Получаем углы вращения по гироакселеромету
  //вычисл. параметры наклонов A=A*(1-k)+B*(k)
  //где к-коэф. А-корректируемой значение, В-корректировочное значения
  Gy_roll_X = Gy_roll_X * (1 - Gy_roll_camp_fo_XY) +
              Ac_roll_X * (Gy_roll_camp_fo_XY);  // внимательно смотрим как
                                                 // расположены между собой
                                                 // датчики
  Gy_roll_Y = Gy_roll_Y * (1 - Gy_roll_camp_fo_XY) +
              Ac_roll_Y * (Gy_roll_camp_fo_XY);  //
  float SINx = sin(Gy_roll_X * TO_Radians);
  float COSx = cos(Gy_roll_X * TO_Radians);
  float SINy = sin(Gy_roll_Y * TO_Radians);
  float COSy = cos(Gy_roll_Y * TO_Radians);
  //доворачиваем вектор МП до положение датчика
  MGx = (int16_t)((float)MGx * COSy + 
                  (float)MGy * SINx * SINy +
                  (float)MGz * COSx * SINy);
  MGy = (int16_t)((float)MGy * COSx - (float)MGz * SINx);
  //по превед. вектору вычисляем отклонение оси Х от Направление на север
  //получили +/- отклонения Х от направ. на маг.Север
  azimuth = atan2((float)MGy, (float)MGx) * TO_Degrees;
  // значение Gy_roll_Z-azimuth подбирается из расчета скорости реакции
  if (abs(Gy_roll_Z - azimuth) > 15) {
    Gy_roll_Z = azimuth;
  }
  //на случай критического отставания
  Gy_roll_Z =
      Gy_roll_Z * (1 - Gy_roll_camp_fo_Z) + azimuth * (Gy_roll_camp_fo_Z);
  if (Gy_roll_Z < 0) {
    azimuth += 360;
  }
}

void setup() {
  initMagnetometr();  //инициализация магн.
  initAccelgyro();    //инициализация гиро.
  //вычисляем дрейф гироскопа
  Corect_fo_Ac(&Corect_fo_Ac_X, &Corect_fo_Ac_Y, &Corect_fo_Ac_Z);  
  Wire.begin();
  Serial.begin(230400);
  //инициализация серво.
  // myservo.attach(9);
  // myservo.write(0);
}

void loop() {
  timer = micros();
  readMag(&MGx, &MGy, &MGz);  //читаем сырые данные из магнитометра
  //читаем сырые данные из гироакселерометра
  readAccGir(&AcX, &AcY, &AcZ, &Tmp, &GyX, &GyY, &GyZ);
  calculate();  //пересчитываем полученные данные
  // int G=(int)(-Gy_roll_Z+90);
  // myservo.write(G);
  //выводим данные
  Serial.print(" ");
  Serial.print(Gy_roll_X);
  Serial.print(" ");
  Serial.print(Gy_roll_Y);
  Serial.print(" ");
  Serial.print(Gy_roll_Z);
  // Serial.print(" ");
  // Serial.print(azimuth);
  // Serial.print(" ");*/
  Serial.println(" ");
  deltaTimer =
      (float)(micros() - timer) / 1000000.0;  //получаем время между измерениями
}