智能魔法棒（手势控制器）———嵌入式篇

已于 2022-01-25 19:19:08 修改
阅读量5.6k 收藏 37
点赞数 13
分类专栏： # 智能魔法棒 文章标签： 物联网 iot 教程
于 2021-12-24 15:29:42 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/sandwich_iot/article/details/122129051
版权

  在《 智能魔法棒———硬件篇 》中已经为大家介绍了魔法棒的硬件方案和结构设计，接下来介绍一下嵌入式软件设计方案。
  注：本文涉及了有关 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 。

get-tuya-ble-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 芯片对应的软件包进行安装，如下图所示：

install-soft-pack

1.4 烧录授权

  • 模组+授权码购买

  涂鸦云模组可以在【硬件开发】页面购买。需要先【新增自定义固件】（开发前可以使用 SDK Demo 中的默认固件），然后点击右侧的【立即购买】 进入 模组+授权码 购买流程。涂鸦提供多种烧录授权方式，可根据不同的开发阶段、芯片类型和生产方式进行选择，详情点击 固件烧录授权。（BN 系列模组烧录授权、PHY6222芯片烧录授权）

  • 仅授权码购买

  如果已有模组或开发板，也可以只购买授权码。可通过左侧导航栏的 采购 - 采购商品 - 生产研发采购 中进入 通用备货采购 页面，选择 授权码-通用版 进行购买。新用户也可以免费领取 2 个授权码：

get-free-licenses
  调试阶段可以选择交付形式为 【授权码清单】，提交订单后即可获得 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.3VVCC
GNDGND
SWDIOSWDIO
SWCSWCLK
  • 固件下载

  在 Demo 固件下载前，必须先下载协议栈固件（同一块模组只需操作 1 次）。在 J-Link 官网下载 J-Link 软件开发包。安装完成后，打开 J-Flash 软件，点击 File - New Project 创建工程，芯片选择 nRF52832_xxAA 后点击 OK，如下图所示：

J-Flash-new-project

  点击 File - Open data file 打开涂鸦 SDK \pca10040\s132\arm5_no_packs\hex\material 目录下的 s132_nrf52_6.1.1_softdevice.hex 文件，如下图所示：

open-softdevice-file

  点击 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 软件，会自动弹出以下对话框：

RTT-Viewer-config

  按上图内容完成设置后，点击 OK。成功连接就可以看到设备日志了，软件日志窗口也会出现如下提示：

view-log

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 的软件架构和应用功能设计，魔法棒的软件框图如下：
software-block-diagram

3）软件流程

  魔法棒的基本工作流程如下：
