基于RP2040制作水平仪

内容介绍

1 项目描述

1.1 选题与要求

本次选题为项目8——利用姿态传感器制作一个水平仪，效果图如图1-1。

FiSgwIhtDTIMMBi5zNpOUWnkeJmj

图1-1 水平仪

其中要求，在LCD屏上设计一款水平仪，通过一个滚动的小球或气泡，来显示当前板子的倾斜度，当板子处于水平位置的时候，小球停在屏幕的正中间，倾斜板子，小球偏移，并能够显示偏移的角度（二维信息）。

1.2 姿态传感器

如图1-2所示，RP2040有姿态传感器、红外接收发射、蜂鸣器、显示屏、四向摇杆和按键、拓展接口等功能。本项目主要使用了姿态传感器和显示屏。

FoetIbnxax4QfbqxQEA1ijcI75QJ

图1-2功能框架

对于在三维空间里的一个参考系，任何坐标系的取向，都可以用三个欧拉角来表现。参考系又称为实验室参考系，静止不动；而坐标系则固定于刚体，随着刚体的运动而运动，如图1-3.

FryRjX9fY5WwtfMYDQ4NpC-GiNYz

图1-3 欧拉角

姿态传感器是基于MEMS技术的高性能三维运动姿态测量系统。它包含三轴陀螺仪、三轴加速度计、三轴电子罗盘等辅助运动传感器，通过内嵌的低功耗ARM处理器输出校准过的角速度、加速度、磁数据等，通过基于四元数的传感器数据算法进行运动姿态测量，实时输出以四元数、欧拉角等表示的零漂移三维姿态数据。

RP2040中有MMA7660FC传感器供使用。MMA7660FC是±1.5G三轴输出的I2C、超低功率、紧凑型电容式微电机的加速度计。本次水平仪的数据来源就是MAA7660FC。本次项目中拟利用MMA7660FC传感器得出α、β、γ的数值，并且将此数值以图形的形式显示在LCD屏幕上。

2 设计思路

毫无疑问，首先需要调用MMA7660FC，获得相关数据。其次，对得到的数据进行处理，并且以图形的方式呈现。最终，再对得到的数据或图形进行微调、优化。

Flpb8gQcf_uxPBGCavrBzZjdB8B3

图2-1 设计思路

2.1 数据获取

数据获取这部分，我主要是参考了GitHub上官方的文档——”eetree-mpy-lecture-code-main\bsp\mma7660.py”、”eetree-mpy-lecture-code-main\app\mma7660-print.py”。再结合电子森林官方的电路图设置broad.py，即可实时获取MMA7660FC上的数据，如图2-2。

FoHsJD4CQ1pmo1XlXNT6f7iA0UcU

图2-2 MMA7660FC实时数据

2.2 数据处理

结合官方代码注释可知，图2-2中的数据表示加速度。

FjIP61QcjwHxbECuvmaCDaVOr1Mb

图2-3 文档注释解释数据含义

我最终的目标是：使小球或气泡在水平仪中移动的距离与欧拉角的大小成正比。所以，我需要通过理论分析，对现有的数据进行处理，从加速度数据中推算出角度数据。

如图2-4，设x轴方向的加速度大小为Ax，其余水平线的夹角为，与重力加速度的夹角为；y轴方向的加速度大小为Ay，其余水平线的夹角为，与重力加速度的夹角为；z轴方向的加速度大小为Az，其余水平线的夹角为，与重力加速度的夹角为。

FrBOkxDJMK0NqXx9HzlqPpjxpqej

图2-4 加速度与角度关系示意图

由图2-4，再结合空间几何知识可推出x轴、y轴、z轴与水平线的弧度关系。

300

从弧度制转到角度值，只需要对以上三式乘以一个系数，即。这样，我们就得到了xyz三个维度的角度数据。根据题目要求，我选择了x轴和y轴两个维度的信息以图形的方式显示在LCD屏幕上。相关代码如下。

    rx=180/3.1415926*atan(r[0]/sqrt(r[1]*r[1]+r[2]*r[2]))

    ry=-180/3.1415926*atan(r[1]/sqrt(r[0]*r[0]+r[2]*r[2]))

    rz=180/3.1415926*atan(r[2]/sqrt(r[1]*r[1]+r[0]*r[0]))

