【.NET 与树莓派】六轴飞控传感器(MPU 6050)
阅读原文时间:2021年05月12日阅读:1

所谓“飞控”,其实是重力加速度计和陀螺仪的组合,因为多用于控制飞行器的平衡(无人机、遥控飞机)。有同学会问,这货为什么会有六轴呢?咱们常见的不是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

手机扫一扫

移动阅读更方便

阿里云服务器
腾讯云服务器
七牛云服务器

你可能感兴趣的文章