前篇硬體和結構已經準備完成，接下來就要進行嵌入式部分的開發，

1 環境搭建

1.1 產品創建

??在進行魔法棒的韌體開發之前，我們需要先在 涂鴉IoT平臺 上創建一個智能產品，還不熟悉產品創建的同學可以通過涂鴉開發者平臺的檔案中心進行了解，詳情點擊【快速入門】，

??魔法棒產品可以選擇【找不到品類】進行自定義創建，本 Demo 選用的是涂鴉 Bluetooth LE 模組 TYBN1，其芯片平臺是 nRF52832 ，因此，選擇通訊協議為【藍牙】，云端接入方式為【TuyaOS】，云端接入硬體為【Nordic nRF52832】，當然也可以選擇其他模組，可以參考開發者平臺的 涂鴉云模組規格書 進行選型，

??為實作云端接收魔法棒識別結果的目的，需在【功能定義】頁面自定義一個DP點，引數如下：

DP ID	功能點名稱	識別符號	資料傳輸型別	資料型別	功能點屬性
101	手勢	gesture	只上報 (ro)	列舉型 (Enum)	列舉值：none，ges1，ges2 …

1.2 SDK 獲取

• 原廠 SDK

??涂鴉 Bluetooth LE 云模組 TYBN1 使用的芯片平臺是 Nordic 的 nRF52832，我們可以在 Nordic 官網找到 nRF5 系列的 SDK (點擊進入下載頁面：nRF5 SDK downloads)，選擇 15.3.0 版本，SoftDevices 選擇版本介紹中涉及 nRF52832 的即可，最后點擊 Download files (.zip)，

• 涂鴉 SDK

??涂鴉提供了適用于 nRF52832 平臺的 SDK，可以在創建好的魔法棒產品的 硬體開發 頁面左下角找到最新版的 SDK ，下載完成后，需將涂鴉 SDK 檔案 tuya-ble-sdk-demo-project-nrf52832 拷貝到原廠 SDK 的nRF5_SDK_15.3.0_59ac345\examples\ble_peripheral目錄下，( 開發指南：藍牙 SDK 開發 )

1.3 IDE 準備

??涂鴉 SDK 中的工程是使用 MDK-ARM Keil μVision 創建的，還未安裝的同學可以前往官網下載，安裝好 IDE 后，雙擊涂鴉 SDK 中的工程檔案ble_app_uart_pca10040_s132.uvprojx，打開時會自動安裝軟體包，如果安裝失敗，可以在 Pack Installer 中找到 nRF52832 芯片對應的軟體包進行安裝，如下圖所示：

1.4 燒錄授權

• 模組購買

??如果同學們手邊沒有涂鴉云模組的話，可以在【硬體開發】頁面先【新增自定義韌體】，然后點擊右側的【立即購買】 進入模組購買流程，涂鴉提供多種燒錄授權方式，可根據不同的開發階段、芯片型別和生產方式進行選擇，詳情點擊 韌體燒錄授權，

• 激活碼領取

??單個模組也可以通過購買單個激活碼進行授權，新用戶可以在【硬體開發】頁面免費領取2個激活碼：

??交付形式選擇【授權碼清單】后提交訂單，就可以獲得2組激活碼，包括 uuid、authkey 和 mac地址，

• 授權資訊修改

??將產品 ID (PID) 和激活碼 (選取 1 組) 填入 tuya_ble_sdk_demo.h 檔案的以下位置：

#define TY_DEVICE_NAME        "demo"
#define TY_DEVICE_PID         "xxxxxxxx" /* PID */
#define TY_DEVICE_MAC         "xxxxxxxx" /* mac */
#define TY_DEVICE_DID         "xxxxxxxx" /* uuid */
#define TY_DEVICE_AUTH_KEY    "xxxxxxxx" /* authkey */

??然后還需要在 tuya_ble_sdk_demo.c 檔案中將初始化引數 use_ext_license_key 和device_id_len 的值分別修改為 1 和 16，以使激活碼生效：