main-flow-chart

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, &reg_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, &reg_addr, 1);
    i2c_start();
    i2c_rcv_bytes(MPU6050_ADDR_CMD_READ, &reg_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标识符说明
7DEVICE_RESET设备复位。
0：复位完成；
1：将所有内部寄存器重置为其默认值，复位完成后，该位自动清除为0。
6SLEEP睡眠模式设置。
0：设备正常工作；
1：进入低功耗睡眠模式。
5CYCLE循环模式设置。
0：禁用循环模式；
1：SLEEP=0时，MPU-60X0将进入循环模式，设备在睡眠和唤醒之间循环。
3TEMP_DIS温度传感器设置。
0：启用温度传感器；
1：禁用温度传感器。
2:0CLKSEL[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:1WHO_AM_I[6:1]设备地址高 6 位，默认为 0x68。

  >> 陀螺仪配置寄存器（0x1B，GYRO_CONFIG，R/W，0x00）

Bit标识符说明
7:5XYZG_ST陀螺仪 X/Y/Z 轴自检控制。
0：关闭自检；
1：激活自检。
4:3FS_SEL[1:0]陀螺仪满量程范围设置。
00：±250°/s；
01：±500°/s；
10：±1000°/s；
11：±2000°/s。

  >> 加速度计配置寄存器（0x1C，ACCEL_CONFIG，R/W，0x00）

Bit标识符说明
7:5XYZA_ST加速度 X/Y/Z 轴自检控制。
0：关闭自检；
1：激活自检。
4:3AFS_SEL[1:0]加速度计满量程范围设置。
00：±2g；
01：±4g；
10：±8g；
11：±16g。

  >> 采样率分频寄存器（0x19，SMPRT_DIV，R/W，0x00）

Bit标识符说明
7:0SMPLRT_DIV[7:0]陀螺仪输出速率分频设置。
- 采样率 = 陀螺仪输出速率 / (1 + SMPLRT_DIV)；
- 禁用 DLPF (DLPF_CFG=0或7) 时，陀螺仪输出速率为 8kHz；
- 启用 DLPF 时，陀螺仪输出速率为 1kHz。

  >> 配置寄存器（0x1A，CONFIG，R/W，0x00）

Bit标识符说明
5:3EXT_SYNC_SET[2:0]外部帧同步 (FSYNC) 引脚采样设置。
2:0DLPF_CFG[2:0]数字低通滤波器 (DLPF) 设置。

  >> 中断引脚配置寄存器（0x37，INT_PIN_CFG，R/W，0x00）

Bit标识符说明
7INT_LEVELINT 引脚中断电平设置。
0：高电平有效；
1：低电平有效。
6INT_OPENINT 引脚输出模式设置。
0：推挽输出；
1：开漏输出。
5LATCH_INT_EN中断保持方式设置。
0：产生 50us 脉冲；
1：保持高电平直到中断清除。
4INT_RD_CLEAR中断状态清除方式设置。
0：仅通过读取 INT_STATUS 来清除中断状态位；
1：通过任何读操作清除中断状态位。
3FSYNC_INT_LEVELFSYNC 引脚中断电平设置。
0：高电平有效；
1：低电平有效。
2FSYNC_INT_ENFSYNC 引脚中断功能设置。
0：禁止 FSYNC 引脚作为主处理器的中断引脚；
1：允许 FSYNC 引脚作为主处理器的中断引脚。
1I2C_BYPASS_EN辅助 I2C 总线访问权限设置。
0：禁止主处理器直接访问辅助 I2C 总线；
1：I2C_MST_EN = 0 时，允许主处理器直接访问辅助 I2C 总线。

  >> 中断使能寄存器（0x38，INT_ENABLE，R/W，0x00）

Bit标识符说明
6MOT_EN运动中断设置。
0：禁用中断；
1：启用中断。
4FIFO_OFLOW_ENFIFO缓存区溢出中断设置。
0：禁用中断；
1：启用中断。
3I2C_MST_INT_ENI2C主机中断设置。
0：禁用中断；
1：启用中断。
0DATA_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-orientation

  由于 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​(θ)=⎣⎡​100​0cosθ−sinθ​0sinθcosθ​⎦⎤​Ry​(θ)=⎣⎡​cosθ0sinθ​010​−sinθ0cosθ​⎦⎤​Rz​(θ)=⎣⎡​cosθ−sinθ0​sinθcosθ0​001​⎦⎤​

  假设设备坐标系一开始与地面坐标系重合，按 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)⎣⎡​0dtdp​0​⎦⎤​+⎣⎡​dtdr​00​⎦⎤​=⎣⎡​100​0cos(r)−sin(r)​−sin(p)cos(p)sin(r)cos(p)cos(r)​⎦⎤​⎣⎡​dtdr​dtdp​dtdy​​⎦⎤​
  再反解得到姿态更新需要的角速度，也就是求逆矩阵：
[ 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] ⎣⎡​dtdr​dtdp​dtdy​​⎦⎤​=⎣⎡​100​sin(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 ⎣⎡​ax​ay​az​​⎦⎤​=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. ⎩⎪⎪⎪⎨⎪⎪⎪⎧​​az​ay​​=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=arctanaz​ay​​p=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​+Buk​Pk−​=APk−1​AT+Q​更新方程：⎩⎪⎪⎪⎨⎪⎪⎪⎧​​Kk​=CPk−​CT+RPk−​CT​x^k​=x^k−​+Kk​(yk​−Cx^k−​)Pk​=(I−Kk​C)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. {θk​eω​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θ​)0​0var(weω​​)​]=[Qθ​0​0Qeω​​​]
  将 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−1​AT+Q⇕Pk−​=(A)[10​−Δt1​]​Pk−1​(AT)[1−Δt​01​]​+(Q)[Qθ​0​0Qeω​​​]​
  显然 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} [ac​bd​]k−​=[a−(b+c)Δt+dΔt2+Qθ​c−dΔt​b−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)[1​0​]​[θ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​=[1​0​][ac​bd​]k−​[10​]+var(vθ​a​)[ac​bd​]k−​[10​]​=ak−​+Rθa​​[ac​]k−​​↓Kk​=[K0​K1​​]k​=⎣⎡​ak−​+Rθa​​ak−​​ak−​+Rθa​​ck−​​​⎦⎤​
  将计算好的卡尔曼增益代入第四个方程，就得到了更新状态估计的表达式：
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−​+[K0​K1​​]k​(θa​k​−θk−​)↓{​θk​=θk−​+K0​(θa​k​−θk−​)eω​k​=eω​k−​+K1​(θa​k​−θ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−Kk​C)Pk−​=([10​01​]−[K0​K1​​]k​[1​0​])Pk−​=[1−K0​−K1​​01​]k​Pk−​

  • 代码实现

  通过上面的公式推导，按照编程思路将公式整理如下：

  ① 预测 - 状态估计

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−1​AT+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​⎩⎪⎪⎪⎨⎪⎪⎪⎧​K0​k​=ak−​+Rθa​​ak−​​K1​k​=ak−​+Rθa​​ck−​​​
  ④ 更新 - 状态估计
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θa​​k​=θa​k​−θ^k−​θ^k​=θ^k−​+K0​k​eθa​​k​eω​^​k​=eω​^​k−​+K1​k​eθa​​k​​
  ⑤ 更新 - 估计误差协方差
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−Kk​C)Pk−​⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧​​ak​=ak−​−K0​k​ak−​bk​=bk−​−K0​k​bk−​ck​=ck−​−K1​k​ak−​dk​=dk−​−K1​k​bk−​​
  ​现在，我们已经可以快速敲出代码了。需要说明的是，由于采样时间一般很小，所以 ② 式中的二次项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 轴的加速度差分绝对值之和作为手势起终点判断的特征量。该特征量大于设定阈值时，判定为手势起点；小于设定阈值时，判定为手势终点。

data-action-occurs
  同样地，当我们以手臂为中心轴翻转魔法棒时，加速度的数据变化情况也符合预期。

data-action-occurs

  • 代码实现

  以下函数用于计算加速度值的差分绝对值之和，并进行移动平均滤波。

#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）甩动手势识别

  • 方案说明

  甩动手势分为上下甩动和左右甩动，上下甩动主要体现在俯仰角的变化上，左右甩动主要体现在偏航角的变化上，如下图所示。那么我们就可以通过比较俯仰角和偏航角的变化量的大小来区分上下甩动还是左右甩动。

up-down

left-right

  确认了是上下甩动还是左右甩动之后，我们还需要判断甩动具体方向。由上图可见，向上甩动时，俯仰角逐渐减小，向下甩动时，俯仰角逐渐增大，那么通过判断这个变化趋势就可以确定方向。

  • 代码实现
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）配网处理

  • 处理流程

  根据功能定义中对配网状态重置、配网等待时限和配网状态指示的描述，可知配网处理流程如下：

network-flow-chart

  • 代码实现

  首先编写初始化函数，如下所示。
  其中，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操作等，实现魔法棒对子设备的控制
三明治开发社区
关注
  • 13
    点赞
  • 踩
  • 37
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
专栏目录
智能魔法棒———硬件篇
三明治开发社区的博客
12-23 3178
大家都听过《哈利波特》吧，在《哈利波特》的世界中，魔法物品很多，但是没有任何一样物品能像魔杖一样重要，因为它与巫师建立起了一种难以言表的紧密联系。大家看到魔法世界里的巫师们，轻轻挥动几下他们的魔法棒，就可以施放各种各样的魔法，是不是即惊叹又羡慕。 今天给大家带来一款家庭版的魔法棒，开发者们只要轻轻挥动它，就可以控制家里的智能设备，体验一次当哈利波特的感觉，是不是特别神奇。它还可以充当探照小灯，比如东西掉到狭小又深的地方时，可以将身材修长苗条是魔法棒伸进去，点亮顶部的小灯，来寻找物品。
嵌入式图形用户界面中的非触摸式手势交互
Crank_Storyboard的博客
10-24 772
我们将解释为什么产品支持非接触式交互会具有商业意义，以及设计师和开发人员在其项目中需要考虑的一些方面。
基于惯性传感器MPU6050的手势识别方法.pdf
03-06
基于惯性传感器MPU6050的手势识别方法
【嵌入式开发】114
宅男很神经
02-21 1000
在Keil uVision5集成开发环境（IDE）中，“魔术棒”通常指的是用于快速配置项目设置的工具。这个工具提供了一个图形化界面，允许开发者轻松地管理和配置项目的各个方面，如目标设备、输出设置、包含路径、库文件等。通过“魔术棒”，开发者可以避免手动编辑复杂的配置文件，从而加快开发速度并减少错误。
基于单片机的智能手势识别控制系统
最新发布
毕业作品网站
05-11 990
对于手指弯曲动作的解析运用了神经网络算法，可以较准确地解算出手掌和手指的空间旋转状态，但是该数据手套没有相关的定位措施，只能应用于静态手势识别，2015 年，台湾国立台北大学研制了一款基于9 轴惯性传感器与力敏传感器的数据手套，用于手部医疗康复训练。主机的输入包括两个部分，第一部分是蓝牙模块将检测的数据通过该模块传输到从机的显示模块上，第二部分是供电模块，给主机进行供电；提出了一种手势识别智能控制器，进行了相关电路设计，并搭建了系统仿真实验，编制了相应的复杂手势识别模块。2.2.2 显示模块方案选择 9。
嵌入式毕设选题 手势检测识别系统设计与实现 - arduino 物联网
MDC_sir的博客
01-11 519
🔥 这两年开始毕业设计和毕业答辩的要求和难度不断提升，传统的毕设题目缺少创新和亮点，往往达不到毕业答辩的要求，这两年不断有学弟学妹告诉学长自己做的项目系统达不到老师的要求。为了大家能够顺利以及最少的精力通过毕设，学长分享优质毕业设计项目，今天要分享的是🚩基于单片机的手势检测识别系统🥇学长这里给一个题目综合评分(每项满分5分)难度系数：4分工作量：4分创新点：3分PIR传感器OLED显示模块（128X64）热阵列传感器。
我用全志V851s做了一个魔法棒，使用Keras训练手势识别模型控制一切电子设备
DOT小文哥的博客
02-04 1719
这是一个可以直接启动原神的魔法棒~原神，启动！这是一个万全的解决方案！只需要花80元再动动手，就可以将哈利波特的魔杖与人工智能结合到一起！它就是用全志V851s做的。
毕业设计 嵌入式 手势识别设计与实现
MDC_sir的博客
02-21 550
Hi，大家好，学长今天向大家介绍一个学长做的单片机项目基于单片机得手势识别系统大家可用于 课程设计 或 毕业设计。
嵌入式系统/ARM技术中的基于嵌入式系统实时交互的手势识别方法
10-21
摘要： 提出了一种在单摄像头条件下基于嵌入式系统的手势识别方法。通过拟合手势图的外接多边形，找出其所对应的手势缺陷图，并建立手势与手势缺陷图的一一映射，利用手势缺陷图的特征来匹配和识别不同的手势。算法还将手势的跟踪与识别有机地统一起来，通过预测下一帧中手势出现的粗略位置大大降低识别步骤的计算量。该算法在实际应用的嵌入式平台下，能快速、准确地实现手势的识别，能够满足实时人机交互的要求。 　　0 引言 　　手势交互是人机交互领域近年来的研究热点，特别是利用摄像头来实现对手势信息的非接触性捕获，并由计算机进行分析理解，然后完成交互任务，由于其自然和符合人自身行为习惯的交互方式而备受青睐。手势的形
基于嵌入式系统实时交互的手势识别方法
10-19
提出了一种在单摄像头条件下基于嵌入式系统的手势识别方法。通过拟合手势图的外接多边形，找出其所对应的手势缺陷图，并建立手势与手势缺陷图的一一映射，利用手势缺陷图的特征来匹配和识别不同的手势。算法还将手势的跟踪与识别有机地统一起来，通过预测下一帧中手势出现的粗略位置大大降低识别步骤的计算量。该算法在实际应用的嵌入式平台下，能快速、准确地实现手势的识别，能够满足实时人机交互的要求。
现实版哈利·波特，智能“魔法棒”开发，内含详细开发资料+原理图
weixin_50563088的博客
12-17 1379
小时候，总有个“魔法梦”， ​挥动魔法棒，骑上飞天扫帚， 与邪恶斗争，挥洒正义与热血。 从电视剧《巴啦啦小魔仙》到奇幻的《哈利·波特》系列电影，一根“魔法棒”承载了我们对未知世界的想象与创造。 假如，真的拥有一根魔法棒，你会做什么？ 看下去，这里将会给你答案~ 开发者们只要轻轻挥动它，就可以控制家里的智能设备，体验一次当哈利·波特的感觉。此外，它还可以充当探照小灯，比如东西掉到狭小又深的地方时，魔法棒利用自身“身材”修长的优势，并点亮顶部的小灯，来寻找物品。...
手势控制小车_MPU6050_hc-05_arduino
09-10
本设计包含两个部分，手势遥控器（利用mpu6050读取手势信息）利用蓝牙模块将控制信号发送到小车端，小车端接收到控制信号后做出相应的动作。
【Demo】 AI魔法声音——一个结合AI的变声器.zip
06-24
【Demo】 AI魔法声音——一个结合AI的变声器 AiSound AiSound AI魔法声音 是一个结合AI的变声器，基于fmod实现，使用fmod来处理音频的变声效果， 支持试听和保存音效文件 核心库 库名 版本 下载地址 fmod 1.10.15 ...
设计师的魔法棒—格式塔心理学
03-02
格式塔原理——图形与背景、接近、相似、闭合、连续、简单、均衡——格式塔心理学科是认知心理学中的一个重要理论，在视觉设计中已经有较大的影响。若去深究格式塔原理，可能需要相当长的时间来学习理解，作为交互...
魔法棒 Magic Robots 网络监控软件、Tkping网络工具
04-29
Magic Robots 程序主要功能说明： 1、首先您需要在高级配置里配备一条钉钉或企业微信 WebHook 地址链接，教程在【显示服务】的帮助里面有官方教程。 钉钉机器人安全设置里“自定义关键词”里一定要添加【我是小智】...
Third Person Controller第三人称控制器unity包
12-09
第三人称控制器是您创建任何第三人称游戏的最终解决方案。第三人称控制器是一个专业的，运动学的字符控制器设计的灵活性，模块化，和性能。这个包包括射击，近战，魔法，可投掷和手电筒物品在第三人称视角。 已经...
使用Arduino的基于加速度计的手势控制机器人
acktomas的博客
04-27 719
原文地址：https://circuitdigest.com/microcontroller-projects/accelerometer-based-hand-gesture-controlled-robot-using-arduino 使用Arduino的基于加速度计的手势控制机器人 ARDUINO的 经过**萨达姆** 2015年7月28日135 机器人在建筑，军事，医疗，制造等各个领域的自动化领域中都起着重要作用。在制造出一些基本的机器人（如直线跟随器机器人，计算机控制机器人等）之后，我们通过使用
基于STM32单片机智能MPU6050交警交通指挥手势信号识别设计23-286
Danpianjihuizong的博客
03-07 260
23-286、STM32交警交通指挥手势信号识别装置设计-MPU6050-LCD1602功能描述：本设计由STM32F103C8T6单片机电路+MPU6050陀螺仪传感器电路+LCD1602液晶显示电路+电源电路组成。1、通过陀螺仪检测手势。2、手势：左翻、右翻、上翻、下翻，则液晶显示为Left、Right、Stop、Straight。
keil5魔法棒是灰色的
04-08
Keil5魔法棒是一款集成开发环境（IDE），用于嵌入式系统的软件开发。它是由Keil Software公司开发的，主要用于编写、调试和测试嵌入式系统的程序。魔法棒是Keil5的图标，通常显示为一个灰色的魔法棒形状。 Keil5魔法棒的灰色外观是为了与其他图标进行区分，并且在视觉上更加突出。它代表了Keil5作为一个强大而神奇的工具，可以帮助开发人员在嵌入式系统开发中实现各种功能和任务。
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值