【.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}\n角速度量程：{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 += $"\n角速度：x={gy_x}, y={gy_y}, z={gy_z}"; Console.WriteLine(output); Console.Write("\n"); 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