static tuya_ble_device_param_t tuya_ble_device_param = {
    .use_ext_license_key = 1,	// 1-info in tuya_ble_sdk_demo.h, 0-auth info
    .device_id_len       = 16,	// DEVICE_ID_LEN,
    .p_type              = TUYA_BLE_PRODUCT_ID_TYPE_PID,
    .product_id_len      = 8,
    .adv_local_name_len  = 4,
    .firmware_version    = TY_DEVICE_FVER_NUM,
    .hardware_version    = TY_DEVICE_HVER_NUM,
};

• 硬體連接

??編譯通過后，將 J-Link 燒錄器連接到開發板，連線方式如下：

模組引腳	J-Link 引腳
3.3V	VCC
GND	GND
SWDIO	SWDIO
SWC	SWCLK

• 韌體下載

??在 Demo 韌體下載前，必須先下載協議堆疊韌體（同一塊模組只需操作 1 次），在 J-Link 官網下載 J-Link 軟體開發包，安裝完成后，打開 J-Flash 軟體，點擊 File - New Project 創建工程，芯片選擇 nRF52832_xxAA 后點擊 OK，如下圖所示：

??點擊 File - Open data file 打開涂鴉 SDK \pca10040\s132\arm5_no_packs\hex\material 目錄下的 s132_nrf52_6.1.1_softdevice.hex 檔案，如下圖所示：

??點擊 Target - Connect 連接芯片，成功連接后點擊 Target - Production Programming 開始下載，下載完成后點擊 Target - Disonnect 斷開連接，Demo 韌體可以直接通過 Keil 進行下載，

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：向右甩動； …
識別功能開關	可通過按鍵控制識別功能打開或關閉： - 按鍵按下時，手勢識別功能打開； - 按鍵釋放時，手勢識別功能關閉，
手勢資料上報	可將手勢資料上報至云端，以實作手勢動作對其他設備的控制，
配網狀態重置	可通過配網鍵實作設備端解綁： - 長按配網鍵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驅動 */
|    ├── sdk
|    |    ├── tuya_ble_bulk_data_demo.h    /* 大資料通道例程 */
|    |    ├── tuya_ble_product_test_demo.h /* 整機產測例程 */
|    |    └── tuya_ble_sdk_test.h          /* 實作tuya_ble_sdk測驗的串口指令 */
|    ├── tuya_ble_sdk_demo.h               /* 實作tuya_ble_sdk的初始化，應用程式入口 */
|    ├── tuya_imu_daq.h                    /* 傳感資料采集 */
|    ├── tuya_gesture_controller.h         /* 手勢控制器管理中心 */
|    ├── tuya_ges_act_rec.h                /* 手勢動作識別 */
|    ├── tuya_net_proc.h                   /* 設備聯網處理 */
|    └── tuya_svc_angle_calc.h             /* 姿態解算服務 */
└── src
     ├── driver
     |    ├── tuya_key.c                   /* 按鍵驅動 */
     |    ├── tuya_led.c                   /* LED驅動 */
     |    └── tuya_mpu6050.c               /* MPU6050驅動 */
     ├── platform
     |    └── tuya_gpio_nRF52832.c         /* nRF52832平臺關聯GPIO驅動 */
     ├── sdk
     |    ├── tuya_ble_bulk_data_demo.c    /* 大資料通道例程 */
     |    ├── tuya_ble_product_test_demo.c /* 整機產測例程 */
     |    └── tuya_ble_sdk_test.c          /* SDK測驗程式 */
     ├── tuya_ble_sdk_demo.c               /* 實作tuya_ble_sdk的初始化，應用程式入口 */
     ├── tuya_gesture_controller.c         /* 手勢控制器管理中心 */
     ├── tuya_imu_daq.c                    /* 傳感資料采集 */
     ├── tuya_ges_act_rec.c                /* 手勢動作識別 */
     ├── tuya_net_proc.c                   /* 設備聯網處理 */
     └── tuya_svc_angle_calc.c             /* 姿態解算服務 */

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	識別符號	說明
7	DEVICE_RESET	設備復位， 0：復位完成； 1：將所有內部暫存器重置為其默認值，復位完成后，該位自動清除為0，
6	SLEEP	睡眠模式設定， 0：設備正常作業； 1：進入低功耗睡眠模式，
5	CYCLE	回圈模式設定， 0：禁用回圈模式； 1：SLEEP=0時，MPU-60X0將進入回圈模式，設備在睡眠和喚醒之間回圈，
3	TEMP_DIS	溫度傳感器設定， 0：啟用溫度傳感器； 1：禁用溫度傳感器，
2:0	CLKSEL[2:0]	系統時鐘源選擇， 000：內部 8M RC 晶振； 001：PLL，使用X軸陀螺儀作為參考； 010：PLL，使用Y軸陀螺儀作為參考； 011：PLL，使用Z軸陀螺儀作為參考； 100：PLL，使用外部 32.768kHz 作為參考； 101：PLL，使用外部 19.2MHz 作為參考； 110：保留； 111：關閉時鐘，保持時序產生電路復位狀態，