2.3 图形可视化

得到数据之后，接下来需要做的，就是把数据转换成图形显示在LCD屏幕上。具体来说，我选择了x轴和y轴两个维度的角度数据作为小球或气泡的圆心。当MMA7660FC的数据发生变化，小球或气泡的位置也将跟着变化。

在图形可视化部分，主要参考了GitHub上的官方文档”RP2040_Game_Kit-main\lecture\ball.py”。屏幕驱动文件主要参考了ball.py的”breakout_colourlcd240x240”，由于没有”breakout_colourlcd240x240”的源代码，不了解该屏幕驱动文件的具体函数，通过摸索，我在本次任务中仅使用了”circle”、”rectangle”、”text”、”updata”、”clear”、”set_pen”、”create_pen”。主要任务是设置图形界面以及实时显示相关数据。

图形可视化部分主要是设置矩形框、圆、文字等的大小和位置，不做详细解释，截取部分代码展示如下。

    for i in range (9):

        #print(i)

        display.set_pen(balls[i].pen)

        display.circle(balls[i].x, balls[i].y, balls[i].r)

        if i==4:

            display.set_pen(0)

            display.rectangle(0,139,200,2)

            display.rectangle(99,40,2,200)

            #two lines

            display.text("45",155,145,0,1)

            display.text("-45",35,145,0,1)

            display.text("45",90,80,0,1)

            display.text("-45",90,200,0,1)

            display.text("x",185,125,0,2)

            display.text("y",90,40,0,2)

            display.set_pen(0xff,0xff,0xff)

            display.rectangle(200,0,40,80)

            #right up rectangle

            display.set_pen(0)

            display.text("k:"+str(k),201,60,0,2)

            display.text("x:"+str(balls[6].x-100),201,0,0,2)

            display.text("y:"+str(-(balls[6].y-140)),201,20,0,2)

            display.text("z:"+str(-rz),201,40,0,2)

            #add the region lines-start

            display.set_pen(0,0,0)

            display.rectangle(48,8,104,24)#up

            display.rectangle(208,88,24,104)#right

            #add the region lines-end

            display.set_pen(0,0xff,0)

            display.rectangle(50,10,100,20)

            display.rectangle(210,90,20,100)

            #up and right rectangle

            balls[7].x=int((balls[6].x-100)/2+100)

            balls[7].y=20

            balls[8].y=int((balls[6].y-140)/2+140)

            balls[8].x=220

        display.set_pen(0)

        display.rectangle(100,10,2,20)

        display.rectangle(75,10,2,20)

        display.rectangle(125,10,2,20)

        display.rectangle(210,140,20,2)

        display.rectangle(210,115,20,2)

        display.rectangle(210,165,20,2)

2.4 优化

完成以上的数据获取、数据处理以及图形可视化后，本次项目以及基本完成，但是在运行的时候发现一个非常明显的问题——静止状态下，小球或气泡仍然有明显的晃动。

于是进行思考，明明是静止状态，为什么小球或气泡仍然有明显的晃动？

运行官方例程并转动RP2040，观察实时数据。可以发现单项数据（比如r[0]）在静止状态下仍有波动，并且其取值约在-22至22之间，而水平仪的直径为200个像素点。以线性关系进行大致估计：若r[0]波动值为1，小球或气泡将波动5个像素点。（实际并不是线性关系，此处仅做估计）

2.4.1卡尔曼滤波

于是猜想，出现晃动的原因是MMA7660FC传感器本身精度有限，实时数据本身不稳定。通过微信群中各位同学和老师的提醒，我知道了需要通过卡尔曼滤波来进行优化。

如图2-5，其中蓝色曲线为真实值，黄色曲线为传感器的测量值，绿色曲线为经卡尔曼滤波后的曲线。由图可知，黄色曲线在真实值附近上下波动，经过卡尔曼滤波后，绿色曲线虽仍有波动，但较黄色曲线而言已经更为接近真实值。因此，使用卡尔曼滤波确实有利于改善MMA7660FC的测量值。

FoK3xInKa95bzBuVntbFNmpxHcm5

图2-5 卡尔曼滤波成效图

那么什么是卡尔曼滤波，具体应该怎么做才能实现卡尔曼滤波呢？

卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可以看做是滤波的过程。

比如，一辆小车开始向远方行驶，某一时刻得知它距离起点1000m，速度为5m/s。那么假设小车速度不变，理论估计小车下一秒应该距离起点1005m。但是实际上传感器测得小车距离起点是1006m。此时是应该相信理论估计值还是传感器测量值呢？进行折中选择，令结果为1006*k+1005*(1-k)，其中k为卡尔曼增益，代表传感器的靠谱程度。

卡尔曼增益的具体值与方差有关，在本次任务中不对方差进行具体计算得出具体的卡尔曼增益，而是把卡尔曼增益设置成可调的。这样一来，不仅可以省去计算方差的复杂过程，还能通过实时改变卡尔曼增益的值观察卡尔曼增益对小球或气泡的影响。

从上文小车的例子中得到启发，可以通过下文的方法对小球或气泡实现卡尔曼滤波。

首先，把小球或气泡的圆心当做位置向量，把他们当前的位置与上一时刻位置之差作为速度向量，如下文代码。(x,y)为位置向量，(dx,dy)为速度向量，pen为颜色。

class Ball:

    def __init__(self, x, y, r, dx, dy, pen):

        self.x = x

        self.y = y

        self.r = r

        self.dx = dx

        self.dy = dy

        self.pen = pen

由于本代码中屏幕每0.1s刷新一次，于是假设这一刻的速度与0.1s后那一时刻的速度相等，那么可以由这一时刻的(x,y)和(dx,dy)推算出0.1s后那一时刻的(x,y)，0.1s后的(dx,dy)由前后两个时刻的(x,y)之差决定。

即(x2,y2)=(x1+dx,y1+dy),(dx,dy)=(x2-x1,y2-y1)。若屏幕刷新时间小于0.1s，速度的估计更准确。

这样一来，就能从这一刻的状态推算出下一时刻的状态。接着，提取出传感器得到的数据，并且与估计值一同进行卡尔曼滤波，得到滤波后的数值。以下为实现该过程的核心代码。

def kalman(dx_now,dx_previous,k):

    return dx_now*k+(1-k)*dx_previous

    dy=ry-balls[6].y

    dx=rx-balls[6].x

    balls[6].dy=kalman(dy,balls[6].dy,k)

    balls[6].dx=kalman(dx,balls[6].dx,k)

    balls[6].y=balls[6].y+balls[6].dy

    balls[6].x=balls[6].x+balls[6].dx#record the angle

当k=1时，相当于不添加卡尔曼滤波。此时，静止状态下，小球或气泡仍然存在明显的晃动，但是能瞬间到达最终位置；当k越小，静止状态下，小球或气泡就越稳定。但是，k越小，小球到达最终位置所需的时间就越长。换言之，小球具备一定的惯性，能够保持原先的运动状态一小段时间。

这一结果是符合预期的。当k越小，传感器的可靠性越低，此时小球或气泡的运动就越依赖估计值。小球或气泡之所以会需要更多的时间到达最终位置，是因为花费了一定时间运动在估计值的轨道上。

2.4.2 实时调整卡尔曼增益

正如前文所述，卡尔曼增益理应通过方差计算得出，为了节省计算量，本次项目并没有通过方差计算得出卡尔曼增益。因此，本项目采取另一种方法——实时调整卡尔曼增益。这样不仅便于代码实现，还能方便观察到卡尔曼增益的值对小球或气泡运动轨迹的影响。

本部分代码参考了”RP2040_Game_Kit-main\example\joystick.py”，核心代码部分如下。

buttonB = Pin(5,Pin.IN, Pin.PULL_UP) #B

buttonA = Pin(6,Pin.IN, Pin.PULL_UP) #A

k=0.5#kalman

    buttonValueA = buttonA.value()

    buttonValueB = buttonB.value()

    if buttonValueA==0:

        k=round((k+0.1)%1.1,1)

    if buttonValueB==0:

        k=round((k-0.1)%1.1,1)

优化后最终成果图如图2-6。右上角白色矩形框中为实时更新的数据，x、y、z为其余水平线的夹角，k为可通过按键A、B调节的卡尔曼增益；左下角为主界面——二维水平仪，蓝色小球的圆心位置有右上角的x、y值决定；上方和右方的位置为一维水平仪。

