Джойстик с датчиком положения MPU6050 для STM32F4

В этом демонстрационном примере рассмотрена модель Engee для считывания углов осевого вращения с фильтром Калмана, преобразования полученных углов и передачи ограниченных углов поворота в последовательный порт.

Введение

Проект, базирующийся на модели Engee stm32_mpu6050.engee, выполняет следующие задачи:

• получение по каналу I2C контроллера STM32F446RE ускорения и скорости поворота по трём осям вращения от датчика MPU6050;
• преобразование этих величин в осевые повороты - крен, тангаж, рыскание с фильтрацией фильтром Калмана;
• получение дискретных сигналов "Фиксация", "Переключение масштаба";
• преобразование осевых поворотов в отклонения координат вектора нормали джойстика в декартовой системе координат;
• преобразование отклонений координат в углы $\vartheta, \varphi$ поворота джойстика в сферических координатах;
• ограничение полученных углов $\left[ 0; \ \frac{\pi}{2} \right], \left[ 0; \ 2\pi \right]$ соответственно;
• вывод переменных: углы $\vartheta, \varphi$, "Фиксация", "Переключение масштаба" в универсальный приемо-передатчик (USART).

Элементы и окружение:

• Отладочная плата: NUCLEO-F446RE
• Микроконтроллер: STM32F446RE
• Входные каналы: I2C, 2$\times$ DI
• Выходные каналы: USART (COM)
• Датчик трёхосевого акселерометра-гироскопа: MPU6050 (GY-521)
• Среда разработки: Engee -> VS Code 1.92.1 + PlatformIO 6.1.15
• Framework PlatformIO: stm32duino

Используемый framework PlatformIO - stm32duino использован ввиду наличия готовых отлаженных подключаемых файлов для работы с датчиком MPU6050 и фильтром Калмана для микроконтроллеров Arduino.

Файлы проекта

Директория проекта в файловом браузере:

• for_PlatformIO - папка с файлами для проекта PlatformIO:
• • include - папка с подключаемыми файлами:
• • • I2Cdev.h - заголовочный файл интерфейса I2C;
• • • Kalman.h - заголовочный файл фильтра Калмана (также подключается в модели!);
• • • MPU6050.h - заголовочный файл датчика MPU6050 (также подключается в модели!);
• • • stm32_mpu6050.h - генерируемый заголовочный файл модели Engee;
• • source - папка с исходными файлами С/С++:
• • • I2Cdev.cpp - исходный файл интерфейса I2C;
• • • main.cpp - файл основной пользовательской программы;
• • • MPU6050.cpp - исходный файл датчика MPU6050;
• • • stm32_mpu6050.cpp - переименованный генерируемый исходный файл модели Engee;
• joystick_description.ngscript - текущий скрипт Engee;
• stm32_mpu6050.engee - модель Engee данного проекта.

Добавим пути и имена некоторых файлов и папок для командного управления моделированием, генерацией кода и работой с файлами.

Pkg.add(["FilePathsBase"])

имя_модели = "stm32_mpu6050";
папка_проекта = "$(@__DIR__)/";
путь_модели = папка_проекта * имя_модели * ".engee";
путь_генератора_кода = папка_проекта * "model_code/";
путь_platformIO_CPP = папка_проекта * "for_PlatformIO/src/";
путь_platformIO_H = папка_проекта * "for_PlatformIO/include/";

Описание модели

Модель stm32_mpu6050.engee данного проекта можно разбить на следующие функциональные группы блоков:

• блоки преобразования углов осевых поворотов,
• блоки взаимодействия с периферией микроконтроллера.

Вычисления, реализуемые подсистемой EulersToDecart выполняют преобразования значений углов поворота по осям, полученных фильтром Калмана, в отклонения в декартовых координатах вектора нормали джойстика, исходящего из начала координат:

$$\Delta X = \sin(KalmanYAngle)\cdot\cos(KalmanZAngle)$$ $$\Delta Y = \sin(KalmanYAngle)\cdot\sin(KalmanZAngle)$$ $$\Delta Z = \cos(KalmanYAngle)$$

Подсистема DecartToSpheric по-сути, выполняет обратные преобразования:

$$\vartheta = \tan^{-1}\frac{\sqrt{\Delta X^2 + \Delta Y^2 }}{\Delta Z}$$ $$\varphi = \tan^{-1}\frac{\Delta Y}{\Delta X}$$

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

Блоки периферии

В модели проекта содержатся 4 блока периферии микроконтроллера, реализуемые при помощи блоков C Function:

• I2C1_MPU6050_Kalman - для инициализации интерфейса I2C_1, получения данных от MPU6050 и фильтрации углов по осям вращения;
• GPIO10_INPUT - для инициализации контакта PB6 в качестве дискретного входа, получения его состояния;
• GPIO11_INPUT - для инициализации контакта PA7 в качестве дискретного входа, получения его состояния;
• USART2_ToSerial - для инициализации интерфейса USART_2 и передачи по последовательному порту на скорости 9600 бод, выдачи массива значений в последовательный порт.

Для подключения в файлах результатов генерации кода готового кода для I2C, MPU6050 и фильтра Калмана, эти файлы и путь к ним указаны в блоке C_Function "I2C1_MPU6050_Kalman":

Также блок C Function "I2C1_MPU6050_Kalman" формирует изменение углов вращения датчика вокруг осей Y и Z в процессе моделирования.

Подробное описание работы кода из блоков C Function дано в его комментариях.

Результаты работы модели

Для моделирования преобразования значений углов вращения загрузим и запустим модель stm32_mpu6050.engee:

