带位置传感器 MPU6050 的操纵杆,用于 STM32F4¶
该演示将介绍使用卡尔曼滤波器读取轴向旋转角度的 Engee 模型,转换得到的角度,并将受约束的旋转角度传输到串行端口。
简介¶
该项目基于 Engee 型号stm32_mpu6050.engee
,执行以下任务:
- 通过 STM32F446RE 控制器的 I2C 通道从 MPU6050 传感器接收三个旋转轴的加速度和转速;
- 通过卡尔曼滤波器过滤,将这些值转换为轴向旋转--滚动、俯仰、偏航;
- 接收离散信号 "固定"、"刻度转换";
- 将轴向旋转转换为笛卡尔坐标系中操纵杆法向量的坐标偏差;
- 将坐标偏差转换为球面坐标系中操纵杆旋转的角度$\vartheta, \varphi$ ;
- 相应限制获得的角度$\left[ 0; \ \frac{\pi}{2} \right], \left[ 0; \ 2\pi \right]$ ;
- 向通用收发器(USART)输出变量:角度$\vartheta, \varphi$ 、"固定"、"刻度切换"。
要素和环境:
- 调试板:*nucleo -F446re.
- 微控制器*STM32F446RE.
- 输入通道:I2C,2$\times$ DI
- 输出通道:USART (COM)
- 三轴加速度计-陀螺仪传感器:三轴加速度计-陀螺仪传感器: mpu6050 (gy-521)
- 开发环境:开发环境: *Engee -> VS Code 1.92.1 + PlatformIO 6.1.15。
- PlatformIO 框架:*stm32duino.
使用 PlatformIO 框架--stm32duino,是因为有现成的调试插件文件,可用于Arduino微控制器的MPU6050传感器和卡尔曼滤波器。
项目文件¶
文件浏览器中的项目目录:
-for_PlatformIO
- 包含 PlatformIO 项目文件的文件夹:
-include
- 包含插件文件的文件夹:
- -
I2Cdev.h
- I2C接口头文件; -Kalman.h
- 卡尔曼滤波头文件(也插入模型中!); -MPU6050.h
- MPU6050传感器头文件(也插入到模型中!); -stm32_mpu6050.h
- 生成的 Engee 模型头文件; -source
- 包含 C/C++ 源文件的文件夹: - -
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(путь_модели, путь_генератора_кода); # генерация кода из модели
由于集成开发环境 VS Code + PlatformIO 中的项目将由 C++ 文件进一步构建,因此为了成功构建,有必要将生成文件的扩展名.c
更改为.cpp
。首先,让我们进入生成文件的文件夹,查看其中的内容:
cd(путь_генератора_кода); # переход в директорию
readdir() # вывод содержимого директории
此处:stm32_mpu6050.c
是由上一脚本单元代码生成的 C 文件。让我们更改新生成的 C 文件的扩展名。要以现有名称覆盖文件,请应用属性force = true
。之后,我们将再次显示包含代码生成结果的文件夹内容。
mv(имя_модели * ".c", имя_модели * ".cpp"; force=true); # изменение расширения сгенерированного файла
readdir() # вывод содержимого директории
新生成的 C 文件扩展名已更改。 在将项目文件添加到集成开发环境之前,仍需将生成的文件移动到 PlatformIO 项目的src
和include
文件夹中。为此,可以使用库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
现在可以继续进行硬件配置并使用集成开发环境。
硬件配置¶
项目的硬件部分由前面提到的元件组成,其连接方案如下:MPU6050 传感器连接到 NUCLEO F446RE 调试板上的 I2C_1 接口,离散信号的按钮触点连接到 GPIO10 和 GPIO11 引脚。
控制器的编程和 USART 数据的读取通过 USB 完成。
项目组装¶
在 VSCode+PlatformIO 开发环境的工作区创建一个项目,配置文件设置如下platformio.ini
:
[env:nucleo_f446re]。
平台 = ststm32
板 = nucleo_f446re
框架 = arduino
在include
和src
目录中,添加当前项目Engee文件浏览器中相应目录的文件。
之后,让我们运行 "构建 "项目,确保构建成功:
在 STM32F4 上执行代码¶
将带有传感器外围和两个按钮的 STM32F4 控制器连接到串行端口,然后启动 "上传",将编译好的项目代码下载到控制器中,并确保上传成功:
操纵杆正常倾斜的可变角度值以及锁定和缩放信号被传输到计算机的串行端口:
本项目使用SerialPlot
程序来显示串行端口的信号图。
结论¶
在本演示中,开发了Engee模型,用于转换MPU6050位置传感器绕轴的旋转角度。该模型生成的代码一旦加载到 STM32F446RE 微控制器中,就能重现嵌入式功能。
在Engee文档中还提供了其他帮助,以帮助完成此项目: