【.NET 与树莓派】六轴飞控传感器（MPU 6050）

所谓“飞控”，其实是重力加速度计和陀螺仪的组合，因为多用于控制飞行器的平衡（无人机、遥控飞机）。有同学会问，这货为什么会有六轴呢？咱们常见的不是X、Y、Z三轴吗？重力加速度有三轴，陀螺仪也有三轴，那我问你，两个加起来多少轴？

贴片常见的有 MPU-6000、MPU-6050、MPU-9250 。MPU 9250 是九轴传感器。哟，吓死阿伟了，怎么变成了九轴了？它弄了个磁场感应嘛。

老周在淘宝“琉璃厂”淘到的模块是正点原子的 MPU 6050。万能法则——找最便宜的入，别相信那些叫你买贵的，你不妨把便宜的和贵各买一个对比看看，最后你会一刻拍案惊奇地发现——两个一模一样。网上卖东西，有些店就是瞎喊价格的。他们真不会做生意，想想网购这玩意儿，我完全可以货比万家的，一样的商品，当然谁便宜买谁了。反正过程一样，都是坐和等待 + 三通一达。

 MPU 6050 使用的是 IIC/i2c 通信协议。也就是说你很熟悉了，除了供电两根线，就是数据线 SDA 和时钟线 SCL。

MPU 6050 的操作方式是读写寄存器，输出的模拟量是 16 位有符号整数。2 的 16 次方有65536个数值，包含0，无符号整数是0 - 65535，但有符号就不同了，因为最高位用作符号位，故范围是 -32768 ~ +32767。这个范围也就是MPU 6050的输出分辨率。

咱们在使用时要注意，这货有多种量程设置，不同量程下输出结果的精度不同。下面老周具体扯一下。

先看重力加速度，可配置的量程有：

1、±2g：g 就是我们以前上物理课时的老熟人了——重力加速度。故，此量程可测量两倍 g 的加速度，包含负值。

2、±4g：原理同上，量程为四倍的 g 的加速度，包含正负值。

3、±8g：八倍于 g ，含正负值。

4、±16g：十六倍的g，含正负值。

前面提到了，模块输出的是16位有符号整数，那么

若量程为 +/- 2g，正负值加起来，倍数是4，16位有65536个数值，所以，65536 ÷ 4 = 16384。也就是说，每一倍的 g 可以划分为 16384 等分来描述，精度是最高的。同样的计算方法，4g、8g、16g的分值也能算出来：

  你可以看看，如果要测量 ±16 个g的量程，那么每个g只能划分为2048个等分了。可见：量程越小，精度越高；量程越大，精度越低。

* 由于正负两边是对轴的，也可以只算一边，即 +/-2g => 32768 / 2 = 16384。 

陀螺仪是测量某个轴上的旋转速度，与加速度一样，角速度也可以设置量程。

±250° / s：速度每秒旋转 250 度。同样，65536 ÷ (250 * 2) = 131，因为速度有正负值，所以250要乘以2。其他几个值也是这样算。

配置重力加速度的量程的寄存器地址为 0x1C，一个字节，各二进制位的参数如下：

这里咱们只关心 bit3 和 bit4 即可，bit5 到 bit7是用来模块自测的，不必管他。AFS_SEL 两个二进制位可以产生四个值（00、01、10、11），这样就和上面咱们提到的量程对应上了。

默认是0，即 +/-2g，向寄存器写入 b0000_.0000。如果要+/-4g的量程，就向寄存器写入 b0000_1000。

-----------------------------------------------------------------

配置陀螺仪量程的寄存器地址是 0x1B。

 和上一个寄存器一样，咱们只关心 FS_SEL 两个二进制位即可，也是四个值，分别与前文中提到的角速度量程一一对应。

接下来，要关注的是电源管理寄存器，地址为 0x6B。

 这里最关键的是 bit6，也就是参数 SLEEP。MPU6050 刚通电时，会默认进入休眠状态（可能别的厂家不是这样），这时候，SLEEP 位上的值是 1，要唤醒模块，就要把这个二进制位改为 0。由于正点原子这个模块上面还有个温度传感器，所以，如果 TEMP_DIS 位为0，表示使用温度传感器，从寄存器 0x41 和 0x42 可以读到温度值；咱们使用这个模块主要是读重力加速度和角速度，所以要禁用温度计的话就把该位设置为 1。

接下来是核心，如何读加速度和角速度的值。一个值是16位有符号整数，两个字节，因此需要两个寄存器；而加速度有三个轴的值，总共需要六个寄存器来存放。这六个寄存器是连续的，地址从  0x3B 到 0x40。依次读出来的是：X轴的高位字节 > X轴的低位字节 > Y轴的高位字节 > Y轴的低位字节 > Z轴的高位字节 > Z轴的低位字节。读取时是高位字节先出，低位字节后出。

读取角速度也一样，需要连续的六个寄存器—— 从 0x43 到 0x48。X、Y、Z三轴供六个字节，也是高字节在前，低字节在后。

连接的时候，VCC接树莓派 5V，GND接树莓派GND，至于另外两根线，这里老周顺便提一下，如何让 Pi 4 开启多路 i2c。咱们通过 raspi-config 工具（或直接改 config.txt 文件）所使用的是默认的总线——i2c-1，也就是 GPIO2 和 GPIO3 引脚。

i2c-0 是给专用扩展板通信的，官方文档建议咱们不要使用（引脚 GPIO0 和 GPIO1），在树莓派上电时会检测 i2c-0 总线，因此这一路是留给 EEPROM 专属。

但不用担心，除了 i2c-0、i2c-1 外，还有四路我们可以选：i2c-3、i2c-4、i2c-5和i2c-6。根据文档说明，只有 BCM 2711 才能开启多路 i2c 接口。在树莓派上执行一下：

cat /proc/cpuinfo

然后，你会看到让人兴奋的一幕。

 而 Raspberry Pi 4B 规格文档上的描述也印证了，4 代是支持开启多路 i2c 接口的（可用多个总线）。

为什么要启用其他 i2c 总线？可以有以下理由：

1、相同的器件挂到同一个总线上，有的模块可以设置地址，但有的不可以。为了不冲突，可以考虑地址相同的模块连到不同的总线上；

2、GPIO2 或 GPIO3 用不了。当然，这里不是指针脚坏了，而是说另作他用。比如，你要给树莓派弄一个开机按钮；又或者，你在 5V 和 GND上接了风扇，有的散热风扇两根线是并在一起的，而且用的是插电脑主板的那种端子，既没法选其他引脚又占用空间，把GPIO2和GPIO3的位置都挡住了。

哦，上面提到了为树莓派添加开机按钮的事，咱们先聊正题，待会儿正题扯完了，老周再补充。

树莓派4B可用 GPIO 有 28 个，也就是说，GPIO 的 BCM 码最多只到 27，什么 40、45 号接口的就别做梦了。依据文档，咱们一起来瞧瞧这可用的四路 i2c 总线的参数。

1、i2c-3：有两组引脚可用。GPIO2、GPIO3 与 i2c-1 是重叠的；所以可以选另一个组——GPIO4 和 GPIO5。

2、i2c-4：也是有两组引脚可选。第一组是 GPIO6 和 GPIO7；第二组是 GPIO8 和GPIO9。

3、i2c-5：也是有两组引脚可用。第一组 GPIO10 和 GPIO11；第二组 GPIO12 和 GPIO13。如果使用 PWM 的话，注意 12、13 的冲突。

4、i2c-6：第一组引脚 GPIO0 和 GPIO1，这个前面提到过，保留分配给专用扩展板，建议不使用；第二组是 GPIO23 和 GPIO23。

这里老周选用了 i2c-4，所以总线 Bus id 是 4，引脚是 6 和 7，打开 /boot/config.txt 文件，加入以下配置：

dtoverlay=i2c4,pins_6_7

这个配置与 raspi-config 中对 i2c 的配置是独立的，也就是说，就算你禁用了 i2c，就像这样：

dtparam=i2c_arm=off

i2c-4 仍然可以正常工作，所以，i2c-3 到 i2c-6 的配置不受默认 i2c 的启用状态影响，只要我配置有 i2c-4，哪怕禁用了i2c接口也能使用。

好了，剩下的工作就是写代码。先上MPU6050类。代码我整个贴了。

    public class Mpu6050 : IDisposable
    {
        /// <summary>
        /// 默认从机地址
        /// </summary>
        public const int DEFAULT_ADDR = 0x68;
        /// <summary>
        /// 重力加速度
        /// </summary>
        public const float G = 9.8f;

        #region 寄存器列表
        // 电源管理，用于唤醒模块
        const byte REG_POWER_MGR = 0x6b;

        // 配置加速度的量程
        const byte REG_ACCEL_CONFIG = 0x1c;

        // 配置角速度的量程
        const byte REG_GYRO_CONFIG = 0x1b;

        // 读取重力加速度
        const byte REG_ACCL_MS_BASE = 0x3b;

        // 读取角速度
        const byte REG_GYRO_MS_BASE = 0x43;
        #endregion
    
        private I2cDevice _device = default;
        
        // 构造函数
        public Mpu6050(int i2cBusid, int devAddress = DEFAULT_ADDR)
        {
            I2cConnectionSettings cs = new I2cConnectionSettings(i2cBusid, devAddress);
            _device = I2cDevice.Create(cs);
        }

        public void Dispose() => _device?.Dispose();

        #region 私有方法
        private void WriteReg(byte reg, byte val)
        {
            Span<byte> data = stackalloc byte[2]
            {
                reg,
                val
            };
            _device.Write(data);
        }
        private byte ReadReg(byte reg)
        {
            _device.WriteByte(reg);
            for(int i =0; i<13; i++)
            {
                System.Threading.Thread.SpinWait(1);
            }
            return _device.ReadByte();
        }
        private void ReadBytes(byte reg, Span<byte> data)
        {
            _device.WriteByte(reg);
            for(int x = 0; x < data.Length; x++)
            {
                data[x] = 0;
            }
            _device.Read(data);
        }
        #endregion
    
        /// <summary>
        /// 唤醒
        /// </summary>
        public void WakeUp()
        {
            // 或者写入 0x08（禁用温度计输出）
            WriteReg(REG_POWER_MGR, 0x00);
        }

        /// <summary>
        /// 进入休眠
        /// </summary>
        public void Sleep()
        {
            WriteReg(REG_POWER_MGR, 0x40);
        }

        /// <summary>
        /// 重力加速度的量程
        /// </summary>
        public AcclRange AccelerRange
        {
            get
            {
                byte v = ReadReg(REG_ACCEL_CONFIG);
                // 由于测量范围的配置在第4、5位，所以读出来的值要右移三位
                return (AcclRange)(byte)((v >> 3) & 0x03);
            }
            set
            {
                byte x = (byte)value;
                // 存入时要左移三位
                WriteReg(REG_ACCEL_CONFIG, (byte)(x << 3));
            }
        }

        /// <summary>
        /// 陀螺仪的量程
        /// </summary>
        public GyroRange GyroRange
        {
            get
            {
                byte v = ReadReg(REG_GYRO_CONFIG);
                // 同样，要右移三位
                return (GyroRange)(byte)((v >> 3) & 0x03);
            }
            set
            {
                byte c = (byte)value;
                // 左移三位
                WriteReg(REG_GYRO_CONFIG,  (byte)(c << 3));
            }
        }

        /// <summary>
        /// 读取加速度值
        /// </summary>
        public (float ax, float ay, float az) GetAccelerometer()
        {
            // 可以以 0x3b 为基址，批量读取
            // 因为地址是连续的
            Span<byte> buffer = stackalloc byte[6];
            ReadBytes(REG_ACCL_MS_BASE, buffer);
            // 合成读数
            short x = BinaryPrimitives.ReadInt16BigEndian(buffer);
            short y = BinaryPrimitives.ReadInt16BigEndian(buffer[2..]);
            short z = BinaryPrimitives.ReadInt16BigEndian(buffer[4..]);
            // 转换倍数
            float fac = AccelerRange switch
            {
                AcclRange.x2g       => 2.0f,
                AcclRange.x4g       => 4.0f,
                AcclRange.x8g       => 8.0f,
                AcclRange.x16g      => 16.0f,
                _                   => 0.0f
            };
            return (
                fac * G / 32768f * x,
                fac * G / 32768f * y,
                fac * G / 32768f * z
            );
        }

        /// <summary>
        /// 读取陀螺仪数据
        /// </summary>
        public (float gx, float gy, float gz) GetGyroscope()
        {
            Span<byte> buffer = stackalloc byte[6];
            ReadBytes(REG_GYRO_MS_BASE, buffer);
            short x = BinaryPrimitives.ReadInt16BigEndian(buffer[..]);
            short y = BinaryPrimitives.ReadInt16BigEndian(buffer[2..]);
            short z = BinaryPrimitives.ReadInt16BigEndian(buffer[4..]);
            // 转换倍数
            float rf = GyroRange switch
            {
                GyroRange.x250dps       => 250f,
                GyroRange.x500dps       => 500f,
                GyroRange.x1000dps      => 1000f,
                GyroRange.x2000dps      => 2000f,
                _                       => 0f
            };
            return (
                rf * x / 32768f,
                rf * y / 32768f,
                rf * z /32768f
            );
        }
    }

    public enum AcclRange : byte
    {
        x2g = 0,
        x4g = 1,
        x8g = 2,
        x16g = 3
    }

    public enum GyroRange : byte
    {
        x250dps = 0,
        x500dps = 1,
        x1000dps = 2,
        x2000dps = 3
    }

