智能魔法棒（手势控制器）———嵌入式篇
前篇硬件和结构已经准备完成，接下来就要进行嵌入式部分的开发1 环境搭建1.1 产品创建 在进行魔法棒的固件开发之前，我们需要先在 涂鸦IoT平台 上创建一个智能产品，还不熟悉产品创建的同学可以通过涂鸦开发者平台的文档中心进行了解，详情点击【快速入门】。 魔法棒产品可以选择【找不到品类】进行自定义创建。本 Demo 选用的是涂鸦 Bluetooth LE 模组 TYBN1，其芯片平台是 nRF5
在《 智能魔法棒———硬件篇 》中已经为大家介绍了魔法棒的硬件方案和结构设计，接下来介绍一下嵌入式软件设计方案。
注：本文涉及了有关 MPU6050 的使用方法、姿态解算方法、卡尔曼滤波建模及公式推导过程的介绍，有兴趣的同学可以通过目录快速定位到相关内容进行参考。
1 环境搭建
1.1 产品创建
在进行魔法棒的固件开发之前，我们需要先在 涂鸦IoT平台 上创建一个智能产品，还不熟悉产品创建的同学可以通过涂鸦开发者平台的文档中心进行了解，详情点击【快速入门】。
魔法棒产品可以选择【找不到品类】进行自定义创建。本 Demo 选用的是涂鸦 Bluetooth LE 模组 TYBN1，其芯片平台是 nRF52832 。因此，选择通讯协议为【蓝牙】，云端接入方式为【TuyaOS】，云端接入硬件为【Nordic nRF52832】。当然也可以选择其他模组，可以参考 涂鸦云模组规格书 进行选型。比如选择了 BP3L 模组，那么在【硬件开发】页面选择“云端接入硬件”时选择【BP3L 模组】即可。
为实现云端接收魔法棒识别结果的目的，需在【功能定义】页面自定义一个DP点，参数如下：
| DP ID | 功能点名称 | 标识符 | 数据传输类型 | 数据类型 | 功能点属性 |
|---|---|---|---|---|---|
| 101 | 手势 | gesture | 只上报 (ro) | 枚举型 (Enum) | 枚举值：none，ges1，ges2 … |
1.2 SDK 获取
涂鸦提供了适用于涂鸦云模组各个芯片平台的 SDK，可以在魔法棒产品的 硬件开发 页面的左下角找到最新版的 SDK 。
下载完成后，请先查看 SDK 中的 README_zh-CN.md 文件！以 nRF52832 平台为例，文件中提示了还需下载原厂 nRF5 SDK 15.3.0 文件。选择 15.3.0 版本，SoftDevices 选择版本介绍中涉及 nRF52832 的即可，最后点击 Download files (.zip)。原厂 SDK 下载完成并解压后，需将涂鸦 SDK 文件 tuya-ble-sdk-demo-project-nrf52832 拷贝到 nRF5_SDK_15.3.0_59ac345\examples\ble_peripheral目录下。涂鸦 SDK 提供了各种 API 来帮助开发者快速实现产品功能，开发指南请参考 蓝牙 SDK 开发。
1.3 IDE 准备
nRF52832 平台使用的 IDE 是 MDK-ARM Keil µVision ，还未安装的同学可以前往官网下载。安装好 IDE 后，双击涂鸦 SDK 中的工程文件ble_app_uart_pca10040_s132.uvprojx，打开时会自动安装软件包。如果安装失败，可以在 Pack Installer 中找到 nRF52832 芯片对应的软件包进行安装，如下图所示：
1.4 烧录授权
- 模组+授权码购买
涂鸦云模组可以在【硬件开发】页面购买。需要先【新增自定义固件】（开发前可以使用 SDK Demo 中的默认固件），然后点击右侧的【立即购买】 进入 模组+授权码 购买流程。涂鸦提供多种烧录授权方式，可根据不同的开发阶段、芯片类型和生产方式进行选择，详情点击 固件烧录授权。（BN 系列模组烧录授权、PHY6222芯片烧录授权）
- 仅授权码购买
如果已有模组或开发板，也可以只购买授权码。可通过左侧导航栏的 采购 - 采购商品 - 生产研发采购 中进入 通用备货采购 页面，选择 授权码-通用版 进行购买。新用户也可以免费领取 2 个授权码：
调试阶段可以选择交付形式为 【授权码清单】，提交订单后即可获得 Excel 形式的授权码清单，包括 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 进行下载。
1.5 日志查看
开发过程中可以通过查看日志来进行功能调试，日志功能默认关闭，开启需对代码作如下修改：
#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。成功连接就可以看到设备日志了，软件日志窗口也会出现如下提示：
2 固件设计
2.1 功能定义
魔法棒要实现的功能定义如下：
| 功能 | 说明 |
|---|---|
| 手势动作识别 | 可识别出下列手势动作： - 手势动作 1：向上甩动； - 手势动作 2：向下甩动； - 手势动作 3：向左甩动； - 手势动作 4：向右甩动； - 手势动作 5：顺时针翻转； - 手势动作 6：逆时针翻转； … |
| 识别功能开关 | 可通过按键控制识别功能打开或关闭： - 按键按下时，手势识别功能打开； - 按键释放时，手势识别功能关闭。 |
| 手势数据上报 | 可将手势数据上报至云端，以实现手势动作对其他设备的控制。 |
| 配网状态重置 | 可通过配网键实现设备端解绑： - 长按配网键3秒，设备主动解绑。 |
| 配网等待时限 | 可在等待配网超过1分钟后结束等待，禁止用户绑定设别： - 上电时设备状态为未绑定或通过配网键重置时，设备进入配网等待； - 1分钟后仍未被绑定，则结束等待。 |
| 配网状态指示 | 可通过指示灯提示设备状态： - 等待配网时，指示灯快闪； - 配网结束时，指示灯关闭。 |
| 手电筒功能 | 可通过配网键短按切换指示灯开/关。 |
2.2 软件方案
1）模块划分
魔法棒的核心功能是识别不同的手势动作，从而达到通过涂鸦云平台控制其他设备的目的。这次硬件方案采取了 MPU6050 六轴惯性传感器来采集手势数据，包括 3 轴加速度和 3 轴角速度，由此可计算出设备的姿态角，也可以通过内置的数字运动处理器 (DMP) 获取四元数来计算姿态。结合这些数据对手势特征进行提取，就可以实现特定手势识别。根据魔法棒的硬件方案和功能定义，可以将 Demo 程序划分为以下 5 个模块：
| 模块 | 处理内容 |
|---|---|
| 数据采集模块 | MPU6050 传感器的数据采集处理 |
| 姿态解算模块 | 根据角速度、加速度计算姿态角，并用卡尔曼滤波法进行数据融合处理 |
| 手势识别模块 | 应用采集到的数据和姿态解算结果，实现各个手势动作识别 |
| 联网处理模块 | 设备配网与解绑、配网状态指示、手势数据上报等 |
| 其他功能模块 | 手电筒功能、低功耗处理等 |
各模块的具体方案将在 4.3 功能开发 中进行介绍。
2）软件框图
结合涂鸦 Bluetooth LE SDK 的软件架构和应用功能设计，魔法棒的软件框图如下：
3）软件流程
魔法棒的基本工作流程如下：
4）文件结构
应用入口：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驱动 */
| | └── tuya_pwr_mgmt.h /* 平台关联低功耗模式处理 */
| ├── 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_gesture_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驱动 */
| └── tuya_pwr_mgmt_nRF52832.c /* nRF52832平台关联低功耗模式处理 */
├── 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_gesture_rec.c /* 手势动作识别 */
├── tuya_net_proc.c /* 设备联网处理 */
└── tuya_svc_angle_calc.c /* 姿态解算服务 */
可将以上代码放至 SDK 的 tuya_ble_sdk_demo\app 目录下。如果使用的不是 nRF52832 芯片，需要对芯片平台相关的驱动部分进行移植。其中，platform 文件夹存放的是平台关联组件，目的是实现应用代码与芯片平台解耦，方便程序在不同芯片平台之间移植，也可以删除这些文件，在 tuya_key.c、tuya_mpu6050.c 等文件中直接修改外设驱动相关的代码。
3 功能开发
下面详细介绍魔法棒各项功能的实现过程。Demo 仓库：tuya-iotos-embeded-demo-ble-gesture-controller
3.1 数据采集模块
这次开发采用的惯性测量单元是 MPU6050，其主要功能已经在硬件设计方案中进行了介绍。数据采集模块的目标是：通过 I2C 接口从 MPU6050 中读取设备的加速度数据和角速度数据。
1）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
2）传感器初始化
- 初始化步骤
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，0x00）
| 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"
/* 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;
}
}
3）数据读取
- 中断处理
开启数据就绪中断后，当 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);
}
4）补充说明
对于角速度和加速度这样的矢量来说，方向的确定尤为重要。
这里引入一个概念叫做右手坐标系，它遵循右手定则，相应地也有遵循左手定则的左手坐标系。右手定则一般可以描述为：以右手握住 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.2 姿态解算模块
欧拉角 是常用的用于描述 三维空间旋转 的一种方式，简单来说就是通过 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°（如果允许俯仰角超过这个范围，会导致同一种姿态可用两套姿态角来表示）。
描述姿态的方式还有 轴-角、旋转矩阵 (方向余弦矩阵) 和 四元数，大家可以多多查阅资料来了解它们之间的转换关系和每种方法的优缺点。
1）姿态角计算
那么如何通过角速度和加速度计算出姿态角呢？在介绍计算方法之前，我们先对系统做如下约定：设定地面坐标系和设备坐标系都以 向前为 X 轴正方向，向左为 Y 轴正方向，向上为 Z 轴正方向，符合右手系。再设定旋转顺序为 z-y’-x"，为方便记忆，将三个欧拉角定义为 y-p-r，即 Yaw、Pitch 、Roll 的首字母。
- 角速度 -> 姿态角
已知，当质点做匀速圆周运动时，将角速度乘以时间就可以知道这段时间的角度变化。那么对于非匀速运动来说，可以通过角速度对时间积分来计算角度变化量；当初始角度确定时，就可以计算出当前角度，即：
θ
(
t
)
=
θ
0
+
∫
0
t
ω
(
t
)
d
t
\theta(t) = \theta_0 + \int_{0}^{t}\omega(t)dt \\
θ(t)=θ0+∫0tω(t)dt
但是，MPU6050 采集的角速度数据是相对于其自身坐标系的某一时刻的瞬时角速度，而事实上姿态更新需要的是相对于地面坐标系的欧拉角，或者说需要的是由一次姿态变化所分解的三次旋转的瞬时角速度。比如，我们将魔法棒朝某个方向甩动一段距离，实际只是发生了一次旋转（绕空间中的某一轴旋转了一个角度，该轴不一定是XYZ），但是对于欧拉角来说，它会将这次甩动拆分成按照约定顺序（如 z-y’-x"）的三次旋转。因为角速度是矢量，所以可以借助旋转矩阵来将设备坐标系下的角速度投影到地面坐标系中，来实现角速度的转换。以下是即将用到的三个基本旋转矩阵，遵循右手定则，分别表示将向量绕 x 轴、y 轴、z 轴旋转一个角度 θ：
R x ( θ ) = [ 1 0 0 0 c o s θ s i n θ 0 − s i n θ c o s θ ] R y ( θ ) = [ c o s θ 0 − s i n θ 0 1 0 s i n θ 0 c o s θ ] R z ( θ ) = [ c o s θ s i n θ 0 − s i n θ c o s θ 0 0 0 1 ] R_x(\theta) = \left[ \begin{matrix} 1 & 0& 0 \\ 0 & cos\theta & sin\theta \\ 0 & -sin\theta & cos\theta \end{matrix} \right] \quad R_y(\theta) = \left[ \begin{matrix} cos\theta & 0 & -sin\theta \\ 0 & 1 & 0 \\ sin\theta & 0 & cos\theta \end{matrix} \right] \quad R_z(\theta) = \left[ \begin{matrix} cos\theta & sin\theta & 0 \\ -sin\theta & cos\theta & 0 \\ 0 & 0 & 1 \end{matrix} \right] Rx(θ)=⎣⎡1000cosθ−sinθ0sinθcosθ⎦⎤Ry(θ)=⎣⎡cosθ0sinθ010−sinθ0cosθ⎦⎤Rz(θ)=⎣⎡cosθ−sinθ0sinθcosθ0001⎦⎤
假设设备坐标系一开始与地面坐标系重合，按 z-y’-x’’ 序列经过 y-p-r 旋转到当前姿态，用 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 就是 陀螺仪的测量值，相当于是三次旋转在各轴上所合成的等效角速度，即：
[
ω
x
ω
y
ω
z
]
=
R
x
(
r
)
R
y
(
p
)
[
0
0
d
y
d
t
]
+
R
x
(
r
)
[
0
d
p
d
t
0
]
+
[
d
r
d
t
0
0
]
=
[
1
0
−
s
i
n
(
p
)
0
c
o
s
(
r
)
c
o
s
(
p
)
s
i
n
(
r
)
0
−
s
i
n
(
r
)
c
o
s
(
p
)
c
o
s
(
r
)
]
[
d
r
d
t
d
p
d
t
d
y
d
t
]
\left[ \begin{matrix} \omega_x \\ \omega_y \\ \omega_z \end{matrix} \right] = R_x(r)R_y(p)\left[ \begin{matrix} 0 \\ 0 \\ \frac{dy}{dt} \end{matrix} \right] + R_x(r)\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(p) \\ 0 & cos(r) & cos(p)sin(r) \\ 0 & -sin(r) & cos(p)cos(r) \end{matrix} \right] \left[ \begin{matrix} \frac{dr}{dt} \\ \frac{dp}{dt} \\ \frac{dy}{dt} \end{matrix} \right]
⎣⎡ωxωyωz⎦⎤=Rx(r)Ry(p)⎣⎡00dtdy⎦⎤+Rx(r)⎣⎡0dtdp0⎦⎤+⎣⎡dtdr00⎦⎤=⎣⎡1000cos(r)−sin(r)−sin(p)cos(p)sin(r)cos(p)cos(r)⎦⎤⎣⎡dtdrdtdpdtdy⎦⎤
再反解得到姿态更新需要的角速度，也就是求逆矩阵：
[
d
r
d
t
d
p
d
t
d
y
d
t
]
=
[
1
s
i
n
(
r
)
t
a
n
(
p
)
c
o
s
(
r
)
t
a
n
(
p
)
0
c
o
s
(
r
)
−
s
i
n
(
r
)
0
s
i
n
(
r
)
/
c
o
s
(
p
)
c
o
s
(
r
)
/
c
o
s
(
p
)
]
[
ω
x
ω
y
ω
z
]
\left[ \begin{matrix} \frac{dr}{dt} \\ \frac{dp}{dt} \\ \frac{dy}{dt} \end{matrix} \right] = \left[ \begin{matrix} 1 & sin(r)tan(p) & cos(r)tan(p) \\ 0 & cos(r) & -sin(r) \\ 0 & sin(r)/cos(p) & cos(r)/cos(p) \end{matrix} \right] \left[ \begin{matrix} \omega_x \\ \omega_y \\ \omega_z \end{matrix} \right]
⎣⎡dtdrdtdpdtdy⎦⎤=⎣⎡100sin(r)tan(p)cos(r)sin(r)/cos(p)cos(r)tan(p)−sin(r)cos(r)/cos(p)⎦⎤⎣⎡ωxωyωz⎦⎤
而内旋 z(α)-y’(β)-x"(γ) 等价于外旋 x(γ)-y(β)-z(α)，所以将上面求得的角速度代入积分方程就能计算出相对于地面坐标系的欧拉角。
- 代码实现
角度的单位有“度”和“弧度”，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，即：
[
a
x
a
y
a
z
]
=
R
x
(
r
)
R
y
(
p
)
R
z
(
y
)
[
0
0
g
]
=
[
−
s
i
n
(
p
)
c
o
s
(
p
)
s
i
n
(
r
)
c
o
s
(
p
)
c
o
s
(
r
)
]
g
\left[ \begin{matrix} a_x \\ a_y \\ a_z \end{matrix} \right] = R_x(r)R_y(p)R_z(y)\left[ \begin{matrix} 0 \\ 0 \\ g \end{matrix} \right] = \left[ \begin{matrix} -sin(p) \\ cos(p)sin(r) \\ cos(p)cos(r) \end{matrix} \right] g
⎣⎡axayaz⎦⎤=Rx(r)Ry(p)Rz(y)⎣⎡00g⎦⎤=⎣⎡−sin(p)cos(p)sin(r)cos(p)cos(r)⎦⎤g
解方程可以得到 Roll 和 Pitch：
{
a
y
a
z
=
c
o
s
(
p
)
s
i
n
(
r
)
g
c
o
s
(
p
)
s
i
n
(
r
)
g
=
s
i
n
(
r
)
c
o
s
(
r
)
=
t
a
n
(
r
)
a
x
=
−
s
i
n
(
p
)
g
a
y
2
+
a
z
2
=
c
o
s
2
(
p
)
[
s
i
n
2
(
r
)
+
c
o
s
2
(
r
)
]
g
2
=
c
o
s
2
(
p
)
g
2
→
{
r
=
a
r
c
t
a
n
a
y
a
z
p
=
a
r
c
t
a
n
−
a
x
a
y
2
+
a
z
2
\left\{ \begin{aligned} & \frac{a_y}{a_z} = \frac{cos(p)sin(r)g}{cos(p)sin(r)g} = \frac{sin(r)}{cos(r)} = tan(r) \\ & a_x = -sin(p)g \\ & a_y^2+a_z^2 = cos^2(p)[sin^2(r)+cos^2(r)]g^2 = cos^2(p)g^2 \end{aligned} \right. \quad \to \quad \left\{ \begin{aligned} & r = arctan\frac{a_y}{a_z} \\ & p = arctan\frac{-a_x}{\sqrt{a_y^2+a_z^2}} \end{aligned} \right.
⎩⎪⎪⎪⎨⎪⎪⎪⎧azay=cos(p)sin(r)gcos(p)sin(r)g=cos(r)sin(r)=tan(r)ax=−sin(p)gay2+az2=cos2(p)[sin2(r)+cos2(r)]g2=cos2(p)g2→⎩⎪⎪⎪⎨⎪⎪⎪⎧r=arctanazayp=arctanay2+az2−ax
可见，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;
}
2）卡尔曼滤波
现在我们已经知道了如何通过加速度或角速度来计算姿态角，但传感器存在测量误差，且根据上面的介绍可以发现：通过角速度通计算的角度误差会随着时间的推移不断增大，即存在累积误差；而加速度只能在设备静止时计算出较准确的姿态角，即动态响应较差。所以我们还需要通过滤波算法对数据进行融合处理，以获得更精确的姿态数据。对姿态解算而言，常用的滤波算法有一阶互补滤波和卡尔曼滤波，这里我们采用卡尔曼滤波。
卡尔曼滤波（Kalman filter）是一种高效率的递归滤波器，它能从一系列的不完全或包含噪声的测量中，估计动态系统的状态。卡尔曼滤波通过 2 个阶段 来估计动态系统的状态：① 预测 - 使用上一时刻的最优估计值，预测当前时刻的估计值；② 更新 - 利用当前时刻的观测值，优化预测阶段获得的估计值。
- 卡尔曼滤波方程
网络上关于卡尔曼滤波的建模思想和方程推导过程有很多不错的文章和视频教程，大家可以自行查阅进行学习，这里将卡尔曼滤波的五个方程总结如下：
预 测 方 程 ： { x ^ k − = A x ^ k − 1 + B u k P k − = A P k − 1 A T + Q 更 新 方 程 ： { K k = P k − C T C P k − C T + R x ^ k = x ^ k − + K k ( y k − C x ^ k − ) P k = ( I − K k C ) P k − 预测方程： \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. 预测方程：{x^k−=Ax^k−1+BukPk−=APk−1AT+Q更新方程：⎩⎪⎪⎪⎨⎪⎪⎪⎧Kk=CPk−CT+RPk−CTx^k=x^k−+Kk(yk−Cx^k−)Pk=(I−KkC)Pk−
方程中的字母含义：
| 字母 | 含义 |
|---|---|
| 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 时刻的角度存在如下关系：
θ k = θ k − 1 + ( ω k − e ω ) Δ t = θ k − 1 − e ω Δ t + ω k Δ t \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+(ωk−eω)Δt=θk−1−eωΔt+ωkΔt
显然角速度就是这个系统的输入量，但和状态方程相比，可以发现还多一个角速度漂移的分式。如果把角速度漂移也作为一个状态量，并假设 k 时刻和 k-1 时刻的角速度漂移是相同的，可以表示为：
e
ω
k
=
e
ω
k
−
1
{e_{\omega}}_{k} = {e_{\omega}}_{k-1}
eωk=eωk−1
将上面两个式子进行拆分，整理成一一对应的形式：
{
θ
k
=
1
∗
θ
k
−
1
−
Δ
t
∗
e
ω
k
−
1
+
Δ
t
∗
ω
k
e
ω
k
=
0
∗
θ
k
−
1
+
1
∗
e
ω
k
−
1
+
0
∗
ω
k
\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.
{θkeωk=1∗θk−1−Δt∗eωk−1+Δt∗ωk=0∗θk−1+1∗eωk−1+0∗ωk
我们用矩阵的形式来表示它，就得到了卡尔曼滤波的第一个方程，状态变量 x = (θ, eω)，输入变量 u = ω：
[
θ
e
ω
]
k
=
[
1
−
Δ
t
0
1
]
(
A
)
[
θ
e
ω
]
k
−
1
+
[
Δ
t
0
]
(
B
)
ω
k
⇕
x
^
k
−
=
A
x
^
k
−
1
+
B
u
k
\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
[θeω]k=(A)[10−Δt1][θeω]k−1+(B)[Δt0]ωk⇕x^k−=Ax^k−1+Buk
在实际情况下，上面的方程并不能完全成立，因为上一时刻的角度可能存在误差，角速度漂移也存在时间漂移、温度漂移，这些噪声就是过程噪声 w，角度噪声与角速度漂移噪声互相独立，所以 w 的协方差矩阵 Q 可以表示为：
Q
=
[
c
o
v
(
w
θ
,
w
θ
)
c
o
v
(
w
θ
,
w
e
ω
)
c
o
v
(
w
e
ω
,
w
θ
)
c
o
v
(
w
e
ω
,
w
e
ω
)
]
=
[
v
a
r
(
w
θ
)
0
0
v
a
r
(
w
e
ω
)
]
=
[
Q
θ
0
0
Q
e
ω
]
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]
Q=[cov(wθ,wθ)cov(weω,wθ)cov(wθ,weω)cov(weω,weω)]=[var(wθ)00var(weω)]=[Qθ00Qeω]
将 A 和 Q 代入卡尔曼滤波的第二个方程，就可以得到误差协方差 P 的表达式：
P
k
−
=
A
P
k
−
1
A
T
+
Q
⇕
P
k
−
=
[
1
−
Δ
t
0
1
]
(
A
)
P
k
−
1
[
1
0
−
Δ
t
1
]
(
A
T
)
+
[
Q
θ
0
0
Q
e
ω
]
(
Q
)
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)}
Pk−=APk−1AT+Q⇕Pk−=(A)[10−Δt1]Pk−1(AT)[1−Δt01]+(Q)[Qθ00Qeω]
显然 P 是一个2×2的矩阵，如果将其中的元素用 a-b-c-d 表示，并进行计算，那么可以得到：
[
a
b
c
d
]
k
−
=
[
a
−
(
b
+
c
)
Δ
t
+
d
Δ
t
2
+
Q
θ
b
−
d
Δ
t
c
−
d
Δ
t
d
+
Q
e
ω
]
k
−
1
\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}
[acbd]k−=[a−(b+c)Δt+dΔt2+Qθc−dΔtb−dΔtd+Qeω]k−1
以上就是预测阶段的公式推导，得到了姿态角状态的预估值和它的误差协方差矩阵。
接下来看更新阶段，我们还需要一个观测量来修正系统。可以使用加速度计测量值计算的角度 θa 作为观测量，显然它等于 θ 加上传感器误差引起的角度误差 v(θa)，代入到观测方程，就得到了矩阵 C = [1, 0]：
y
k
=
C
x
k
+
v
⇔
[
θ
a
]
k
=
[
1
0
]
(
C
)
[
θ
e
ω
]
k
+
v
θ
a
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_{(C)} \left[ \begin{matrix} \theta \\ e_{\omega} \end{matrix} \right]_{k} + v_{\theta_a}
yk=Cxk+v⇔[θa]k=(C)[10][θeω]k+vθa
而测量噪声协方差 R 可以表示为 v(θa) 的方差，代入第三个方程，可以发现卡尔曼增益是一个2×1矩阵：
K
k
=
P
k
−
C
T
C
P
k
−
C
T
+
R
=
[
a
b
c
d
]
k
−
[
1
0
]
[
1
0
]
[
a
b
c
d
]
k
−
[
1
0
]
+
v
a
r
(
v
θ
a
)
=
[
a
c
]
k
−
a
k
−
+
R
θ
a
↓
K
k
=
[
K
0
K
1
]
k
=
[
a
k
−
a
k
−
+
R
θ
a
c
k
−
a
k
−
+
R
θ
a
]
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]
Kk=CPk−CT+RPk−CT=[10][acbd]k−[10]+var(vθa)[acbd]k−[10]=ak−+Rθa[ac]k−↓Kk=[K0K1]k=⎣⎡ak−+Rθaak−ak−+Rθack−⎦⎤
将计算好的卡尔曼增益代入第四个方程，就得到了更新状态估计的表达式：
x
^
k
=
x
^
k
−
+
K
k
(
y
k
−
C
x
^
k
−
)
⇕
[
θ
e
ω
]
k
=
[
θ
e
ω
]
k
−
+
[
K
0
K
1
]
k
(
θ
a
k
−
θ
k
−
)
↓
{
θ
k
=
θ
k
−
+
K
0
(
θ
a
k
−
θ
k
−
)
e
ω
k
=
e
ω
k
−
+
K
1
(
θ
a
k
−
θ
k
−
)
{\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.
x^k=x^k−+Kk(yk−Cx^k−)⇕[θeω]k=[θeω]k−+[K0K1]k(θak−θk−)↓{θk=θk−+K0(θak−θk−)eωk=eωk−+K1(θak−θk−)
最后用第五个方程计算更新的协方差矩阵：
P
k
=
(
I
−
K
k
C
)
P
k
−
=
(
[
1
0
0
1
]
−
[
K
0
K
1
]
k
[
1
0
]
)
P
k
−
=
[
1
−
K
0
0
−
K
1
1
]
k
P
k
−
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^-
Pk=(I−KkC)Pk−=([1001]−[K0K1]k[10])Pk−=[1−K0−K101]kPk−
- 代码实现
通过上面的公式推导，按照编程思路将公式整理如下：
① 预测 - 状态估计
x ^ k − = A x ^ k − 1 + B u k { θ ^ k − = θ ^ k − 1 + ( ω k − e ω ^ k − 1 ) Δ t e ω ^ k − = e ω ^ k − 1 {\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. x^k−=Ax^k−1+Buk{θ^k−=θ^k−1+(ωk−eω^k−1)Δteω^k−=eω^k−1
② 预测 - 估计误差协方差
P
k
−
=
A
P
k
−
1
A
T
+
Q
{
a
k
−
=
a
k
−
1
−
(
b
k
−
1
+
c
k
−
1
)
Δ
t
+
d
k
−
1
Δ
t
2
+
Q
θ
b
k
−
=
b
k
−
1
−
d
k
−
1
Δ
t
c
k
−
=
c
k
−
1
−
d
k
−
1
Δ
t
d
k
−
=
d
k
−
1
+
Q
e
ω
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.
Pk−=APk−1AT+Q⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧ak−=ak−1−(bk−1+ck−1)Δt+dk−1Δt2+Qθbk−=bk−1−dk−1Δtck−=ck−1−dk−1Δtdk−=dk−1+Qeω
③ 更新 - 卡尔曼增益
K
k
=
P
k
−
C
T
C
P
k
−
C
T
+
R
{
K
0
k
=
a
k
−
a
k
−
+
R
θ
a
K
1
k
=
c
k
−
a
k
−
+
R
θ
a
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.
Kk=CPk−CT+RPk−CT⎩⎪⎪⎪⎨⎪⎪⎪⎧K0k=ak−+Rθaak−K1k=ak−+Rθack−
④ 更新 - 状态估计
x
^
k
=
x
^
k
−
+
K
k
(
y
k
−
C
x
^
k
−
)
{
e
θ
a
k
=
θ
a
k
−
θ
^
k
−
θ
^
k
=
θ
^
k
−
+
K
0
k
e
θ
a
k
e
ω
^
k
=
e
ω
^
k
−
+
K
1
k
e
θ
a
k
{\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.
x^k=x^k−+Kk(yk−Cx^k−)⎩⎪⎪⎨⎪⎪⎧eθak=θak−θ^k−θ^k=θ^k−+K0keθakeω^k=eω^k−+K1keθak
⑤ 更新 - 估计误差协方差
P
k
=
(
I
−
K
k
C
)
P
k
−
{
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
−
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.
Pk=(I−KkC)Pk−⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧ak=ak−−K0kak−bk=bk−−K0kbk−ck=ck−−K1kak−dk=dk−−K1kbk−
现在，我们已经可以快速敲出代码了。需要说明的是，由于采样时间一般很小，所以 ② 式中的二次项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.3 手势识别模块
手势识别，简单来说就是找到各个手势信号的变化规律，然后从采集到的手势数据中找到符合规律的数据，从而达到识别的目的。因此，建议同学们在开发时准备一个 串口数据波形显示 工具，以便能更直观地感受数据变化。
1）手势信号提取
- 方案说明
在识别具体手势之前，需要先确定是否发生了预期的动作，比如从桌上拿起或放下魔法棒时，相关数据也会发生变化，需要排除这些干扰数据。从理论上来说，当我们挥动魔法棒时，相当于是对魔法棒施加力的过程，根据牛顿第二定律可知，力的作用会表现在物体运动的加速度上。实验证明，当我们快速挥动魔法棒时，加速度数据会发生剧烈变化，朝不同的方向甩动时，各轴的加速度变化情况也不同。因此，可以用 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;
}
2）手势类型判断
- 方案说明
在获得了有效手势数据之后，我们还可以对一些有相似特征的手势进行归类，这样就可以使用类似的识别算法进一步区分同一类手势，这也取决于开发者具体想要识别哪些手势，比如我们要识别的上下甩动和左右甩动就都可以归类为甩动类。当然，不具备相似特征的手势也可以单独处理。
对于甩动类手势而言，其特征表现为手势长度较长且加速度变化比较剧烈。可以简单以采样点个数作为手势长度，而加速度的变化情况，可以通过对提取到的有效手势数据的“3轴差分绝对值之和”再求和来表示。
注：Demo 程序中已追加翻转手势的识别。
- 代码实现
#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;
}
3）甩动手势识别
- 方案说明
甩动手势分为上下甩动和左右甩动，上下甩动主要体现在俯仰角的变化上，左右甩动主要体现在偏航角的变化上，如下图所示。那么我们就可以通过比较俯仰角和偏航角的变化量的大小来区分上下甩动还是左右甩动。
确认了是上下甩动还是左右甩动之后，我们还需要判断甩动具体方向。由上图可见，向上甩动时，俯仰角逐渐减小，向下甩动时，俯仰角逐渐增大，那么通过判断这个变化趋势就可以确定方向。
- 代码实现
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;
}
3.4 联网处理模块
1）配网处理
- 处理流程
根据功能定义中对配网状态重置、配网等待时限和配网状态指示的描述，可知配网处理流程如下：
- 代码实现
首先编写初始化函数，如下所示。
其中，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();
}
2）数据上报
- 上报机制
完成手势识别后，可以将手势数据上报至云端，来实现 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();
}
3.5 其他功能模块
1）手电筒功能
- 处理内容
手电筒功能比较简单，在配网键短按事件发生时切换指示灯的状态即可。但考虑到配网指示优先级较高，所以当指示灯快闪时，不处理开/关灯的请求。
- 代码实现
/**
* @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;
}
}
2）低功耗处理
- 低功耗模式
nRF52832 芯片有 SYSTOM ON 和 SYSTEM OFF 两种模式，相关说明可以参考这篇文章：NRF52832学习笔记（33）——低功耗实现 。这里选用 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();
}
/* ... */
}
}
4 总结
了解到有些同学还不太清除如何入手软件开发，这里给大家整理了一下针对魔法棒的开发思路：
| No. | 内容 |
|---|---|
| 1 | 搭建好软件开发环境，包括调试环境 |
| 2 | 确认要实现哪些功能，虽然可以后面继续追加，但最好先确定一个初期目标 |
| 3 | 对这些功能进行模块划分，理清模块之间的关系 |
| 4 | 确定软件的框架、总体流程、工作时序等 |
| 5 | 实现按键、指示灯、MPU6050 等外设驱动部分 |
| 6 | 对旋转表示和姿态解算相关知识进行学习，选择合适的姿态解算方案 |
| 7 | 观察并总结手势数据变化规律，实现识别算法 |
| 8 | 学会设备配网、数据上报、APP操作等，实现魔法棒对子设备的控制 |
开放原子开发者工作坊旨在鼓励更多人参与开源活动，与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动，如meetup、训练营等，主打技术交流，干货满满，真诚地邀请各位开发者共同参与！
更多推荐
16
0- 0
已为社区贡献16条内容
所有评论(0)