其他分享
首页 > 其他分享> > 【.NET 与树莓派】六轴飞控传感器(MPU 6050)以及开关机按钮

【.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

 

 

出处:https://www.cnblogs.com/tcjiaan/p/14751964.html

标签:开关机,树莓,量程,AcclRange,六轴飞控,加速度,byte,i2c,GyroRange
来源: https://www.cnblogs.com/mq0036/p/14874664.html