Микронавигатор на STM32F100

Oleg Katkov
Aug 6, 2019 · 13 min read
Image for post
Image for post
Прототип выглядит так

The English version is available here.

Предыстория

  1. Весьма неочевидное управление. В некоторых приложениях я так и не понял, как заставить стрелку показывать в нужном мне направлении. Показывать на север — сколько угодно. Как задать другую точку назначения — почти всегда проблема.
  2. Часть приложений начинает показывать на юг, а не на север, если перевернуть телефон.
  3. Мой смартфон работает без подзарядки дня 3, не больше. Некоторые работают вообще 1 день. Маловато, честно говоря.

Есть разные аналоги подобного устройства — GPS-возвращатели для грибников, путешественников и т.п. Если бы не было требования получать направление из любого положения устройства в пространстве — можно было бы существенно упростить конструкцию, а то и просто порекомендовать один из таких девайсов. Но одной из ключевых особенностей является возможность показывать направление из любого положения в пространстве.

Постановка задачи

  1. Показывать направление из текущей точки и ориентации в пространстве в сторону заранее определенной координаты (например мой дом или местная достопримечательность).
  2. Иметь простой интерфейс и малые размеры.
  3. Питаться от батарейки и работать в автономном режиме без подзарядки не меньше месяца.
  4. Рассчитано на “эконом” сектор рынка.

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

И всё же самыми главными требованиями оставались миниатюрность, экономичность и точность навигации.

Определение азимута

Image for post
Image for post
By derivative work: Pbroks13 (talk)RechtwKugeldreieck.svg: Traced by User:Stannered from a PNG by en:User:Rt66lt — RechtwKugeldreieck.svg, Public Domain, https://commons.wikimedia.org/w/index.php?curid=4375381

Геодезическая задача связана с определением взаимного положения точек земной поверхности и подразделяется на прямую и обратную задачу. Прямой геодезической задачей называют вычисление геодезических координат — широты и долготы некоторой точки, лежащей на земном эллипсоиде, по координатам другой точки и по длине и азимуту геодезической линии, соединяющей эти точки. Обратная задача заключается в определении по геодезическим координатам двух точек на земном эллипсоиде длины и азимута геодезической линии между этими точками. В зависимости от длины геодезической линии, соединяющей рассматриваемые точки, применяются различные методы и формулы, разработанные в геодезии. По размерам принятого земного эллипсоида составляются таблицы, облегчающие решение геодезической задачи и рассчитанные на использование определённой системы формул.

Определение текущей ориентации

Куда должна показывать стрелка?

Image for post
Image for post
Так выглядит нормальный режим работы, когда устройство показывает туда, куда нужно, т.е. угол dX = О.

Наверняка есть более простые и аккуратные способы вычисления угла dX. Если у вас есть предложения, как это улучшить, пожалуйста, отпишите в комментариях.

Итак, у нас есть кватернион, описывающий текущее положение устройства в глобальной системе координат и азимут. Кватернион текущего положения обозначим через qCurrent. Возьмем проекцию единичного вектора на ось X и обозначим её через iX. Повернув iX кватернионом qCurrent, получаем вектор srcX. Если же повернуть iX на величину азимута, то получим вектор dstX. Разницу между этими векторами и нужно отобразить на дисплее. Для определения угла между этими векторами используется скалярное произведение векторов:

Image for post
Image for post

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

Image for post
Image for post
Взято отсюда: https://ru.onlinemschool.com/math/library/vector/multiply1/

Собственно на этом вся математика в проекте заканчивается :). У нас есть угол, на который нужно повернуть стрелку дисплея, чтоб она указывала в нужном направлении. При этом если dX будет равен 0, то компонент рысканье (можно вычислить из текущего кватерниона) текущего кватерниона будет равен найденному азимуту.

Используемые компоненты

  1. Микроконтроллер stm32f100rbt6
  2. LCD дисплей Nokia 5110
  3. Акселерометр + гироскоп MPU6050
  4. GPS приемник Neo-6m
  5. Магнитометр LIS3MDL

В конечном варианте пришлось заменить магнитометр на QMC5883L. Связано это с тем, что все магнитометры LIS3MDL у меня сгорели. Не знаю, как по-другому назвать это явление. Один из модулей сразу перестал отзываться по I2C шине, 2 других выдают рандомные данные. Использовались эти магнитометры в виде модулей от российского производителя, которого я не буду называть. Но, судя по всему, эти модули можно использовать только с контроллерами, которые работают от 5В, но не от 3.3В. В документации, конечно, сказано, что можно использовать и так и так, но на деле ничего не получилось.