??>> 設備ID校驗暫存器（0x75，WHO_AM_I，R，0x68）

Bit	識別符號	說明
6:1	WHO_AM_I[6:1]	設備地址高 6 位，默認為 0x68，

??>> 陀螺儀配置暫存器（0x1B，GYRO_CONFIG，R/W，0x00）

Bit	識別符號	說明
7:5	XYZG_ST	陀螺儀 X/Y/Z 軸自檢控制， 0：關閉自檢； 1：激活自檢，
4:3	FS_SEL[1:0]	陀螺儀滿量程范圍設定， 00：±250°/s； 01：±500°/s； 10：±1000°/s； 11：±2000°/s，

??>> 加速度計配置暫存器（0x1C，ACCEL_CONFIG，R/W，0x00）

Bit	識別符號	說明
7:5	XYZA_ST	加速度 X/Y/Z 軸自檢控制， 0：關閉自檢； 1：激活自檢，
4:3	AFS_SEL[1:0]	加速度計滿量程范圍設定， 00：±2g； 01：±4g； 10：±8g； 11：±16g，

??>> 采樣率分頻暫存器（0x19，SMPRT_DIV，R/W，0x00）

Bit	識別符號	說明
7:0	SMPLRT_DIV[7:0]	陀螺儀輸出速率分頻設定， - 采樣率 = 陀螺儀輸出速率 / (1 + SMPLRT_DIV)； - 禁用 DLPF (DLPF_CFG=0或7) 時，陀螺儀輸出速率為 8kHz； - 啟用 DLPF 時，陀螺儀輸出速率為 1kHz，

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

Bit	識別符號	說明
5:3	EXT_SYNC_SET[2:0]	外部幀同步 (FSYNC) 引腳采樣設定，
2:0	DLPF_CFG[2:0]	數字低通濾波器 (DLPF) 設定，

??>> 中斷引腳配置暫存器（0x37，INT_PIN_CFG，R/W）

Bit	識別符號	說明
7	INT_LEVEL	INT 引腳中斷電平設定， 0：高電平有效； 1：低電平有效，
6	INT_OPEN	INT 引腳輸出模式設定， 0：推挽輸出； 1：開漏輸出，
5	LATCH_INT_EN	中斷保持方式設定， 0：產生 50us 脈沖； 1：保持高電平直到中斷清除，
4	INT_RD_CLEAR	中斷狀態清除方式設定， 0：僅通過讀取 INT_STATUS 來清除中斷狀態位； 1：通過任何讀操作清除中斷狀態位，
3	FSYNC_INT_LEVEL	FSYNC 引腳中斷電平設定， 0：高電平有效； 1：低電平有效，
2	FSYNC_INT_EN	FSYNC 引腳中斷功能設定， 0：禁止 FSYNC 引腳作為主處理器的中斷引腳； 1：允許 FSYNC 引腳作為主處理器的中斷引腳，
1	I2C_BYPASS_EN	輔助 I2C 總線訪問權限設定， 0：禁止主處理器直接訪問輔助 I2C 總線； 1：I2C_MST_EN = 0 時，允許主處理器直接訪問輔助 I2C 總線，

??>> 中斷使能暫存器（0x38，INT_ENABLE，R/W，0x00）

Bit	識別符號	說明
6	MOT_EN	運動中斷設定， 0：禁用中斷； 1：啟用中斷，
4	FIFO_OFLOW_EN	FIFO快取區溢位中斷設定， 0：禁用中斷； 1：啟用中斷，
3	I2C_MST_INT_EN	I2C主機中斷設定， 0：禁用中斷； 1：啟用中斷，
0	DATA_RDY_EN	資料就緒中斷設定， 0：禁用中斷； 1：啟用中斷，