if имя_модели in [m.name for m in engee.get_all_models()]
    m = engee.open(имя_модели);
else
    m = engee.load(путь_модели);
end

данные = engee.run(m);

Из полученных данных моделирования извлечём переменные для построения сигналов:

• KalmanY - смоделированный угол поворота вокруг оси Y,
• KalmanZ - смоделированный угол поворота вокруг оси Z,
• SatTheta - рассчитанный угол $\vartheta$,
• SatPhi - рассчитанный угол $\varphi$.

# из данных модели извлекаем переменные для построения
KalmanY = Base.stack(данные["KalmanXYZ"].value, dims = 1)[:, 2];
KalmanZ = Base.stack(данные["KalmanXYZ"].value, dims = 1)[:, 3];
SatTheta = данные["SatTheta"].value;
SatPhi = данные["SatPhi"].value;

using Plots;
gr();
plot(
     plot(данные["KalmanXYZ"].time, [KalmanY, KalmanZ];
          label=["Вращение по Y, рад" "Вращение по Z, рад"], lw=1, legend=:bottomright),
     plot(данные["SatTheta"].time, [SatTheta, SatPhi];
          label=["Угол ϑ, рад" "Угол φ, рад"], lw=1, legend=:bottomright);
     layout=(1,2), size=(900,300)
)

Как видно из графиков, сформированные блоком C Function "I2C1_MPU6050_Kalman" углы поворота датчика вокруг осей Y и Z преобразуются в углы $\vartheta, \varphi$ без изменения значений.

Генерация кода

Сгенерируем код из модели для последующей загрузки управляющего алгоритма в микроконтроллер.

engee.generate_code(путь_модели, путь_генератора_кода); # генерация кода из модели

[ Info: Generated code and artifacts: /user/start/examples/codegen/stm32_mpu6050_joystick/model_code

Так как проект в IDE VS Code + PlatformIO далее будет собираться из файлов C++, необходимо для успешной сборки изменить расширение .c сгенерированного файла на .cpp. Для начала перейдём в папку сгенерированных файлов и просмотрим её содержимое:

cd(путь_генератора_кода);   # переход в директорию
readdir()   # вывод содержимого директории

3-element Vector{String}:
 "main.c"
 "stm32_mpu6050.c"
 "stm32_mpu6050.h"

Здесь: stm32_mpu6050.c - файл Си, сгенерированный кодом из предыдущей ячейки скрипта. Изменим расширение нового сгенерированного файла Си. Для перезаписи файла с существующим названием применим атрибут force = true. После чего снова выведем содержимое папки с результатами генерации кода.

mv(имя_модели * ".c", имя_модели * ".cpp"; force=true); # изменение расширения сгенерированного файла
readdir()   # вывод содержимого директории

3-element Vector{String}:
 "main.c"
 "stm32_mpu6050.cpp"
 "stm32_mpu6050.h"

Расширение нового сгенерированного файла Си изменено, перед добавлением файлов проекта в IDE осталось перенести полученыне файлы в папки src и include для проекта PlatformIO. Для этого удобно воспользоваться библиотекой FilePathsBase.jl

## При необходимости раскомментируйте ячейку для скачивания и установки библиотеки
# import Pkg;
# Pkg.add("FilePathsBase")

using FilePathsBase

# переносим файлы по папкам для проекта PlatformIO
mv(AbstractPath(имя_модели * ".cpp"),
   AbstractPath(путь_platformIO_CPP * имя_модели * ".cpp");
   force = true);
mv(AbstractPath(имя_модели * ".h"),
   AbstractPath(путь_platformIO_H * имя_модели * ".h");
   force = true);

Сгенерированный main.c использоваться в проекте не будет, можно удалить файл и его директорию:

cd("..") # переходим на уровень выше в файловой браузере
rm(путь_генератора_кода; recursive=true) # удаляем папку и main.c

Теперь можно перейти к настройке аппаратной части и работой с IDE.

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

Аппаратная часть проекта, состоящая из указанных ранее элементов, имеет следующую схему соединений: Датчик MPU6050 подключен к интерфейсу I2C_1 на отладочной плате NUCLEO F446RE, кнопочные контакты для подачи дискретных сигналов - к пинам GPIO10 и GPIO11.

Программирование контроллера и считывание данных, поступающих по интерфейсу USART, осуществляется через USB.

Сборка проекта

Создадим проект в рабочей области среды разработки VSCode+PlatformIO со следующими настройками файла конфигурации platformio.ini:

[env:nucleo_f446re]
platform = ststm32
board = nucleo_f446re
framework = arduino

В директории include и src добавим файлы из соответствующих директорий из браузера файлов Engee текущего проекта.

После этого запустим сборку проекта "Build" и убедимся в успешности сборки:

Выполнение кода на STM32F4

Подключим контроллер STM32F4 c периферией из датчика и двух кнопочных контактов к последовательному порту, после чего запустим загрузку "Upload" скомпилированного кода проекта в контроллер и убедимся в успешности загрузки:

В последовательный порт компьютера передаются значения переменных углов наклона нормали джойстика и сигналов фиксации и переключения масштаба:

Для отображения графиков сигналов из последовательного порта в этом проекте используется программа SerialPlot.

Заключение

В этом демонстрационном примере была разработана модель Engee для преобразования углов вращения датчика положения MPU6050 вокруг осей. Сгенерированный из модели код после загрузки в микроконтроллер STM32F446RE воспроизводит заложенные функции.

В документации Engee можно получить дополнительную справку, которая поможет при работе с данным проектом:

• концепция Модельно-ориентированного проектирования;
• генератор кода Engee;
• работа с блоком C Function;
• программное управление симуляцией модели.