С QMC5883L тоже дружба завязалась не сразу. Дело в том, что я заказал магнитометры HMC5883L. Когда стал запрашивать данные по адресу 0x1E — получал ошибку, что устройства с таким адресом на шине нет. Быстренько собрав сканер I2C я убедился, что какое-то устройство имеет адрес 0x0d. Оказалось, что корпусы похожи и часто QMC принимают за HMC. Если вдруг кто-то удивляется, почему его HMC не отвечает — имейте в виду.

GPS используется всего для 2х вещей:

  1. Коррекция времени, если разница со значением в RTC (встроенные часы реального времени) больше, чем 5 секунд.
  2. Коррекция своего положения и азимута.

В финальной версии прошивки GPS приемник будет включаться только по запросу в целях экономии батарейки.

Скорее всего заменим дисплей на какой-нибудь OLED и улучшим подсветку.

Аппаратная часть

Image for post
Image for post
Сделано в https://easyeda.com/

В конечном итоге получилось следующее:

  1. Контроллер общается с дисплеем по SPI.
  2. Магнитометр, гироскоп и акселерометр висят на шине I2C1.
  3. GPS модуль NEO-6M посылает данные по USART3.

Каждый из компонентов подключен через транзистор, что дает мне возможность включать/выключать устройства по мере надобности. Можно было бы магнитометр и гироскоп запитать от ног контроллера, но ток 5мА показался мне великоватым. В документации говорится, что IO выводы могут выдержать до 20мА, но на деле контроллер иногда просто перезагружался. Решилось дело хорошим стабилизатором и правильной разводкой питания, так что транзисторы для магнитометра и гироскопа остались просто как исторический элемент.

Что не так с I2C в stm32f100x?

I2С гарантирует неплохую скорость работы при относительной простоте разработки. Есть 2 режима:

• Обычный режим — до 100КГц
• Быстрый режим — до 400КГц

I2С использует две линии, которые подтянуты к напряжению питания. Одна линия — линия данных, другая — линия тактирования. В интернете очень много подробных спецификаций и описания того, что это за шина и как она работает, поэтому сразу перейдем к особенностям реализации в контроллерах stm32f100x

Основные характеристики шины:

Каждое устройство может выступать как в роли Master, так и Slave.

В режиме Master реализуется:

  • Генерация тактового сигнала
  • Генерация старт-сигнала и стоп-сигнала

В режиме Slave реализуется:

  • Механизм подтверждения адреса
  • Использование двух Slave адресов
  • Обнаружение стоп-бита, выданного ведущим на линию
  • Генерация и определение 7/10 битных адресов
  • Поддержка разных скоростей передачи данных
  • Наличие множества флагов, сигнализирующих о событиях, а также об ошибках на линии

Возможна работа в одном из следующих режимов:

  • Slave приемник
  • Slave передатчик
  • Master приемник
  • Master передатчик

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

В нашем случае используются только 2 режима: master приемник и master передатчик. Причем как передатчик наш контроллер используется только для инициализации и настройки модулей.

Само собой, что на все события I2C можно повесить своё прерывание.

Image for post
Image for post
Image for post
Image for post
Взято из официальной документации

Итак, как выглядит типичное чтение данных по шине I2C? Ну, приблизительно вот так:

Image for post
Image for post

Вроде бы ничего сложного, правда? То есть что делаем:

  1. Устанавливаем START (S)
  2. Ждем, когда установится бит SB (прерывание это или постоянный опрос регистра состояния — не имеет значения).
  3. Посылаем адрес устройства, с которым хотим общаться и признак того, что будем писать в устройство.
  4. Получаем ACK от него.
  5. Посылаем адрес регистра устройства, из которого хотим читать.
  6. Получаем ACK.
  7. Повторно посылаем START, чтоб другой мастер не перехватил управление линией.
  8. Посылаем адрес устройства, но на этот раз с признаком того, что будем читать из устройства.
  9. Читаем.
  10. После последнего принятого байта посылаем NACK и устанавливаем STOP состояние.

Предельно простой алгоритм, который выпил из меня всю кровь и развлекал целую неделю. Собака, как говорится, порылась в деталях :) Самое первое с чем я столкнулся — перед пунктом 1 должен быть пункт 0 — подождать, пока линия I2C освободится. Веселое в этом ожидании то, что конкретно эта модель может там зависнуть сразу после подачи питания навсегда. В errata пишут, что это связано с неправильной работой аналогового фильтра I2C и есть обходной путь. Т.е. сначала запускается таймер, который отключается как только линия становится свободной. Если же будет прерывание таймера, то скорее всего линия занята и надо действовать по рецепту из errata.

Было решено, что нужно организовать асинхронное чтение и запись данных по I2C. Во-первых, синхронное чтение через 20 секунд переставало работать - контроллер намертво зависал. Конечно, ошибки были в готовой библиотеке работы с MPU6050, но всё равно как-то неприятно. Во-вторых, каждый из датчиков имеет признак того, что данные готовы и можно их считывать. Они даже генерируют прерывания, сигнализирующие о готовности данных, поэтому было грех не воспользоваться таким механизмом. Да и разгрузить контроллер для других дел никогда не мешает. Для этого был реализован простой конечный автомат:

