Как сделать электронный компас на датчике MPU-9250 и Arduino
Разобьём всю задачу на более мелкие подзадачи. Так сказать, проведём декомпозицию. И по шагам опишем весь процесс создания электронного компаса. Для проекта нам понадобятся:
- многофункциональный датчик MPU-9250;
- 3-разрядный 7-сегментный индикатор 3361AS;
- драйвер CD4511 для управления индикатором;
- 7 резисторов по 220 Ом (рекомендую набор резисторов с номиналами от 10 Ом до 1 МОм);
- пьезоизлучатель;
- Arduino UNO или иная совместимая плата;
- соединительные провода (например, вот такой набор);
- макетная плата;
- персональный компьютер со средой разработки Arduino IDE.
1Подключение датчика MPU-9250к Arduino
Датчик MPU-9250 – это по сути несколько датчиков, расположенных на одном чипе. Так, он реализует функции 3-осевого гироскопа, 3-осевого акселерометра и 3-осевого магнитометра. В данном проекте мы будем использовать только магнитометр. Остальные датчики можно отключить в целях уменьшения потребляемого тока, т.к. будем делать портативное устройство, питающееся от батареи «Крона».
Модуль имеет 10 выводов. Вот их назначение:
Вывод | Назначение вывода модуля с MPU-9250 (MPU-9255) |
---|---|
VCC | Внешнее питание 3.3 В. |
GND | Общий. |
SCL | Линия тактовых импульсов I2C и SPI. |
SDA | Линия данных для I2C или SPI. |
EDA | Линия данных при подключении внешних датчиков по шине I2C. |
ECL | Линия тактов при подключении внешних датчиков по шине I2C. |
AD0 | Для выставления адреса I2C в режиме I2C. В режиме SPI это линия данных от датчика. |
INT | Линия прерываний. Срабатывание настраивается при конфигурировании датчика MPU-9250. |
NCS | В режиме SPI – линия выбора ведомого (chip select). В режиме I2C не соединяется ни с чем. |
FSYNC | Зависит от конфигурации. |
Прежде чем подключать датчик MPU-9250 к Arduino, проверим его работоспособность с помощью моей любимой платы с микросхемой FT2232H. Для самого простого теста прочитаем содержимое регистра датчика, в котором содержится постоянное значение. Таким регистром может служить, например, регистр, в котором хранится идентификатор магнитометра, равный 0x48. Подключаться будем по интерфейсу I2C (для сокращения числа проводников). Как и большинство датчиков, MPU-9250 является ведомым на I2C шине. Схема подключения предельно проста: питание +3.3 В подаётся на вывод VCC, земля – GND, тактовая частота приходит на вывод SCL с ножки ADBUS0 микросхемы FT2232, линия данных SDA подключается одновременно к выводам ADBUS1 и ADBUS2 микросхемы FT2232.
Теперь, когда всё подключено, запускаем в режиме I2C программу SPI via FTDI, многократно описанную нами ранее. Оставляем настройки по умолчанию и сканируем шину I2C. Мы увидим, что программа обнаружила на шине одно устройство по адресу 0x68. Если прочитать из него 127 байтов (именно столько регистров имеет датчик MPU9250, техническое описание можно скачать в приложении к статье), то увидим следующее:
Здесь нет показаний магнитометра. Магнитометр (он называется AK8963) – это отдельное устройство на кристалле, которое не активно при подаче питания на датчик MPU-9250. Его нужно активировать явно. Для этого необходимо в регистр под номером 0x37 (INT_PIN_CFG) записать значение 0x02. Для этого в программе в поле записи укажем команду "37 02", как на рисунке, и нажмём кнопку «Записать». Полное описание регистра приводится далее.
Карта регистров магнитометра AK8963 представляет собой довольно короткую таблицу, состоящую всего из 13-ти байтов:
Как видно, по адресу 0x00 размещается постоянный Device ID, который должен быть равен 0x48. В нашем случае так и есть. Это хороший признак. Значит, магнитометр как минимум отвечает осмысленные данные, а мы можем их читать.
Схема подключений датчика MPU-9250 к Arduino остаётся предельно простой:
Вывод датчика MPU-9250 | Вывод Arduino |
---|---|
SCL | A5 |
SDA | A4 |
VCC | +3.3V |
GND | GND |
Для того чтобы использовать датчик MPU-9250 в режиме магнитометра, следует придерживаться такой последовательности действий:
- настроить регистр управления питанием PWR_MGMT_1 по адресу 0x6B;
- настроить пользовательский регистр контроля USER_CTRL по адресу 0x6A чтобы отключить мастер шины I2C (не будем его использовать);
- настроить регистр конфигурации прерываний INT_PIN_CFG по адресу 0x37 чтобы включить магнитометр;
- настроить регистр управления магнитометра CNTL1 по адресу 0x0A на непрерывное проведение измерений.
Реализуем это в следующем коде.
Скетч для чтения показаний датчика MPU-9255 (разворачивается)
#include <Wire.h> const byte MPU9255_I2C_ADDR = 0x68; // I2C адрес датчика MPU-9255 const byte MPU9255_MAG_I2C_ADDR = 0x0C; // I2C адрес магнитометра // Регистры датчика MPU-9255: const byte USER_CTRL = 0x6A; // { - | FIFO_EN | I2C_MST_EN | I2C_IF_DIS | - | FIFO_RST | I2C_MST_RST | SIG_COND_RST } const byte PWR_MGMT_1 = 0x6B; // { H_RESET | SLEEP | CYCLE | GYRO_STANDBY | PD_PTAT | CLKSEL[2:0] } const byte INT_PIN_CFG = 0x37; // { ACTL | OPEN | LATCH_INT_EN | INT_ANYRD_2CLEAR | ACTL_FSYNC | FSYNC_INT_MODE_EN | BYPASS_EN | - } const byte CNTL1 = 0x0A; // { 0 | 0 | 0 | BIT | MODE3 | MODE2 | MODE1 | MODE0 } void setup() { Serial.begin(115200); Wire.begin(); Write(PWR_MGMT_1, 0x8); // Отключаем гироскоп Write(USER_CTRL, 0); // Отключаем I2C мастер Write(INT_PIN_CFG, 0x02); // включаем режим bypass WriteMag(CNTL1, 0x12); // задаём непрерывный режим измерений с точностью 16 разрядов int mag_id = ReadMag(00); // читаем идентификатор датчика MPU-9255 Serial.print("Magnetometer ID="); Serial.println(mag_id, HEX); } void loop() { int xh = ReadMag(0x04); // читаем показания оси X, старший байт int xl = ReadMag(0x03); // читаем показания оси X, младший байт int yh = ReadMag(0x06); // аналогично для осей Y и Z int yl = ReadMag(0x05); int zh = ReadMag(0x08); int zl = ReadMag(0x07); int status = ReadMag(0x09); // сообщаем датчику, что прочитали значения, и можно продолжать измерения // Собираем значения: int x = word(xh, xl); // или так: (xh << 8) | (xl & 0xFF); int y = word(yh, yl); // или так: (yh << 8) | (yl & 0xFF); int z = word(zh, zl); // или так: (zh << 8) | (zl & 0xFF); // Выводим показания в мкТл: Serial.print("x="); Serial.print(GetMagneticFlux(x)); Serial.print(", y="); Serial.print(GetMagneticFlux(y)); Serial.print(", z="); Serial.print(GetMagneticFlux(z)); Serial.println(" uT"); delay(200); } // Читает по шине I2C из заданного регистра датчика MPU-9255 byte Read(int reg) { Wire.beginTransmission(MPU9255_I2C_ADDR); // начинаем обмен по I2C Wire.write(reg); Wire.endTransmission(false); Wire.requestFrom(MPU9255_I2C_ADDR, 1, false); // запрашиваем 1 байт от MPU-9255 byte val = Wire.read(); Wire.endTransmission(true); return val; } // Записывает по шине I2C в заданный регистр датчика MPU-9255 void Write(int reg, int data) { Wire.beginTransmission(MPU9255_I2C_ADDR); // начинаем обмен по I2C Wire.write(reg); Wire.write(data); Wire.endTransmission(true); } // Читает значение регистра магнитометра byte ReadMag(int reg) { Wire.beginTransmission(MPU9255_MAG_I2C_ADDR); Wire.write(reg); Wire.endTransmission(false); Wire.requestFrom(MPU9255_MAG_I2C_ADDR, 1, false); // запрашиваем у магнитометра 1 байт byte val = Wire.read(); Wire.endTransmission(true); return val; } // Записывает значение в регистр магнитометра void WriteMag(int reg, int data) { Wire.beginTransmission(MPU9255_MAG_I2C_ADDR); Wire.write(reg); Wire.write(data); Wire.endTransmission(true); } // Преобразует показания с датчика MPU-9255 в значения магнитного потока в микротеслах float GetMagneticFlux(int value){ return (value * 0.15); }
Теперь остаётся перевести показания индукции магнитного поля в азимут. Для этого воспользуемся следующей формулой:
float GetAzimuth(int x, int y) { float azimuth = atan2(x, y) * 180 / PI; return azimuth; }
Будем двигаться к следующей задаче.
2Подключение 7-сегментного светодиодногоиндикатора к Arduino
В качестве индикатора для вывода показаний компаса будем использовать семисегментный индикатор 3361AS-1. Он построен по принципу индикатора с общим катодом.
Светодиодный индикатор с общим катодом – это тип индикатора, состоящий из нескольких светодиодов в одном корпусе, у которых общая земля, а питание на каждый светодиод подаётся отдельно.
Напомню, что 7-сегментным индикатор называется из-за того, что он состоит из 7-ми светодиодов, которые расположены в форме цифры "8". Зажигая определённые сегменты, можно изображать разные цифры. Это похоже на цифры индекса на почтовом конверте: закрашивая определённые участки, мы пишем разные индексы. Зачастую дополнительно к 7-ми сегментам, индикатор содержит десятичную точку. Также индикатор может иметь несколько цифр – разрядов. Сегменты индикатора обозначаются латинскими буквами от A до G, как на рисунке.
Воспользуемся популярным способом управления 7-сегментным индикатором с помощью драйвера CD4511. Это микросхема двоично-десятичного преобразователя, который переводит двоичный код числа в напряжение на соответствующих цифре сегментах индикатора. Такой преобразователь использует всего 4 ножки Arduino. Например, если необходимо отобразить на индикаторе десятичное число "7", следует выставить на входе преобразователя двоичное "0111". Микросхема CD4511 выполняется в разных типах корпусов. Назначение выводов в исполнении с 16-тью ножками, такое:
Отечественными аналогами данного преобразователя являются микросхемы серий ИД1…ИД7.
При подключении двоичного декодера будем руководствоваться следующей таблицей:
Вывод CD4511 | Назначение | Примечание |
---|---|---|
A0...A3 | Входы двоичного преобразователя | Соответствуют разрядам двоичного числа. |
a...g | Выходы на сегменты индикатора | Подключаются через токоограничительные резисторы к соответствующим сегментам светодиодного индикатора. |
Lamp Test# | Тест индикатора (включает все сегменты) | Подключим к питанию, не использовать его. |
Blanking# | Очистка индикатора (отключает все сегменты) | Подключим к питанию, чтобы не использовать его. |
Latch Enabled# | Выход активен | Будет подключен к земле, чтобы выход был всегда активен. |
VDD | Питание микросхемы и индикатора | От 3 до 15 В. |
GND | Земля | Общая у CD4511, Arduino, 7-сегментного индикатора. |
Индикатор 3361AS не имеет токоограничительных резисторов, поэтому необходимо озаботиться этим самому, подключая индикатор. При напряжении питания 5 В сопротивление на каждый сегмент должно быть около 200 Ом.
Желательно также подключить керамический конденсатор ёмкостью примерно 1 мкФ между землёй и питанием микросхемы CD4511.
Нам нужно одновременно управлять тремя разрядами десятичного числа, используя только один преобразователь CD4511. Но чисто физически это невозможно. Однако можно добиться иллюзии постоянного свечения всех разрядов светодиодного индикатора. Для этого придётся быстро переключаться между разрядами, постоянно обновляя показание каждого разряда. Мы будем поочерёдно активировать каждый из разрядов индикатора 3361AS, выставлять на нём с помощью двоичного преобразователя CD4511 нужную цифру, а затем переключаться на следующий разряд.
Для человеческого глаза такое переключение между разрядами будет незаметно, но если результат снять на видео, то можно увидеть, как мерцают разряды чисел при переключении между разрядами, и даже мерцание отдельных светодиодов.
Скетч для управления трёхразрядным 7-сегментным индикатором (разворачивается)
// Выводы Arduino для управления двоичным конвертером CD4511: const byte bit0 = 11; const byte bit1 = 10; const byte bit2 = 9; const byte bit3 = 8; // Выводы Arduino для выбора десятичных разрядов индикатора 3361AS: const byte B_0 = 5; const byte B_1 = 6; const byte B_2 = 7; #define seconds() (millis()/1000) // макрос определения секунд, прошедших с начала работы скетча void setup() { pinMode(bit0, OUTPUT); pinMode(bit1, OUTPUT); pinMode(bit2, OUTPUT); pinMode(bit3, OUTPUT); pinMode(B_0, OUTPUT); pinMode(B_1, OUTPUT); pinMode(B_2, OUTPUT); digitalWrite(B_0, HIGH); digitalWrite(B_1, HIGH); digitalWrite(B_2, HIGH); } void loop() { // Каждую секунду увеличиваем показания индикатора на 1: int sec = seconds(); for (int i=0; i<1000; i++) { while (sec == seconds()) { printNumber(i); } sec = seconds(); } } // Выводит 3-разрядное число на 7-сегментный индикатор. void printNumber(int n) { setDigit(B_0, n/100); // выводим сотни десятичного числа setDigit(B_1, n/10 ); // выводим десятки setDigit(B_2, n/1 ); // выводим единицы } // Выводит заданное число на заданный разряд индикатора. void setDigit(byte digit, int value) { digitalWrite(digit, LOW); // выбираем разряд индикатора 3361AS-1 setNumber(value); // выводим на этот разряд число delay(4); digitalWrite(digit, HIGH); // снимаем выбор разряда индикатора } // Выставляет двоичный код на входе преобразователя CD4511 void setNumber(int n) { static const struct number { byte b3; byte b2; byte b1; byte b0; } numbers[] = { {0, 0, 0, 0}, // 0 {0, 0, 0, 1}, // 1 {0, 0, 1, 0}, // 2 {0, 0, 1, 1}, // 3 {0, 1, 0, 0}, // 4 {0, 1, 0, 1}, // 5 {0, 1, 1, 0}, // 6 {0, 1, 1, 1}, // 7 {1, 0, 0, 0}, // 8 {1, 0, 0, 1}, // 9 }; digitalWrite(bit0, numbers[n%10].b0); digitalWrite(bit1, numbers[n%10].b1); digitalWrite(bit2, numbers[n%10].b2); digitalWrite(bit3, numbers[n%10].b3); }
Итак, теперь мы умеем выводить трёхзначные числа на 7-сегментный индикатор, что нам понадобится для отображения азимута.
3Подключение динамика / пьезоизлучателяк Arduino
Для оповещения об отклонении от азимута, как было решено, будем использовать звуковой пьезоизлучатель. Мы уже обсуждали в отдельной статье, как подключить пьезоизлучатель к Arduino. Поэтому останавливаться подробно здесь не будем. Напомню ключевые моменты.
Схема подключения излучателя к Arduino очень простая: объединяем земли, а в цепь питания ставим резистор сопротивлением около 100 Ом (для защиты порта Ardunio).
Для Arduino есть специальные функции tone() и noTone(), которые используются для извлечения звука заданной частоты:
tone(piezoPin, freq); // подача звука с частотой freq (Гц) tone(piezoPin, freq, duration); // подача звука с частотой freq (Гц) длительностью duration (мс) noTone(piezoPin); // остановка звука
Здесь piezoPin – номер вывода Arduino, к которой подключён звуковой извещатель. Давайте изменим в предыдущем скетче функцию loop() таким образом (изменения выделены жирным):
void loop() { // Каждую секунду увеличиваем показания индикатора на 1: int sec = seconds(); for (int i=0; i<1000; i++) { while (sec == seconds()) { print_number(i); if (sec % 60 == 0){ // каждые 60 секунд tone(piezoPin, 1000, 500); // издавать звуковой сигнал } } sec = seconds(); } }
И конечно же, не забудем объявить в начале скетча piezoPin и задать ему режим работы OUTPUT. Теперь каждую минуту излучатель будет подавать звуковой сигнал продолжительностью 500 мс и частотой 1000 Гц.
4Собираем всё вместе:компас на Arduino
Схема нашего устройства будет такой (нарисована в DipTrace Schematic):
Здесь ARD1 – это Arduino Nano, CD4511 – драйвер управления 7-сегментным дисплеем 3361AS, MPU-9255 – собственно, сам модуль с магнитным датчиком, SW1 – кнопка для запуска и останова отслеживания азимута, BUZ – звуковой извещатель, а PWR – клемма для подачи внешнего питания от батареи «Крона» на устройство.
Монтаж компаса будем производить на печатной плате, которую «разведём» в программе DipTrace PCB Layout.
Закажем печатную плату здесь. На этом предприятии делают всё быстро и качественно. Например, изготовление данной печатной платы заняло около суток от момента заказа до отправки. Единственный минус – придётся долго ждать доставки из Китая (2-4 недели).
Распаяем элементы на плате.
После распайки компонентов плата электронного компаса будет выглядеть так:
Останется только придумать какой-то корпус для платы с компасом.
В процессе экспериментов выяснились несколько деталей, которые потребовали доработки. Во-первых, динамик вносит искажения в показания компаса. Величина искажения зависит от типа динамика и его близости к датчику. Поэтому его желательно отнести подальше от датчика, а не размещать непосредственно на плате.
Во-вторых, изначальный скетч определения азимута выводит довольно приблизительные и нестабильные измерения. Поэтому в части работы компаса всё было переделано. Я взял за основу скетч, представленный в этой статье. Он отличается тем, что используются показания акселерометра для коррекции наклона датчика, а также вводятся дополнительные коррекции, связанные с индивидуальными особенностями датчика (в частности, чувствительность ASAX, ASAY, ASAZ). Для нормальной работы этого скетча необходимо сделать следующее.
- Определить магнитное наклонение географического места, в котором вы находитесь. Сделать это можно, например, на сайте magnetic-declination.com. Значение магнитного наклонения необходимо присвоить переменной Declination скетча.
- Скорректировать остаточный угол наклона акселерометра MPU-9255. Для этого следует положить датчик на ровную горизонтальную поверхность и добиться того, чтобы в выводе монитора последовательного порта значения accel_pitch и accel_roll были максимально близкими к нулю. Это достигается в несколько итераций, последовательными приближениями. Начните с нулевых значений.
- Ввести калибровочные коэффициенты. Для этого необходимо задать Record_data = true. Скомпилировать и загрузить в плату скетч. Медленно вращать компас во всех направлениях, пока в мониторе порта не появятся значения коэффициентов. Скопировать из монитора порта эти значения в соответствующие поля в скетче (они там обозначены). Вернуть Record_data = false, скомпилировать и загрузить скетч в Arduino с новыми коэффициентами.
Для всего описанного ниже переменная DEBUG в скетче должна быть true. Более подробные пояснения приводятся в самом скетче.
От того, насколько качественно вы сделаете предварительную работу, будет сильно зависеть работа и стабильность показаний компаса.
Скетч для магнитного компаса на датчике MPU-9250/MPU-9255 (разворачивается)
#include <ButtonDebounce.h> #include <Wire.h> #define DEBUG true // для вывода отладочных сообщений ButtonDebounce button(2, 50); // кнопка #define Piezo 4 // пищалка #define Fsync 3 // вывод FSYNC датчика MPU9250/MPU9255 #define Intr 12 // вывод прерывания датчика MPU9250 // Выводы Arduino для управления двоичным конвертером CD4511: #define Bit0 11 #define Bit1 10 #define Bit2 9 #define Bit3 8 // Выводы Arduino для выбора десятичных разрядов индикатора 3361AS: #define B_0 5 #define B_1 6 #define B_2 7 bool TrackingOn = false; // признак включения/выключения режима отслеживания float CurrentAzimuth = 0; // текущий азимут float TrackingAzimuth = 0; // азимут, который необходимо отслеживать float AzmThreshold = 3; // максимальное отклонение от азимута отслеживания, градусы, +/- int Counter = 0; // счётчик измерений для сглаживания азимута const int SmoothingBy = 5; // по скольким элементам сглаживать показания азимута float AzimuthArray[SmoothingBy]; // массив азимутов, по которым проводится усреднение // Регистры датчика MPU9250: #define MPU9255_I2C_ADDR 0x68 // I2C адрес датчика MPU-9255 #define PWR_MGMT_1 0x6B // регистр управления питанием (H_RESET|SLEEP|CYCLE|GYRO_STANDBY|PD_PTAT|CLKSEL[2:0]) #define ACCEL_CONFIG 0x1C // регистр конфигурации акселерометра (ax_st_en|ay_st_en|az_st_en|ACCEL_FS_SEL[1:0]|-[2:0]) #define GYRO_CONFIG 0x1B // регистр конфигурации гироскопа (XGYRO_Ct_en|YGYRO_Ct_en|ZGYRO_Ct_en|GYRO_FS_SEL[1:0]|-|FCHOICE_B[1:0]) #define USER_CTRL 0x6A // USER_CTRL[5]=I2C_MST_EN #define INT_PIN_CFG 0x37 // INT_PIN_CFG[1]=BYPASS_EN // Гироскоп bool Gyro_synchronised = false; // флаг синхронизации гироскопа #define Frequency 125 // интервал сэмплирования = 8 мс #define Gyro_Sensitivity 65.5 // чувствительность гироскопа (см. datasheet Gyro_Sensitivity) int Gyro_x, Gyro_y, Gyro_z; long Gyro_x_cal, Gyro_y_cal, Gyro_z_cal; float Gyro_pitch, Gyro_roll, Gyro_yaw; float Gyro_pitch_output, Gyro_roll_output; // Акселерометр long Accel_x, Accel_y, Accel_z; // Магнитометр #define AK8963_I2C_ADDR 0x0C // I2C адрес магнитометра AK8963 #define CNTL1 0x0A // CNTL[3:0] - режим работы магнитометра #define AK8963_status_reg_1 0x02 // ST1[0] - готовность данных DRDY #define AK8963_data_ready_mask 0b00000001 // маска готовности данных #define AK8963_overflow_mask 0b00001000 // маска признака переполнения магнитометра #define AK8963_data_start 0x03 // начальный адрес данных по X,Y,Z #define AK8963_ASA 0x10 // коррекция чувствительности (ASA) по осям X,Y,Z (fuze ROM) float Declination = +11.532; // магнитное склонение в градусах. УКАЖИТЕ ЗДЕСЬ СВОЁ ЗНАЧЕНИЕ. int Mag_x, Mag_y, Mag_z; // "сырые" данные магнитометра float Mag_x_dampened, Mag_y_dampened, Mag_z_dampened; // со сглаженными флуктуациями float Mag_x_hor, Mag_y_hor; float Mag_pitch, Mag_roll; // Калибровочные данные: /* При первом использовании для учёта аппаратных и программных искажений необходимо откалибровать компас. Для этого: 1) Задать (Record_data = true). 2) Открыть монитор последовательного порта. 3) Медленно вращать компас во всех направлениях. 4) Скопировать значения из монитора порта в поля ниже. 5) Задать (Record_data = false), скомпилировать и загрузить скетч в Arduino. */ bool Record_data = false; // ЗАДАЙТЕ TRUE ДЛЯ КАЛИБРОВКИ. int Mag_x_offset = 162, Mag_y_offset = 69, Mag_z_offset = 92; // аппаратные отклонения (hard-iron) float Mag_x_scale = 0.72, Mag_y_scale = 0.79, Mag_z_scale = 2.96; // программные поправочные коэффициенты (soft-iron) float ASAX = 1.20, ASAY = 1.20, ASAZ = 1.16; /* значения коэффициентов чувствительности (A)sahi (S)ensitivity (A)djustment, * записанные в однократно программируемое ПЗУ (fuse ROM) */ // Макросы: #define Sensor_to_deg 1/(Gyro_Sensitivity*Frequency) // макрос преобразования показаний датчика в градусы #define Sensor_to_rad Sensor_to_deg*DEG_TO_RAD // макрос преобразования показаний датчика в радианы #define Loop_time 1000000/Frequency // макрос вычисления времени цикла (мкс) long Loop_start; // время начала цикла измерения, мкс long Loop_start_time; void setup() { pinMode(Bit0, OUTPUT); pinMode(Bit1, OUTPUT); pinMode(Bit2, OUTPUT); pinMode(Bit3, OUTPUT); pinMode(B_0, OUTPUT); pinMode(B_1, OUTPUT); pinMode(B_2, OUTPUT); digitalWrite(B_0, HIGH); digitalWrite(B_1, HIGH); digitalWrite(B_2, HIGH); pinMode(Intr, INPUT); pinMode(Fsync, OUTPUT); digitalWrite(Fsync, LOW); pinMode(Piezo, OUTPUT); button.setCallback(buttonChanged); // назначаем обработчик нажатия кнопки Wire.begin(); // инициализация шины I2C Wire.setClock(400000); // с заданной скоростью delay(100); #if DEBUG Serial.begin(115200); #endif int mag_id = ReadMag(0x00, 1); // читаем идентификатор датчика MPU-9255 #if DEBUG Serial.print("Magnetometer ID = "); Serial.println(mag_id, HEX); #endif if (mag_id != 0x48) { // если идентификатор датчика неверный (датчик отсутствует или неисправен) do { buzz(1, 1000); // бесконечный цикл звукового оповещения о неготовности датчика } while (true); } configure_magnetometer(); // конфигурируем магнитометр // Калибруем магнитометр (по необходимости): if (Record_data) { calibrate_magnetometer(); } config_mpu9255(); // конфигурируем датчик MPU-9250/9255 calibrate_gyro(); // калибруем гироскоп Loop_start_time = micros(); // запоминаем время начала цикла измерения для контроля скорости обновления данных гироскопа } void loop() { button.update(); // определяем состояние кнопки //////////////////////////////////////////// // ВЫЧИСЛЕНИЕ УГЛА НАКЛОНА // //////////////////////////////////////////// /* Ориентация MPU-9250; ось X смотрит на север */ read_mpu_9250_data(); // читаем "сырые" данные акселерометра и гироскопа MPU-6050 Gyro_x -= Gyro_x_cal; // вычитаем смещение X Gyro_y -= Gyro_y_cal; // вычитаем смещение Y Gyro_z -= Gyro_z_cal; // вычитаем смещение Z // Вычисление углов /* Pitch - тангаж, наклон вперёд/назад; roll - крен, наклон вправо/влево. Далее я не буду расшифровывать эти термины и буду называть их pitch и roll. Подстройка знаков показаний X,Y,Z гироскопа: Pitch (Nose - up) = +ve reading Roll (Right - wing down) = +ve reading Yaw (Clock - wise rotation) = +ve reading */ // Суммируем с "сырыми" показаниями гироскопа: Gyro_pitch += -Gyro_y * Sensor_to_deg; // по оси Y Gyro_roll += Gyro_x * Sensor_to_deg; // по оси X Gyro_yaw += -Gyro_z * Sensor_to_deg; // по оси Z // Компенсация угла крена гироскопа: Gyro_pitch += Gyro_roll * sin(Gyro_z * Sensor_to_rad); Gyro_roll -= Gyro_pitch * sin(Gyro_z * Sensor_to_rad); // Вычисление угла наклона акселерометра: long accel_total_vector = sqrt((Accel_x * Accel_x) + (Accel_y * Accel_y) + (Accel_z * Accel_z)); // полный вектор float accel_pitch = asin((float)Accel_x / accel_total_vector) * RAD_TO_DEG; // угол крена pitch float accel_roll = asin((float)Accel_y / accel_total_vector) * RAD_TO_DEG; // угол крена roll /* * Учёт остаточных отклонений показаний акселерометра: * 1) поместите акселерометр на ровную горизонтальную поверхность; * 2) подберите эти 2 значения, пока углы крена pitch и roll не станут "0". */ accel_pitch -= 1.0f; // корректировка угла крена (pitch) - ЗАДАЙТЕ СВОЁ ЗНАЧЕНИЕ accel_roll -= 1.0f; // корректировка угла крена (roll) - ЗАДАЙТЕ СВОЁ ЗНАЧЕНИЕ // Коррекция показаний гироскопа: if (Gyro_synchronised) // если гироскоп и акселерометр синхронизированы { // коррекция крена гироскопа: Gyro_pitch = 0.9996 * Gyro_pitch + 0.0004 * accel_pitch; // угол pitch Gyro_roll = 0.9996 * Gyro_roll + 0.0004 * accel_roll; // угол roll } else // иначе синхронизируем гироскоп и акселерометр { // задаём равными углы отклонения pitch и roll гироскопа и акселерометра: Gyro_pitch = accel_pitch; Gyro_roll = accel_roll; Gyro_synchronised = true; // устанавливаем флаг синхронизации } // Учёт флуктуаций: Gyro_pitch_output = 0.9 * Gyro_pitch_output + 0.1 * Gyro_pitch; // берём 90% от выходного значения угла pitch и добавляем 10% "сырого" угла pitch Gyro_roll_output = 0.9 * Gyro_roll_output + 0.1 * Gyro_roll; // берём 90% от выходного значения угла roll и добавляем 10% "сырого" угла roll //////////////////////////////////////////// // ВЫЧИСЛЕНИЕ ПОКАЗАНИЙ МАГНИТОМЕТРА // //////////////////////////////////////////// read_magnetometer(); // читаем показания магнитометра // Коррекция угла наклона и знака азимута: /* Оси X и Y гироскопа MPU-9250 и магнитометра AK8963 ориентированы под углом 90 градусов друг к другу, поэтому Mag_pitch совпадает с Gyro_roll, а Mag_roll совпадает с Gyro_pitch. Ось Z датчика MPU-9520 и ось Z магнитометра AK8963 направлены в противоположных направлениях, поэтому для компенсации знак Mag_pitch должен быть взят с противоположным знаком. */ Mag_pitch = -Gyro_roll_output * DEG_TO_RAD; Mag_roll = Gyro_pitch_output * DEG_TO_RAD; // Применяем стандартные формулы наклона: Mag_x_hor = Mag_x * cos(Mag_pitch) + Mag_y * sin(Mag_roll) * sin(Mag_pitch) - Mag_z * cos(Mag_roll) * sin(Mag_pitch); Mag_y_hor = Mag_y * cos(Mag_roll) + Mag_z * sin(Mag_roll); // Учёт флуктуаций: Mag_x_dampened = 0.9 * Mag_x_dampened + 0.1 * Mag_x_hor; Mag_y_dampened = 0.9 * Mag_y_dampened + 0.1 * Mag_y_hor; // Вычисляем азимут. Азимут - в направлении оси X магнитометра: float Heading = atan2(Mag_x_dampened, Mag_y_dampened) * RAD_TO_DEG; // направление на магнитный полюс /* Для вычисления направления на географический полюс необходимо учесть магнитное склонение. * Магнитное склонение Declination для своего географического местоположения * можно найти, например, на сайте http://www.magnetic-declination.com/ */ Heading += Declination; // направление на географический северный полюс /* * Магнитное отклонение положительно, когда магнитный полюс расположен * на востоке от географического севера, и отрицательно - когда на западе. * Для перевода в систему 0-360, необходимо скорректировать значение. */ // Перевод азимута в систему 0..360 градусов: if (Heading >= 360.0) Heading -= 360.0; if (Heading < 0.0) Heading += 360.0; // Сглаживаем показания азимута: if (Counter < SmoothingBy) { AzimuthArray[Counter] = Heading; // заполняем массив азимутов Counter +=1; } else // когда массив заполнен, вычисляем среднее арифметическое значение азимута { CurrentAzimuth = 0; for (int i=0; i<SmoothingBy; i++) { CurrentAzimuth += AzimuthArray[i]; // если значения около "0", то может быть ошибка усреднения и заниженный результат; // но ситуация довольно редкая, поэтому оставим пока без обработки. } CurrentAzimuth /= SmoothingBy; // считаем среднее арифметическое Counter = 0; // сбрасываем счётчик } // Выводим азимут: #if DEBUG Serial.print((String)CurrentAzimuth + "\t"); Serial.print((String)accel_pitch + "\t"); // ...и углы наклона pitch и roll акселерометра Serial.println((String)accel_roll); #endif printNumber(round(CurrentAzimuth)); // выводим азимут на 7-сегментный дисплей if (TrackingOn) // если включено отслеживание... { // ...проверяем, что не отклонились от заданного азимута больше допустимого: float delta = abs(CurrentAzimuth - TrackingAzimuth); if (delta > AzmThreshold) // при превышении допуска { buzz(1, 1000); // издаём звуковой сигнал digitalWrite(LED_BUILTIN, HIGH); // и зажигаем светодиод статуса } else // если азимут в допуске { digitalWrite(LED_BUILTIN, LOW); // гасим светодиод статуса } } // Контроль за циклом измерений while ((micros() - Loop_start_time) < 8000); // ждём окончания цикла Loop_start_time = micros(); // запоминаем время начала нового цикла измерений } // Конфигурирование магнитометра. void configure_magnetometer() { /* * Для доступа к магнитометру AK8963 шина I2C датчика MPU-9250 * должна быть переведена в режим pass-through. Для этого необходимо: * - отключить мастер I2C; * - включить режим bypass. */ // Отключаем мастер шины I2C датчика MPU9250: Wire.beginTransmission(MPU9255_I2C_ADDR); Wire.write(USER_CTRL); Wire.write(0x00); // отключаем мастер интерфейса I2C Wire.endTransmission(); // Активируем режим bypass: Wire.beginTransmission(MPU9255_I2C_ADDR); Wire.write(INT_PIN_CFG); Wire.write(0x02); // включаем режим bypass Wire.endTransmission(); /* Доступ к коэффициентам чувствительности AK8963. * Заводские значения коэффициентов чувствительности для осей X,Y,Z хранятся в однократно программируемом ПЗУ (fuse ROM). * Для доступа к ним мы должны изменить режим работы магнитометра AK9863. */ Wire.beginTransmission(AK8963_I2C_ADDR); Wire.write(CNTL1); // CNTL1[3:0] - биты управления режимом Wire.write(0b00011111); // задаём режим: выход 16-битный; доступ к ПЗУ Wire.endTransmission(); delay(100); // ожидание смены режима // Читаем заводские установки чувствительности по X,Y,Z из ПЗУ магнитометра: Wire.beginTransmission(AK8963_I2C_ADDR); Wire.write(AK8963_ASA); Wire.endTransmission(); Wire.requestFrom(AK8963_I2C_ADDR, 3); // запрашиваем 3 байта данных (регистры ASAX, ASAY, ASAZ) while (Wire.available() < 3); // ждём данные // Коррекция коэффициентов чувствительности: ASAX = (Wire.read() - 128) * 0.5 / 128 + 1; // по X ASAY = (Wire.read() - 128) * 0.5 / 128 + 1; // по Y ASAZ = (Wire.read() - 128) * 0.5 / 128 + 1; // по Z /* Это получается из формулы на странице 53 документа "MPU-9250, Register Map and Decriptions": Hadj = H*(((ASA-128)*0.5)/128) + 1, где H - считанное из регистра значение; ASA - значение чувствительности по оси; Hadj - измеренное скорректированное значение. */ // Выключаем магнитометр AK8963 пока идёт смена режима: Wire.beginTransmission(AK8963_I2C_ADDR); Wire.write(CNTL1); Wire.write(0); // выключаем магнитометр Wire.endTransmission(); delay(100); // ожидание смены режима // Задаём режим 2: Wire.beginTransmission(AK8963_I2C_ADDR); Wire.write(CNTL1); Wire.write(0b00010110); // выход 16-битный, измерения непрерывные с частотой 100 Гц Wire.endTransmission(); delay(100); // ожидание смены режима } // Калибровка магнитометра. void calibrate_magnetometer() { #if DEBUG Serial.println("Slowly rotate compass "); #endif // Максимумы: int mag_x_min = 0x7FFF; int mag_y_min = 0x7FFF; int mag_z_min = 0x7FFF; int mag_x_max = 0x8000; int mag_y_max = 0x8000; int mag_z_max = 0x8000; // Запоминаем min/max показания компаса по осям X,Y,Z: for (int cnt=0; cnt<16000; cnt++) // по 16000 значениям { Loop_start = micros(); // запоминаем время начала цикла Wire.beginTransmission(AK8963_I2C_ADDR); Wire.write(AK8963_status_reg_1); // регистр статуса ST1 Wire.endTransmission(); Wire.requestFrom(AK8963_I2C_ADDR, 1); // читаем 1 байт из регистра статуса ST1 while (Wire.available() < 1); if (Wire.read() & AK8963_data_ready_mask) // проверяем бит готовности данных (бит 0 регистра - DRDY) { // Читаем данные по осям X,Y,Z: Wire.requestFrom(AK8963_I2C_ADDR, 7); while (Wire.available() < 7); // Комбинируем LSB,MSB, применяем коррекцию ASA по X,Y,Z: int mag_x = (Wire.read() | Wire.read() << 8) * ASAX; int mag_y = (Wire.read() | Wire.read() << 8) * ASAY; int mag_z = (Wire.read() | Wire.read() << 8) * ASAZ; int status2 = Wire.read(); // читаем из регистра ST2; тем самым сообщаем, что закончили чтение данных // Проверка прочитанных данных: if (!(status2 & AK8963_overflow_mask)) // проверка флага переполнения HOFL в ST2[3] { // Определяем max/min значения: mag_x_min = min(mag_x, mag_x_min); mag_x_max = max(mag_x, mag_x_max); mag_y_min = min(mag_y, mag_y_min); mag_y_max = max(mag_y, mag_y_max); mag_z_min = min(mag_z, mag_z_min); mag_z_max = max(mag_z, mag_z_max); } } delay(4); // время между чтениями магнитометра } // Вычисляем аппаратные поправки (hard-iron) как средние значения: Mag_x_offset = (mag_x_max + mag_x_min) / 2; Mag_y_offset = (mag_y_max + mag_y_min) / 2; Mag_z_offset = (mag_z_max + mag_z_min) / 2; // Вычисляем программные коэффициенты шкалы (soft-iron): float chord_x = ((float)(mag_x_max - mag_x_min)) / 2; float chord_y = ((float)(mag_y_max - mag_y_min)) / 2; float chord_z = ((float)(mag_z_max - mag_z_min)) / 2; float chord_average = (chord_x + chord_y + chord_z) / 3; // вычисляем среднее по всем трём осям // Вычисляем коэффициенты шкалы: Mag_x_scale = chord_average / chord_x; Mag_y_scale = chord_average / chord_y; Mag_z_scale = chord_average / chord_z; // В режиме первичной калибровки выводит данные магнитометра в последовательный порт: if (Record_data) { #if DEBUG // Выводим пределы: Serial.print("XYZ Max/Min: "); Serial.print((String)mag_x_min + "\t"); Serial.print((String)mag_x_max + "\t"); Serial.print((String)mag_y_min + "\t"); Serial.print((String)mag_y_max + "\t"); Serial.print((String)mag_z_min + "\t"); Serial.println((String)mag_z_max); // Выводим аппаратные отклонения: Serial.print("Hard-iron: "); Serial.print((String)Mag_x_offset + "\t"); Serial.print((String)Mag_y_offset + "\t"); Serial.println(Mag_z_offset); // Выводим программные коэффициенты: Serial.print("Soft-iron: "); Serial.print((String)Mag_x_scale + "\t"); Serial.print((String)Mag_y_scale + "\t"); Serial.println(Mag_z_scale); // Выводим значения чувствительности из ПЗУ магнитометра: Serial.print("ASA: "); Serial.print((String)ASAX + "\t"); Serial.print((String)ASAY + "\t"); Serial.println(ASAZ); #endif while (true); // дальше не идём (входим бесконечный цикл) } } // Читает показания магнитометра. void read_magnetometer() { Wire.beginTransmission(AK8963_I2C_ADDR); Wire.write(AK8963_status_reg_1); // указатель на бит статуса ST1[0] Wire.endTransmission(); Wire.requestFrom(AK8963_I2C_ADDR, 1); // запрашиваем 1 байт данных while (Wire.available() < 1); // ждём данные if (Wire.read() & AK8963_data_ready_mask) // проверяем бит готовности данных { // Читаем данные по осям: Wire.requestFrom(AK8963_I2C_ADDR, 7); // запрашиваем 7 байтов данных while (Wire.available() < 7); // ждём данные // Объединяем LSB и MSB, применяем коррекцию ASA по X,Y,Z: int mag_x = (Wire.read() | Wire.read() << 8) * ASAX; int mag_y = (Wire.read() | Wire.read() << 8) * ASAY; int mag_z = (Wire.read() | Wire.read() << 8) * ASAZ; int status2 = Wire.read(); // читаем из регистра ST2; тем самым сообщаем, что закончили чтение данных // Проверяем данные: if (!(status2 & AK8963_overflow_mask)) // проверяем флаг HOFL в ST2[3] { Mag_x = (mag_x - Mag_x_offset) * Mag_x_scale; Mag_y = (mag_y - Mag_y_offset) * Mag_y_scale; Mag_z = (mag_z - Mag_z_offset) * Mag_z_scale; } } } // Конфигурирование датчика MPU-9250/9255. void config_mpu9255() { // Активируем MPU9255: Wire.beginTransmission(MPU9255_I2C_ADDR); Wire.write(PWR_MGMT_1); // в регистр управления питанием записываем, что... Wire.write(0x00); // ...будем использовать внутренний источник тактового сигнала 20 МГц Wire.endTransmission(); // Конфигурируем акселерометр: Wire.beginTransmission(MPU9255_I2C_ADDR); Wire.write(ACCEL_CONFIG); Wire.write(0x10); // задаём предел шкалы измерений (+/-8g) Wire.endTransmission(); // Конфигурируем гироскоп: Wire.beginTransmission(MPU9255_I2C_ADDR); Wire.write(GYRO_CONFIG); Wire.write(0x08); // выбираем шкалу 500dps (DPS – degrees per second, градусы в секунду) Wire.endTransmission(); } // Калибровка гироскопа. void calibrate_gyro() { digitalWrite(LED_BUILTIN, HIGH); // покажем, что начали калибровку, включив светодиод buzz(3, 1000); #if DEBUG Serial.println("Calibrating gyro"); #endif for (int cnt=0; cnt<2000; cnt++) // усредним показания гироскопа по 2000 значениям { Loop_start = micros(); read_mpu_9250_data(); // читаем "сырые" данные акселерометра и гироскопа MPU-6050 Gyro_x_cal += Gyro_x; // добавляем отклонение по X Gyro_y_cal += Gyro_y; // добавляем отклонение по Y Gyro_z_cal += Gyro_z; // добавляем отклонение по Z while (micros() - Loop_start < Loop_time); // ждём истечения периода времени Loop_time } // Вычисляем среднее отклонение по X,Y,Z: Gyro_x_cal /= 2000; Gyro_y_cal /= 2000; Gyro_z_cal /= 2000; #if DEBUG Serial.println("Calibrating gyro done"); #endif digitalWrite(LED_BUILTIN, LOW); // гасим светодиод buzz(3, 1000); } // Читает "сырые" данные с MPU-9250/9255. void read_mpu_9250_data() { Wire.beginTransmission(MPU9255_I2C_ADDR); Wire.write(0x3B); // адрес регистра начала данных (ACCEL_XOUT_H) Wire.endTransmission(); Wire.requestFrom(MPU9255_I2C_ADDR, 14); // запрашиваем 14 байтов у MPU-6050 while (Wire.available() < 14); // ждём данные // Показания акселерометра по X,Y,Z: Accel_x = Wire.read() << 8 | Wire.read(); Accel_y = Wire.read() << 8 | Wire.read(); Accel_z = Wire.read() << 8 | Wire.read(); int temp = Wire.read() << 8 | Wire.read(); /* данные о температуре * (не используем, но прочитать нужно, чтобы продвинуться на 2 регистра вперёд) */ // Показания гироскопа по X,Y,Z: Gyro_x = Wire.read() << 8 | Wire.read(); Gyro_y = Wire.read() << 8 | Wire.read(); Gyro_z = Wire.read() << 8 | Wire.read(); } // Читает num число байтов, начиная с заданного регистра магнитометра. byte ReadMag(int reg, int num) { Wire.beginTransmission(AK8963_I2C_ADDR); Wire.write(reg); Wire.endTransmission(false); Wire.requestFrom(AK8963_I2C_ADDR, num, false); // запрашиваем у магнитометра num байтов byte val = Wire.read(); Wire.endTransmission(true); return val; } // ПЬЕЗО ******************************************************** // Издаёт звуковой сигнал заданное число раз с заданной частотой звука. void buzz(int num, int freq) { for (int i=0; i<num-1; i++) { tone(Piezo, freq, 50); delay(100); } tone(Piezo, freq, 50); } // КНОПКА ******************************************************* // Обработчик нажатия кнопки. // Если кнопка нажата, то включает слежение за азимутом. // Повторное нажатие выключает отслеживание азимута. void buttonChanged(int state) { if (state) { TrackingOn = !TrackingOn; if (TrackingOn) { TrackingAzimuth = CurrentAzimuth; // запоминаем азимут, который нужно отслеживать buzz(1, 500); // сигнализируем о начале отслеживания азимута } else { digitalWrite(LED_BUILTIN, LOW); // гасим светодиод buzz(2, 500); // сигнализируем о завершении отслеживания азимута } #if DEBUG Serial.print("Tracking="); Serial.println(TrackingOn); #endif } } // 7-СЕГМЕНТНЫЙ ДИСПЛЕЙ ***************************************** // Выводит 3-разрядное число на 7-сегментный индикатор. void printNumber(int n) { setDigit(B_0, n/100); // выводим сотни десятичного числа setDigit(B_1, n/10); // выводим десятки setDigit(B_2, n/1); // выводим единицы } // Выводит заданное число на заданный разряд индикатора. void setDigit(byte digit, int value) { digitalWrite(digit, LOW); // выбираем разряд индикатора 3361AS-1 setNumber(value); // выводим на этот разряд число delay(5); digitalWrite(digit, HIGH); // снимаем выбор разряда индикатора } // Выставляет двоичный код на входе преобразователя CD4511 void setNumber(int n) { static const struct number { byte b3; byte b2; byte b1; byte b0; } numbers[] = { {0, 0, 0, 0}, // 0 {0, 0, 0, 1}, // 1 {0, 0, 1, 0}, // 2 {0, 0, 1, 1}, // 3 {0, 1, 0, 0}, // 4 {0, 1, 0, 1}, // 5 {0, 1, 1, 0}, // 6 {0, 1, 1, 1}, // 7 {1, 0, 0, 0}, // 8 {1, 0, 0, 1}, // 9 }; // массив структур типа number digitalWrite(Bit0, numbers[n%10].b0); digitalWrite(Bit1, numbers[n%10].b1); digitalWrite(Bit2, numbers[n%10].b2); digitalWrite(Bit3, numbers[n%10].b3); }
Скачать вложения:
- Скачать datasheet на датчик MPU9250 (957 Скачиваний)
- Карта регистров датчика MPU-9250 (902 Скачиваний)
- Скачать datasheet на двоичный преобразователь CD4511 (842 Скачиваний)
- Скачать техническое описание на микросхемы ИД1...ИД7 (781 Скачиваний)
- Техническое описание магнитометра AK8963 (4027 Скачиваний)
Поблагодарить автора:
Поделиться
Похожие материалы (по тегу)
7 комментарии
-
Dima 08.05.2020 11:09 Комментировать
А как сделать так, чтобы работал гироскоп, акселерометр и компас?
-
aave1 09.05.2020 15:08 Комментировать
Dima, это слишком общий вопрос. На такой вопрос - общий ответ: следовать документации на датчик MPU-9250. Он имеет большие возможности и огромное число настраиваемых параметров. Краткий ответ: использовать готовые библиотеки для Arduino, они есть. Просто мне интереснее в данном случае разобраться, как взаимодействовать с датчиком MPU-9250 самому, без "посредников".
-
неАдмин 28.03.2021 16:53 Комментировать
Откуда брали адреса регистров и значения? Можно ли их вычислить не имея datasheet? Есть микросхема магнитометр + акселерометр с i2c. Знаю только куда провода цеплять.
-
aave1 28.03.2021 17:41 Комментировать
неАдмин, адреса регистров можно узнать только из документации на модуль.
-
Дмитрий 28.08.2021 19:55 Комментировать
// Применяем стандартные формулы наклона:
Mag_x_hor = Mag_x * cos(Mag_pitch) + Mag_y * sin(Mag_roll) * sin(Mag_pitch) - Mag_z * cos(Mag_roll) * sin(Mag_pitch);
Mag_y_hor = Mag_y * cos(Mag_roll) + Mag_z * sin(Mag_roll);
// Компенсация наклона:
Mag_x_hor = Mag_x;
Mag_y_hor = Mag_y;
Что-то не особо понятна логика данного участка кода. Сначала производятся вычисления углов наклона с сохранением их в переменные Mag_x_hor и Mag_y_hor, а потом эти же переменные перезаписываются другими значениями. Здесь ошибка? -
aave1 04.09.2021 21:16 Комментировать
Дмитрий, вы правы. В финальном варианте скетча этого нет, а тут забыл убрать. Недоглядел. Спасибо.
-
match 25.12.2021 11:29 Комментировать
Спасибо за статью!
пытаюсь разобраться, но возникает много вопросов, например
float ASAX = 1.20, ASAY = 1.20, ASAZ = 1.16; /* значения коэффициентов чувствительности (A)sahi (S)ensitivity (A)djustment,
* записанные в однократно программируемое ПЗУ (fuse ROM) */
Почему, зачем вы пытаетесь преобразовать во float значения из восьмибитного регистра?