• 代碼實作

#include "tuya_mpu6050.h"
#include "ty_i2c.h"

/* device address */
#define MPU6050_DEV_ADDR_AD0_LOW        0x68
#define MPU6050_DEV_ADDR_AD0_HIGH       0x69
#define MPU6050_DEV_ADDR     		    MPU6050_DEV_ADDR_AD0_LOW
#define MPU6050_DEV_ID                  0x68

/* I2C R/W command */
#define I2C_CMD_WRITE                   0
#define I2C_CMD_READ                    1
#define MPU6050_ADDR_CMD_WRITE          ((MPU6050_DEV_ADDR << 1) | I2C_CMD_WRITE)
#define MPU6050_ADDR_CMD_READ           ((MPU6050_DEV_ADDR << 1) | I2C_CMD_READ)

/* register map */
#define MPU6050_RA_SMPRT_DIV            0x19
#define MPU6050_RA_CONFIG               0x1A
#define MPU6050_RA_GYRO_CONFIG          0x1B
#define MPU6050_RA_ACCEL_CONFIG         0x1C
#define MPU6050_RA_INT_PIN_CFG          0x37
#define MPU6050_RA_INT_ENABLE           0x38
#define MPU6050_RA_PWR_MGMT_1           0x6B
#define MPU6050_RA_WHO_AM_I             0x75

/* MPU6050 Gyro full-scale range */
typedef BYTE_T MPU_GYRO_FSR_E;
#define MPU_GYRO_FS_250             	0x00    /* 250dps */
#define MPU_GYRO_FS_500             	0x01    /* 500dps */
#define MPU_GYRO_FS_1000            	0x02    /* 1000dps */
#define MPU_GYRO_FS_2000            	0x03    /* 2000dps */

/* MPU6050 Accel full-scale range */
typedef BYTE_T MPU_ACCEL_FSR_E;
#define MPU_ACCEL_FS_2              	0x00    /* 2g */
#define MPU_ACCEL_FS_4              	0x01    /* 4g */
#define MPU_ACCEL_FS_8              	0x02    /* 8g */
#define MPU_ACCEL_FS_16             	0x03    /* 16g */

STATIC FLOAT_T sg_gyro_sens = 0.0f;
STATIC USHORT_T sg_accel_sens = 0;

/**
 * @brief reset MPU6050
 * @param[in] none
 * @return none
 */
STATIC VOID_T __mpu6050_reset(VOID_T)
{
    __mpu6050_write_register_bit(MPU6050_RA_PWR_MGMT_1, MPU_RA_BIT_DEVICE_RESET, MPU_RA_BIT_DEVICE_RESET);
    tuya_ble_device_delay_ms(100);
}

/**
 * @brief get the identity of the device (default: 0x68)
 * @param[in] none
 * @return device id
 */
STATIC UCHAR_T __mpu6050_get_device_id(VOID_T)
{
    return __mpu6050_read_register(MPU6050_RA_WHO_AM_I);
}

/**
 * @brief check if MPU6050 is connected
 * @param[in] none
 * @return TRUE - connected, FALSE - unconnected
 */
STATIC BOOL_T __mpu6050_is_connected(VOID_T)
{
    if (__mpu6050_get_device_id() == MPU6050_DEV_ID) {
        return TRUE;
    } else {
        return FALSE;
    }
}

/**
 * @brief enable or disable sleep mode
 * @param[in] enabled: TRUE - sleep, FALSE - work
 * @return none
 */
STATIC VOID_T __mpu6050_set_sleep_mode(_IN CONST BOOL_T enabled)
{
    if (enabled) {
        __mpu6050_write_register_bit(MPU6050_RA_PWR_MGMT_1, MPU_RA_BIT_SLEEP, MPU_RA_BIT_SLEEP);
    } else {
        __mpu6050_write_register_bit(MPU6050_RA_PWR_MGMT_1, ~MPU_RA_BIT_SLEEP, MPU_RA_BIT_SLEEP);
    }
}

/**
 * @brief enable or disable sleep mode
 * @param[in] enabled: TRUE - sleep, FALSE - work
 * @return none
 */