两个枚举类型：AcclRange 表示重力加速度的量程，即 2g、4g等；GyroRange 表示陀螺仪的量程，像 500 度/秒。

这里重点看看计数的读取。在读取加速度时，要把读到的 16 位有符号整数进行处理。实际上就是读数除以量程，比如，±2g，就用 32768 / 2 = 16384。假设读数为x，就用x除以16384，这样就知道是多少个 g 了。通用公式是：

 其中，r 是读数，g 是重力加速度，一般取值 9.8。量程就是前面说的2、4、8、16。所以才有这个代码：

            // 转换倍数
            // 获取倍数（量程）
            float fac = AccelerRange switch
            {
                AcclRange.x2g       => 2.0f,
                AcclRange.x4g       => 4.0f,
                AcclRange.x8g       => 8.0f,
                AcclRange.x16g      => 16.0f,
                _                   => 0.0f
            };
            return (
                fac * G / 32768f * x,
                fac * G / 32768f * y,
                fac * G / 32768f * z
            );

陀螺仪的原理也一样，可以看上面贴的完整代码。

最后，做个测试。

    class Program
    {
        static void Main(string[] args)
        {
            using Devices.Mpu6050 mpudev = new(i2cBusid: 4,
                                         devAddress: Devices.Mpu6050.DEFAULT_ADDR);
            // 唤醒
            mpudev.WakeUp();
            // 设定重力加速度量程为 4g
            mpudev.AccelerRange = Devices.AcclRange.x4g;
            // 设定陀螺仪的量程为 500 d/s
            mpudev.GyroRange = Devices.GyroRange.x500dps;
            // 输出验证
            Console.WriteLine("加速度量程：{0}
角速度量程：{1}",
                        mpudev.AccelerRange switch
                        {
                            Devices.AcclRange.x2g   => "+/- 2g",
                            Devices.AcclRange.x4g   => "+/- 4g",
                            Devices.AcclRange.x8g   => "+/- 8g",
                            Devices.AcclRange.x16g  => "+/- 16g",
                            _                       => "未知"
                        },
                        mpudev.GyroRange switch
                        {
                            Devices.GyroRange.x250dps       => "+/- 250dps",
                            Devices.GyroRange.x500dps       => "+/- 500dps",
                            Devices.GyroRange.x1000dps      => "+/- 1000dps",
                            Devices.GyroRange.x2000dps      => "+/- 2000dps",
                            _                               => "未知"
                        });
            Console.WriteLine("------------------------");
            bool looping=true;
            Console.CancelKeyPress += (_,_)=> looping = false;

            Console.WriteLine("每一输输出后会暂停，以方便观察数据，可按任意键继续。");

            while(looping)
            {
                // 分别读出加速度和角速度
                float acc_x, acc_y, acc_z;
                (acc_x, acc_y, acc_z) = mpudev.GetAccelerometer();
                float gy_x, gy_y, gy_z;
                (gy_x, gy_y, gy_z) = mpudev.GetGyroscope();
                string output = $"加速度：x={acc_x}, y={acc_y}, z={acc_z}";
                output += $"
角速度：x={gy_x}, y={gy_y}, z={gy_z}";
                Console.WriteLine(output);
                Console.Write("
");
                Console.ReadKey(true);
            }
        }
    }

随即 build 源码，上传到树莓派上运行一下。

 数据是读出来了，至于怎么去用，那得看你的用途了。多数时候，MPU6050会用在无人机上，不过，姿态运算的算法真的太复杂了，老周也没弄明白，所以这里也没办法跟大伙聊了。不过要判断是不是有人拿模块在做“摇一摇”运动还是好办的，因为剧烈晃动时陀螺仪的读数会增大，加速度x、y的读数也会增大。

========================================================

 最后，咱们聊聊给大草莓添加开机按钮的事。很简单，因为这是硬件上设定好的，你也不用改什么配置（根本没法配置），方法就是：向 GPIO3 引脚输出低电平，树莓派就会开机。树莓派在上电后会自动开机的，这里加开机按钮的用途是当你关机后想再开机，如果不加个按钮，你就要拔掉电源线再接上，重新上电，或者关掉插座再通电。如果加了按钮，按一下就会开机了。

那按钮怎么接呢？最简单方案就是 GPIO3 -- 按钮 -- GND，即在 GPIO3 和 GND 之间接个按钮。原理就是 GND 是相对 0V，它就是输出低电平的最简单方案。只要和 GPIO3 接通，GPIO3 读到的就是低电平，所以就会开机。当然了，你用两根线把 GPIO3 和 GND 短接一下也可以开机的。

如果想用关机键，就要配置了。开机是硬件层定义的，但关机是系统驱动集成的，应该算是软件层定义的。所以，给草莓派加关机按钮就要配置了。打开 /boot/config.txt

sudo nano /boot/config.txt

加上：

dtoverlay=gpio-shutdown, gpio_pin=11

gpio_pin 指定用哪个引脚来触发关机，默认是 GPIO3，这里我配置了11。如果省略 gpio_pin 参数，就是3。于是，如果你打算用一个按钮来完成关机和开机动作，那就保持默认。这样一来，在开机状态下按一下按钮，就会关机；关机后再按一下就开机。

关机信号默认也是低电平触发，所以你把用来关机的引脚和 GND 短接一下也能关机的。如果希望高电平触发，可以用 active_low 参数来配置，如果为1，表明低电平触发，在高电平向低电平跳转（过渡，下降沿）的时候发送关机命令；如果配置为0，表示高电平触发，当电平从低跳转到高时发送关机命令。

dtoverlay=gpio-shutdown, gpio_pin=11, active_low=0