Микронавигатор на STM32F100
The English version is available here.
Предыстория
Эта статья в некотором роде продолжение предыдущей статьи, связанной с инерциальными системами отсчета, но на этот раз речь пойдет о том, как собрать и заставить работать навигатор, используя микроконтроллер и набор недорогих датчиков. Законный вопрос — нафиг он нужен, если почти в каждом телефоне есть необходимый набор датчиков и приложений типа “компас”?
- Весьма неочевидное управление. В некоторых приложениях я так и не понял, как заставить стрелку показывать в нужном мне направлении. Показывать на север — сколько угодно. Как задать другую точку назначения — почти всегда проблема.
- Часть приложений начинает показывать на юг, а не на север, если перевернуть телефон.
- Мой смартфон работает без подзарядки дня 3, не больше. Некоторые работают вообще 1 день. Маловато, честно говоря.
Есть разные аналоги подобного устройства — GPS-возвращатели для грибников, путешественников и т.п. Если бы не было требования получать направление из любого положения устройства в пространстве — можно было бы существенно упростить конструкцию, а то и просто порекомендовать один из таких девайсов. Но одной из ключевых особенностей является возможность показывать направление из любого положения в пространстве.
Постановка задачи
Итак, необходимо с нуля собрать устройство, которое будет:
- Показывать направление из текущей точки и ориентации в пространстве в сторону заранее определенной координаты (например мой дом или местная достопримечательность).
- Иметь простой интерфейс и малые размеры.
- Питаться от батарейки и работать в автономном режиме без подзарядки не меньше месяца.
- Рассчитано на “эконом” сектор рынка.
В ходе обсуждения деталей подумали, что было бы неплохо, если бы устройство также показывало текущую дату и время, уровень заряда и имело на борту будильник. Дополнительно добавили bootloader для того чтобы облегчить обновление прошивки и замену целевых координат по USB и возможность работы от аккумулятора с контроллером зарядки/разрядки.
И всё же самыми главными требованиями оставались миниатюрность, экономичность и точность навигации.
Определение азимута
Для того, чтобы показать направление куда-то, нужно знать всего 2 вещи — где мы, собственно, находимся и ориентацию в пространстве. Зная текущую координату можно получить азимут, решая обратную геодезическую задачу.
Геодезическая задача связана с определением взаимного положения точек земной поверхности и подразделяется на прямую и обратную задачу. Прямой геодезической задачей называют вычисление геодезических координат — широты и долготы некоторой точки, лежащей на земном эллипсоиде, по координатам другой точки и по длине и азимуту геодезической линии, соединяющей эти точки. Обратная задача заключается в определении по геодезическим координатам двух точек на земном эллипсоиде длины и азимута геодезической линии между этими точками. В зависимости от длины геодезической линии, соединяющей рассматриваемые точки, применяются различные методы и формулы, разработанные в геодезии. По размерам принятого земного эллипсоида составляются таблицы, облегчающие решение геодезической задачи и рассчитанные на использование определённой системы формул.
Определение текущей ориентации
Для определения текущей ориентации использовали фильтр Маджвика — ПО с открытым исходным кодом, рассчитанное, в первую очередь, на низкую вычислительную мощность целевой системы. В качестве входных данных он использует показания акселерометра, гироскопа и магнитометра. На выходе получается кватернион, описывающий положение устройства в пространстве. Для лучшего понимания работы кватерниона рекомендую этот ресурс. Фильтр работает действительно быстро — на моём контроллере с тактовой частотой 24МГц он успевает сработать 360 раз в секунду, с учетом того, что еще тратится время на получение данных от датчиков. Фильтр почти не тратит ресурсы (в документации утверждается, что используется 160 операций сложения, 172 умножения, 5 делений и 5 извлечений квадратного корня) и имеет всего 2 параметра: частота опроса датчиков и коэффициент усиления. Можно модифицировать функцию и использовать не частоту получения данных, а подсчитывать разницу времени между двумя порциями измерений. Это улучшает показатели фильтра. Коэффициент усиления нужно подбирать в поисках компромисса между скоростью стабилизации и точностью.
Куда должна показывать стрелка?
Зная азимут и текущее положение устройства в пространстве, можно определить угол, на который надо повернуть стрелку, чтобы она указывала в нужном направлении. В этой реализации используются сразу 2 угла : dX и dY (соответственно угол, на который надо повернуть ось X и Y). Два угла нужны, если одна из осей вдруг совпадет с осью Z. В этом случае просто используется второй угол.
Наверняка есть более простые и аккуратные способы вычисления угла dX. Если у вас есть предложения, как это улучшить, пожалуйста, отпишите в комментариях.
Итак, у нас есть кватернион, описывающий текущее положение устройства в глобальной системе координат и азимут. Кватернион текущего положения обозначим через qCurrent. Возьмем проекцию единичного вектора на ось X и обозначим её через iX. Повернув iX кватернионом qCurrent, получаем вектор srcX. Если же повернуть iX на величину азимута, то получим вектор dstX. Разницу между этими векторами и нужно отобразить на дисплее. Для определения угла между этими векторами используется скалярное произведение векторов:
Для определения знака угла использовал векторное произведение векторов, точнее знак определителя.
Собственно на этом вся математика в проекте заканчивается :). У нас есть угол, на который нужно повернуть стрелку дисплея, чтоб она указывала в нужном направлении. При этом если dX будет равен 0, то компонент рысканье (можно вычислить из текущего кватерниона) текущего кватерниона будет равен найденному азимуту.
Используемые компоненты
В качестве контроллера решили использовать ARM по многим причинам. Основная — экономичность. Они дешевые, 32 битные, имеют гибкие настройки энергопотребления и т.п. Выбор пал на stm32f100rbt6 (180 рублей). Отладочная плата именно этой модели была у меня в наличии сразу, контроллер по характеристикам вполне подходил. Конечно жаль, что на нем нет аппаратного USB. С ним было бы ещё проще сделать bootloader. Да и по потреблению тока есть модели поинтересней. Но в сравнении с тем сколько потребляет GPS разница не очень заметна :). Остальные компоненты подбирались по мере доступности в нашем городе. Изначальный сет модулей выглядел так:
- Микроконтроллер stm32f100rbt6
- LCD дисплей Nokia 5110
- Акселерометр + гироскоп MPU6050
- GPS приемник Neo-6m
- Магнитометр LIS3MDL
В конечном варианте пришлось заменить магнитометр на QMC5883L. Связано это с тем, что все магнитометры LIS3MDL у меня сгорели. Не знаю, как по-другому назвать это явление. Один из модулей сразу перестал отзываться по I2C шине, 2 других выдают рандомные данные. Использовались эти магнитометры в виде модулей от российского производителя, которого я не буду называть. Но, судя по всему, эти модули можно использовать только с контроллерами, которые работают от 5В, но не от 3.3В. В документации, конечно, сказано, что можно использовать и так и так, но на деле ничего не получилось.
С QMC5883L тоже дружба завязалась не сразу. Дело в том, что я заказал магнитометры HMC5883L. Когда стал запрашивать данные по адресу 0x1E — получал ошибку, что устройства с таким адресом на шине нет. Быстренько собрав сканер I2C я убедился, что какое-то устройство имеет адрес 0x0d. Оказалось, что корпусы похожи и часто QMC принимают за HMC. Если вдруг кто-то удивляется, почему его HMC не отвечает — имейте в виду.
GPS используется всего для 2х вещей:
- Коррекция времени, если разница со значением в RTC (встроенные часы реального времени) больше, чем 5 секунд.
- Коррекция своего положения и азимута.
В финальной версии прошивки GPS приемник будет включаться только по запросу в целях экономии батарейки.
Скорее всего заменим дисплей на какой-нибудь OLED и улучшим подсветку.
Аппаратная часть
Сразу скажу, что разводка печатной платы в Kicad выглядит значительно лучше. Здесь схема приведена в чисто ознакомительных целях.
В конечном итоге получилось следующее:
- Контроллер общается с дисплеем по SPI.
- Магнитометр, гироскоп и акселерометр висят на шине I2C1.
- GPS модуль NEO-6M посылает данные по USART3.
Каждый из компонентов подключен через транзистор, что дает мне возможность включать/выключать устройства по мере надобности. Можно было бы магнитометр и гироскоп запитать от ног контроллера, но ток 5мА показался мне великоватым. В документации говорится, что IO выводы могут выдержать до 20мА, но на деле контроллер иногда просто перезагружался. Решилось дело хорошим стабилизатором и правильной разводкой питания, так что транзисторы для магнитометра и гироскопа остались просто как исторический элемент.
Что не так с I2C в stm32f100x?
Как я писал выше, магнитометр, гироскоп и акселерометр общаются с контроллером по шине I2C.
I2С гарантирует неплохую скорость работы при относительной простоте разработки. Есть 2 режима:
• Обычный режим — до 100КГц
• Быстрый режим — до 400КГц
I2С использует две линии, которые подтянуты к напряжению питания. Одна линия — линия данных, другая — линия тактирования. В интернете очень много подробных спецификаций и описания того, что это за шина и как она работает, поэтому сразу перейдем к особенностям реализации в контроллерах stm32f100x
Основные характеристики шины:
Каждое устройство может выступать как в роли Master, так и Slave.
В режиме Master реализуется:
- Генерация тактового сигнала
- Генерация старт-сигнала и стоп-сигнала
В режиме Slave реализуется:
- Механизм подтверждения адреса
- Использование двух Slave адресов
- Обнаружение стоп-бита, выданного ведущим на линию
- Генерация и определение 7/10 битных адресов
- Поддержка разных скоростей передачи данных
- Наличие множества флагов, сигнализирующих о событиях, а также об ошибках на линии
Возможна работа в одном из следующих режимов:
- Slave приемник
- Slave передатчик
- Master приемник
- Master передатчик
По умолчанию установлен режим slave, но как только устройство генерирует старт-бит оно сразу же превращается из подчиненного в ведущего.
В нашем случае используются только 2 режима: master приемник и master передатчик. Причем как передатчик наш контроллер используется только для инициализации и настройки модулей.
Само собой, что на все события I2C можно повесить своё прерывание.
Итак, как выглядит типичное чтение данных по шине I2C? Ну, приблизительно вот так:
Вроде бы ничего сложного, правда? То есть что делаем:
- Устанавливаем START (S)
- Ждем, когда установится бит SB (прерывание это или постоянный опрос регистра состояния — не имеет значения).
- Посылаем адрес устройства, с которым хотим общаться и признак того, что будем писать в устройство.
- Получаем ACK от него.
- Посылаем адрес регистра устройства, из которого хотим читать.
- Получаем ACK.
- Повторно посылаем START, чтоб другой мастер не перехватил управление линией.
- Посылаем адрес устройства, но на этот раз с признаком того, что будем читать из устройства.
- Читаем.
- После последнего принятого байта посылаем NACK и устанавливаем STOP состояние.
Предельно простой алгоритм, который выпил из меня всю кровь и развлекал целую неделю. Собака, как говорится, порылась в деталях :) Самое первое с чем я столкнулся — перед пунктом 1 должен быть пункт 0 — подождать, пока линия I2C освободится. Веселое в этом ожидании то, что конкретно эта модель может там зависнуть сразу после подачи питания навсегда. В errata пишут, что это связано с неправильной работой аналогового фильтра I2C и есть обходной путь. Т.е. сначала запускается таймер, который отключается как только линия становится свободной. Если же будет прерывание таймера, то скорее всего линия занята и надо действовать по рецепту из errata.
Было решено, что нужно организовать асинхронное чтение и запись данных по I2C. Во-первых, синхронное чтение через 20 секунд переставало работать - контроллер намертво зависал. Конечно, ошибки были в готовой библиотеке работы с MPU6050, но всё равно как-то неприятно. Во-вторых, каждый из датчиков имеет признак того, что данные готовы и можно их считывать. Они даже генерируют прерывания, сигнализирующие о готовности данных, поэтому было грех не воспользоваться таким механизмом. Да и разгрузить контроллер для других дел никогда не мешает. Для этого был реализован простой конечный автомат:
Здесь происходит следующее. Изначально конечный автомат в состоянии init. Как только нужно начать получать какие-то данные, генерируется состояние START. Дальше всё обрабатывается в прерываниях. Вроде бы тоже ничего сложного. Однако здесь есть проблема, которой я объяснения не нашел. Проявляется так:
- Генерируем START.
- До состояния 2 включительно всё отрабатывает как надо. В состоянии 2 генерируется повторный START.
- После повторного старта возникает прерывание, но ни один из флагов прерывания I2C не выставлен. Ну т.е. вообще ни одного флага. Нет флагов ошибок, нет ни одного флага из таблицы прерываний, приведенной выше. Однако следующее чтение регистра SR поясняет, что всё таки выставлен флаг SB.
Собственно когда нет флага прерывания довольно сложно его обработать. В нашем случае я просто ввёл маску всех возможных прерываний. Если ни одно не совпадает — прерывание игнорируется. На форуме stm какое-то объяснение есть. Там говорится, что при повторном состоянии START генерируется “раннее” прерывание со всеми очищенными флагами. Особенность реализации I2C в контроллерах STM32F100x. Я же для себя решил, что запрос на прерывание приходит быстрее, чем выставляется флаг SB, и когда я игнорирую такое прерывание, этот флаг не очищается. Генерируется повторное прерывание, там уже флаг установлен и всё отрабатывает нормально.
Ну и вторая проблема с тем, что нужно 2 разных конечных автомата на чтение данных. Если количество байт N ≥ 2 — нужно использовать конечный автомат, приведённый выше. Если N < 2, то там нужно отключать ACK и генерировать STOP сразу после того, как отправлен адрес устройства с флагом чтения. Тогда устройство отправит 1 байт, после получения которого контроллер не отправит ему ACK и сгенерирует STOP.
Вывод следующий: всегда нужно очень внимательно читать документацию к контроллеру и его errata. К примеру, первые версии автомата работали по событию RxNE, но только если нет прерываний от других источников. Если добавляются прерывания от таймеров, USART и т.д. — конечный автомат разваливался из-за установки флага BTF. Установка самого высокого приоритета для I2C прерываний не решает проблему.
Что не так с магнитометрами?
Победив в конечном итоге I2C, я начал уверенно получать данные от акселерометра, гироскопа и магнитометра. Казалось бы всё, можно скормить данные фильтру Маджвика, получить кватернион и вычислить необходимый угол поворота. Но тут опять всё пошло не так :). Стрелка упорно показывала в каком угодно направлении, но только не на магазин в 500 метрах от офиса, который я использовал как тестовую точку назначения. При этом приложение, написанное под Android, отрабатывало на ура. Кстати, разработать приложение под андроид, обложить всё тестами и убедиться, что вся математика работает было очень хорошей идеей. В поисках проблемы я для интереса вывел на экран данные, которые получаю от магнитометра. Сразу стало понятно, что просто магнитометр не откалиброван. Я вообще сильно удивился, что когда ось X направлена на север — магнитометр показывает одно значение. Если же повернуть Y в направлении севера, то магнитометр выдает совершенно отличное от X значение. Поначалу казалось, что это не имеет никакого смысла, но всё объясняется просто.
Существуют два типа искажений, действующих на магнитометр. Первое называется искажением твердого железа (Hard Iron Distortion). Оно по своей природе является аддитивным, то есть к изначально измеряемому полю добавляется дополнительное, создаваемое постоянным магнитом (в моем случае транзистором и остальными элементами на плате). При неизменной ориентации такого магнита относительно датчика, смещение, вносимое им, будет также неизменно. Ко второму типу относится искажение мягкого железа (Soft Iron Distortion). Оно создается посторонними предметами, искажающими уже имеющееся магнитное поле.
Бороться с искажением твёрдого железа легко. Для этого нужно несколько раз повернуть устройство вокруг оси Z и получить минимальное и максимальное значения для каждой из осей магнитометра. Затем то же самое сделать с осью Z магнитометра (т.е. повернуть устройство боком и покрутить вокруг оси X или Y). Получив максимальные и минимальные значения по осям вычисляем смещение как (Max — Min) / 2. Изначально я описывал магнитометром фигуру “восьмерка”, но этот метод давал худший результат, чем описан здесь. Видимо в моем регионе вертикальное магнитное отклонение большое.
С искажением мягкого железа бороться сложнее. В основном предлагается использовать приложение Mag Master, с помощью которого можно получить компенсирующую матрицу и значения смещений. Но оно, во-первых, написано на C# и работает под windows. Во-вторых, нужно дорабатывать свою прошивку, чтоб нормально работать с этой программой. Мы сделали кое-что не такое сложное, но в том же духе. Мы взяли уже рассчитанные минимальные/максимальные значения и использовали их для масштабирования данных магнитометра, чтобы выровнять отклик по трем осям измерения.
#define MagXMin 1880#define MagXMax 5547#define MagYMin 6185#define MagYMax 10157#define MagZMin -6530#define MagZMax 5305#define MagXBias ((MagXMin + MagXMax) / 2)#define MagYBias ((MagYMin + MagYMax) / 2)#define MagZBias ((MagZMin + MagZMax) / 2)#define MagXAvgDelta ((MagXMax - MagXMin) / 2.0f)#define MagYAvgDelta ((MagYMax - MagYMin) / 2.0f)#define MagZAvgDelta ((MagZMax - MagZMin) / 2.0f)#define MagAvgDelta ((MagXAvgDelta + MagYAvgDelta + MagZAvgDelta) / 3.0f)#define MagXScale (MagAvgDelta / MagXAvgDelta)#define MagYScale (MagAvgDelta / MagYAvgDelta)#define MagZScale (MagAvgDelta / MagZAvgDelta)mx = (float)(m_magRaw.data.mx - MagXBias) * MagXScale;my = (float)(m_magRaw.data.my - MagYBias) * MagYScale;mz = (float)(m_magRaw.data.mz - MagZBias) * MagZScale;
Таким образом, данные от магнитометра пришли в более-менее нормальный вид. Во всяком случае теперь при вращении нашим девайсом и визуализации полученных данных получалась фигура, похожая на сферу, а не 3 разных эллипса.
Однако всё равно в конечном варианте прошивки будет использоваться матрица 3x3 для коррекции показаний магнитометра.
Борьба с магнитным склонением
На самом деле я знал, что с ним придется что-то делать. Суть этого явления в том, что магнитометр показывает не на реальный север, а на магнитный. И разница этих направлений называется магнитным склонением.
Национальное агентство геопространственной разведки (NGA) предоставляет исходный код приложения, написанного на C, для определения значения магнитного склонения на основе WMM (world magnetic model). Там же предоставляется файл с данными, который обновляется каждые 5 лет. Если в прошлый раз мне не приходилось задумываться об этом (в Android уже реализован расчет магнитного склонения), то в этот раз пришлось разбираться с кодом, который это агентство предоставляет. Немного запутанно, но после рефакторинга всё встало на свои места и мы смогли адаптировать его для использования на микроконтроллере.
После учета магнитного склонения показания стрелки стали ещё точнее. В моем городе это всего 5 градусов разница, но в Москве (к примеру) это уже заметные 11 градусов.
Результат
В результате есть вот такой вот прототип и вариант, спаянный на макетке. Из оставшейся работы:
- Нормальное меню для настроек. Необходимо дать возможность менять свою временную зону.
- Добавить возможность удобно калибровать девайс.
- Добить PCB и заказать пару экземпляров.
Используйте и развивайте
Ссылка на репозиторий: https://github.com/maddevsio/mad-navigator.
Для предложений по улучшению используйте issues. Для помощи и консультации по внедрению библиотеки так же создавайте задачи в репозитории.
Данное решение распространяется под лицензией MIT. Используйте и не забывайте ставить копирайты с предупреждениями :)
Ссылки на используемые материалы
https://www.rlocman.ru/review/article.html?di=143960 — очень хорошее и подробное объяснение эффектов Hard и Soft Iron Distortion с примерами компенсации для двумерного случая.
https://teslabs.com/articles/magnetometer-calibration/ — еще более подробное объяснение того, как бороться с Hard и Soft iron distortion с примерами кода на питоне. Заключительная прошивка устройства будет опираться на решения, описанные здесь.
https://www.nxp.com/docs/en/application-note/AN4246.pdf — ещё одно техническое описание эффектов Hard и Soft iron distortion.
https://appelsiini.net/2018/calibrate-magnetometer/ — самое простое объяснение эффектов Hard и Soft iron distortion с примерами компенсации. Примечательно, что здесь описан значительно менее ресурсоемкий метод компенсации эффекта мягкого железа, используемый в текущей прошивке.
Следующие 3 ссылки объяснили ошибки, допущенные при сборке макета на начальном этапе и помогли их исправить:
http://easyelectronics.ru/razvedenie-pitaniya.html
http://cxem.net/comp/comp40.php
http://caxapa.ru/lib/emc_immunity.html
http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/ — всё, что связано с фильтром Маджвика, включая реализацию.
https://www.ngdc.noaa.gov/geomag/models.shtml — модель и код для вычисления характеристик магнитного поля.
