智能魔法棒原型开发
概况
大家都听过《哈利波特》吧，在《哈利波特》的世界中，魔法物品很多，但是没有任何一样物品能像魔杖一样重要，因为它与巫师建立起了一种难以言表的紧密联系。大家看到魔法世界里的巫师们，轻轻挥动几下他们的魔法棒，就可以施放各种各样的魔法，是不是即惊叹又羡慕。
今天给大家带来一款家庭版的魔法棒，开发者们只要轻轻挥动它，就可以控制家里的智能设备，体验一次当哈利波特的感觉，是不是特别神奇。它还可以充当探照小灯，比如东西掉到狭小又深的地方时，可以将身材修长苗条是魔法棒伸进去，点亮顶部的小灯，来寻找物品。
想拥有这样的魔法棒吗，一起来看看吧。
步骤
第 1 步：硬件设计
电路主要分为主控板和灯板两部分。
主控板搭载锂电池和充电管理系统，使魔法棒具备可循环的持久续航能力，采用一颗高精度六轴传感器，可以识别用户手势姿态，判断用户指令，还搭载涂鸦智能出品的通信模组，赋予魔法棒联网的能力，可与手机 APP 通讯，实现设备端，手机端，云端三方数据通信，从而实现用户通过挥动魔法棒控制智能设备，起到魔术一样的神奇效果。
灯板可以根据开发者喜好自由发挥，添加各种颜色，各种规格的灯，使魔法棒在"施法"时显的更加绚丽多彩。
整体硬件框图如下：
主控板
考虑到便捷环保和重复使用，魔法棒选择 3.7V 锂电池供电，电池的尺寸和容量可以根据自身需求选择。原理图中的 U3 为 TP4056，是一颗锂电池专用充放电芯片。
这是一款恒定电流/恒定电压线性充电芯片，采用了内部 PMOSFET 架构，内置防倒充电路。芯片具有热反馈功能，可对充电电流进行自动调节，以便在大功率操作或高环境温度条件下对芯片温度加以限制。充电电压固定于 4.2V，而充电电流可通过一个电阻器进行外部设置。当充电电流在达到最终浮充电压之后降至设定值 1/10 时，TP4056 将自动终止充电循环。
V_IN 为 USB 输入的 5V 电压，V_BAT 连接锂电池的正极（锂电池的负极记得连 GND），CE 脚是芯片使能端，只有 CE 脚高电平的时候，TP4056 才会正常工作。
- STDBY 脚是电池充电完成指示端。当电池充电完成时，该脚被内部开关拉到低电平，表示充电完成。除此之外，该管脚都将处于高阻态。
- CHRG 脚是漏极开路输出的充电状态指示端。当充电器向电池充电时， 该管脚被内部开关拉到低电平，表示充电正在进行；否则该管脚处于高阻态。
因此，开发者可以在 STDBY 和 CHRG 脚分别加了绿色和红色两个 LED。这样，当锂电池正在充电时，红灯点亮，当充电完成，绿灯点亮。
- PROG 脚是恒流充电电流设置和充电电流监测端。从 PROG 管脚连接一个外部电阻到地端可以对充电电流进行编程。在预充电阶段，此管脚的电压被调制在 0.1V；在恒流充电阶段，此管脚的电压被固定在 1V。在充电状态的所有模式，测量该管脚的电压都可以根据下面的公式来估算充电电流 Ibat=1200*Vprog/Rprog。
为了防止大电流充电，芯片和电池温度上升，开发者把充电最大电流设置为 0.8A。根据公式可知，Rprog=1200*Vprog/Ibat。
其中 Ibat 取 0.8A，Vprog 取 1V，计算得 Rprog=1.5K 欧姆。如果开发者想加大电流，在此基础上适当减小 Rprog 电阻（原理图中的 R8）即可。
- TEMP 脚是电池温度检测输入端。将 TEMP 管脚接到电池的 NTC 传感器的输出端。如果 TEMP 管脚的电压小于输入电压的 45％或者大于输入电压的 80％，意味着电池温度过低或过高，则充电被暂停。如果 TEMP 直接接 GND，电池温度检测功能取消，其他充电功能正常
这里为了减少开发周期，开发者可以将 R16 或者 R14 焊 0 欧姆电阻，取消该功能。如果开发者希望启用，可以联系涂鸦智能，涂鸦智能会提供专业技术支持进行帮助。查看 TP4056 数据手册
U4 是一颗低功耗型的 LDO，型号是 MD5333，它的作用将锂电池输出的 3.6V 电压降压到 3.3V，由此给控制器，传感器等设备供电。
MD5333 最大的优势在于它的低功耗，它的静态漏电流只有 1~2uA，可以大大减少魔法棒在待机情况下的功耗，从而保证魔法棒的续航时间。开发者也可以选择其他型号的 LDO 芯片，但是静态漏电流建议要在 2uA 以下哦。
控制器
控制核心依旧是开发者们所熟悉的涂鸦智能云模组。
涂鸦有各种通信协议，各种尺寸规格，各种工作温度，各种焊接方式的一系列超高性价比自研模组，开发者可根据自身的需求进行灵活选择。本次的魔法棒，如果考虑产品化，涉及低功耗等要求，推荐开发者选择低功耗蓝牙模组或者低功耗 Zigbee 模组。
魔法棒会用到如下几个控制引脚用于控制外设，开发者在设计电路的时候需要注意一下：
引脚名称 引脚功能 LED LED 灯控制引脚，用于控制 LED 灯亮灭或闪烁 KEY 按键引脚，用于唤醒涂鸦云模组，退出低功耗模式 MPU_INT MPU6050 数据就绪引脚，当数据就绪时会产生高电平以提示主控来读取数据 SCL MPU6050 IIC 接口的时钟引脚 SDA MPU6050 IIC 接口的数据引脚 CT_POW MPU6050 电源控制引脚 传感器
如何识别用户挥手的手势姿态信息呢？这里就需要一颗姿态采集传感器，本方案采用的是 MPU6050。
它是 InvenSense 公司推出的全球首个整合性 6 轴运动处理器件，内带 3 轴陀螺仪和 3 轴加速度传感器，并且含有一个辅助 IIC 接口，可用于连接外部磁力传感器，利用数字运动处理器 DMP 硬件加速引擎，通过主 IIC 接口，可以向应用端输出完整的 9 轴姿态融合演算数据。
MPU6050 对陀螺仪和加速度计分别用了三个 16 位的 ADC（0~65535），将其测量的模拟量转化为可输出的数字量。为了精确跟踪快速和慢速的运动，传感器的测量范围都是用户可控的。
MPU6050 三个独立的振动 MEMS 速率陀螺仪，可检测旋转角度 X 轴，Y 轴和 Z 轴。 当陀螺仪围绕任何感应轴旋转时，科里奥利效应就会产生电容式传感器检测到的振动。 所得到的信号被放大，解调和滤波产生与角速度成比例的电压。 该电压使用单独的片内数字化 16 位模数转换器（ADC）对每个轴进行采样。 陀螺仪传感器可以全面范围的被数字编程为每秒±250，±500，±1000 或±2000 度（dps）。 ADC 样本速率可以从每秒 8,000 个采样点编程到每秒 3.9 个采样点，并且可由用户选择低通滤波器可实现广泛的截止频率。
MPU6050 的 3 轴加速度计为每个轴使用单独的检测质量。 加速沿着一条特定轴在相应的检测质量上引起位移，并且电容式传感器检测到该位移位移有差别。MPU6050 的架构降低了加速度计的敏感度制造变化以及热漂移。 当设备放置在平坦的表面上时，将进行测量在 X 和 Y 轴上为 0g，在 Z 轴上为+ 1g。 加速度计的比例因子在工厂进行校准并且在名义上与电源电压无关。 每个传感器都有一个专用 ADC 来提供数字输出。 数字输出的满量程范围可以调整到±2g，±4g，±8g 或±16g。
传感器的引脚功能说明如下：
引脚编号 引脚名称 说明 1 CLKIN 可选的外部时钟输入，如果不用则接 GND 6 AUX_DA I2C 主串行数据，用于外接传感器 7 AUX_CL I2C 主串行时钟，用于外接传感器 8 VLOGIC 数字 I/O 供电电压 9 AD0 I2C Slave 地址 LSB 10 REGOUT 校准滤波电容连线 11 FSYNC 帧同步数字输入 12 INT 中断数字输出（推挽或开漏） 13 VDD 电源电压及数字 I/O 供电电压 18 GND 电源地 19、20、21 RESV 预留，不接 20 CPOUT 电荷泵电容连线 23 SCL I2C 串行时钟 SCL 24 SDA I2C 串行数据 SDA 2、3、4、5、14、15、16、17 NC 悬空 功能板
这部分的电路，开发者可以任意发挥，给魔法棒附加额外的创意功能，展现各种天马行空的奇思妙想，简单的，比如加个灯，让魔法棒具备探照的功能，也可以加蜂鸣器，当按键按下，可以制造美妙的音乐。这里以灯为例，开发者可以参考下面原理图。
设计时需要注意一下几点：
- 所选 LED 灯的压降不能超过供电电压。
- 需要考虑 LED 允许的最大持续电流，选择合作阻值的限流电阻。
- 注意限流电阻的功率，选择合适封装，0603 为 1/10W，0805 为 1/8W，1206 为 1/4W。
第 2 步：整机搭建
第 3 步：软件开发
环境搭建
产品创建
在进行魔法棒的固件开发之前，我们需要先在 涂鸦 IoT 开发平台 上创建一个智能产品，还不熟悉产品创建的您可以通过涂鸦开发者平台的文档中心进行了解，详情请参考快速入门**。
魔法棒产品可以选择找不到品类进行自定义创建。本 Demo 选用的是涂鸦 Bluetooth LE 模组 TYBN1，其芯片平台是 nRF52832 。因此，选择通讯协议为蓝牙，云端接入方式为TuyaOS，云端接入硬件为Nordic nRF52832。当然也可以选择其他模组，可以参考开发者平台的 涂鸦云模组规格书 进行选型。
为实现云端接收魔法棒识别结果的目的，需在功能定义页面自定义一个 DP 点，参数如下：
DP ID 功能点名称 标识符 数据传输类型 数据类型 功能点属性 101 手势 gesture 只上报 (ro) 枚举型 (Enum) 枚举值：none，ges1，ges2 …
获取 SDK
- 原厂 SDK
涂鸦 Bluetooth LE 云模组 TYBN1 使用的芯片平台是 Nordic 的 nRF52832，我们可以在 Nordic 官网找到 nRF5 系列的 SDK (单击进入下载页面：nRF5 SDK downloads)，选择 15.3.0 版本，SoftDevices 选择版本介绍中涉及 nRF52832 的即可，最后单击 Download files (.zip)。
- 涂鸦 SDK
涂鸦提供了适用于 nRF52832 平台的 SDK，可以在创建好的魔法棒产品的 硬件开发 页面左下角找到最新版的 SDK 。下载完成后，需将涂鸦 SDK 文件
tuya-ble-sdk-demo-project-nrf52832拷贝到原厂 SDK 的nRF5_SDK_15.3.0_59ac345\examples\ble_peripheral目录下。( 开发指南：蓝牙 SDK 开发 )
准备 IDE
涂鸦 SDK 中的工程是使用 MDK-ARM Keil µVision 创建的，还未安装的您可以前往官网下载。安装好 IDE 后，双击涂鸦 SDK 中的工程文件
ble_app_uart_pca10040_s132.uvprojx，打开时会自动安装软件包。如果安装失败，可以在 Pack Installer 中找到 nRF52832 芯片对应的软件包进行安装，如下图所示：
烧录授权
- 模组购买
如果您手边没有涂鸦云模组的话，可以在硬件开发页面先新增自定义固件，然后单击右侧的立即购买 进入模组购买流程。涂鸦提供多种烧录授权方式，可根据不同的开发阶段、芯片类型和生产方式进行选择，详情单击 固件烧录授权。
- 激活码领取
单个模组也可以通过购买单个激活码进行授权。新用户可以在硬件开发页面免费领取 2 个激活码：
交付形式选择授权码清单后提交订单，就可以获得 2 组激活码，包括 uuid、authkey 和 mac 地址。
- 授权信息修改
将产品 ID (PID) 和激活码 (选取 1 组) 填入
tuya_ble_sdk_demo.h文件的以下位置：#define TY_DEVICE_NAME "demo" #define TY_DEVICE_PID "xxxxxxxx" /* PID */ #define TY_DEVICE_MAC "xxxxxxxx" /* mac */ #define TY_DEVICE_DID "xxxxxxxx" /* uuid */ #define TY_DEVICE_AUTH_KEY "xxxxxxxx" /* authkey */然后还需要在
tuya_ble_sdk_demo.c文件中将初始化参数use_ext_license_key和device_id_len的值分别修改为 1 和 16，以使激活码生效：static tuya_ble_device_param_t tuya_ble_device_param = { .use_ext_license_key = 1, // 1-info in tuya_ble_sdk_demo.h, 0-auth info .device_id_len = 16, // DEVICE_ID_LEN, .p_type = TUYA_BLE_PRODUCT_ID_TYPE_PID, .product_id_len = 8, .adv_local_name_len = 4, .firmware_version = TY_DEVICE_FVER_NUM, .hardware_version = TY_DEVICE_HVER_NUM, };- 硬件连接
编译通过后，将 J-Link 烧录器连接到开发板，连线方式如下：
模组引脚 J-Link 引脚 3.3V VCC GND GND SWDIO SWDIO SWC SWCLK - 固件下载
在 Demo 固件下载前，必须先下载协议栈固件（同一块模组只需操作 1 次）。在 J-Link 官网下载 J-Link 软件开发包。安装完成后，打开 J-Flash 软件，单击 File - New Project 创建工程，芯片选择 nRF52832_xxAA 后单击 OK，如下图所示：
单击 File - Open data file 打开涂鸦 SDK
\pca10040\s132\arm5_no_packs\hex\material目录下的s132_nrf52_6.1.1_softdevice.hex文件，如下图所示：
单击 Target - Connect 连接芯片，成功连接后单击 Target - Production Programming 开始下载，下载完成后单击 Target - Disonnect 断开连接。Demo 固件可以直接通过 Keil 进行下载。
查看日志
开发过程中可以通过查看日志来进行功能调试，日志功能默认关闭，开启需对代码作如下修改：
#define TUYA_APP_LOG_ENABLE 1 /* 位于 custom_tuya_ble_config.h 文件中，0-关闭，1-开启 */ #define TY_LOG_ENABLE 1 /* 位于 board.h 文件，0-关闭，1-开启 */将修改后的固件编译并烧录至开发板，然后打开 J-link RTT Viewer 软件，会自动弹出以下对话框：
按上图内容完成设置后，单击 OK。成功连接就可以看到设备日志了，软件日志窗口也会出现如下提示：
固件设计
功能定义
魔法棒要实现的功能定义如下：
功能 说明 手势动作识别 可识别出下列手势动作：
- 手势动作 1：向上甩动；
- 手势动作 2：向下甩动；
- 手势动作 3：向左甩动；
- 手势动作 4：向右甩动；
…识别功能开关 可通过按键控制识别功能打开或关闭：
- 按键按下时，手势识别功能打开；
- 按键释放时，手势识别功能关闭。手势数据上报 可将手势数据上报至云端，以实现手势动作对其他设备的控制。 配网状态重置 可通过配网键实现设备端解绑：
- 长按配网键 3 秒，设备主动解绑。配网等待时限 可在等待配网超过 1 分钟后结束等待：
- 上电时设备状态为未绑定或通过配网键重置时，设备进入配网等待；
- 1 分钟后仍未被绑定，则结束等待。配网状态指示 可通过指示灯提示设备状态：
- 等待配网时，指示灯快闪；
- 配网结束时，指示灯关闭。手电筒功能 可通过配网键短按切换指示灯开/关。
软件方案
模块划分
魔法棒的核心功能是识别不同的手势动作，从而达到通过涂鸦云平台控制其他设备的目的。这次硬件方案采取了 MPU6050 六轴惯性传感器来采集手势数据，包括 3 轴加速度和 3 轴角速度，由此可计算出设备的姿态角，也可以通过内置的数字运动处理器 (DMP) 获取四元数来计算姿态。结合这些数据对手势特征进行提取，就可以实现特定手势识别。根据魔法棒的硬件方案和功能定义，可以将 Demo 程序划分为以下 5 个模块：
模块 处理内容 数据采集模块 MPU6050 传感器的数据采集处理 姿态解算模块 根据角速度、加速度计算姿态角，并用卡尔曼滤波法进行数据融合处理 手势识别模块 应用采集到的数据和姿态解算结果，实现各个手势动作识别 联网处理模块 设备配网与解绑、配网状态指示、手势数据上报等 其他功能模块 手电筒功能、低功耗处理等 各模块的具体方案将在 4.3 功能开发 中进行介绍。
软件框图
结合涂鸦 Bluetooth LE SDK 的软件架构和应用功能设计，魔法棒的软件框图如下：
软件流程
魔法棒的基本工作流程如下：
文件结构
应用入口：
tuya_ble_sdk_demo.c -> tuya_gesture_controller.c├── include | ├── common | | └── tuya_common.h /* 通用类型和宏定义 */ | ├── driver | | ├── tuya_key.h /* 按键驱动 */ | | ├── tuya_led.h /* LED 驱动 */ | | └── tuya_mpu6050.h /* MPU6050 驱动 */ | ├── platform | | └── tuya_gpio.h /* 平台关联 GPIO 驱动 */ | ├── sdk | | ├── tuya_ble_bulk_data_demo.h /* 大数据通道例程 */ | | ├── tuya_ble_product_test_demo.h /* 整机产测例程 */ | | └── tuya_ble_sdk_test.h /* 实现 tuya_ble_sdk 测试的串口指令 */ | ├── tuya_ble_sdk_demo.h /* 实现 tuya_ble_sdk 的初始化，应用程序入口 */ | ├── tuya_imu_daq.h /* 传感数据采集 */ | ├── tuya_gesture_controller.h /* 手势控制器管理中心 */ | ├── tuya_ges_act_rec.h /* 手势动作识别 */ | ├── tuya_net_proc.h /* 设备联网处理 */ | └── tuya_svc_angle_calc.h /* 姿态解算服务 */ └── src ├── driver | ├── tuya_key.c /* 按键驱动 */ | ├── tuya_led.c /* LED 驱动 */ | └── tuya_mpu6050.c /* MPU6050 驱动 */ ├── platform | └── tuya_gpio_nRF52832.c /* nRF52832 平台关联 GPIO 驱动 */ ├── sdk | ├── tuya_ble_bulk_data_demo.c /* 大数据通道例程 */ | ├── tuya_ble_product_test_demo.c /* 整机产测例程 */ | └── tuya_ble_sdk_test.c /* SDK 测试程序 */ ├── tuya_ble_sdk_demo.c /* 实现 tuya_ble_sdk 的初始化，应用程序入口 */ ├── tuya_gesture_controller.c /* 手势控制器管理中心 */ ├── tuya_imu_daq.c /* 传感数据采集 */ ├── tuya_ges_act_rec.c /* 手势动作识别 */ ├── tuya_net_proc.c /* 设备联网处理 */ └── tuya_svc_angle_calc.c /* 姿态解算服务 */
功能开发
下面详细介绍魔法棒各项功能的实现过程。Demo 仓库：tuya-iotos-embeded-demo-ble-gesture-controller
数据采集模块
这次开发采用的惯性测量单元是 MPU6050，其主要功能已经在硬件设计方案中进行了介绍。数据采集模块的目标是：通过 I2C 接口从 MPU6050 中读取设备的加速度数据和角速度数据。
I2C 通信
我们先来解决通信问题，在 MPU6050 产品规格书 的 9.2~9.4 节可以找到关于 I2C 的介绍。
- 从机地址
MPU6050 的 7 位从机地址由 AD0 引脚决定，当 AD0 = 0 时，从机地址为 0x68；AD0 = 1 时，从机地址为 0x69。我们采用的方案是 AD0 接地，所以从机地址为 0x68。
- 读写时序
我们可以在产品规格书的 9.3 节找到关于 MPU6050 的寄存器读写时序的描述，包括单字节读写和多字节读写。其中，命令字节由从机地址 (bit[7:1]) 和读/写命令 (bit0) 组成，写命令为 0，读命令为 1。
- 代码实现
涂鸦 SDK 提供了一些有关 I2C 通信的函数接口，需要包含
ty_i2c.h文件。我们使用软件 I2C 来实现，编写寄存器读写函数：#include "tuya_mpu6050.h" #include "ty_i2c.h" /* slave address */ #define MPU6050_DEV_ADDR_AD0_LOW 0x68 #define MPU6050_DEV_ADDR_AD0_HIGH 0x69 #define MPU6050_DEV_ADDR MPU6050_DEV_ADDR_AD0_LOW /* I2C R/W command */ #define I2C_CMD_WRITE 0 #define I2C_CMD_READ 1 #define MPU6050_ADDR_CMD_WRITE ((MPU6050_DEV_ADDR << 1) | I2C_CMD_WRITE) #define MPU6050_ADDR_CMD_READ ((MPU6050_DEV_ADDR << 1) | I2C_CMD_READ) /** * @brief read data of MPU6050 * @param[in] reg_addr: register address * @param[in] len: data length * @param[out] data: data buffer * @return none */ STATIC VOID_T __mpu6050_read_data(_IN UCHAR_T reg_addr, _IN CONST UCHAR_T len, _OUT UCHAR_T *data) { i2c_start(); i2c_send_bytes(MPU6050_ADDR_CMD_WRITE, ®_addr, 1); i2c_start(); i2c_rcv_bytes(MPU6050_ADDR_CMD_READ, data, len); i2c_stop(); } /** * @brief read register of MPU6050 * @param[in] reg_addr: register address * @return register value */ STATIC UCHAR_T __mpu6050_read_register(_IN UCHAR_T reg_addr) { UCHAR_T reg_val; i2c_start(); i2c_send_bytes(MPU6050_ADDR_CMD_WRITE, ®_addr, 1); i2c_start(); i2c_rcv_bytes(MPU6050_ADDR_CMD_READ, ®_val, 1); i2c_stop(); return reg_val; } /** * @brief write register of MPU6050 * @param[in] reg_addr: register address * @param[in] reg_val: value to be written * @return none */ STATIC VOID_T __mpu6050_write_register(_IN CONST UCHAR_T reg_addr, _IN UCHAR_T reg_val) { i2c_soft_cfg(MPU6050_ADDR_CMD_WRITE, reg_addr, reg_val); } /** * @brief write register of MPU6050 * @param[in] reg_addr: register address * @param[in] data: data to be written * @param[in] valid_bit: the code of valid bits * @return none */ STATIC VOID_T __mpu6050_write_register_bit(_IN CONST UCHAR_T reg_addr, _IN CONST UCHAR_T data, _IN CONST UCHAR_T valid_bit) { UCHAR_T reg_val; if (valid_bit == 0xFF) { reg_val = data; } else { reg_val = __mpu6050_read_register(reg_addr); reg_val = (reg_val & (~valid_bit)) | (data & valid_bit); } i2c_soft_cfg(MPU6050_ADDR_CMD_WRITE, reg_addr, reg_val); }此外，还需要确认配置的 I2C 引脚是否符合硬件设计，在
ty_i2c_nRF52832.c中修改：#define TY_I2C_PIN_SCL 14 #define TY_I2C_PIN_SDA 11
传感器初始化
- 初始化步骤
MPU6050 初始化的基本步骤和需要配置的寄存器如下表所示：
No. 步骤 操作方法 1 初始化 I2C 接口 软件 I2C：初始化 SCL 和 SDA 引脚； 2 复位设备 设置寄存器 PWR_MGMT_1 的 bit7，复位后需延时至少 100ms，否则初始化不成功； 3 检查设备连接 读取寄存器 WHO_AM_I 进行校验，0x68 表示已连接； 4 解除休眠 设置寄存器 PWR_MGMT_1 的 bit6，初值为 1，即休眠模式； 5 选择时钟源 设置寄存器 PWR_MGMT_1 的 bit2:0，一般选择陀螺仪某一轴的时钟源，以保证数据精度； 6 设置陀螺仪满量程范围 设置寄存器 GYRO_CONFIG 的 bit4:3，4 种可选； 7 设置加速度计满量程范围 设置寄存器 ACCEL_CONFIG 的 bit4:3，4 种可选； 8 设置陀螺仪输出分频 设置寄存器 SMPRT_DIV； 9 设置数字低通滤波器 设置寄存器 CONFIG 的 bit2:0； 10 启用数据就绪中断 设置寄存器 INT_PIN_CFG 的 bit7:4 和寄存器 INT_ENABLE 的 bit0。 下面对初始化涉及到的寄存器做简单说明，也可以直接查看 MPU6050 寄存器手册，用上表中的寄存器名称进行搜索即可。注：括号内表示寄存器地址、寄存器名称、可读写情况和初值；省略的位为保留位，始终为 0。
>> 电源管理寄存器 1（0x6B，PWR_MGMT_1，R/W，0x40）
Bit 标识符 说明 7 DEVICE_RESET 设备复位。
0：复位完成；
1：将所有内部寄存器重置为其默认值，复位完成后，该位自动清除为 0。6 SLEEP 睡眠模式设置。
0：设备正常工作；
1：进入低功耗睡眠模式。5 CYCLE 循环模式设置。
0：禁用循环模式；
1：SLEEP=0 时，MPU-60X0 将进入循环模式，设备在睡眠和唤醒之间循环。3 TEMP_DIS 温度传感器设置。
0：启用温度传感器；
1：禁用温度传感器。2:0 CLKSEL[2:0] 系统时钟源选择。
000：内部 8M RC 晶振；
001：PLL，使用 X 轴陀螺仪作为参考；
010：PLL，使用 Y 轴陀螺仪作为参考；
011：PLL，使用 Z 轴陀螺仪作为参考；
100：PLL，使用外部 32.768kHz 作为参考；
101：PLL，使用外部 19.2MHz 作为参考；
110：保留；
111：关闭时钟，保持时序产生电路复位状态。>> 设备 ID 校验寄存器（0x75，WHO_AM_I，R，0x68）
Bit 标识符 说明 6:1 WHO_AM_I[6:1] 设备地址高 6 位，默认为 0x68。 >> 陀螺仪配置寄存器（0x1B，GYRO_CONFIG，R/W，0x00）
Bit 标识符 说明 7:5 XYZG_ST 陀螺仪 X/Y/Z 轴自检控制。
0：关闭自检；
1：激活自检。4:3 FS_SEL[1:0] 陀螺仪满量程范围设置。
00：±250°/s；
01：±500°/s；
10：±1000°/s；
11：±2000°/s。>> 加速度计配置寄存器（0x1C，ACCEL_CONFIG，R/W，0x00）
Bit 标识符 说明 7:5 XYZA_ST 加速度 X/Y/Z 轴自检控制。
0：关闭自检；
1：激活自检。4:3 AFS_SEL[1:0] 加速度计满量程范围设置。
00：±2g；
01：±4g；
10：±8g；
11：±16g。>> 采样率分频寄存器（0x19，SMPRT_DIV，R/W，0x00）
Bit 标识符 说明 7:0 SMPLRT_DIV[7:0] 陀螺仪输出速率分频设置。
- 采样率 = 陀螺仪输出速率 / (1 + SMPLRT_DIV)；
- 禁用 DLPF (DLPF_CFG=0 或 7) 时，陀螺仪输出速率为 8kHz；
- 启用 DLPF 时，陀螺仪输出速率为 1kHz。>> 配置寄存器（0x1A，CONFIG，R/W，0x00）
Bit 标识符 说明 5:3 EXT_SYNC_SET[2:0] 外部帧同步 (FSYNC) 引脚采样设置。 2:0 DLPF_CFG[2:0] 数字低通滤波器 (DLPF) 设置。 >> 中断引脚配置寄存器（0x37，INT_PIN_CFG，R/W）
Bit 标识符 说明 7 INT_LEVEL INT 引脚中断电平设置。
0：高电平有效；
1：低电平有效。6 INT_OPEN INT 引脚输出模式设置。
0：推挽输出；
1：开漏输出。5 LATCH_INT_EN 中断保持方式设置。
0：产生 50us 脉冲；
1：保持高电平直到中断清除。4 INT_RD_CLEAR 中断状态清除方式设置。
0：仅通过读取 INT_STATUS 来清除中断状态位；
1：通过任何读操作清除中断状态位。3 FSYNC_INT_LEVEL FSYNC 引脚中断电平设置。
0：高电平有效；
1：低电平有效。2 FSYNC_INT_EN FSYNC 引脚中断功能设置。
0：禁止 FSYNC 引脚作为主处理器的中断引脚；
1：允许 FSYNC 引脚作为主处理器的中断引脚。1 I2C_BYPASS_EN 辅助 I2C 总线访问权限设置。
0：禁止主处理器直接访问辅助 I2C 总线；
1：I2C_MST_EN = 0 时，允许主处理器直接访问辅助 I2C 总线。>> 中断使能寄存器（0x38，INT_ENABLE，R/W，0x00）
Bit 标识符 说明 6 MOT_EN 运动中断设置。
0：禁用中断；
1：启用中断。4 FIFO_OFLOW_EN FIFO 缓存区溢出中断设置。
0：禁用中断；
1：启用中断。3 I2C_MST_INT_EN I2C 主机中断设置。
0：禁用中断；
1：启用中断。0 DATA_RDY_EN 数据就绪中断设置。
0：禁用中断；
1：启用中断。- 代码实现
#include "tuya_mpu6050.h" #include "ty_i2c.h" /* device address */ #define MPU6050_DEV_ADDR_AD0_LOW 0x68 #define MPU6050_DEV_ADDR_AD0_HIGH 0x69 #define MPU6050_DEV_ADDR MPU6050_DEV_ADDR_AD0_LOW #define MPU6050_DEV_ID 0x68 /* I2C R/W command */ #define I2C_CMD_WRITE 0 #define I2C_CMD_READ 1 #define MPU6050_ADDR_CMD_WRITE ((MPU6050_DEV_ADDR << 1) | I2C_CMD_WRITE) #define MPU6050_ADDR_CMD_READ ((MPU6050_DEV_ADDR << 1) | I2C_CMD_READ) /* register map */ #define MPU6050_RA_SMPRT_DIV 0x19 #define MPU6050_RA_CONFIG 0x1A #define MPU6050_RA_GYRO_CONFIG 0x1B #define MPU6050_RA_ACCEL_CONFIG 0x1C #define MPU6050_RA_INT_PIN_CFG 0x37 #define MPU6050_RA_INT_ENABLE 0x38 #define MPU6050_RA_PWR_MGMT_1 0x6B #define MPU6050_RA_WHO_AM_I 0x75 /* MPU6050 Gyro full-scale range */ typedef BYTE_T MPU_GYRO_FSR_E; #define MPU_GYRO_FS_250 0x00 /* 250dps */ #define MPU_GYRO_FS_500 0x01 /* 500dps */ #define MPU_GYRO_FS_1000 0x02 /* 1000dps */ #define MPU_GYRO_FS_2000 0x03 /* 2000dps */ /* MPU6050 Accel full-scale range */ typedef BYTE_T MPU_ACCEL_FSR_E; #define MPU_ACCEL_FS_2 0x00 /* 2g */ #define MPU_ACCEL_FS_4 0x01 /* 4g */ #define MPU_ACCEL_FS_8 0x02 /* 8g */ #define MPU_ACCEL_FS_16 0x03 /* 16g */ STATIC FLOAT_T sg_gyro_sens = 0.0f; STATIC USHORT_T sg_accel_sens = 0; /** * @brief reset MPU6050 * @param[in] none * @return none */ STATIC VOID_T __mpu6050_reset(VOID_T) { __mpu6050_write_register_bit(MPU6050_RA_PWR_MGMT_1, MPU_RA_BIT_DEVICE_RESET, MPU_RA_BIT_DEVICE_RESET); tuya_ble_device_delay_ms(100); } /** * @brief get the identity of the device (default: 0x68) * @param[in] none * @return device id */ STATIC UCHAR_T __mpu6050_get_device_id(VOID_T) { return __mpu6050_read_register(MPU6050_RA_WHO_AM_I); } /** * @brief check if MPU6050 is connected * @param[in] none * @return TRUE - connected, FALSE - unconnected */ STATIC BOOL_T __mpu6050_is_connected(VOID_T) { if (__mpu6050_get_device_id() == MPU6050_DEV_ID) { return TRUE; } else { return FALSE; } } /** * @brief enable or disable sleep mode * @param[in] enabled: TRUE - sleep, FALSE - work * @return none */ STATIC VOID_T __mpu6050_set_sleep_mode(_IN CONST BOOL_T enabled) { if (enabled) { __mpu6050_write_register_bit(MPU6050_RA_PWR_MGMT_1, MPU_RA_BIT_SLEEP, MPU_RA_BIT_SLEEP); } else { __mpu6050_write_register_bit(MPU6050_RA_PWR_MGMT_1, ~MPU_RA_BIT_SLEEP, MPU_RA_BIT_SLEEP); } } /** * @brief enable or disable sleep mode * @param[in] enabled: TRUE - sleep, FALSE - work * @return none */ VOID_T tuya_mpu6050_set_sleep_mode(_IN CONST BOOL_T enabled) { __mpu6050_set_sleep_mode(enabled); } /** * @brief set clock source * @param[in] src: clock source * @return none */ STATIC VOID_T __mpu6050_set_clk_src(UCHAR_T src) { __mpu6050_write_register_bit(MPU6050_RA_PWR_MGMT_1, src, MPU_RA_BIT_CLKSEL); } /** * @brief set gyroscope's full-scale range * @param[in] range: gyroscope's full-scale range value * @return none */ STATIC VOID_T __mpu6050_set_gyro_fsr(_IN CONST MPU_GYRO_FSR_E range) { __mpu6050_write_register_bit(MPU6050_RA_GYRO_CONFIG, range<<3, MPU_RA_BIT_FS_SEL); } /** * @brief set accelerometer's full-scale range * @param[in] range: new full-scale accelerometer range value * @return none */ STATIC VOID_T __mpu6050_set_accel_fsr(_IN CONST MPU_ACCEL_FSR_E range) { __mpu6050_write_register_bit(MPU6050_RA_ACCEL_CONFIG, range<<3, MPU_RA_BIT_AFS_SEL); } /** * @brief set MPU6050's sample rate * @param[in] sr: sample rate gyroscope output rate divider value * @return none */ STATIC VOID_T __mpu6050_set_sample_rate(_IN USHORT_T sr) { UCHAR_T div; if (sr > MPU_GYRO_OUTPUT_RATE) { sr = MPU_GYRO_OUTPUT_RATE; } if (sr < (MPU_GYRO_OUTPUT_RATE/MPU_SMPRT_DIV_MAX)) { sr = (MPU_GYRO_OUTPUT_RATE/MPU_SMPRT_DIV_MAX); } div = MPU_GYRO_OUTPUT_RATE / sr - 1; __mpu6050_write_register(MPU6050_RA_SMPRT_DIV, div); } /** * @brief set MPU6050's DLPF * @param[in] bw: baud width * @return none */ STATIC VOID_T __mpu6050_set_dlpf(_IN CONST USHORT_T bw) { UCHAR_T cfg = 0; if (bw >= MPU_DLPF_BW_CFG_1) { cfg = 1; } else if (bw >= MPU_DLPF_BW_CFG_2) { cfg = 2; } else if (bw >= MPU_DLPF_BW_CFG_3) { cfg = 3; } else if (bw >= MPU_DLPF_BW_CFG_4) { cfg = 4; } else if (bw >= MPU_DLPF_BW_CFG_5) { cfg = 5; } else { cfg = 6; } __mpu6050_write_register(MPU6050_RA_CONFIG, cfg); } /** * @brief set intterupt * @param[in] active_low: TRUE - active low, FALSE - active high * @return none */ STATIC VOID_T __mpu6050_set_int(BOOL_T active_low) { UCHAR_T reg_value = 0; if (active_low) { __mpu6050_write_register(MPU6050_RA_INT_PIN_CFG, 0x90); } else { __mpu6050_write_register(MPU6050_RA_INT_PIN_CFG, 0x50); } __mpu6050_write_register(MPU6050_RA_INT_ENABLE, 0x01); } /** * @brief MPU6050 sensor driver init * @param[in] clk: clock source * @param[in] g_fsr: gyroscope's full-scale range * @param[in] a_fsr: accelerometer's full-scale range * @param[in] smp_rt: sample rate * @param[in] pin: interrupt pin * @param[in] type: interrupt type * @param[in] int_cb: interrupt callback function * @return operation result */ MPU_RET tuya_mpu6050_init(_IN CONST MPU_CLK_E clk, _IN CONST MPU_GYRO_FSR_E g_fsr, _IN CONST MPU_ACCEL_FSR_E a_fsr, _IN CONST USHORT_T smp_rt, _IN CONST TY_GPIO_PORT_E pin, _IN CONST TY_GPIO_IRQ_TYPE_E type, _IN TY_GPIO_IRQ_CB int_cb) { /* I2C init */ i2c_soft_gpio_init(); /* reset MPU6050 */ __mpu6050_reset(); /* check communication */ if (!__mpu6050_is_connected()) { return MPU_ERR_UNCONN; } /* MPU6050 init */ __mpu6050_set_sleep_mode(FALSE); /* wakeup MPU6050 */ __mpu6050_set_clk_src(clk); /* set clock source */ __mpu6050_set_gyro_fsr(g_fsr); /* set gyroscope's full-scale range */ __mpu6050_set_accel_fsr(a_fsr); /* set accelerometer's full-scale range */ __mpu6050_set_sample_rate(smp_rt); /* set sample rate */ __mpu6050_set_dlpf(smp_rt/2); /* set DLPF */ /* save sensitivity scale factor */ sg_gyro_sens = 32768.0 / ((1 << g_fsr) * 250); sg_accel_sens = 32768.0 / ((1 << a_fsr) * 2); /* interrupt init */ if (int_cb != NULL) { if (tuya_gpio_irq_init(pin, type, int_cb)) { return MPU_ERR_IRQ_INIT_FAILED; } if (TY_GPIO_IRQ_FALLING == type) { __mpu6050_set_int(TRUE); } else { __mpu6050_set_int(FALSE); } } return MPU_OK; }另外，这次的硬件方案选择了用模组引脚来控制 MPU6050 的电源，所以在初始化前要先拉高 MPU6050 的 VLOGIC 引脚（模组的 CT_POW 引脚）来为其供电：
#include "tuya_mpu6050.h" #include "tuya_gpio.h" #define DAQ_TIME_MS 5 #define MPU_CT_POW_PIN TY_GPIO_16 #define MPU_INT_PIN TY_GPIO_2 STATIC DAQ_END_CB sg_daq_end_cb = NULL; /** * @brief set MPU6050 power on * @param[in] pin: VLOGIC pin number * @param[in] active_low: TRUE - active low, FALSE - active high * @return none */ VOID_T tuya_mpu6050_power_on(_IN CONST TY_GPIO_PORT_E pin, _IN CONST BOOL_T active_low) { if (!sg_pwr_pin_used) { tuya_gpio_init(pin, FALSE, active_low); sg_pwr_pin_used = TRUE; } tuya_gpio_write(pin, !active_low); tuya_ble_device_delay_ms(100); } /** * @brief IMU DAQ module init * @param[in] none * @return none */ VOID_T tuya_imu_daq_init(VOID_T) { tuya_mpu6050_power_on(MPU_CT_POW_PIN, TRUE); MPU_RET ret = tuya_mpu6050_init(MPU_CLK_PLL_XGYRO, MPU_GYRO_FS_2000, MPU_ACCEL_FS_16, 1000/DAQ_TIME_MS, MPU_INT_PIN, TY_GPIO_IRQ_FALLING, __new_data_ready_cb); if (MPU_OK != ret) { TUYA_APP_LOG_ERROR("tuya_mpu6050_init error: %d.", ret); return; } }
数据读取
- 中断处理
开启数据就绪中断后，当 MPU6050 完成数据采集时，INT 引脚的电平就会发生翻转，使得主控模组检测到外部中断，这时我们就可以通过读寄存器操作来获得数据。
加速度计和陀螺仪测得的数据是 16 位带符号数，由 2 个寄存器组成，高 8 位存放于低地址。加速度计数据寄存器的地址为 3BH~40H，陀螺仪数据寄存器的地址为 43H~48H，按顺序分别为 X 轴、Y 轴、Z 轴的数据。- 单位换算
从寄存器中读取的数据大小和设定的满量程范围有关，可以根据需要进行单位转换。MPU6050 输出的数据类型为 signed short，所以数据范围是 -32768~32767。
当陀螺仪的量程设置为 ±2000°/s 时，可计算出灵敏度为 32768/2000 = 16.4。假设从陀螺仪读数为 ω，那么角速度值为 ω/16.40，单位是[°/s]；或者为 ω/16.4/57.30，单位是[rad/s]。
当加速度的量程设置为 16g 时，可计算出灵敏度为 32768/16 = 2048。假设加速度计读数为 a，那么加速度值为 a/2048，单位是[g]，或者为 a*9.8/2048，单位是[m/s^2]。
各量程对应的灵敏度值也可以直接查看产品规格书的 6.1~6.2 节。- 代码实现
#include "tuya_mpu6050.h" /* register map */ #define MPU6050_RA_ACCEL_XOUT_H 0x3B #define MPU6050_RA_GYRO_XOUT_H 0x43 /* unit conversion parameters */ #define ACCEL_OF_G 9.8f #define RPS_TO_DPS 57.3f /* MPU6050 Gyro data type */ typedef BYTE_T MPU_GYRO_DT_E; #define MPU_GDT_RAW 0x00 /* raw data */ #define MPU_GDT_DPS 0x01 /* unit: dps */ #define MPU_GDT_RPS 0x02 /* unit: rps */ /* MPU6050 Accel data type */ typedef BYTE_T MPU_ACCEL_DT_E; #define MPU_ADT_RAW 0x00 /* raw data */ #define MPU_ADT_G 0x01 /* unit: g */ #define MPU_ADT_MPS2 0x02 /* unit: m/s^2 */ /** * @brief read accelerometer data (raw data) * @param[out] a_x: accelerometer data of X-axis * @param[out] a_y: accelerometer data of Y-axis * @param[out] a_z: accelerometer data of Z-axis * @return none */ STATIC VOID_T __read_accel_raw(_OUT SHORT_T *a_x, _OUT SHORT_T *a_y, _OUT SHORT_T *a_z) { UCHAR_T tmp_buf[6]; /* read data from MPU6050 */ __mpu6050_read_data(MPU6050_RA_ACCEL_XOUT_H, 6, tmp_buf); /* get acceleration */ *a_x = ((SHORT_T)tmp_buf[0] << 8) | tmp_buf[1]; *a_y = ((SHORT_T)tmp_buf[2] << 8) | tmp_buf[3]; *a_z = ((SHORT_T)tmp_buf[4] << 8) | tmp_buf[5]; } /** * @brief convert accelerometer data's unit to g * @param[in] data: accelerometer data * @return data in g */ STATIC FLOAT_T __accel_cnv_unit_to_g(_IN CONST SHORT_T data) { FLOAT_T new_data; new_data = (FLOAT_T)data / sg_accel_sens; return new_data; } /** * @brief convert accelerometer data's unit to m/s^2 * @param[in] data: accelerometer data * @return data in mps2 */ STATIC FLOAT_T __accel_cnv_unit_to_mps2(_IN CONST SHORT_T data) { FLOAT_T new_data; new_data = data * ACCEL_OF_G / sg_accel_sens; return new_data; } /** * @brief convert accelerometer data's unit * @param[in] ax: raw data of X-axis * @param[in] ay: raw data of Y-axis * @param[in] az: raw data of Z-axis * @param[out] a_x: converted data of X-axis * @param[out] a_y: converted data of Y-axis * @param[out] a_z: converted data of Z-axis * @param[in] unit: accelerometer unit * @return none */ STATIC VOID_T __cnv_accel_unit(_IN CONST SHORT_T ax, _IN CONST SHORT_T ay, _IN CONST SHORT_T az, _OUT FLOAT_T *a_x, _OUT FLOAT_T *a_y, _OUT FLOAT_T *a_z, _IN CONST MPU_ACCEL_DT_E unit) { if (unit == MPU_ADT_G) { *a_x = __accel_cnv_unit_to_g(ax); *a_y = __accel_cnv_unit_to_g(ay); *a_z = __accel_cnv_unit_to_g(az); } else { *a_x = __accel_cnv_unit_to_mps2(ax); *a_y = __accel_cnv_unit_to_mps2(ay); *a_z = __accel_cnv_unit_to_mps2(az); } } /** * @brief read accelerometer data from MPU6050 (specified unit) * @param[out] a_x: output data of X-axis * @param[out] a_y: output data of Y-axis * @param[out] a_z: output data of Z-axis * @param[in] unit: accelerometer unit * @return none */ VOID_T tuya_mpu6050_read_accel_spec_unit(_OUT FLOAT_T *a_x, _OUT FLOAT_T *a_y, _OUT FLOAT_T *a_z, _IN CONST MPU_ACCEL_DT_E unit) { SHORT_T ax, ay, az; __read_accel_raw(&ax, &ay, &az); __cnv_accel_unit(ax, ay, az, a_x, a_y, a_z, unit); } /** * @brief read gyroscope data (raw data) * @param[out] g_x: gyroscope data of X-axis * @param[out] g_y: gyroscope data of Y-axis * @param[out] g_z: gyroscope data of Z-axis * @return none */ STATIC VOID_T __read_gyro_raw(_OUT SHORT_T *g_x, _OUT SHORT_T *g_y, _OUT SHORT_T *g_z) { UCHAR_T tmp_buf[6]; /* read data from MPU6050 */ __mpu6050_read_data(MPU6050_RA_GYRO_XOUT_H, 6, tmp_buf); /* get angular rate */ *g_x = ((SHORT_T)tmp_buf[0] << 8) | tmp_buf[1]; *g_y = ((SHORT_T)tmp_buf[2] << 8) | tmp_buf[3]; *g_z = ((SHORT_T)tmp_buf[4] << 8) | tmp_buf[5]; } /** * @brief convert gyroscope data's unit to dps * @param[in] data: gyroscope data * @return data in dps */ STATIC FLOAT_T __gyro_cnv_unit_to_dps(_IN CONST SHORT_T data) { FLOAT_T new_data; new_data = data / sg_gyro_sens; return new_data; } /** * @brief convert gyroscope data's unit to rps * @param[in] data: gyroscope data * @return data in rps */ STATIC FLOAT_T __gyro_cnv_unit_to_rps(_IN CONST SHORT_T data) { FLOAT_T new_data; new_data = data / sg_gyro_sens / RPS_TO_DPS; return new_data; } /** * @brief convert gyroscope data's unit * @param[in] gx: raw data of X-axis * @param[in] gy: raw data of Y-axis * @param[in] gz: raw data of Z-axis * @param[out] g_x: converted data of X-axis * @param[out] g_y: converted data of Y-axis * @param[out] g_z: converted data of Z-axis * @param[in] unit: gyroscope unit * @return none */ STATIC VOID_T __cnv_gyro_unit(_IN CONST SHORT_T gx, _IN CONST SHORT_T gy, _IN CONST SHORT_T gz, _OUT FLOAT_T *g_x, _OUT FLOAT_T *g_y, _OUT FLOAT_T *g_z, _IN CONST MPU_GYRO_DT_E unit) { if (unit == MPU_GDT_DPS) { *g_x = __gyro_cnv_unit_to_dps(gx); *g_y = __gyro_cnv_unit_to_dps(gy); *g_z = __gyro_cnv_unit_to_dps(gz); } else { *g_x = __gyro_cnv_unit_to_rps(gx); *g_y = __gyro_cnv_unit_to_rps(gy); *g_z = __gyro_cnv_unit_to_rps(gz); } } /** * @brief read gyroscope data from MPU6050 (specified unit) * @param[out] g_x: output data of X-axis * @param[out] g_y: output data of Y-axis * @param[out] g_z: output data of Z-axis * @param[in] unit: gyroscope unit * @return none */ VOID_T tuya_mpu6050_read_gyro_spec_unit(_OUT FLOAT_T *g_x, _OUT FLOAT_T *g_y, _OUT FLOAT_T *g_z, _IN CONST MPU_GYRO_DT_E unit) { SHORT_T gx, gy, gz; __read_gyro_raw(&gx, &gy, &gz); __cnv_gyro_unit(gx, gy, gz, g_x, g_y, g_z, unit); } /** * @brief sensor new data ready callback * @param[in] none * @return none */ STATIC VOID_T __new_data_ready_cb(VOID_T) { FLOAT_T gyro[3], accel[3]; tuya_mpu6050_read_gyro_spec_unit(gyro, gyro+1, gyro+2, MPU_GDT_DPS); tuya_mpu6050_read_accel_spec_unit(accel, accel+1, accel+2, MPU_ADT_MPS2); }
补充说明
对于角速度和加速度这样的矢量来说，方向的确定尤为重要。
这里引入一个概念叫做右手坐标系，它遵循右手定则，相应地也有遵循左手定则的左手坐标系。右手定则一般可以描述为：以右手握住 z 轴，当右手的四个手指从 x 轴正方向以 90° 转向 y 轴正方向时，大拇指指向就是 z 轴的正方向。（或者：伸出右手，使大拇指与食指垂直，再将中指弯向掌心方向使之与食指垂直；如果此时满足大拇指指向 X 轴正方向，食指指向 Y 轴正方向，中指指向 Z 轴正方向，那么这就是一个右手系。）；确定为右手系之后，用右手握住坐标轴，并竖起大拇指指向该轴正方向，此时其余四指就指向物体绕该轴旋转时的正旋转方向。
下面我们来看一下 MPU6050，如下图所示，当芯片正面放置且芯片上的小圆点在左上角时，Y 轴向前，X 轴向右，Z 轴向上，显然这是一个右手系，图中画出的正旋转方向也符合右手定则。
由于 MPU6050 芯片可能以各种状态被安装在设备上，如果设定的设备方向与芯片默认方向不一致时，就需要重新确认寄存器各项数据实际的含义，编码时需进行一些转换处理。
另外，值得一提的是，InvenSense 还提供了一个嵌入式运动驱动库 Motion Driver，包含 MPU 设备的驱动层程序文件和基于特定芯片平台的示例程序文件，可以在官网下载。使用该驱动库可以快速实现驱动 数字运动处理器 DMP 来获得 四元数。魔法棒的 Demo 程序也提供了使用该驱动库的参考代码，通过使能
tuya_mpu6050.h文件中的INV_MOTION_DRIVER即可切换为使用 DMP 输出的数据。
姿态解算模块
欧拉角 是常用的用于描述 三维空间旋转 的一种方式，简单来说就是通过 3 个角 (α/β/γ 或 φ/θ/ψ) 来表示物体相对于三维直角坐标系的坐标轴的姿态变化，所以也可称之为姿态角。
欧拉角可分为 经典欧拉角 (z-x-z, x-y-x, y-z-y, z-y-z, x-z -x, y-x-y) 和 泰特-布莱恩角 (x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z)，前者在第三次旋转和第一次旋转时使用相同的轴，后者则是绕三个不同的轴旋转。括号内是在不考虑内旋与外旋区别的情况下存在的旋转序列，一共 12 种，使用不同的旋转顺序会得到不同的结果。因此，在给定一组欧拉角表示两个坐标系之间的姿态关系时，一定要同时指定对应的转轴顺序才有意义。
那么什么是内旋和外旋呢？比如，物体原始的坐标轴为 x、y、z，物体先绕 z 轴旋转 α 得到了新的坐标轴 x’、y’、z’，再绕 y’ 轴旋转 β 得到了新坐标轴为 x"、y"、z"，最后绕 x" 轴旋转 γ 达到最终姿态，也就是经过了 z-y’-x" 旋转，这就是一种内在旋转，即绕旋转坐标轴发生的旋转。而 x-y-z 表示的旋转就是一种外在旋转，即绕固定坐标轴发生的旋转。
不同领域定义欧拉角的习惯不同。在航空航天领域，通常将遵循 z-y’-x" 序列(内旋)的三个角定义为 偏航角/航向角(Yaw) - 俯仰角(Pitch) - 横滚角/滚转角(Roll)，而遵循 x-y-z 序列(外旋)的欧拉角也可称为 RPY 角。这是因为，内旋 z(α)-y’(β)-x"(γ) 和 外旋 x(γ)-y(β)-z(α) 达到的效果是一样的，可以通过 旋转矩阵 来证明，这里不做过多介绍。对于 z-y’-x" 定义的姿态角来说，偏航角和横滚角的取值范围可设置为 0~360° 或 -180°~180°，而俯仰角的取值范围必须为 -90°~90°（如果允许俯仰角超过这个范围，会导致同一种姿态可用两套姿态角来表示）。
描述姿态的方式还有轴-角、旋转矩阵(方向余弦矩阵)和四元数，大家可以多多查阅资料来了解它们之间的转换关系和每种方法的优缺点。
姿态角计算
那么如何通过角速度和加速度计算出姿态角呢？在介绍计算方法之前，我们先对系统做如下约定：设定地面坐标系和设备坐标系都以 向前为 X 轴正方向，向左为 Y 轴正方向，向上为 Z 轴正方向，符合右手系。再设定旋转顺序为 z-y’-x"，为方便记忆，将三个欧拉角定义为 y-p-r，即 Yaw、Pitch 、Roll 的首字母。
- 角速度 -> 姿态角
已知，当质点做匀速圆周运动时，将角速度乘以时间就可以知道这段时间的角度变化。那么对于非匀速运动来说，可以通过角速度对时间积分来计算角度变化量；当初始角度确定时，就可以计算出当前角度，即：
$$
\theta(t) = \theta_0 + \int_{0}^{t}\omega(t)dt \
$$
但是，MPU6050 采集的角速度数据是相对于其自身坐标系的某一时刻的瞬时角速度，而事实上姿态更新需要的是相对于地面坐标系的欧拉角，或者说需要的是由一次姿态变化所分解的三次旋转的瞬时角速度。比如，我们将魔法棒朝某个方向甩动一段距离，实际只是发生了一次旋转（绕空间中的某一轴旋转了一个角度，该轴不一定是 XYZ），但是对于欧拉角来说，它会将这次甩动拆分成按照约定顺序（如 z-y’-x"）的三次旋转。因为角速度是矢量，所以可以借助旋转矩阵来将设备坐标系下的角速度投影到地面坐标系中，来实现角速度的转换。以下是即将用到的三个基本旋转矩阵，遵循右手定则，分别表示将向量绕 x 轴、y 轴、z 轴旋转一个角度 θ：
$$
R_x(\theta) = \left[ \begin{matrix}
1 & 0& 0 \
0 & cos\theta & sin\theta \
0 & -sin\theta & cos\theta
\end{matrix} \right] \quadR_y(\theta) = \left[ \begin{matrix}
cos\theta & 0 & -sin\theta \
0 & 1 & 0 \
sin\theta & 0 & cos\theta
\end{matrix} \right] \quadR_z(\theta) = \left[ \begin{matrix}
cos\theta & sin\theta & 0 \
-sin\theta & cos\theta & 0 \
0 & 0 & 1
\end{matrix} \right]
$$
假设设备坐标系一开始与地面坐标系重合，经过 z(y)-y’§-x"® 旋转到当前姿态，用 dy/dt、dp/dt、dr/dt 表示三次旋转的瞬时角速度，则角速度的转换过程为：
① 绕 z 轴旋转 y，用矩阵表示三轴角速度就是 [0, 0, dy/dt]T，简写为 A；
② 绕 y’ 轴旋转 p，相当于 A 左乘旋转矩阵 Ry§ 再叠加 [0, dp/dt, 0]T，简写为 B；
③ 绕 x" 轴旋转 r，相当于 B 左乘旋转矩阵 Rx® 再叠加 [dr/dt, 0, 0]T，简写为 C；
最终得到的 C 就是 陀螺仪的测量值，相当于是三次旋转在各轴上所合成的等效角速度，即：
$$
\left[ \begin{matrix}
\omega_x \
\omega_y \
\omega_z
\end{matrix} \right] =
R_x®R_y§\left[ \begin{matrix}
0 \
0 \
\frac{dy}{dt}
\end{matrix} \right] +
R_x®\left[ \begin{matrix}
0 \
\frac{dp}{dt} \
0
\end{matrix} \right] +
\left[ \begin{matrix}
\frac{dr}{dt} \
0 \
0
\end{matrix} \right] =
\left[ \begin{matrix}
1 & 0 & -sin§ \
0 & cos® & cos§sin® \
0 & -sin® & cos§cos®
\end{matrix} \right]
\left[ \begin{matrix}
\frac{dr}{dt} \
\frac{dp}{dt} \
\frac{dy}{dt}
\end{matrix} \right]
$$
再反解得到姿态更新需要的角速度，也就是求逆矩阵：
$$
\left[ \begin{matrix}
\frac{dr}{dt} \
\frac{dp}{dt} \
\frac{dy}{dt}
\end{matrix} \right] =
\left[ \begin{matrix}
1 & sin®tan§ & cos®tan§ \
0 & cos® & -sin® \
0 & sin®/cos§ & cos®/cos§
\end{matrix} \right]
\left[ \begin{matrix}
\omega_x \
\omega_y \
\omega_z
\end{matrix} \right]
$$而内旋 z(y)-y’§-x"® 等价于外旋 x®-y§-z(y)，所以将上面求得的角速度代入积分方程就能计算出相对于地面坐标系的欧拉角。
- 代码实现
角度的单位有“度”和“弧度”，
math.h提供的三角函数需要传入弧度值，编码时要注意。#include <math.h> #define RAD_TO_DEG 57.3f /** * @brief convert gyro data (intrinsic rotation to extrinsic rotation) * @param[inout] gx: gyro data of X-axis * @param[inout] gy: gyro data of Y-axis * @param[inout] gz: gyro data of Z-axis * @param[in] roll: the angle rotated around the X-axis * @param[in] pitch: the angle rotated around the Y-axis * @param[in] unit: angle unit (TRUE - degree, FALSE - radian) * @return none */ STATIC VOID_T __conv_gyro_intr_to_extr(_INOUT FLOAT_T *gx, _INOUT FLOAT_T *gy, _INOUT FLOAT_T *gz, _IN FLOAT_T roll, _IN FLOAT_T pitch, _IN CONST BOOL_T unit) { FLOAT_T omega_x = *gx; FLOAT_T omega_y = *gy; FLOAT_T omega_z = *gz; if (unit) { roll /= RAD_TO_DEG; pitch /= RAD_TO_DEG; } *gx = omega_x + sin(roll) * tan(pitch) * omega_y + cos(roll) * tan(pitch) * omega_z; *gy = cos(roll) * omega_y - sin(roll) * omega_z; *gz = sin(roll) / cos(pitch) * omega_y + cos(roll) / cos(pitch) * omega_z; } /** * @brief get euler angle * @param[in] dt: smple time * @param[in] gx: gyro data of x-axis * @param[in] gy: gyro data of y-axis * @param[in] gz: gyro data of z-axis * @param[out] roll: the angle rotated around the x-axis * @param[out] pitch: the angle rotated around the y-axis * @param[out] yaw: the angle rotated around the y-axis * @return none */ VOID_T tuya_calc_angles(_IN CONST FLOAT_T dt, _IN FLOAT_T gx, _IN FLOAT_T gy, _IN FLOAT_T gz, _OUT FLOAT_T *roll, _OUT FLOAT_T *pitch, _OUT FLOAT_T *yaw) { __conv_gyro_intr_to_extr(&gx, &gy, &gz, *roll, *pitch, TRUE); *roll += (gx * dt); *pitch += (gy * dt); *yaw += (gz * dt); }- 加速度 -> 姿态角
由于重力的存在，当设备静止时，它必然受到了与重力大小相等，方向相反的力的作用，即设备静止时，z 轴加速度不为 0，其大小等于重力加速度 g，方向为竖直向上。当加速度计旋转到某一个姿态时，重力加速度会在 3 个轴上产生相应的分量，此时加速度计的测量值相当于是向量 [0, 0, g]T 按 z-y’-x" 顺序经过 y-p-r 三次旋转后得到的新向量[ax, ay, az]T，即：
$$
\left[ \begin{matrix}
a_x \
a_y \
a_z
\end{matrix} \right] =
R_x®R_y§R_z(y)\left[ \begin{matrix}
0 \
0 \
g
\end{matrix} \right] =
\left[ \begin{matrix}
-sin§ \
cos§sin® \
cos§cos®
\end{matrix} \right]
g
$$
解方程可以得到 Roll 和 Pitch：
$$
\left{ \begin{aligned}
& \frac{a_y}{a_z} = \frac{cos§sin®g}{cos§sin®g} = \frac{sin®}{cos®} = tan® \
& a_x = -sin§g \
& a_y2+a_z2 = cos2§[sin2®+cos2®]g2 = cos2§g2
\end{aligned} \right. \quad \to \quad
\left{ \begin{aligned}
& r = arctan\frac{a_y}{a_z} \
& p = arctan\frac{-a_x}{\sqrt{a_y2+a_z2}}
\end{aligned} \right.
$$可见，Yaw 无法通过加速度计算，当物体仅绕 Z 轴发生转动时，重力加速度的方向始终不变。
- 代码实现
同样地，
math.h提供的反三角函数输出的是弧度值，注意单位转换问题。#include <math.h> #define RAD_TO_DEG 57.3f /** * @brief get euler angle * @param[in] ax: accel data of x-axis * @param[in] ay: accel data of y-axis * @param[in] az: accel data of z-axis * @param[out] roll: the angle rotated around the x-axis * @param[out] pitch: the angle rotated around the y-axis * @return none */ VOID_T tuya_calc_angles(_IN CONST FLOAT_T ax, _IN CONST FLOAT_T ay, _IN CONST FLOAT_T az, _OUT FLOAT_T *roll, _OUT FLOAT_T *pitch) { *roll = atan2(ay, az) * RAD_TO_DEG; *pitch = atan2(-ax, sqrt(ay*ay + az*az)) * RAD_TO_DEG; }
卡尔曼滤波
现在我们已经知道了如何通过加速度或角速度来计算姿态角，但传感器存在测量误差，且根据上面的介绍可以发现：通过角速度通计算的角度误差会随着时间的推移不断增大，即存在累积误差；而加速度只能在设备静止时计算出较准确的姿态角，即动态响应较差。所以我们还需要通过滤波算法对数据进行融合处理，以获得更精确的姿态数据。对姿态解算而言，常用的滤波算法有一阶互补滤波和卡尔曼滤波，这里我们采用卡尔曼滤波。
卡尔曼滤波（Kalman filter）是一种高效率的递归滤波器，它能从一系列的不完全或包含噪声的测量中，估计动态系统的状态。卡尔曼滤波通过 2 个阶段 来估计动态系统的状态：① 预测 - 使用上一时刻的最优估计值，预测当前时刻的估计值；② 更新 - 利用当前时刻的观测值，优化预测阶段获得的估计值。
- 卡尔曼滤波方程
网络上关于卡尔曼滤波的建模思想和方程推导过程有很多不错的文章和视频教程，大家可以自行查阅进行学习，这里将卡尔曼滤波的五个方程总结如下：
$$
预测方程：
\left{ \begin{aligned}
& {\hat {x}}k^- = A{\hat {x}}{k-1} + Bu_k \
& P_k^- = AP_{k-1}A^T + Q
\end{aligned} \right. \qquad
更新方程：
\left{ \begin{aligned}
& K_k = \frac {P_k^- C^T} {C P_k^- C^T + R} \
& {\hat {x}}_k = {\hat {x}}_k^- + K_k(y_k-C{\hat {x}}_k^-) \
& P_k = (I-K_kC) P_k^-
\end{aligned} \right.
$$方程中的字母含义：
字母 含义 k 时刻 x 状态量，加”^“表示估计值，加”-“表示预测阶段的预估值，初值一般取 0 P 状态估计误差的协方差矩阵，初值一般取 1，不可取 0 u 输入量，即外部控制量，无输入时为 0 A 转移矩阵，表示 x 在没有系统输入影响时从 k-1 到 k 的转移方式 B 控制矩阵，表示 u 如何影响 x Q 过程噪声 w 的协方差矩阵，w~(0, Q) y 观测量，与系统状态 x 存在相关性 K 卡尔曼增益 C 观测矩阵，反映系统状态 x 和观测量 y 之间的关系 R 观测噪声 v 的协方差矩阵，v~(0, R) I 单位矩阵（对角线元素为 1，其余为 0）
- 姿态解算建模与公式推导
下面我们来梳理一下基于角速度和加速度进行姿态解算的卡尔曼滤波器建模过程。
对于姿态解算来说，姿态角的角度是我们最关心的状态量。已知通过角速度 ω 积分可以得到角度 θ，且角速度存在漂移 eω，那么 k 时刻和 k-1 时刻的角度存在如下关系：
$$
\theta_k = \theta_{k-1}+(\omega_k-{e_{\omega}})\Delta t = \theta_{k-1} - {e_{\omega}} \Delta t +\omega_k \Delta t
$$显然角速度就是这个系统的输入量，但和状态方程相比，可以发现还多一个角速度漂移的分式。如果把角速度漂移也作为一个状态量，并假设 k 时刻和 k-1 时刻的角速度漂移是相同的，可以表示为：
$$
{e_{\omega}}{k} = {e{\omega}}{k-1}
$$
将上面两个式子进行拆分，整理成一一对应的形式：
$$
\left{ \begin{aligned}
\theta_k &= 1 * \theta{k-1} - \Delta t * {e_{\omega}}{k-1} + \Delta t * \omega_k \
{e{\omega}}{k} &= 0 * \theta{k-1} + 1 * {e_{\omega}}{k-1} + 0 * \omega_k \
\end{aligned} \right.
$$
我们用矩阵的形式来表示它，就得到了卡尔曼滤波的第一个方程，状态变量 x = (θ, eω)，输入变量 u = ω：
$$
\left[ \begin{matrix}
\theta \
e{\omega}
\end{matrix} \right]k =
\mathop{ \left[ \begin{matrix}
1 & -\Delta t \
0 & 1
\end{matrix} \right]} \limits{(A)}
\left[ \begin{matrix}
\theta \
e_{\omega}
\end{matrix} \right]{k-1} +
\mathop{ \left[ \begin{matrix}
\Delta t \
0
\end{matrix} \right]} \limits{(B)}
\omega_k \
\Updownarrow \
{\hat {x}}k^- = A{\hat {x}}{k-1} + Bu_k
$$
在实际情况下，上面的方程并不能完全成立，因为上一时刻的角度可能存在误差，角速度漂移也存在时间漂移、温度漂移，这些噪声就是过程噪声 w，角度噪声与角速度漂移噪声互相独立，所以 w 的协方差矩阵 Q 可以表示为：
$$
Q = \left[ \begin{matrix}
cov(w_{\theta}, w_{\theta}) & cov(w_{\theta}, w_{e_\omega}) \
cov(w_{e_\omega}, w_{\theta}) & cov(w_{e_\omega}, w_{e_\omega})
\end{matrix} \right] =
\left[ \begin{matrix}
var(w_{\theta}) & 0 \
0 & var(w_{e_\omega})
\end{matrix} \right] =
\left[ \begin{matrix}
Q_{\theta} & 0 \
0 & Q_{e_\omega}
\end{matrix} \right]
$$
将 A 和 Q 代入卡尔曼滤波的第二个方程，就可以得到误差协方差 P 的表达式：
$$
P_k^- = AP_{k-1}A^T + Q \
\Updownarrow \
P_k^- =
\mathop{ \left[ \begin{matrix}
1 & -\Delta t \
0 & 1
\end{matrix} \right]} \limits_{(A)}
P_{k-1}
\mathop{ \left[ \begin{matrix}
1 & 0 \
-\Delta t & 1
\end{matrix} \right]} \limits_{(A^T)} +
\mathop{ \left[ \begin{matrix}
Q_{\theta} & 0 \
0 & Q_{e_\omega}
\end{matrix} \right]} \limits_{(Q)}
$$
显然 P 是一个 2×2 的矩阵，如果将其中的元素用 a-b-c-d 表示，并进行计算，那么可以得到：
$$
\left[ \begin{matrix}
a & b \
c & d
\end{matrix} \right]k^- =
\left[ \begin{matrix}
a - (b+c) \Delta t + d \Delta t^2 + Q{\theta} & b - d \Delta t \
c - d \Delta t & d + Q_{e_\omega}
\end{matrix} \right]{k-1}
$$
以上就是预测阶段的公式推导，得到了姿态角状态的预估值和它的误差协方差矩阵。
接下来看更新阶段，我们还需要一个观测量来修正系统。可以使用加速度计测量值计算的角度 θa 作为观测量，显然它等于 θ 加上传感器误差引起的角度误差 v(θa)，代入到观测方程，就得到了矩阵 C = [1, 0]：
$$
y_k = Cx_k+v
\quad \Leftrightarrow \quad
\left[ \begin{matrix}
\theta_a
\end{matrix} \right]{k} =
\mathop{ \left[ \begin{matrix}
1 & 0 \
\end{matrix} \right]} \limits_{©}
\left[ \begin{matrix}
\theta \
e_{\omega}
\end{matrix} \right]{k} +
v{\theta_a}
$$
而测量噪声协方差 R 可以表示为 v(θa) 的方差，代入第三个方程，可以发现卡尔曼增益是一个 2×1 矩阵：
$$
K_k = \frac {P_k^- C^T} {C P_k^- C^T + R} =
\frac{
\left[ \begin{matrix}
a & b \
c & d
\end{matrix} \right]_k^-
\left[ \begin{matrix}
1 \
0
\end{matrix} \right] }
{ \left[ \begin{matrix}
1 & 0
\end{matrix} \right]
\left[ \begin{matrix}
a & b \
c & d
\end{matrix} \right]k^-
\left[ \begin{matrix}
1 \
0
\end{matrix} \right] +
var({v{\theta}}a) } =
\frac
{ \left[ \begin{matrix}
a \
c
\end{matrix} \right]k^- }
{ a_k^- + R{\theta_a} } \
\downarrow \
K_k = \left[ \begin{matrix}
K_0 \
K_1
\end{matrix} \right]k =
\left[ \begin{matrix}
\frac{a_k-}{a_k-+R{\theta_a}} \
\frac{c_k-}{a_k-+R{\theta_a}}
\end{matrix} \right]
$$
将计算好的卡尔曼增益代入第四个方程，就得到了更新状态估计的表达式：
$$
{\hat {x}}_k = {\hat {x}}k^- + K_k(y_k-C{\hat {x}}k^-) \
\Updownarrow \
\left[ \begin{matrix}
\theta \
e{\omega}
\end{matrix} \right]k =
\left[ \begin{matrix}
\theta \
e{\omega}
\end{matrix} \right]{k}^{-} +
\left[ \begin{matrix}
K_0 \
K_1
\end{matrix} \right]_k
({\theta_a}k - {\theta}{k}^{-}) \
\downarrow \
\left{ \begin{aligned}
& {\theta}_k = {\theta}k^- + K_0({\theta_a}k - {\theta}{k}^{-}) \
& {e\omega}k = {e\omega}_k^- + K_1({\theta_a}k - {\theta}{k}^{-})
\end{aligned} \right.
$$
最后用第五个方程计算更新的协方差矩阵：
$$
P_k = (I-K_kC)P_k^- =
(\left[ \begin{matrix}
1 & 0 \
0 & 1
\end{matrix} \right] -
\left[ \begin{matrix}
K_0 \
K_1
\end{matrix} \right]_k
\left[ \begin{matrix}
1 & 0
\end{matrix} \right])
P_k^- =
\left[ \begin{matrix}
1-K_0 & 0 \
-K_1 & 1
\end{matrix} \right]_k
P_k^-
$$- 代码实现
通过上面的公式推导，按照编程思路将公式整理如下：
① 预测 - 状态估计
$$
{\hat {x}}k^- = A{\hat {x}}{k-1} + Bu_k
\left{ \begin{aligned}
& {\hat {\theta}}k^- = {\hat {\theta}}{k-1} + (\omega_k - {\hat {e_{\omega}}}{k-1}) \Delta t \
& {\hat {e{\omega}}}k^- = {\hat {e{\omega}}}_{k-1}
\end{aligned} \right.
$$② 预测 - 估计误差协方差
$$
P_k^- = AP_{k-1}A^T + Q
\left{ \begin{aligned}
& a_k^- = a_{k-1} - (b_{k-1}+c_{k-1})\Delta t +d_{k-1} {\Delta t}^2 + Q_{\theta} \
& b_k^- = b_{k-1} - d_{k-1} {\Delta t} \
& c_k^- = c_{k-1} - d_{k-1} {\Delta t} \
& d_k^- = d_{k-1} + Q_{e_\omega}
\end{aligned} \right.
$$
③ 更新 - 卡尔曼增益
$$
K_k = \frac {P_k^- C^T} {C P_k^- C^T + R}
\left{ \begin{aligned}
{K_0}k = \frac{a_k-}{a_k-+R{\theta_a}} \
{K_1}k = \frac{c_k-}{a_k-+R{\theta_a}}
\end{aligned} \right.
$$
④ 更新 - 状态估计
$$
{\hat {x}}_k = {\hat {x}}_k^- + K_k(y_k-C{\hat {x}}k^-)
\left{ \begin{aligned}
& {e{\theta_a}}_k = {\theta_a}k - {\hat {\theta}}{k}^{-} \
& {\hat {\theta}}_k = {\hat {\theta}}_k^- + {K_0}k{e{\theta_a}}k \
& {\hat {e\omega}}k = {\hat {e\omega}}_k^- + {K_1}k{e{\theta_a}}_k
\end{aligned} \right.
$$
⑤ 更新 - 估计误差协方差
$$
P_k = (I-K_kC)P_k^-
\left{ \begin{aligned}
& a_k = a_k^- - {K_0}_k a_k^- \
& b_k = b_k^- - {K_0}_k b_k^-\
& c_k = c_k^- - {K_1}_k a_k^- \
& d_k = d_k^- - {K_1}_k b_k^- \
\end{aligned} \right.
$$
现在，我们已经可以快速敲出代码了。需要说明的是，由于采样时间一般很小，所以 ② 式中的二次项dΔt^2一般可以省略。最终编写代码如下：/* 相关参数 */ #define ERR_COV_Q_ANGLE 0.001f /* 过程噪声 w 的协方差 Q - 角度 */ #define ERR_COV_Q_GYRO 0.003f /* 过程噪声 w 的协方差 Q - 角速度漂移(来自陀螺仪) */ #define ERR_COV_R_ACC_ANG 0.5f /* 测量噪声 v 的协方差 R - 角度(来自加速度计) */ #define KF_CH_NUM 2 /* 姿态角通道数(俯仰角或横滚角) */ /* 姿态角通道 */ typedef BYTE_T KF_CH_E; #define KF_CH_PITCH 0x00 /* 俯仰角 */ #define KF_CH_ROLL 0x01 /* 横滚角 */ /* 估计误差协方差矩阵 P 类型 */ typedef struct { FLOAT_T a; /* P[0][0] */ FLOAT_T b; /* P[0][1] */ FLOAT_T c; /* P[1][0] */ FLOAT_T d; /* P[1][1] */ } KF_COV_MT_T; /* 卡尔曼滤波器数据类型 */ typedef struct { FLOAT_T angle; /* 状态量 x0 - 角度 */ FLOAT_T err_gyro; /* 状态量 x1 - 角速度漂移 */ FLOAT_T k0; /* 卡尔曼增益 K0 - 用于角度 */ FLOAT_T k1; /* 卡尔曼增益 K1 - 用于角速度漂移 */ KF_COV_MT_T err_cov; /* 估计误差协方差矩阵 P */ } ANGLE_KF_T; /* 卡尔曼滤波器结构体定义 */ STATIC ANGLE_KF_T sg_angle_kf[KF_CH_NUM] = { { 0.0f, 0.0f, 0.0f, 0.0f, {1.0f, 0.0f, 0.0f, 1.0f} }, { 0.0f, 0.0f, 0.0f, 0.0f, {1.0f, 0.0f, 0.0f, 1.0f} } }; /** * @brief 用于姿态解算的卡尔曼滤波器 * @param[in] dt: 采样时间 * @param[in] acc_ang_m: 由加速度计算的角度 * @param[in] gyro_m: 由陀螺仪测量的角速度 * @param[in] ch: 姿态角通道 * @return none */ STATIC VOID_T __angle_calc_kalman_filter(_IN CONST FLOAT_T dt, _IN CONST FLOAT_T acc_ang_m, _IN CONST FLOAT_T gyro_m, _IN CONST KF_CH_E ch) { KF_COV_MT_T mt_tmp; FLOAT_T err_acc_ang; /* 1. 预测状态估计 */ sg_angle_kf[ch].angle += (gyro_m - sg_angle_kf[ch].err_gyro_m) * dt; /* 2. 预测估计协方差 */ mt_tmp.a = sg_angle_kf[ch].err_cov.a; mt_tmp.b = sg_angle_kf[ch].err_cov.b; mt_tmp.c = sg_angle_kf[ch].err_cov.c; mt_tmp.d = sg_angle_kf[ch].err_cov.d; sg_angle_kf[ch].err_cov.a += ERR_COV_Q_ANGLE - (mt_tmp.b + mt_tmp.c) * dt; sg_angle_kf[ch].err_cov.b -= mt_tmp.d * dt; sg_angle_kf[ch].err_cov.c -= mt_tmp.d * dt; sg_angle_kf[ch].err_cov.d += ERR_COV_Q_GYRO; /* 3. 更新卡尔曼增益 */ sg_angle_kf[ch].k0 = sg_angle_kf[ch].err_cov.a / (sg_angle_kf[ch].err_cov.a + ERR_COV_R_ACC_ANG); sg_angle_kf[ch].k1 = sg_angle_kf[ch].err_cov.c / (sg_angle_kf[ch].err_cov.a + ERR_COV_R_ACC_ANG); /* 4. 更新状态估计 */ err_acc_ang = acc_ang_m - sg_angle_kf[ch].angle; sg_angle_kf[ch].angle += sg_angle_kf[ch].k0 * err_acc_ang; sg_angle_kf[ch].err_gyro += sg_angle_kf[ch].k1 * err_acc_ang; /* 5. 更新估计协方差 */ mt_tmp.a = sg_angle_kf[ch].err_cov.a; mt_tmp.b = sg_angle_kf[ch].err_cov.b; sg_angle_kf[ch].err_cov.a -= sg_angle_kf[ch].k0 * mt_tmp.a; sg_angle_kf[ch].err_cov.c -= sg_angle_kf[ch].k1 * mt_tmp.b; sg_angle_kf[ch].err_cov.b -= sg_angle_kf[ch].k0 * mt_tmp.a; sg_angle_kf[ch].err_cov.d -= sg_angle_kf[ch].k1 * mt_tmp.b; }结合之前的姿态角计算方法，基于卡尔曼滤波器的姿态解算函数可整合如下：
/** * @brief get euler angles * @param[in] dt: smple time * @param[in] type: angle type * @param[in] gx: gyro data of X-axis * @param[in] gy: gyro data of Y-axis * @param[in] gz: gyro data of Z-axis * @param[in] ax: accel data of X-axis * @param[in] ay: accel data of Y-axis * @param[in] az: accel data of Z-axis * @param[out] roll: the angle rotated around the X-axis * @param[out] pitch: the angle rotated around the Y-axis * @param[out] yaw: the angle rotated around the Z-axis * @return none */ VOID_T tuya_calc_angles(_IN CONST FLOAT_T dt, _IN CONST BOOL_T type, _IN FLOAT_T gx, _IN FLOAT_T gy, _IN FLOAT_T gz, _IN CONST FLOAT_T ax, _IN CONST FLOAT_T ay, _IN CONST FLOAT_T az, _OUT FLOAT_T *roll, _OUT FLOAT_T *pitch, _OUT FLOAT_T *yaw) { FLOAT_T acc_roll_m, acc_pitch_m, tmp_yaw; if (!type) { acc_roll_m = atan2(-ay, az) * RAD_TO_DEG; acc_pitch_m = atan2(ax, az) * RAD_TO_DEG; } else { acc_roll_m = atan2(ay, az) * RAD_TO_DEG; acc_pitch_m = atan2(-ax, sqrt(ay*ay + az*az)) * RAD_TO_DEG; __conv_gyro_intr_to_extr(&gx, &gy, &gz, *roll, *pitch, TRUE); } __angle_calc_kalman_filter(dt, acc_roll_m, gx, KF_CH_ROLL); *roll = sg_angle_kf[KF_CH_ROLL].angle; __angle_calc_kalman_filter(dt, acc_pitch_m, gy, KF_CH_PITCH); *pitch = sg_angle_kf[KF_CH_PITCH].angle; tmp_yaw = *yaw; tmp_yaw += (gz * dt); if (tmp_yaw > 180) { tmp_yaw -= 360; } else if (tmp_yaw <= -180) { tmp_yaw += 360; } else { ; } *yaw = tmp_yaw; }过程噪声的协方差 Q 和测量噪声的协方差 R 需要人工整定。大家可以修改参数来感受一下这些值对滤波效果会产生什么样的影响。另外，我们可以看到代码中偏航角计算只使用了角速度数据，前文也提到了这是因为无法通过加速度来计算出偏航角，那么对于磁场干扰不强的场合，可以尝试增加磁力计来修正偏航角。
另外，Demo 程序中提供了四元数姿态更新算法的参考代码，功能启用宏为
ANGLE_CALC_BY_QUAT。
手势识别模块
手势识别，简单来说就是找到各个手势信号的变化规律，然后从采集到的手势数据中找到符合规律的数据，从而达到识别的目的。因此，建议您在开发时准备一个串口数据波形显示工具，以便能更直观地感受数据变化。
手势信号提取
- 方案说明
在识别具体手势之前，需要先确定是否发生了预期的动作，比如从桌上拿起或放下魔法棒时，相关数据也会发生变化，需要排除这些干扰数据。从理论上来说，当我们挥动魔法棒时，相当于是对魔法棒施加力的过程，根据牛顿第二定律可知，力的作用会表现在物体运动的加速度上。实验证明，当我们快速挥动魔法棒时，加速度数据会发生剧烈变化，朝不同的方向甩动时，各轴的加速度变化情况也不同。因此，可以用 3 轴的加速度差分绝对值之和作为手势起终点判断的特征量。该特征量大于设定阈值时，判定为手势起点；小于设定阈值时，判定为手势终点。
同样地，当我们以手臂为中心轴翻转魔法棒时，加速度的数据变化情况也符合预期。
- 代码实现
以下函数用于计算加速度值的差分绝对值之和，并进行移动平均滤波。
#define ACCEL_AXIS_NUM 3 #define DATA_SMP_NUM 8 STATIC FLOAT_T accel_last[ACCEL_AXIS_NUM]; STATIC FLOAT_T sg_accel_diff_sum[DATA_SMP_NUM]; /** * @brief calculate the sum of the absolute value of the acceleration difference * @param[in] accel_cur: current acceleration * @return the calculation result */ FLOAT_T __calc_accel_diff_abs_sum(FLOAT_T *accel_cur) { UCHAR_T i; FLOAT_T diff = 0.0f; FLOAT_T diff_sum = 0.0f; FLOAT_T ret = 0.0f; for (i = 0; i < ACCEL_AXIS_NUM; i++) { diff = accel_cur[i] - accel_last[i]; diff_sum += ((diff > 0) ? diff : (-diff)); accel_last[i] = accel_cur[i]; } for (i = 0; i < DATA_SMP_NUM-1; i++) { sg_accel_diff_sum[i] = sg_accel_diff_sum[i+1]; ret += sg_accel_diff_sum[i]; } sg_accel_diff_sum[DATA_SMP_NUM-1] = diff_sum; ret = (ret + sg_accel_diff_sum[DATA_SMP_NUM-1]) / DATA_SMP_NUM; return ret; }以下是手势识别功能的入口函数，先确定手势起终点。如果确定为起点，开始记录加速度、角速度和欧拉角数据；如果确定为终点，停止记录，并对保存的手势数据进行处理。
#define GYRO_AXIS_NUM 3 #define ACCEL_AXIS_NUM 3 #define EULER_ANGLE_NUM 3 #define BUFFER_SIZE 100 #define GES_DATA_VALID_THR 5 STATIC FLOAT_T sg_gyro_buf[BUFFER_SIZE][GYRO_AXIS_NUM]; STATIC FLOAT_T sg_accel_buf[BUFFER_SIZE][ACCEL_AXIS_NUM]; STATIC FLOAT_T sg_roll_buf[BUFFER_SIZE]; STATIC FLOAT_T sg_pitch_buf[BUFFER_SIZE]; STATIC FLOAT_T sg_yaw_buf[BUFFER_SIZE]; STATIC BOOL_T sg_ges_valid = FALSE; STATIC FLOAT_T sg_accel_d_s = 0; STATIC UCHAR_T sg_data_index = 0; /** * @brief recognize gesture * @param[in] gyro: gyro data * @param[in] accel: accel data * @param[in] angle: angle data * @return gesture code */ GES_CODE_E tuya_rec_gesture(FLOAT_T *gyro, FLOAT_T *accel, FLOAT_T *angle) { GES_CODE_E ret = GES_NONE; UCHAR_T i; sg_accel_d_s = __calc_accel_diff_abs_sum(accel); if (!sg_ges_valid) { if (sg_accel_d_s >= GES_DATA_VALID_THR) { sg_ges_valid = TRUE; sg_data_index = 0; } } else { if (sg_accel_d_s < GES_DATA_VALID_THR) { sg_ges_valid = FALSE; ret = __rec_gesture(); } } if (sg_ges_valid) { for (i = 0; i < 3; i++) { sg_gyro_buf[sg_data_index][i] = gyro[i]; sg_accel_buf[sg_data_index][i] = accel[i]; } sg_roll_buf[sg_data_index] = angle[0]; sg_pitch_buf[sg_data_index] = angle[1]; sg_yaw_buf[sg_data_index] = angle[2]; sg_data_index++; if (sg_data_index >= BUFFER_SIZE) { sg_data_index = 0; } } return ret; }手势类型判断
- 方案说明
在获得了有效手势数据之后，我们还可以对一些有相似特征的手势进行归类，这样就可以使用类似的识别算法进一步区分同一类手势，这也取决于开发者具体想要识别哪些手势，比如我们要识别的上下甩动和左右甩动就都可以归类为甩动类。当然，不具备相似特征的手势也可以单独处理。
对于甩动类手势而言，其特征表现为手势长度较长且加速度变化比较剧烈。可以简单以采样点个数作为手势长度，而加速度的变化情况，可以通过对提取到的有效手势数据的“3 轴差分绝对值之和”再求和来表示。
- 代码实现
#define ACCEL_AXIS_NUM 3 #define BUFFER_SIZE 100 #define GES_SHAKE_NRG_THR 2000.0f #define GES_SHAKE_LEN_THR 20 typedef BYTE_T GES_TYPE_E; #define GES_TYPE_NONE 0x00 #define GES_TYPE_SHAKE 0x01 STATIC FLOAT_T sg_accel_buf[BUFFER_SIZE][ACCEL_AXIS_NUM]; /** * @brief get gesture length * @param[in] none * @return gesture length */ UCHAR_T __get_ges_len(VOID_T) { return sg_data_index; } /** * @brief calculate the total amount of change in acceleration data * @param[in] none * @return the calculation result */ FLOAT_T __calc_accel_total_change(VOID_T) { UCHAR_T i, j; FLOAT_T diff = 0.0f; FLOAT_T diff_sum = 0.0f; for (i = 1; i < sg_data_index; i++) { for (j = 0; j < ACCEL_AXIS_NUM; j++) { diff = sg_accel_buf[i][j] - sg_accel_buf[0][j]; diff_sum += ((diff > 0) ? diff : (-diff)); } } return diff_sum; } /** * @brief judge the type of gesture * @param[in] none * @return gesture type */ GES_TYPE_E __judge_ges_type(VOID_T) { GES_TYPE_E type = GES_TYPE_NONE; UCHAR_T len = __get_ges_len(); FLOAT_T accel_change = __calc_accel_total_change(); if ((accel_change >= GES_SHAKE_NRG_THR) && (len >= GES_SHAKE_LEN_THR)) { type = GES_TYPE_SHAKE; } else { } return type; } /** * @brief recognize gesture * @param[in] none * @return gesture code */ GES_CODE_E __rec_gesture(VOID_T) { GES_CODE_E ret = GES_NONE; switch (__judge_ges_type()) { case GES_TYPE_SHAKE: ret = __rec_shake_gesture(); break; default: break; } return ret; }甩动手势识别
- 方案说明
甩动手势分为上下甩动和左右甩动，上下甩动主要体现在俯仰角的变化上，左右甩动主要体现在偏航角的变化上，如下图所示。那么我们就可以通过比较俯仰角和偏航角的变化量的大小来区分上下甩动还是左右甩动。
确认了是上下甩动还是左右甩动之后，我们还需要判断甩动具体方向。由上图可见，向上甩动时，俯仰角逐渐减小，向下甩动时，俯仰角逐渐增大，那么通过判断这个变化趋势就可以确定方向。
- 代码实现
FLOAT_T __get_angle_dir_feat(FLOAT_T *angle, UCHAR_T len) { UCHAR_T i; FLOAT_T diff = 0.0f; FLOAT_T dir_feat = 0.0f; for (i = 1; i < len; i++) { diff = angle[i] - angle[i-1]; if ((diff > 300) || (diff <- 300)) { dir_feat += ((diff > 0) ? (diff - 360) : (diff + 360)); } else { dir_feat += diff; } } return dir_feat; } /** * @brief recognize the direction of the shaking gesture * @param[in] none * @return gesture code */ GES_CODE_E __rec_shake_gesture(VOID_T) { GES_CODE_E ret = GES_NONE; FLOAT_T pitch_d_a_s = __calc_angle_diff_abs_sum(sg_pitch_buf, sg_data_index); FLOAT_T yaw_d_a_s = __calc_angle_diff_abs_sum(sg_yaw_buf, sg_data_index); if (pitch_d_a_s > yaw_d_a_s) { if (__get_angle_dir_feat(sg_pitch_buf, sg_data_index) < 0) { ret = GES_SHAKE_UP; TUYA_APP_LOG_DEBUG("Gesture: up"); } else { ret = GES_SHAKE_DOWN; TUYA_APP_LOG_DEBUG("Gesture: down"); } } else { if (__get_angle_dir_feat(sg_yaw_buf, sg_data_index) > 0) { ret = GES_SHAKE_LEFT; TUYA_APP_LOG_DEBUG("Gesture: left"); } else { ret = GES_SHAKE_RIGHT; TUYA_APP_LOG_DEBUG("Gesture: right"); } } return ret; }联网处理模块
配网处理
- 处理流程
根据功能定义中对配网状态重置、配网等待时限和配网状态指示的描述，可知配网处理流程如下：
- 代码实现
首先编写初始化函数，如下所示。
tuya_key.h和tuya_led.h是单独编写的针对按键和 LED 的驱动组件，具体内容可参考完整 Demo 代码。蓝牙连接状态通过调用接口 tuya_ble_connect_status_get 获取。1 分钟定时通过调用软件定时器实现，需要包含tuya_ble_port.h文件，接口函数说明请参考 tuya_ble_timer。#include "tuya_net_proc.h" #include "tuya_ble_port.h" #include "tuya_key.h" #include "tuya_led.h" /* network peripherals */ #define NET_KEY_PIN TY_GPIO_5 #define NET_LED_PIN TY_GPIO_12 #define NET_LED_FLASH_INTV_MS 300 /* timer: 1min */ #define WAIT_BIND_TIME_MS (1*60*1000) /* bind flag */ #define F_BLE_BOUND sg_net_proc_flag.bit0 #define F_WAIT_BINDING sg_net_proc_flag.bit1 typedef BYTE_T NET_LED_STAT; #define NET_LED_OFF 0x00 #define NET_LED_ON 0x01 #define NET_LED_FLASH_QUICK 0x02 STATIC FLAG_BIT sg_net_proc_flag; STATIC LED_HANDLE sg_net_led_handle = NULL; STATIC NET_LED_STAT sg_net_led_status = NET_LED_OFF; STATIC NET_LED_STAT sg_net_led_status_last = NET_LED_OFF; STATIC KEY_DEF_T sg_key_def_s; STATIC tuya_ble_timer_t wait_bind_timer; STATIC VOID_T __net_key_cb(KEY_PRESS_TYPE_E type); STATIC VOID_T __wait_bind_timer_cb(VOID_T); /** * @brief network key init * @param[in] none * @return none */ VOID_T __net_key_init(VOID_T) { KEY_RET ret; sg_key_def_s.port = NET_KEY_PIN; sg_key_def_s.active_low = TRUE; sg_key_def_s.long_press_time1 = 3000; sg_key_def_s.long_press_time2 = 0; sg_key_def_s.key_cb = __net_key_cb; ret = tuya_reg_key(&sg_key_def_s); if (KEY_OK != ret) { TUYA_APP_LOG_ERROR("Network key init error: %d.", ret); } } /** * @brief set network led status * @param[in] status: network led status * @return none */ STATIC VOID_T __set_net_led_status(NET_LED_STAT status) { if (NET_LED_FLASH_QUICK == status) { sg_net_led_status_last = sg_net_led_status; } sg_net_led_status = status; switch (status) { case NET_LED_OFF: TUYA_APP_LOG_DEBUG("Net led is light off."); tuya_set_led_light(sg_net_led_handle, FALSE); break; case NET_LED_ON: TUYA_APP_LOG_DEBUG("Net led is light on."); tuya_set_led_light(sg_net_led_handle, TRUE); break; case NET_LED_FLASH_QUICK: TUYA_APP_LOG_DEBUG("Net led is flashing."); tuya_set_led_flash(sg_net_led_handle, LFM_FOREVER, LFT_STA_ON_END_ON, NET_LED_FLASH_INTV_MS, NET_LED_FLASH_INTV_MS, 0, NULL); break; default: break; } } /** * @brief network led init * @param[in] none * @return none */ VOID_T __net_led_init(VOID_T) { LED_RET ret = tuya_create_led_handle(NET_LED_PIN, TRUE, &sg_net_led_handle); if (LED_OK != ret) { TUYA_APP_LOG_ERROR("Network led init err:%d.", ret); } } /** * @brief network process init * @param[in] none * @return none */ VOID_T tuya_net_proc_init(VOID_T) { tuya_ble_connect_status_t ble_conn_sta; ble_conn_sta = tuya_ble_connect_status_get(); TUYA_APP_LOG_DEBUG("BLE connect status: %d.", ble_conn_sta); __net_key_init(); __net_led_init(); tuya_ble_timer_create(&wait_bind_timer, WAIT_BIND_TIME_MS, TUYA_BLE_TIMER_SINGLE_SHOT, __wait_bind_timer_cb); if ((ble_conn_sta == BONDING_UNCONN) || (ble_conn_sta == BONDING_CONN) || (ble_conn_sta == BONDING_UNAUTH_CONN)) { F_BLE_BOUND = SET; F_WAIT_BINDING = CLR; __set_net_led_status(NET_LED_OFF); } else { F_BLE_BOUND = CLR; F_WAIT_BINDING = SET; __set_net_led_status(NET_LED_FLASH_QUICK); tuya_ble_timer_start(wait_bind_timer); } }1 分钟定时回调时，通过关闭蓝牙广播来禁止用户绑定设备。
ty_ble_start_adv和ty_ble_stop_adv分别为打开蓝牙广播和关闭蓝牙广播的接口函数，需要包含ty_ble.h文件。#include "tuya_net_proc.h" #include "tuya_led.h" #include "ty_ble.h" /** * @brief prohibit users to bind * @param[in] none * @return none */ VOID_T __prohibit_binding(VOID_T) { F_WAIT_BINDING = CLR; ty_ble_stop_adv(); __set_net_led_status(sg_net_led_status_last); } /** * @brief wait for binding timer callback * @param[in] none * @return none */ STATIC VOID_T __wait_bind_timer_cb(VOID_T) { __prohibit_binding(); }按键长按 3 秒事件发生时，重置设备并打开蓝牙广播来等待用户绑定。重置设备通过调用
tuya_ble_api.h文件中定义的函数 tuya_ble_device_unbind 实现。#include "tuya_net_proc.h" #include "tuya_ble_log.h" #include "tuya_ble_port.h" #include "tuya_ble_api.h" #include "tuya_key.h" #include "tuya_led.h" #include "ty_ble.h" /** * @brief allow users to bind * @param[in] none * @return none */ VOID_T __allow_binding(VOID_T) { if (F_WAIT_BINDING) { return; } if (F_BLE_BOUND) { tuya_ble_device_unbind(); } F_WAIT_BINDING = SET; ty_ble_start_adv(); __set_net_led_status(NET_LED_FLASH_QUICK); tuya_ble_timer_start(wait_bind_timer); } /** * @brief network key callback * @param[in] type: key event type * @return none */ STATIC VOID_T __net_key_cb(KEY_PRESS_TYPE_E type) { switch (type) { case SHORT_PRESS: break; case LONG_PRESS_FOR_TIME1: __allow_binding(); break; case LONG_PRESS_FOR_TIME2: break; default: break; } }另外，设备连接时和设备被解绑时还需做如下处理：
/** * @brief ble connected process * @param[in] none * @return none */ VOID_T tuya_net_proc_ble_conn(VOID_T) { if (F_WAIT_BINDING == SET) { F_BLE_BOUND = SET; F_WAIT_BINDING = CLR; tuya_ble_timer_stop(wait_bind_timer); __set_net_led_status(sg_net_led_status_last); } } /** * @brief ble unbound process * @param[in] none * @return none */ VOID_T tuya_net_proc_ble_unbound(VOID_T) { F_BLE_BOUND = CLR; ty_ble_stop_adv(); }
数据上报
- 上报机制
完成手势识别后，可以将手势数据上报至云端，来实现 APP 数据显示和场景联动。上报机制设定如下：
No. 上报节点 上报功能点 备注 1 收到 APP 的查询请求时 所有功能点 即手势功能点 2 设备手势数据更新时 手势功能点 手势识别成功时、手势数据恢复默认时 - 代码实现
数据上报调用的接口是
tuya_ble_dp_data_send，需要包含tuya_ble_api.h文件。DP 上报协议和接口函数说明请查看 tuya_ble_dp_data_send。#include "tuya_net_proc.h" #include "tuya_ble_api.h" /* DP ID */ #define DP_ID_GESTURE 101 /* DP data index */ #define DP_DATA_INDEX_OFFSET_ID 0 #define DP_DATA_INDEX_OFFSET_TYPE 1 #define DP_DATA_INDEX_OFFSET_LEN_H 2 #define DP_DATA_INDEX_OFFSET_LEN_L 3 #define DP_DATA_INDEX_OFFSET_DATA 4 /* DP report */ STATIC UCHAR_T sg_repo_array[255+3]; STATIC UCHAR_T sg_dp_gesture = 0; extern UINT_T g_sn; /** * @brief report one dp data * @param[in] dp_id: DP ID * @param[in] dp_type: DP type * @param[in] dp_len: DP length * @param[in] dp_data: DP data address * @return none */ STATIC VOID_T __report_one_dp_data(_IN CONST UCHAR_T dp_id, _IN CONST UCHAR_T dp_type, _IN CONST USHORT_T dp_len, _IN CONST UCHAR_T *dp_data) { USHORT_T i; sg_repo_array[DP_DATA_INDEX_OFFSET_ID] = dp_id; sg_repo_array[DP_DATA_INDEX_OFFSET_TYPE] = dp_type; sg_repo_array[DP_DATA_INDEX_OFFSET_LEN_H] = (UCHAR_T)(dp_len >> 8); sg_repo_array[DP_DATA_INDEX_OFFSET_LEN_L] = (UCHAR_T)dp_len; for (i = 0; i < dp_len; i++) { sg_repo_array[DP_DATA_INDEX_OFFSET_DATA + i] = *(dp_data + (dp_len-i-1)); } tuya_ble_dp_data_send(g_sn++, DP_SEND_TYPE_ACTIVE, DP_SEND_FOR_CLOUD_PANEL, DP_SEND_WITHOUT_RESPONSE, sg_repo_array, dp_len + DP_DATA_INDEX_OFFSET_DATA); } /** * @brief add one dp data * @param[in] dp_id: DP ID * @param[in] dp_type: DP type * @param[in] dp_len: DP length * @param[in] dp_data: DP data address * @param[in] addr: DP report address * @return total length */ STATIC UCHAR_T __add_one_dp_data(_IN CONST UCHAR_T dp_id, _IN CONST UCHAR_T dp_type, _IN CONST USHORT_T dp_len, _IN CONST UCHAR_T *dp_data, _IN UCHAR_T *addr) { USHORT_T i; *(addr + DP_DATA_INDEX_OFFSET_ID) = dp_id; *(addr + DP_DATA_INDEX_OFFSET_TYPE) = dp_type; *(addr + DP_DATA_INDEX_OFFSET_LEN_H) = (UCHAR_T)(dp_len >> 8); *(addr + DP_DATA_INDEX_OFFSET_LEN_L) = (UCHAR_T)dp_len; for (i = 0; i < dp_len; i++) { *(addr + DP_DATA_INDEX_OFFSET_DATA + i) = *(dp_data + (dp_len-i-1)); } return (dp_len + DP_DATA_INDEX_OFFSET_DATA); } /** * @brief report all dp data * @param[in] none * @return none */ STATIC VOID_T __report_all_dp_data(VOID_T) { UINT_T total_len = 0; total_len += __add_one_dp_data(DP_ID_GESTURE, DT_ENUM, 1, &sg_dp_gesture, sg_repo_array); tuya_ble_dp_data_send(g_sn++, DP_SEND_TYPE_ACTIVE, DP_SEND_FOR_CLOUD_PANEL, DP_SEND_WITHOUT_RESPONSE, sg_repo_array, total_len); } /** * @brief report gesture result * @param[in] gesture: gesture code * @return none */ VOID_T tuya_report_gesture(UCHAR_T gesture) { sg_dp_gesture = gesture; __report_one_dp_data(DP_ID_GESTURE, DT_ENUM, 1, &sg_dp_gesture); } /** * @brief report all dp data * @param[in] none * @return none */ VOID_T tuya_report_all_dp_data(VOID_T) { __report_all_dp_data(); } /** * @brief ble dp query handler * @param[in] none * @return none */ VOID_T tuya_app_ble_dp_query_handler(VOID_T) { /* report initial value */ __report_all_dp_data(); }
其他功能模块
手电筒功能
- 处理内容
手电筒功能比较简单，在配网键短按事件发生时切换指示灯的状态即可。但考虑到配网指示优先级较高，所以当指示灯快闪时，不处理开/关灯的请求。
- 代码实现
/** * @brief set net led turn on or off * @param[in] none * @return none */ VOID_T __set_net_led_power(VOID_T) { if (NET_LED_FLASH_QUICK == sg_net_led_status) { return; } if (NET_LED_OFF == sg_net_led_status) { __set_net_led_status(NET_LED_ON); } else { __set_net_led_status(NET_LED_OFF); } } /** * @brief network key callback * @param[in] type: key event type * @return none */ STATIC VOID_T __net_key_cb(KEY_PRESS_TYPE_E type) { switch (type) { case SHORT_PRESS: __set_net_led_power(); break; case LONG_PRESS_FOR_TIME1: __allow_binding(); break; case LONG_PRESS_FOR_TIME2: break; default: break; } }
低功耗处理
- 低功耗模式
nRF52832 芯片有 SYSTOM ON 和 SYSTEM OFF 两种模式，相关说明可以参考这篇文章：NRF52832 学习笔记（3——低功耗实现 。这里选用 SYSTEM OFF 模式，在设备状态判断为未使用时，关闭已开启的外设，并设置唤醒引脚，最后调用
nrf_pwr_mgmt_shutdown函数进入休眠。设备未使用状态的判断可以通过陀螺仪 3 轴数据连续一段时间内均小于设定阈值来判定。（等待用户绑定时不进行休眠判断）- 代码实现
#define UNUSED_THR 5 #define UNUSED_TIME 6000 /* 5ms * 6000 = 30s */ STATIC UINT_T sg_wakeup_pin[2] = {TY_GPIO_4, TY_GPIO_5}; /** * @brief putting the chip into sleep mode * @param[in] wakeup_pin: wake-up pin * @param[in] cnt: wake-up pin number * @return none */ VOID_T tuya_enter_sleep_mode(UINT_T *wakeup_pin, UCHAR_T cnt) { /* prepare wakeup buttons */ for (UCHAR_T i = 0; i < cnt; i++) { nrf_gpio_cfg_input(wakeup_pin[i], NRF_GPIO_PIN_PULLUP); nrf_gpio_pin_sense_t sense = NRF_GPIO_PIN_SENSE_LOW; nrf_gpio_cfg_sense_set(wakeup_pin[i], sense); } /* go to system-off mode */ nrf_pwr_mgmt_shutdown(NRF_PWR_MGMT_SHUTDOWN_GOTO_SYSOFF); } /** * @brief close all peripheral * @param[in] none * @return none */ VOID_T __close_peripheral(VOID_T) { tuya_gpio_close_all(); ty_rtc_uninit(); ty_uart_uninit(); } /** * @brief device sleep * @param[in] none * @return none */ VOID_T __gesture_controller_sleep(VOID_T) { if (!tuya_ble_sleep_allowed_check()) { return; } tuya_close_imu_daq(); tuya_net_proc_before_sleep(); __close_peripheral(); tuya_enter_sleep_mode(sg_wakeup_pin, 2); } /** * @brief is device unused * @param[in] none * @return TRUE - unused, FALSE - using */ STATIC BOOL_T __is_device_unused(VOID_T) { STATIC USHORT_T s_tm = 0; if (tuya_is_wait_bind()) { return FALSE; } if ((sg_ges_data.gyro[0] < UNUSED_THR) && (sg_ges_data.gyro[1] < UNUSED_THR) && (sg_ges_data.gyro[2] < UNUSED_THR)) { s_tm++; if (s_tm >= UNUSED_TIME) { return TRUE; } } return FALSE; } /** * @brief gesture controller loop * @param[in] none * @return none */ VOID_T tuya_gesture_controller_loop(VOID_T) { if (sg_new_data_ready) { sg_new_data_ready = CLR; /* ... */ if (__is_device_unused()) { __gesture_controller_sleep(); } /* ... */ } }
指引
如果您还不太清除如何入手软件开发，这里给大家整理了一下针对魔法棒的开发思路：
No. 内容 1 搭建好软件开发环境，包括调试环境 2 确认要实现哪些功能，虽然可以后面继续追加，但最好先确定一个初期目标 3 对这些功能进行模块划分，理清模块之间的关系 4 确定软件的框架、总体流程、工作时序等 5 实现按键、指示灯、MPU6050 等外设驱动部分 6 对旋转表式和姿态解算相关知识进行学习，选择合适的姿态解算方案 7 观察并总结手势数据变化规律，实现识别算法 8 学会设备配网、数据上报、APP 操作等，实现魔法棒对子设备的控制
该内容对您有帮助吗？
是我要提建议