VOID_T tuya_mpu6050_set_sleep_mode(_IN CONST BOOL_T enabled)
{
    __mpu6050_set_sleep_mode(enabled);
}

/**
 * @brief set clock source
 * @param[in] src: clock source
 * @return none
 */
STATIC VOID_T __mpu6050_set_clk_src(UCHAR_T src)
{
    __mpu6050_write_register_bit(MPU6050_RA_PWR_MGMT_1, src, MPU_RA_BIT_CLKSEL);
}

/**
 * @brief set gyroscope's full-scale range
 * @param[in] range: gyroscope's full-scale range value
 * @return none
 */
STATIC VOID_T __mpu6050_set_gyro_fsr(_IN CONST MPU_GYRO_FSR_E range)
{
    __mpu6050_write_register_bit(MPU6050_RA_GYRO_CONFIG, range<<3, MPU_RA_BIT_FS_SEL);
}

/**
 * @brief set accelerometer's full-scale range
 * @param[in] range: new full-scale accelerometer range value
 * @return none
 */
STATIC VOID_T __mpu6050_set_accel_fsr(_IN CONST MPU_ACCEL_FSR_E range)
{
    __mpu6050_write_register_bit(MPU6050_RA_ACCEL_CONFIG, range<<3, MPU_RA_BIT_AFS_SEL);
}

/**
 * @brief set MPU6050's sample rate
 * @param[in] sr: sample rate gyroscope output rate divider value
 * @return none
 */
STATIC VOID_T __mpu6050_set_sample_rate(_IN USHORT_T sr)
{
    UCHAR_T div;
    if (sr > MPU_GYRO_OUTPUT_RATE) {
        sr = MPU_GYRO_OUTPUT_RATE;
    }
    if (sr < (MPU_GYRO_OUTPUT_RATE/MPU_SMPRT_DIV_MAX)) {
        sr = (MPU_GYRO_OUTPUT_RATE/MPU_SMPRT_DIV_MAX);
    }
    div = MPU_GYRO_OUTPUT_RATE / sr - 1;
    __mpu6050_write_register(MPU6050_RA_SMPRT_DIV, div);
}

/**
 * @brief set MPU6050's DLPF
 * @param[in] bw: baud width
 * @return none
 */
STATIC VOID_T __mpu6050_set_dlpf(_IN CONST USHORT_T bw)
{
    UCHAR_T cfg = 0;
    if (bw >= MPU_DLPF_BW_CFG_1) {
        cfg = 1;
    } else if (bw >= MPU_DLPF_BW_CFG_2) {
        cfg = 2;
    } else if (bw >= MPU_DLPF_BW_CFG_3) {
        cfg = 3;
    } else if (bw >= MPU_DLPF_BW_CFG_4) {
        cfg = 4;
    } else if (bw >= MPU_DLPF_BW_CFG_5) {
        cfg = 5;
    } else {
        cfg = 6;
    }
    __mpu6050_write_register(MPU6050_RA_CONFIG, cfg);
}

/**
 * @brief set intterupt
 * @param[in] active_low: TRUE - active low, FALSE - active high
 * @return none
 */
STATIC VOID_T __mpu6050_set_int(BOOL_T active_low)
{
    UCHAR_T reg_value = 0;
    if (active_low) {
        __mpu6050_write_register(MPU6050_RA_INT_PIN_CFG, 0x90);
    } else {
        __mpu6050_write_register(MPU6050_RA_INT_PIN_CFG, 0x50);
    }
    __mpu6050_write_register(MPU6050_RA_INT_ENABLE, 0x01);
}

/**
 * @brief MPU6050 sensor driver init
 * @param[in] clk: clock source
 * @param[in] g_fsr: gyroscope's full-scale range
 * @param[in] a_fsr: accelerometer's full-scale range
 * @param[in] smp_rt: sample rate
 * @param[in] pin: interrupt pin
 * @param[in] type: interrupt type
 * @param[in] int_cb: interrupt callback function
 * @return operation result
 */