Image for post
Image for post

Здесь происходит следующее. Изначально конечный автомат в состоянии init. Как только нужно начать получать какие-то данные, генерируется состояние START. Дальше всё обрабатывается в прерываниях. Вроде бы тоже ничего сложного. Однако здесь есть проблема, которой я объяснения не нашел. Проявляется так:

  1. Генерируем START.
  2. До состояния 2 включительно всё отрабатывает как надо. В состоянии 2 генерируется повторный START.
  3. После повторного старта возникает прерывание, но ни один из флагов прерывания I2C не выставлен. Ну т.е. вообще ни одного флага. Нет флагов ошибок, нет ни одного флага из таблицы прерываний, приведенной выше. Однако следующее чтение регистра SR поясняет, что всё таки выставлен флаг SB.

Собственно когда нет флага прерывания довольно сложно его обработать. В нашем случае я просто ввёл маску всех возможных прерываний. Если ни одно не совпадает — прерывание игнорируется. На форуме stm какое-то объяснение есть. Там говорится, что при повторном состоянии START генерируется “раннее” прерывание со всеми очищенными флагами. Особенность реализации I2C в контроллерах STM32F100x. Я же для себя решил, что запрос на прерывание приходит быстрее, чем выставляется флаг SB, и когда я игнорирую такое прерывание, этот флаг не очищается. Генерируется повторное прерывание, там уже флаг установлен и всё отрабатывает нормально.

Ну и вторая проблема с тем, что нужно 2 разных конечных автомата на чтение данных. Если количество байт N ≥ 2 — нужно использовать конечный автомат, приведённый выше. Если N < 2, то там нужно отключать ACK и генерировать STOP сразу после того, как отправлен адрес устройства с флагом чтения. Тогда устройство отправит 1 байт, после получения которого контроллер не отправит ему ACK и сгенерирует STOP.

Вывод следующий: всегда нужно очень внимательно читать документацию к контроллеру и его errata. К примеру, первые версии автомата работали по событию RxNE, но только если нет прерываний от других источников. Если добавляются прерывания от таймеров, USART и т.д. — конечный автомат разваливался из-за установки флага BTF. Установка самого высокого приоритета для I2C прерываний не решает проблему.

Что не так с магнитометрами?

Существуют два типа искажений, действующих на магнитометр. Первое называется искажением твердого железа (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 градусов.

Результат

Image for post
Image for post
Попытка продемонстрировать, что стрелка действительно указывает в одном направлении при вращении устройства.

В результате есть вот такой вот прототип и вариант, спаянный на макетке. Из оставшейся работы:

  1. Нормальное меню для настроек. Необходимо дать возможность менять свою временную зону.
  2. Добавить возможность удобно калибровать девайс.
  3. Добить PCB и заказать пару экземпляров.

Используйте и развивайте

Для предложений по улучшению используйте issues. Для помощи и консультации по внедрению библиотеки так же создавайте задачи в репозитории.

Данное решение распространяется под лицензией MIT. Используйте и не забывайте ставить копирайты с предупреждениями :)

Ссылки на используемые материалы

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 — модель и код для вычисления характеристик магнитного поля.

Mad Devs Blog — Custom Software Development Company

Engineering your growth. Mad Devs is the team behind large scalable projects, globally.

Thanks to sadwoolf, Oleg Puzanov, Kirill Avdeev, and Tamara Mun

Oleg Katkov

Written by

Mad Devs Blog — Custom Software Development Company

Mad Devs is a Cambridge-headquartered IT company developing enterprise-level software solutions for finance, transportation & logistics, security, edtech, and advertising industries. For more information about us, please browse our website: https://maddevs.io/

Oleg Katkov

Written by

Mad Devs Blog — Custom Software Development Company

Mad Devs is a Cambridge-headquartered IT company developing enterprise-level software solutions for finance, transportation & logistics, security, edtech, and advertising industries. For more information about us, please browse our website: https://maddevs.io/

Medium is an open platform where 170 million readers come to find insightful and dynamic thinking. Here, expert and undiscovered voices alike dive into the heart of any topic and bring new ideas to the surface. Learn more

Follow the writers, publications, and topics that matter to you, and you’ll see them on your homepage and in your inbox. Explore

If you have a story to tell, knowledge to share, or a perspective to offer — welcome home. It’s easy and free to post your thinking on any topic. Write on Medium

Get the Medium app

A button that says 'Download on the App Store', and if clicked it will lead you to the iOS App store
A button that says 'Get it on, Google Play', and if clicked it will lead you to the Google Play store