FiUagZSFcN0GeYv71pnbxCl25s0t

图2-6 成果展示

3 遇到的困难

虽然最终我是成功完成了项目，但期间其实遇到了许多大大小小的问题。最后也是磕磕绊绊地不断查阅资料、求助同学老师，才能逐一解决这些问题。遇到的主要问题在数据处理以及图形可视化这两个部分，以及uf2文件的使用这一部分。

3.1数据处理

在群里同学“小行星”的帮助下，我一早就得到MMA7660FC的实时数据，但由于r[i]的值在-22到22之间，我一时分辨不出这个数据的含义。难道是加速度？但是22这个数字跟重力加速度9.8似乎没什么关系；难道是弧度，但是22这个数字跟π好像也难以扯上关系；难道是角度？但是22这个数字跟180好像也没什么关系。

所以有好几天的时间，我都在一直查资料，想了解通过MMA7660FC得到的三个数据到底是什么意思？但是难以有明显的收获，不仅耗费了不少时间，还耗费了不少精力。最后，本想通过自己查资料弄清问题的我，不得已之下只能向朋友求教。在朋友的提示下，我才初步确认r[i]代表的是加速度，后来仔细查看文档，才发现注释中明确写了r[i]代表加速度。

回过头来看，这其实是一个很简单的问题，答案就在文档的注释中，但自己却花了这么多时间，着实不应该。

3.2 图形可视化

通过电子森林的三节官方视频教程学习，我只学会了通过st7789驱动屏幕。很长一段时间内，我也一直打算通过st7789完成本次项目。

但是，我遇到了两个主要的问题。其一，st7789没有能够直接画圆的函数。我都是用pixel画像素点的形式画圆，但是自己写出来的绘图函数生成的圆，尤其是实心圆，会消耗不少时间，导致视觉效果上屏幕的信息不能瞬间显示出来，而是缓慢显示出来；其二，不知道怎么消除小球或气泡的运动轨迹。

针对第一个问题，我通过预先用数组存储生成圆的像素点，能够做到视觉上几乎瞬间生成较小的实心圆。但是对于那些作为边界的圆圈，由于有些圆圈半径过大，像素点过多，哪怕是通过数组预先存好了像素点的位置，遍历数组的过程仍然消耗不少时间，达不到视觉上瞬间呈现图形的效果。所以第一个问题我只能解决一部分，没有能完全解决。

针对第二个问题，如果想要清楚小球或气泡的轨迹，最好的方法是把240x240的屏幕不断刷新、清屏，并且完全重新生成新的界面。但是正如前文所述，用st7789我无法做到视觉上瞬间显示界面的效果，所以如果用刷新、清屏，重新生成全部界面的方法，视觉效果会非常差——屏幕轻则闪烁，重则以肉眼可见速度进行绘图。

我后来用了一个办法，即不对全部界面进行清屏，仅对小球或气泡进行清屏。由于我能做到对像素点较少的图形进行视觉上的瞬间呈现，这样的方法勉强可行。但是会有一个bug——当小球或气泡与边界重合时，此时的清屏还会清除圆圈边界，导致圆圈边界缺失。如图3-1。

FtaY0p6FYUN6t9cMwoDRJIz3F10Q

图3-1 st7789-该位置时出现bug

当然也可以对此时清屏的是否是圆圈边界进行判断，但是此时需要一个庞大的二维数组存储圆圈边界信息，thonny中无法创建这么大的数组，且此时会需要一个庞大的循环进行判断，仍然会消耗不少时间。

在绞尽脑汁之后，以我的水平实在难以解决界面的问题。于是我被卡死了在这个问题上了。

3.3 uf2文件的使用

最后，我关注群中的信息，希望能得到一些有用的帮助。于是巧合之下，我解决了一个困扰我更久的问题——不懂加载固件uf2文件。

在运行一些官方文件的过程中，有时候会报错说缺少了某个文件，但是这个文件在文件夹中确实找不到。这样一来，官方例程就无法运行，群里的老师同学说这个文件在uf2文件中，可能是我天资愚笨，难从老师同学的答复中提取出有效的帮助，一直不懂怎么从uf2文件中调用需要的文件。所以就一直卡在这里，上网搜资料也难以搜到有用的信息。