MPU_RET tuya_mpu6050_init(_IN CONST MPU_CLK_E clk, _IN CONST MPU_GYRO_FSR_E g_fsr, _IN CONST MPU_ACCEL_FSR_E a_fsr,
                          _IN CONST USHORT_T smp_rt, _IN CONST TY_GPIO_PORT_E pin, _IN CONST TY_GPIO_IRQ_TYPE_E type,
                          _IN TY_GPIO_IRQ_CB int_cb)
{
    /* I2C init */
    i2c_soft_gpio_init();
    /* reset MPU6050 */
    __mpu6050_reset();
    /* check communication */
    if (!__mpu6050_is_connected()) {
        return MPU_ERR_UNCONN;
    }

    /* MPU6050 init */
    __mpu6050_set_sleep_mode(FALSE);    /* wakeup MPU6050 */
    __mpu6050_set_clk_src(clk);         /* set clock source */
    __mpu6050_set_gyro_fsr(g_fsr);      /* set gyroscope's full-scale range */
    __mpu6050_set_accel_fsr(a_fsr);     /* set accelerometer's full-scale range */
    __mpu6050_set_sample_rate(smp_rt);  /* set sample rate */
    __mpu6050_set_dlpf(smp_rt/2);       /* set DLPF */

    /* save sensitivity scale factor */
    sg_gyro_sens = 32768.0 / ((1 << g_fsr) * 250);
    sg_accel_sens = 32768.0 / ((1 << a_fsr) * 2);

    /* interrupt init */
    if (int_cb != NULL) {
        if (tuya_gpio_irq_init(pin, type, int_cb)) {
            return MPU_ERR_IRQ_INIT_FAILED;
        }
        if (TY_GPIO_IRQ_FALLING == type) {
            __mpu6050_set_int(TRUE);
        } else {
            __mpu6050_set_int(FALSE);
        }
    }

    return MPU_OK;
}

??另外，這次的硬體方案選擇了用模組引腳來控制 MPU6050 的電源，所以在初始化前要先拉高 MPU6050 的 VLOGIC 引腳（模組的 CT_POW 引腳）來為其供電：

#include "tuya_mpu6050.h"
#include "tuya_gpio.h"

#define DAQ_TIME_MS         5

#define MPU_CT_POW_PIN      TY_GPIO_16
#define MPU_INT_PIN         TY_GPIO_2

STATIC DAQ_END_CB sg_daq_end_cb = NULL;

/**
 * @brief set MPU6050 power on
 * @param[in] pin: VLOGIC pin number
 * @param[in] active_low: TRUE - active low, FALSE - active high
 * @return none
 */
VOID_T tuya_mpu6050_power_on(_IN CONST TY_GPIO_PORT_E pin, _IN CONST BOOL_T active_low)
{
    if (!sg_pwr_pin_used) {
        tuya_gpio_init(pin, FALSE, active_low);
        sg_pwr_pin_used = TRUE;
    }
    tuya_gpio_write(pin, !active_low);
    tuya_ble_device_delay_ms(100);
}

/**
 * @brief IMU DAQ module init
 * @param[in] none
 * @return none
 */
VOID_T tuya_imu_daq_init(VOID_T)
{
    tuya_mpu6050_power_on(MPU_CT_POW_PIN, TRUE);
    MPU_RET ret = tuya_mpu6050_init(MPU_CLK_PLL_XGYRO, MPU_GYRO_FS_2000, MPU_ACCEL_FS_16, 1000/DAQ_TIME_MS, MPU_INT_PIN, TY_GPIO_IRQ_FALLING, __new_data_ready_cb);
    if (MPU_OK != ret) {
        TUYA_APP_LOG_ERROR("tuya_mpu6050_init error: %d.", ret);
        return;
    }
}

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?(θ)=???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 軸的加速度差分絕對值之和作為手勢起終點判斷的特征量，該特征量大于設定閾值時，判定為手勢起點；小于設定閾值時，判定為手勢終點，

??同樣地，當我們以手臂為中心軸翻轉魔法棒時，加速度的資料變化情況也符合預期，

• 代碼實作

??以下函式用于計算加速度值的差分絕對值之和，并進行移動平均濾波，

#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] = *





      
  
      轉載請註明出處，本文鏈接：https://www.uj5u.com/qita/392242.html
      
標籤：其他 
      
上一篇：資訊安全期末題庫1
下一篇：Flash地址空間的資料讀取——STM32