在群聊中，我顺手提醒一个不懂刷机的同学的过程中无意中发现了这个问题的答案——只需要调出U盘模式，然后把uf2文件复制进去即可。如图3-2.

成功导入uf2文件后，ball.py文件我就能够运行了，我得到了之前报错显示缺少的”breakout_colourlcd240x240”文件——它真的就在uf2文件中。利用这个驱动文件，我轻易解决了图形可视化中我遇到的所有问题。我像是突破了瓶颈一般，接下来的一切都水到渠成似的，我花了十天研究的项目，在短短一天内直接完成了将近90%。

FqDTr3cOb9pbZUrZXcG5icUO5pok

图3-1 uf2文件的使用

4 总结

4.1 收获

本次训练之后，我的检索资料能力、独立思考能力、编程能力得到了一定程度的提升；更重要的是，我开阔了自己的视野。

平时在校内也完成过大大小小的编程作业，但本次项目与校内作业不同。完成本次项目还需要结合RP2040的实物电路图等，本更贴近实际生活应用。校内的作业和实际应用还是存在区别，此次寒假的训练让我更加了解了书本知识和实际应用的联系。

此次训练之后，我对生活中电子设备的思维方式发生了微妙的变化。

比如，寒假期间叔叔送给我一个运动手表。这个手表仅靠戴在手腕上就能够测出人的心跳、呼吸、消耗能量、运动步数、睡眠质量等各种各样的信息。最让我好奇的是，睡眠质量报告中给出了我睡着的时间点和睡醒的时间点，竟然几乎完全符合。其中甚至还给出了我快速眼动、深睡、浅睡、清醒的时间段，如图4-1.

Ftdg6fR4zt6ZDofMPBGCICtQbZg_

图4-1 运动手表睡眠检测

经过本次寒假训练后，我对这个手表就更加敏感好奇了。这个手表顶多就能通过传感器测量到我的脉搏，仅通过脉搏能测量出心跳和呼吸尚可理解，究竟是通过什么算法能推算出我的睡眠的具体信息呢？

于是我更加敬佩当今的计算机水平，对知识更多了一分敬畏，也更加明白了自己才疏学浅，未来仍需继续努力。

4.2 不足

虽然我能顺利完成本次项目，但回过头来看，收获有限，仍存在不少不足。

起初，我打算通过研究实物电路图，能够自己写程序调用MMA7660FC。调用MMA7660FC是我所认为的一大难点，但是后来发现官方有了例程，于是这一难点也直接被攻克了。我真正自己独立完成的部分，其实只有数据处理、图形界面生成、以及简单的卡尔曼滤波，收获有限。

正如前文所述，我曾遇到大大小小的问题。其中的一些问题属于非常简单的问题，但是由于我资料检索能力差等问题，短时间内没有得到解决，浪费了不少时间。资料检索能力差是我所存在的非常大的不足。

其次，能够完成本次项目的关键在于，我使用了适合的屏幕驱动文件。原先我一直在使用st7789却没有进展，换了breakout_colourlcd240x240之后一下子就有突破性进展。所以选择正确的方向非常重要，我一开始就走了弯路，导致前面浪费了不少时间。如果我能够认真关注群消息，一开始就懂得导入uf2文件，能够使用恰当的屏幕驱动文件，那么我将更早地完成任务。

5 致谢

最后，我想特别感谢硬禾课堂能够为广大学生提供这样一个机会让我们进行锻炼，并且还能够退款。硬禾学堂是一个真心为学生的好老师，也是一个乐于为社会、为国家做贡献的好组织。由衷感谢硬禾学堂给予我这个锻炼的平台，也由衷感谢群里积极答疑的老师、同学。

6 主要参考文档

RP2040_Game_Kit-main\example\ST7789\LCDTest.py
RP2040_Game_Kit-main\example\ST7789\st7789.py
RP2040_Game_Kit-main\example\py
RP2040_Game_Kit-main\lecture\ball.py
RP2040_Game_Kit-main\lecture/board.py
eetree-mpy-lecture-code-main\app\mma7660-print.py
eetree-mpy-lecture-code-main\bsp\mma7660.py
eetree-mpy-lecture-code-main\ria\mma7660-ws2812b-display.py