一、SH3001简介
二、SH3001寄存器介绍
三、SH3001时序介绍
四、SH3001驱动步骤
五、编程实战1 六轴传感器-读取原始数据
六、姿态解算介绍
- 6.1、姿态角
- 6.2、利用加速度计测量角度（俯仰角与横滚角）
- 6.3、重力计算欧拉角、方向余弦矩阵
- 6.4、利用加速度计测量角度缺陷
- 6.5、利用陀螺仪检测姿态角
- 6.6、利用陀螺仪测量角度缺陷
- 6.7、传感器数据融合
- 6.8、姿态表示的方式：欧拉角、旋转矩阵、四元数
- 6.9、四元数的求解
一、SH3001简介
SH3001是一款由Senodia公司生产的集成三轴加速度计和三轴陀螺仪于一体的六轴姿态传感器，它能够实时测量芯片沿X、Y、Z轴的加速度和角速度，通过融合算法（如互补滤波、卡尔曼滤波等）可以计算出芯片在三维空间中的姿态角（如俯仰角、翻滚角、航向角）。
其主要特性包括：
-
陀螺仪量程：支持多个量程选项，分别为±125 dps、±250 dps、±500 dps、±1000 dps和±2000 dps，表示陀螺仪能够测量的角速度范围。
-
陀螺仪灵敏度：对应陀螺仪不同量程，有不同的灵敏度，例如在±125 dps量程下，灵敏度为262 LSB/s，随着量程增大，灵敏度减小。
-
加速度计量程：可选择±2 g、±4 g、±8 g、±16 g四种量程，用于测量重力加速度及动态加速度。
-
加速度计灵敏度：对应加速度计不同量程，其灵敏度分别为16384 LSB/g、8192 LSB/g、4096 LSB/g、2048 LSB/g，灵敏度越小，测量精度越高。
-
内置数字输出温度传感器：传感器集成了温度感应元件，能够提供数字形式的温度输出，帮助进行温度补偿，提高测量准确性。
SH3001六轴传感器广泛应用于各类消费电子和物联网设备中，如智能手机、平板电脑、智能手表、无人机、电动平衡车、运动相机手持稳定器等，以实现精确的运动跟踪、姿态识别、导航定位等功能。如果需要更多维度的运动参数测量，可以将SH3001与其他传感器（如三轴磁力计、气压计等）结合使用，构建九轴或十轴传感器系统。
SH3001六轴姿态传感器内部集成了三轴加速度计和三轴陀螺仪，通常采用I²C（Inter-Integrated Circuit）或SPI（Serial Peripheral Interface）通信接口与微控制器（MCU）进行数据交换。
结构概述：
- 三轴加速度计：用于检测X、Y、Z三个轴向上的加速度，可以感知静态重力加速度和动态加速度。
- 三轴陀螺仪：用于测量芯片绕X、Y、Z轴旋转的角速度，反映芯片转动情况。
- 数字接口：SH3001通过I²C或SPI接口与MCU通信，传输测量得到的加速度和角速度数据，以及其他配置信息和状态数据。
- 内部处理单元：负责采集传感器原始数据，对其进行处理（如温度补偿、噪声抑制等），并将处理后的数据打包通过接口传输出去。
- 温度传感器：用于检测芯片工作环境的温度，提供温度数据，支持传感器的温度补偿功能。
I²C通信原理：
- SDA（Serial Data Line）：数据线，用来传输数据。
- SCK（Serial Clock Line）：时钟线，由主设备（通常是MCU）提供时钟信号，控制数据传输的节奏。
- 在I²C通信中，MCU作为主设备，SH3001作为从设备，通过设置正确的从设备地址，MCU可以向SH3001发送读取数据或写入配置命令。
- 数据传输时，MCU首先发送起始信号（START），接着发送从设备地址和读写位（R/W位），然后是寄存器地址或数据，最后是STOP信号结束一次通信。
- 若是读取数据，SH3001在接收到读命令后，会通过SDA线发送指定寄存器中的数据。
SPI通信原理（若SH3001支持SPI）：
- SPI通信通常包括MISO（Master Input Slave Output）、MOSI（Master Output Slave Input）、SCLK（Serial Clock）和CS（Chip Select）信号线。
- MCUI通过SCLK提供时钟信号，通过CS选择SH3001开始通信，然后通过MOSI线发送命令或配置信息，从MISO线接收数据。
无论是I²C还是SPI，都需要查阅SH3001的具体数据手册，以了解详细的通信协议、寄存器映射、命令格式等内容，以便正确配置和读取传感器数据。
SH3001六轴传感器的引脚功能如下：
-
SDO（Slave Data Output）：有时也标注为A0，用于设置I²C总线上的器件地址，通过连接不同电平至该引脚，可以改变SH3001在I²C网络中的唯一地址。
-
MSDA（Master-Slave Data Line A）：在SPI模式下，此引脚作为主设备数据输出到从设备的数据线，但在SH3001默认的I²C模式下，可能不使用该引脚。
-
MCLK（Master Clock）：在SPI模式下，此引脚作为主设备向从设备提供的时钟信号线。在I²C模式下，此引脚未被使用。
-
INT（Interrupt）：数字中断输出引脚，当传感器内部的特定事件发生时（如数据准备好、阈值触发等），该引脚会产生中断信号。
-
VDDIO（Digital I/O Supply Voltage）：为数字I/O引脚提供电源电压。
-
GND（Ground）：接地引脚，连接到系统的地线。
7、10、11：RESV（Reserved）：预留引脚，目前未定义功能，建议在设计时保持悬空或接地。
-
VDD（Supply Voltage）：电源电压引脚，用于为SH3001提供工作电压。
-
INT1（Interrupt 1）：第二个中断数字输出引脚，同INT引脚功能相似，但可能对应不同的中断源。
-
SENB（Serial Enable）：I²C和SPI选择引脚，当此引脚为高电平时（1），SH3001工作在I²C模式；为低电平时（0），工作在SPI模式。
-
SCK（Serial Clock）：在I²C模式下，此引脚作为I²C通信时钟线，由主设备（MCU）提供时钟信号。
-
SDA（Serial Data Line）：在I²C模式下，此引脚作为I²C通信数据线，用于数据的传输。
根据描述，SH3001默认工作在I²C模式下，上电后会根据SENB引脚的电平自动选择工作模式。在实际应用中，需要根据具体需求配置好SENB引脚和其他相关引脚，以确保SH3001正常工作并与微控制器进行有效的数据通信。
在SH3001六轴传感器中，电源模式可以根据不同的应用场景和功耗需求进行切换：
-
正常模式（高性能模式，全检测）：
- 在正常模式下，SH3001的所有传感器均处于工作状态，包括三轴加速度计、三轴陀螺仪以及内置的温度传感器。这种模式下可以获得完整的运动和姿态信息，但功耗相对较高。
-
睡眠模式：
- 在睡眠模式中，SH3001仅保留加速度计继续工作，陀螺仪和可能的其他附加传感器（如磁力计等）会关闭。这种模式下可以节省能源，适用于只需要检测加速度信息且无需频繁获取旋转信息的场景，比如设备静止时或低功耗待机状态。
-
掉电模式：
- 掉电模式是最低功耗的模式，此时SH3001几乎不消耗电力，所有传感器（包括加速度计、陀螺仪和温度传感器）都不工作。设备在此模式下无法检测任何运动或姿态数据，仅在唤醒机制触发时恢复到正常工作状态。此模式适用于长时间休眠或者关机状态，以最大限度延长电池寿命。
在实际应用中，开发者可以根据产品需求适时切换这些电源模式，以达到性能、功耗和电池续航的最佳平衡。通常通过向SH3001发送相应的控制命令来切换电源模式。
二、SH3001寄存器介绍
SH3001六轴传感器的寄存器列表及其功能如下：
-
加速度计数据寄存器
- 0x00 和 0x01：ACC_XDATA_L/H，存放加速度计X轴的16位测量数据，低八位在0x00寄存器，高八位在0x01寄存器。
- 0x02 和 0x03：ACC_YDATA_L/H，存放加速度计Y轴的16位测量数据。
- 0x04 和 0x05：ACC_ZDATA_L/H，存放加速度计Z轴的16位测量数据。
-
陀螺仪数据寄存器
- 0x06 和 0x07：GYRO_XDATA_L/H，存放陀螺仪X轴的16位测量数据。
- 0x08 和 0x09：GYRO_YDATA_L/H，存放陀螺仪Y轴的16位测量数据。
- 0x0A 和 0x0B：GYRO_ZDATA_L/H，存放陀螺仪Z轴的16位测量数据。
-
温度数据寄存器
- 0x0C 和 0x0D：TEMP_DATA_L/H，存放温度传感器的12位测量数据，低八位在0x0C寄存器，高四位在0x0D寄存器的低四位。
-
传感器配置寄存器
- 0x22 至 0x23、0x25 至 0x26：ACC_CONFIG_x(0~3)，用于配置加速度计的各项参数，如数字滤波器、采样频率、满量程范围和低通滤波器设置等。
- 0x28 至 0x2B、0x8F、0x9F、0xAF：GYRO_CONFIG_x(0~5)，用于配置陀螺仪的类似参数。
- 0x20 至 0x21、0xD5：T_CONFIGx(0~3)，用于配置温度传感器的采样频率、启用/禁用温度传感器、设置或获取室温值等参数。
-
器件ID寄存器
- 0x0F：CHIP_ID，存储着SH3001的默认器件ID，正常情况下读取值应为0x61，用于验证器件是否正确连接和工作。
在实际使用中，通过I²C或SPI接口与微控制器（MCU）通信，向这些寄存器写入配置信息或从这些寄存器读取测量数据。具体配置细节和寄存器位含义需要查阅SH3001的产品手册以获取详细信息。
如何从SH3001传感器中读取并合并加速度计、陀螺仪和温度数据的低字节和高字节，以获得完整的16位测量值。以下是简要解读：
-
加速度计数据获取：
acc_x = (ACC_XDATA_H << 8) | ACC_XDATA_L; acc_y = (ACC_YDATA_H << 8) | ACC_YDATA_L; acc_z = (ACC_ZDATA_H << 8) | ACC_ZDATA_L;这里将加速度计X、Y、Z轴的高八位数据左移8位与低八位数据进行“或”运算，组合成16位的测量值。
-
陀螺仪数据获取：
gyro_x = (GYRO_XDATA_H << 8) | GYRO_XDATA_L; gyro_y = (GYRO_YDATA_H << 8) | GYRO_YDATA_L; gyro_z = (GYRO_ZDATA_H << 8) | GYRO_ZDATA_L;同样的方式读取陀螺仪X、Y、Z轴的16位测量值。为了得到更精确的数据，可能需要通过厂家提供的函数接口读取并应用补偿系数。
-
温度数据转换：
reg_temp = ((TEMP_DATA_H & 0x0F) << 8) | TEMP_DATA_L; temperature = (reg_temp / 16.0f) / 25.0f; // 直接转换为摄氏温度先将温度数据寄存器的高四位与低八位合并，然后按比例转换为温度值。为了进一步提高精度，可以考虑室温数据：
room_temp = (T_CONFIG0 & 0x0F) | (T_CONFIG1 << 4); // 获取室温值 more_precise_temp = ((reg_temp - room_temp) / 16.0f) / 25.0f; // 计算更精确的温度值此处首先获取室温寄存器中的值，然后从原始温度数据中减去室温值，再进行温度转换计算，以获得更接近真实环境温度的结果。在实际应用中，请根据具体传感器数据手册的指导进行数据处理和温度补偿。
三、SH3001时序介绍
3.1、SH3001寻址
SH3001六轴传感器在I²C总线上的寻址方式如下：
-
设备地址：SH3001的固定部分地址为0b0110110，加上硬件选择位（例如A0引脚的电平状态），共同构成了7位的设备地址。若A0引脚接高电平或悬空，则7位地址为0x36。
-
通讯地址：在I²C通信时，除了7位设备地址外，还需要加上一位数据传输方向位。当这位置1时，表示接下来进行的是读操作；当这位置0时，表示进行的是写操作。
-
写操作地址：若要向SH3001写入数据，则需要将设备地址扩展为8位，并将最低位设置为0，因此写操作地址为0x6C（即0b01101100）。
-
读操作地址：若要从SH3001读取数据，则需要将设备地址扩展为8位，并将最低位设置为1，因此读操作地址为0x6D（即0b01101101）。
在实际的I²C通信过程中，主设备（如微控制器）会根据需要，向正确的写操作地址或读操作地址发送起始信号，然后进行数据的写入或读取。
3.2、读写时序
SH3001通过I²C接口进行读写操作的时序大致如下：
IIC单字节写入时序：
- Start Condition：主设备（通常为微控制器）发出起始信号（S）。
- Send Device Address + Write Bit：主设备发送SH3001的7位设备地址（0x36，取决于A0引脚状态），并在最低位添加写入标志位（0），形成8位的写入地址（0x6C）。
- Wait for ACK：从设备（SH3001）在接收到地址后，若同意通信，则发送一个ACK（Acknowledgement）信号。
- Send Register Address：主设备发送要写入的寄存器地址。
- Wait for ACK：从设备再次发送ACK信号确认寄存器地址已接收。
- Send Data Byte：主设备发送要写入寄存器的数据字节。
- Wait for ACK：从设备发送ACK确认数据已接收。
- Stop Condition：主设备发出停止信号（P），结束此次写入过程。
IIC多字节写入时序：
在单字节写入基础上，只需在发送完一个数据字节后，不发送停止信号而是继续发送下一个要写入的数据字节，直到所有数据发送完毕，最后才发送停止信号。
IIC单字节读取时序：
- Start Condition：主设备发出起始信号（S）。
- Send Device Address + Read Bit：主设备发送SH3001的7位设备地址，并在最低位添加读取标志位（1），形成8位的读取地址（0x6D）。
- Wait for ACK：从设备发送ACK信号。
- Receive Data Byte：从设备将数据字节放在数据线上，主设备读取该数据。
- Send ACK/NAK：主设备根据是否继续读取下一个字节，发送ACK（继续读取）或NAK（结束读取）信号。
- Stop Condition（仅在读取最后一个字节时）：主设备在读取完所有需要的数据后，发出停止信号（P）。
IIC多字节读取时序：
在单字节读取基础上，读取完一个数据字节后，主设备发送ACK信号并再次发出重复起始信号（Repeat Start），接着发送读取地址（0x6D），继续读取下一个数据字节，如此反复，直到所有需要的数据字节都被读取，最后才发送停止信号（P）结束整个读取过程。
四、SH3001驱动步骤
以下是基于您描述的SH3001驱动步骤的详细说明：
-
初始化SH3001
- 调用通用的I²C接口初始化函数
iic_init()，设置I²C通信的基本参数，包括时钟频率、设备地址等，确保MCU能够成功与SH3001建立通信连接。
- 调用通用的I²C接口初始化函数
-
编写SH3001基础读写接口函数
- 编写针对SH3001的专用读写函数，包括但不限于：
write_to_sh3001(uint8_t reg_addr, uint8_t *data, uint8_t len)：用于向SH3001写入多个字节数据，参数包括要写入的寄存器地址、数据缓冲区指针和数据长度。read_from_sh3001(uint8_t reg_addr, uint8_t *data, uint8_t len)：用于从SH3001读取多个字节数据，参数包括要读取的寄存器地址、数据缓冲区指针和数据长度。
- 编写针对SH3001的专用读写函数，包括但不限于：
-
重置内部模块
- 根据SH3001的数据手册，调用相应的控制命令或写入特定寄存器值来启动驱动、ADC复位和CVA复位（如果适用）。
-
配置加速度计、陀螺仪、温度传感器
- 分别访问相应的寄存器，设置加速度计和陀螺仪的输出数据频率、量程、截止频率以及是否启用数字滤波器。
- 配置温度传感器的输出频率和是否开启温度测量功能。
-
选择SH3001电源模式
- 调用特定的函数（例如
set_power_mode_to_normal()）来设置SH3001为正常模式，确保加速度计、陀螺仪和温度传感器都在工作状态。
- 调用特定的函数（例如
-
读取原始数据
- 通过之前编写的读取函数，分别读取加速度计（X、Y、Z轴）、陀螺仪（X、Y、Z轴）和温度传感器的数据。
- 如果需要更精确的数据，可以考虑读取并应用厂家提供的补偿系数来修正原始数据，以减少传感器本身的零点误差和温漂等影响。
在实际开发过程中，还需要密切关注SH3001的数据手册和厂家提供的API函数，确保按照正确的时序和协议进行操作。
五、编程实战1 六轴传感器-读取原始数据
源码
sh3001.c
#include "./BSP/IIC/myiic.h"
#include "./BSP/SH3001/sh3001.h"
#include "./SYSTEM/usart/usart.h"
#include "./SYSTEM/delay/delay.h"
#include "./BSP/LCD/lcd.h"
#include <math.h>
static compCoefType g_compcoef; /* compCoefType结构体,用于存储补偿系数 */
static uint8_t g_store_acc_odr; /* 保存ACC ODR临时变量 */
/**
* @brief 从SH3001读取N个字节数据
* @param devaddr : 器件地址
* @param regaddr : 寄存器地址
* @param length : 读取长度
* @param readbuf : 数据存储buf
* @retval 0, 操作成功
* 其他, 操作失败
*/
uint8_t sh3001_read_nbytes(uint8_t devaddr, uint8_t regaddr, uint8_t length, uint8_t *readbuf)
{
uint8_t i;
iic_start(); /* 主机发送起始信号 */
iic_send_byte(devaddr << 1 | 0X00); /* 主机发送写操作地址(器件地址 + 写命令) */
if (iic_wait_ack()) /* 主机等待SH3001响应 */
{
iic_stop(); /* 接收应答信号失败，直接发送停止信号 */
return SH3001_FALSE;
}
iic_send_byte(regaddr); /* 主机发送寄存器地址 */
iic_wait_ack(); /* 主机等待SH3001响应 */
iic_start(); /* 主机重新发送起始信号 */
iic_send_byte(devaddr << 1 | 0X01); /* 主机发送读操作地址(器件地址 + 读命令) */
iic_wait_ack(); /* 主机等待SH3001响应 */
for (i = 0; i < length; i++) /* 循环读取 数据 */
{
readbuf[i] = iic_read_byte(i == (length - 1) ? 0 : 1); /* 主机读取到寄存器的数据并发送ACK(最后一次读发送NACK) */
}
iic_stop(); /* 主机发送停止信号 */
return SH3001_TRUE;
}
/**
* @brief SH3001写入N个字节数据
* @param devaddr : 器件地址
* @param regaddr : 寄存器地址
* @param length : 写入长度
* @param writebuf : 数据存储buf
* @retval 0, 操作成功
* 1, 操作失败
*/
uint8_t sh3001_write_nbytes(uint8_t devaddr, uint8_t regaddr, uint8_t length, uint8_t *writebuf)
{
uint8_t i;
iic_start(); /* 主机发送起始信号 */
iic_send_byte(devaddr << 1 | 0X00); /* 主机发送写操作地址(器件地址 + 写命令) */
iic_wait_ack(); /* 主机等待SH3001响应 */
iic_send_byte(regaddr); /* 主机发送要操作的寄存器地址 */
iic_wait_ack(); /* 主机等待SH3001响应 */
for (i = 0; i < length; i++) /* 连续写入数据 */
{
iic_send_byte(writebuf[i]); /* 主机发送要写入到寄存器的数据 */
if (iic_wait_ack()) /* 主机等待SH3001响应 */
{
iic_stop(); /* 接收应答信号失败，直接发送停止信号 */
return (SH3001_FALSE);
}
}
iic_stop(); /* 主机发送停止信号 */
return (SH3001_TRUE);
}
/**
* @brief 设置加速度计参数
* @param acc_odr acc_range acc_cutofffreq acc_filter_enble
* SH3001_ODR_1000HZ SH3001_ACC_RANGE_16G SH3001_ACC_ODRX040 SH3001_ACC_FILTER_EN
* SH3001_ODR_500HZ SH3001_ACC_RANGE_8G SH3001_ACC_ODRX025 SH3001_ACC_FILTER_DIS
* SH3001_ODR_250HZ SH3001_ACC_RANGE_4G SH3001_ACC_ODRX011
* SH3001_ODR_125HZ SH3001_ACC_RANGE_2G SH3001_ACC_ODRX004
* SH3001_ODR_63HZ
* SH3001_ODR_31HZ
* SH3001_ODR_16HZ
* SH3001_ODR_2000HZ
* SH3001_ODR_4000HZ
* SH3001_ODR_8000HZ
* @retval 无
*/
static void sh3001_acc_config(uint8_t acc_odr, uint8_t acc_range, uint8_t acc_cutofffreq, uint8_t acc_filter_enble)
{
uint8_t regdata = 0;
/* 使能加速度计数字滤波器 */
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_ACC_CONF0, 1, ®data);
regdata |= 0x01;
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_ACC_CONF0, 1, ®data); /* 对SH3001_ACC_CONF0[0]置1，使能数字滤波器 */
/* 设置采样频率 */
g_store_acc_odr = acc_odr;
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_ACC_CONF1, 1, &acc_odr); /* 对SH3001_ACC_CONF1[3:0]设置 */
/* 设置加速度计满量程范围 */
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_ACC_CONF2, 1, &acc_range); /* 对SH3001_ACC_CONF2[2:0]设置 */
/* 设置低通滤波器 */
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_ACC_CONF3, 1, ®data);
regdata &= 0x17; /* 目的：清零要设置位 */
regdata |= (acc_cutofffreq | acc_filter_enble);
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_ACC_CONF3, 1, ®data); /* 对SH3001_ACC_CONF3[7:5]设置低通滤波器截止频率 SH3001_ACC_CONF3[3]设置是否旁路低通滤波器 */
}
/**
* @brief 设置陀螺仪参数
* @param gyro_odr gyro_range_x,y,z gyro_cutofffreq gyro_filter_enble
* SH3001_ODR_1000HZ SH3001_GYRO_RANGE_125 SH3001_GYRO_ODRX00 SH3001_GYRO_FILTER_EN
* SH3001_ODR_500HZ SH3001_GYRO_RANGE_250 SH3001_GYRO_ODRX01 SH3001_GYRO_FILTER_DIS
* SH3001_ODR_250HZ SH3001_GYRO_RANGE_500 SH3001_GYRO_ODRX02
* SH3001_ODR_125HZ SH3001_GYRO_RANGE_1000 SH3001_GYRO_ODRX03
* SH3001_ODR_63HZ SH3001_GYRO_RANGE_2000
* SH3001_ODR_31HZ
* SH3001_ODR_16HZ
* SH3001_ODR_2000HZ
* SH3001_ODR_4000HZ
* SH3001_ODR_8000HZ
* SH3001_ODR_16000HZ
* SH3001_ODR_32000HZ
* @retval 无
*/
static void sh3001_gyro_config(uint8_t gyro_odr, uint8_t gyro_range_x, uint8_t gyro_range_y, uint8_t gyro_range_z, uint8_t gyro_cutofffreq, uint8_t gyro_filter_enble)
{
uint8_t regdata = 0;
/* 使能陀螺仪数字滤波器 */
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_GYRO_CONF0, 1, ®data);
regdata |= 0x01;
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_GYRO_CONF0, 1, ®data); /* SH3001_GYRO_CONF0[0]置1，使能数字滤波器 */
/* 设置采样频率 */
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_GYRO_CONF1, 1, &gyro_odr); /* SH3001_GYRO_CONF1[0:3]设置 */
/* 设置满量程范围 */
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_GYRO_CONF3, 1, &gyro_range_x); /* SH3001_GYRO_CONF3[2:0]设置X轴 */
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_GYRO_CONF4, 1, &gyro_range_y); /* SH3001_GYRO_CONF4[2:0]设置Y轴 */
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_GYRO_CONF5, 1, &gyro_range_z); /* SH3001_GYRO_CONF5[2:0]设置Z轴 */
/* 设置低通滤波器 */
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_GYRO_CONF2, 1, ®data);
regdata &= 0xE3; /* 目的：清零要设置位 */
regdata |= (gyro_cutofffreq | gyro_filter_enble);
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_GYRO_CONF2, 1, ®data); /* SH3001_GYRO_CONF2[3:2]设置低通滤波器截止频率 SH3001_GYRO_CONF2[4]设置是否旁路低通滤波器 */
}
/**
* @brief 设置温度参数
* @param temp_odr
* SH3001_TEMP_ODR_500
* SH3001_TEMP_ODR_250
* SH3001_TEMP_ODR_125
* SH3001_TEMP_ODR_63
* @param temp_enable
* SH3001_TEMP_EN
* SH3001_TEMP_DIS
* @retval 无
*/
static void sh3001_temp_config(uint8_t temp_odr, uint8_t temp_enable)
{
uint8_t regdata = 0;
/* 设置温度传感器参数 */
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_TEMP_CONF0, 1, ®data);
regdata &= 0x4F; /* 目的：清零要设置位 */
regdata |= (temp_odr | temp_enable);
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_TEMP_CONF0, 1, ®data); /* SH3001_TEMP_CONF0[5:4]设置采样频率 SH3001_TEMP_CONF0[7]设置是否使能温度传感器 */
}
/**
* @brief 读取补偿系数
* @param g_compcoef : compCoefType结构体指针
* @retval 无
*/
static void sh3001_comp_init(compCoefType *g_compcoef)
{
uint8_t coefdata[2] = {0};
/* Acc Cross */
sh3001_read_nbytes(SH3001_ADDRESS, 0x81, 2, coefdata);
g_compcoef->cYX = (int8_t)coefdata[0];
g_compcoef->cZX = (int8_t)coefdata[1];
sh3001_read_nbytes(SH3001_ADDRESS, 0x91, 2, coefdata);
g_compcoef->cXY = (int8_t)coefdata[0];
g_compcoef->cZY = (int8_t)coefdata[1];
sh3001_read_nbytes(SH3001_ADDRESS, 0xA1, 2, coefdata);
g_compcoef->cXZ = (int8_t)coefdata[0];
g_compcoef->cYZ = (int8_t)coefdata[1];
/* Gyro Zero */
sh3001_read_nbytes(SH3001_ADDRESS, 0x60, 1, coefdata);
g_compcoef->jX = (int8_t)coefdata[0];
sh3001_read_nbytes(SH3001_ADDRESS, 0x68, 1, coefdata);
g_compcoef->jY = (int8_t)coefdata[0];
sh3001_read_nbytes(SH3001_ADDRESS, 0x70, 1, coefdata);
g_compcoef->jZ = (int8_t)coefdata[0];
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_GYRO_CONF3, 1, coefdata);
coefdata[0] = coefdata[0] & 0x07;
g_compcoef->xMulti = ((coefdata[0] < 2) || (coefdata[0] >= 7)) ? 1 : (1 << (6 - coefdata[0]));
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_GYRO_CONF4, 1, coefdata);
coefdata[0] = coefdata[0] & 0x07;
g_compcoef->yMulti = ((coefdata[0] < 2) || (coefdata[0] >= 7)) ? 1 : (1 << (6 - coefdata[0]));
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_GYRO_CONF5, 1, coefdata);
coefdata[0] = coefdata[0] & 0x07;
g_compcoef->zMulti = ((coefdata[0] < 2) || (coefdata[0] >= 7)) ? 1 : (1 << (6 - coefdata[0]));
sh3001_read_nbytes(SH3001_ADDRESS, 0x2E, 1, coefdata);
g_compcoef->paramP0 = coefdata[0] & 0x1F;
}
/**
* @brief 复位部分内部模块
* @param 无
* @retval 无
*/
static void sh3001_module_reset(void)
{
const uint8_t regaddr[8] = {0xC0, 0xD3, 0xD3, 0xD5, 0xD4, 0xBB, 0xB9, 0xBA};
/* 芯片版本批次：MCC、MCD、MCF */
/* MCC版本配置 */
const uint8_t mcc_regdata_a[8] = {0x38, 0xC6, 0xC1, 0x02, 0x0C, 0x18, 0x18, 0x18};
const uint8_t mcc_regdata_b[8] = {0x3D, 0xC2, 0xC2, 0x00, 0x04, 0x00, 0x00, 0x00};
/* MCD版本配置 */
const uint8_t mcd_regdata_a[8] = {0x38, 0xD6, 0xD1, 0x02, 0x08, 0x18, 0x18, 0x18};
const uint8_t mcd_regdata_b[8] = {0x3D, 0xD2, 0xD2, 0x00, 0x00, 0x00, 0x00, 0x00};
/* MCF版本配置 */
const uint8_t mcf_regdata_a[8] = {0x38, 0x16, 0x11, 0x02, 0x08, 0x18, 0x18, 0x18};
const uint8_t mcf_regdata_b[8] = {0x3E, 0x12, 0x12, 0x00, 0x00, 0x00, 0x00, 0x00};
uint8_t regdata = 0;
uint8_t *regdata_a = (uint8_t *)mcd_regdata_a; /* 默认用MCD配置 */
uint8_t *regdata_b = (uint8_t *)mcd_regdata_b;
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_CHIP_VERSION, 1, ®data); /* 读取 SH3001_CHIP_VERSION */
if (regdata == SH3001_CHIP_VERSION_MCC) /* MCC版本 */
{
regdata_a = (uint8_t *)mcc_regdata_a;
regdata_b = (uint8_t *)mcc_regdata_b;
}
else if (regdata == SH3001_CHIP_VERSION_MCF) /* MCF版本 */
{
regdata_a = (uint8_t *)mcf_regdata_a;
regdata_b = (uint8_t *)mcf_regdata_b;
}
/* Drive Start */
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[0], 1, ®data_a[0]);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[1], 1, ®data_a[1]);
delay_ms(100);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[0], 1, ®data_b[0]);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[1], 1, ®data_b[1]);
delay_ms(50);
/* ADC Resett */
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[2], 1, ®data_a[2]);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[3], 1, ®data_a[3]);
delay_ms(1);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[2], 1, ®data_b[2]);
delay_ms(1);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[3], 1, ®data_b[3]);
delay_ms(50);
/* CVA Resett */
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[4], 1, ®data_a[4]);
delay_ms(10);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[4], 1, ®data_b[4]);
delay_ms(1);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[5], 1, ®data_a[5]);
delay_ms(10);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[6], 1, ®data_a[6]);
delay_ms(10);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[7], 1, ®data_a[7]);
delay_ms(10);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[5], 1, ®data_b[5]);
delay_ms(10);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[6], 1, ®data_b[6]);
delay_ms(10);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[7], 1, ®data_b[7]);
delay_ms(10);
/* 设置 INT and INT1 脚开漏输出 */
regdata = 0x00;
sh3001_write_nbytes(SH3001_ADDRESS, 0x44, 1, ®data);
}
/**
* @brief 切换SH3001电源模式
* Normal mode: 1.65mA; Sleep mode: 162uA; Acc normal mode:393uA;
* @param powermode
* SH3001_NORMAL_MODE
* SH3001_SLEEP_MODE
* SH3001_POWERDOWN_MODE
* SH3001_ACC_NORMAL_MODE
* @retval SH3001_TRUE 成功
* SH3001_FALSE 异常
*/
uint8_t sh3001_switch_powermode(uint8_t powermode)
{
uint8_t regaddr[10] = {0xCF, 0x22, 0x2F, 0xCB, 0xCE, 0xD3, 0xD4, 0xD5, 0xD6, 0xD7};
uint8_t regdata[10] = {0};
uint8_t i = 0;
uint8_t acc_odr = SH3001_ODR_1000HZ;
if ((powermode != SH3001_NORMAL_MODE) && (powermode != SH3001_SLEEP_MODE) && (powermode != SH3001_POWERDOWN_MODE) && (powermode != SH3001_ACC_NORMAL_MODE))
{
return (SH3001_FALSE);
}
for (i = 0; i < 10; i++)
{
sh3001_read_nbytes(SH3001_ADDRESS, regaddr[i], 1, ®data[i]);
}
switch (powermode)
{
case SH3001_NORMAL_MODE:
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_ACC_CONF1, 1, &g_store_acc_odr); /* 恢复accODR */
regdata[0] = (regdata[0] & 0xF8);
regdata[1] = (regdata[1] & 0x7F);
regdata[2] = (regdata[2] & 0xF7);
regdata[3] = (regdata[3] & 0xF7);
regdata[4] = (regdata[4] & 0xFE);
regdata[5] = (regdata[5] & 0xFC) | 0x02;
regdata[6] = (regdata[6] & 0x9F);
regdata[7] = (regdata[7] & 0xF9);
for (i = 0; i < 8; i++)
{
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[i], 1, ®data[i]);
}
regdata[7] = (regdata[7] & 0x87);
regdata[8] = (regdata[8] & 0x1F);
regdata[9] = (regdata[9] & 0x03);
for (i = 7; i < 10; i++)
{
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[i], 1, ®data[i]);
}
sh3001_module_reset();
return (SH3001_TRUE);
case SH3001_SLEEP_MODE:
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_ACC_CONF1, 1, &g_store_acc_odr); /* 保存当前的acc ODR */
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_ACC_CONF1, 1, &acc_odr); /* 设置acc ODR=1000Hz */
regdata[0] = (regdata[0] & 0xF8) | 0x07;
regdata[1] = (regdata[1] & 0x7F) | 0x80;
regdata[2] = (regdata[2] & 0xF7) | 0x08;
regdata[3] = (regdata[3] & 0xF7) | 0x08;
regdata[4] = (regdata[4] & 0xFE);
regdata[5] = (regdata[5] & 0xFC) | 0x01;
regdata[6] = (regdata[6] & 0x9F);
regdata[7] = (regdata[7] & 0xF9) | 0x06;
for (i = 0; i < 8; i++)
{
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[i], 1, ®data[i]);
}
regdata[7] = (regdata[7] & 0x87);
regdata[8] = (regdata[8] & 0x1F);
regdata[9] = (regdata[9] & 0x03);
for (i = 7; i < 10; i++)
{
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[i], 1, ®data[i]);
}
return (SH3001_TRUE);
case SH3001_POWERDOWN_MODE:
regdata[0] = (regdata[0] & 0xF8);
regdata[1] = (regdata[1] & 0x7F) | 0x80;
regdata[2] = (regdata[2] & 0xF7) | 0x08;
regdata[3] = (regdata[3] & 0xF7) | 0x08;
regdata[4] = (regdata[4] & 0xFE);
regdata[5] = (regdata[5] & 0xFC) | 0x01;
regdata[6] = (regdata[6] & 0x9F) | 0x60;
regdata[7] = (regdata[7] & 0xF9) | 0x06;
for (i = 0; i < 8; i++)
{
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[i], 1, ®data[i]);
}
regdata[7] = (regdata[7] & 0x87);
regdata[8] = (regdata[8] & 0x1F);
regdata[9] = (regdata[9] & 0x03);
for (i = 7; i < 10; i++)
{
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[i], 1, ®data[i]);
}
return (SH3001_TRUE);
case SH3001_ACC_NORMAL_MODE:
regdata[0] = (regdata[0] & 0xF8);
regdata[1] = (regdata[1] & 0x7F);
regdata[2] = (regdata[2] & 0xF7);
regdata[3] = (regdata[3] & 0xF7);
regdata[4] = (regdata[4] | 0x01);
regdata[5] = (regdata[5] & 0xFC) | 0x01;
regdata[6] = (regdata[6] & 0x9F);
regdata[7] = (regdata[7] & 0xF9) | 0x06;
for (i = 0; i < 8; i++)
{
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[i], 1, ®data[i]);
}
regdata[7] = (regdata[7] & 0x87) | 0x78;
regdata[8] = (regdata[8] & 0x1F) | 0xE0;
regdata[9] = (regdata[9] & 0x03) | 0xFC;
for (i = 7; i < 10; i++)
{
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[i], 1, ®data[i]);
}
return (SH3001_TRUE);
default:
return (SH3001_FALSE);
}
}
/**
* @brief 初始化SH3001接口
* @param 无
* @retval SH3001_TRUE, 成功(0)
* SH3001_FALSE, 异常(1)
*/
uint8_t sh3001_init(void)
{
uint8_t i = 0;
uint8_t regdata = 0;
iic_init(); /* 初始化IIC */
/* 读取CHIP ID */
do
{
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_CHIP_ID, 1, ®data); /* 读取器件ID:0x61 */
}while ((regdata != 0x61) && (i++ < 3));
if ((regdata != 0x61))
{
printf("SH3001 CHIP ID:0X%X\r\n", regdata); /* 打印错误的ID */
return SH3001_FALSE;
}
sh3001_module_reset(); /* 重置内部模块 */
/* ACC配置: 500Hz, 16G, cut off Freq(BW)=500*0.25Hz=125Hz, enable filter; */
sh3001_acc_config(SH3001_ODR_500HZ, SH3001_ACC_RANGE_16G, SH3001_ACC_ODRX025, SH3001_ACC_FILTER_EN);
/* GYRO配置: 500Hz, X\Y\Z 2000deg/s, cut off Freq(BW)=181Hz, enable filter; */
sh3001_gyro_config( SH3001_ODR_500HZ, SH3001_GYRO_RANGE_2000, SH3001_GYRO_RANGE_2000, SH3001_GYRO_RANGE_2000, SH3001_GYRO_ODRX00, SH3001_GYRO_FILTER_EN);
/* 温度配置: 输出速率63Hz, 使能温度测量 */
sh3001_temp_config(SH3001_TEMP_ODR_63, SH3001_TEMP_EN);
/* SH3001进入正常工作模式 */
sh3001_switch_powermode(SH3001_NORMAL_MODE);
/* 读取补偿系数 */
sh3001_comp_init(&g_compcoef);
return SH3001_TRUE;
}
/**
* @brief 读温度参数
* @param 无
* @retval 温度值;
*/
float sh3001_get_temperature(void)
{
uint8_t regdata[2] = {0};
uint16_t tempref[2] = {0};
/* 温度数据是无符号的12位 */
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_TEMP_CONF0, 2, ®data[0]); /* SH3001_TEMP_CONF0[3:0]为室温高4位，SH3001_TEMP_CONF1[7:0]为室温低8位 */
tempref[0] = ((uint16_t)(regdata[0] & 0x0F) << 8) | regdata[1];
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_TEMP_ZL, 2, ®data[0]); /* SH3001_TEMP_ZL[7:0]为温度数据低8位，SH3001_TEMP_ZH[3:0]为温度数据高4位 */
tempref[1] = ((uint16_t)(regdata[1] & 0x0F) << 8) | regdata[0];
return ( (((float)(tempref[1] - tempref[0])) / 16.0f) + 25.0f); /* 温度运算公式 */
}
/**
* @brief 读取SH3001陀螺仪和加速度的数据(未作补偿!不推荐,仅供调试)
* @param accdata[3] : acc X,Y,Z;
* @param gyrodata[3] : gyro X,Y,Z;
* @retval 无
*/
void sh3001_get_imu_data(short accdata[3], short gyrodata[3])
{
uint8_t regdata[12] = {0};
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_ACC_XL, 12, regdata);
accdata[0] = ((short)regdata[1] << 8) | regdata[0];
accdata[1] = ((short)regdata[3] << 8) | regdata[2];
accdata[2] = ((short)regdata[5] << 8) | regdata[4];
gyrodata[0] = ((short)regdata[7] << 8) | regdata[6];
gyrodata[1] = ((short)regdata[9] << 8) | regdata[8];
gyrodata[2] = ((short)regdata[11] << 8) | regdata[10];
printf("data: %d %d %d %d %d %d\r\n", accdata[0], accdata[1], accdata[2], gyrodata[0], gyrodata[1], gyrodata[2]);
}
/**
* @brief 读取补偿后SH3001陀螺仪和加速度的数据(推荐使用)
* @param accdata[3] : acc X,Y,Z;
* @param gyrodata[3] : gyro X,Y,Z;
* @retval 无
*/
void sh3001_get_imu_compdata(short accdata[3], short gyrodata[3])
{
uint8_t regdata[15] = {0};
uint8_t paramp;
int acctemp[3], gyrotemp[3];
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_ACC_XL, 15, regdata);
accdata[0] = ((short)regdata[1] << 8) | regdata[0];
accdata[1] = ((short)regdata[3] << 8) | regdata[2];
accdata[2] = ((short)regdata[5] << 8) | regdata[4];
gyrodata[0] = ((short)regdata[7] << 8) | regdata[6];
gyrodata[1] = ((short)regdata[9] << 8) | regdata[8];
gyrodata[2] = ((short)regdata[11] << 8) | regdata[10];
paramp = regdata[14] & 0x1F;
/* 加速度计补偿公式：每个轴原始值还要叠加上其他两个轴及其对应Cross Axis误差补偿系数的乘积 */
acctemp[0] = (int)( accdata[0] + \
accdata[1] * ((float)g_compcoef.cXY / 1024.0f) + \
accdata[2] * ((float)g_compcoef.cXZ / 1024.0f) );
acctemp[1] = (int)( accdata[0] * ((float)g_compcoef.cYX / 1024.0f) + \
accdata[1] + \
accdata[2] * ((float)g_compcoef.cYZ / 1024.0f) );
acctemp[2] = (int)( accdata[0] * ((float)g_compcoef.cZX / 1024.0f) + \
accdata[1] * ((float)g_compcoef.cZY / 1024.0f) + \
accdata[2] );
/* 对非法轴数据做处理 */
if (acctemp[0] > 32767)
{
acctemp[0] = 32767;
}
else if (acctemp[0] < -32768)
{
acctemp[0] = -32768;
}
if (acctemp[1] > 32767)
{
acctemp[1] = 32767;
}
else if (acctemp[1] < -32768)
{
acctemp[1] = -32768;
}
if (acctemp[2] > 32767)
{
acctemp[2] = 32767;
}
else if (acctemp[2] < -32768)
{
acctemp[2] = -32768;
}
accdata[0] = (short)acctemp[0];
accdata[1] = (short)acctemp[1];
accdata[2] = (short)acctemp[2];
/* 陀螺仪补偿公式：每个轴原始值减去对应误差系数乘积 */
gyrotemp[0] = gyrodata[0] - (paramp - g_compcoef.paramP0) * g_compcoef.jX * g_compcoef.xMulti;
gyrotemp[1] = gyrodata[1] - (paramp - g_compcoef.paramP0) * g_compcoef.jY * g_compcoef.yMulti;
gyrotemp[2] = gyrodata[2] - (paramp - g_compcoef.paramP0) * g_compcoef.jZ * g_compcoef.zMulti;
/* 对非法轴数据做处理 */
if (gyrotemp[0] > 32767)
{
gyrotemp[0] = 32767;
}
else if (gyrotemp[0] < -32768)
{
gyrotemp[0] = -32768;
}
if (gyrotemp[1] > 32767)
{
gyrotemp[1] = 32767;
}
else if (gyrotemp[1] < -32768)
{
gyrotemp[1] = -32768;
}
if (gyrotemp[2] > 32767)
{
gyrotemp[2] = 32767;
}
else if (gyrotemp[2] < -32768)
{
gyrotemp[2] = -32768;
}
gyrodata[0] = (short)gyrotemp[0];
gyrodata[1] = (short)gyrotemp[1];
gyrodata[2] = (short)gyrotemp[2];
printf("compdata: %d %d %d %d %d %d\r\n", accdata[0], accdata[1], accdata[2], gyrodata[0], gyrodata[1], gyrodata[2]);
}
sh3001.h
#ifndef __SH3001_H
#define __SH3001_H
#include "./SYSTEM/sys/sys.h"
/* 器件地址 */
#define SH3001_ADDRESS 0x36 /* 由于SDO接GND,SH3001的器件地址为0x36 */
//#define SH3001_WRITE_ADDR 0x6C /* 由器件地址转为写操作地址(0x36 << 1 | 0x0) */
//#define SH3001_READ_ADDR 0x6D /* 由器件地址转为读操作地址(0x36 << 1 | 0x0) */
/******************************************************************
* SH3001寄存器地址
******************************************************************/
#define SH3001_ACC_XL (0x00)
#define SH3001_ACC_XH (0x01)
#define SH3001_ACC_YL (0x02)
#define SH3001_ACC_YH (0x03)
#define SH3001_ACC_ZL (0x04)
#define SH3001_ACC_ZH (0x05)
#define SH3001_GYRO_XL (0x06)
#define SH3001_GYRO_XH (0x07)
#define SH3001_GYRO_YL (0x08)
#define SH3001_GYRO_YH (0x09)
#define SH3001_GYRO_ZL (0x0A)
#define SH3001_GYRO_ZH (0x0B)
#define SH3001_CHIP_ID (0x0F)
#define SH3001_TEMP_ZL (0x0C)
#define SH3001_TEMP_CONF0 (0x20)
#define SH3001_TEMP_CONF1 (0x21)
#define SH3001_ACC_CONF0 (0x22)
#define SH3001_ACC_CONF1 (0x23)
#define SH3001_ACC_CONF2 (0x25)
#define SH3001_ACC_CONF3 (0x26)
#define SH3001_GYRO_CONF0 (0x28)
#define SH3001_GYRO_CONF1 (0x29)
#define SH3001_GYRO_CONF2 (0x2B)
#define SH3001_GYRO_CONF3 (0x8F)
#define SH3001_GYRO_CONF4 (0x9F)
#define SH3001_GYRO_CONF5 (0xAF)
#define SH3001_CHIP_VERSION (0xDD)
#define SH3001_CHIP_ID1 (0xDF)
#define SH3001_AUX_I2C_CONF (0xFD)
/******************************************************************
* 加速度计配置相关宏定义
******************************************************************/
#define SH3001_ODR_1000HZ (0x00)
#define SH3001_ODR_500HZ (0x01)
#define SH3001_ODR_250HZ (0x02)
#define SH3001_ODR_125HZ (0x03)
#define SH3001_ODR_63HZ (0x04)
#define SH3001_ODR_31HZ (0x05)
#define SH3001_ODR_16HZ (0x06)
#define SH3001_ODR_2000HZ (0x08)
#define SH3001_ODR_4000HZ (0x09)
#define SH3001_ODR_8000HZ (0x0A)
#define SH3001_ODR_16000HZ (0x0B)
#define SH3001_ODR_32000HZ (0x0C)
#define SH3001_ACC_RANGE_16G (0x02)
#define SH3001_ACC_RANGE_8G (0x03)
#define SH3001_ACC_RANGE_4G (0x04)
#define SH3001_ACC_RANGE_2G (0x05)
#define SH3001_ACC_ODRX040 (0x00)
#define SH3001_ACC_ODRX025 (0x20)
#define SH3001_ACC_ODRX011 (0x40)
#define SH3001_ACC_ODRX004 (0x60)
#define SH3001_ACC_FILTER_EN (0x00)
#define SH3001_ACC_FILTER_DIS (0x08)
/******************************************************************
* 陀螺仪配置相关宏定义
******************************************************************/
#define SH3001_GYRO_RANGE_125 (0x02)
#define SH3001_GYRO_RANGE_250 (0x03)
#define SH3001_GYRO_RANGE_500 (0x04)
#define SH3001_GYRO_RANGE_1000 (0x05)
#define SH3001_GYRO_RANGE_2000 (0x06)
#define SH3001_GYRO_ODRX00 (0x00)
#define SH3001_GYRO_ODRX01 (0x04)
#define SH3001_GYRO_ODRX02 (0x08)
#define SH3001_GYRO_ODRX03 (0x0C)
#define SH3001_GYRO_FILTER_EN (0x00)
#define SH3001_GYRO_FILTER_DIS (0x10)
/******************************************************************
* 温度配置相关宏定义
******************************************************************/
#define SH3001_TEMP_ODR_500 (0x00)
#define SH3001_TEMP_ODR_250 (0x10)
#define SH3001_TEMP_ODR_125 (0x20)
#define SH3001_TEMP_ODR_63 (0x30)
#define SH3001_TEMP_EN (0x80)
#define SH3001_TEMP_DIS (0x00)
/******************************************************************
* 中断引脚配置相关宏
******************************************************************/
#define SH3001_INT_LOWG (0x8000)
#define SH3001_INT_HIGHG (0x4000)
#define SH3001_INT_INACT (0x2000)
#define SH3001_INT_ACT (0x1000)
#define SH3001_INT_DOUBLE_TAP (0x0800)
#define SH3001_INT_SINGLE_TAP (0x0400)
#define SH3001_INT_FLAT (0x0200)
#define SH3001_INT_ORIENTATION (0x0100)
#define SH3001_INT_FIFO_GYRO (0x0010)
#define SH3001_INT_GYRO_READY (0x0008)
#define SH3001_INT_ACC_FIFO (0x0004)
#define SH3001_INT_ACC_READY (0x0002)
#define SH3001_INT_FREE_FALL (0x0001)
#define SH3001_INT_UP_DOWN_Z (0x0040)
#define SH3001_INT_ENABLE (0x01)
#define SH3001_INT_DISABLE (0x00)
#define SH3001_INT_MAP_INT1 (0x01)
#define SH3001_INT_MAP_INT (0x00)
#define SH3001_INT0_LEVEL_LOW (0x80)
#define SH3001_INT0_LEVEL_HIGH (0x7F)
#define SH3001_INT_NO_LATCH (0x40)
#define SH3001_INT_LATCH (0xBF)
#define SH3001_INT1_LEVEL_LOW (0x20)
#define SH3001_INT1_LEVEL_HIGH (0xDF)
#define SH3001_INT_CLEAR_ANY (0x10)
#define SH3001_INT_CLEAR_STATUS (0xEF)
#define SH3001_INT1_NORMAL (0x04)
#define SH3001_INT1_OD (0xFB)
#define SH3001_INT0_NORMAL (0x01)
#define SH3001_INT0_OD (0xFE)
/******************************************************************
* Orientation Blocking Config Macro Definitions
******************************************************************/
#define SH3001_ORIENT_BLOCK_MODE0 (0x00)
#define SH3001_ORIENT_BLOCK_MODE1 (0x04)
#define SH3001_ORIENT_BLOCK_MODE2 (0x08)
#define SH3001_ORIENT_BLOCK_MODE3 (0x0C)
#define SH3001_ORIENT_SYMM (0x00)
#define SH3001_ORIENT_HIGH_ASYMM (0x01)
#define SH3001_ORIENT_LOW_ASYMM (0x02)
/******************************************************************
* Flat Time Config Macro Definitions
******************************************************************/
#define SH3001_FLAT_TIME_500MS (0x40)
#define SH3001_FLAT_TIME_1000MS (0x80)
#define SH3001_FLAT_TIME_2000MS (0xC0)
/******************************************************************
* 活动和非活动中断相关宏定义
******************************************************************/
#define SH3001_ACT_AC_MODE (0x80)
#define SH3001_ACT_DC_MODE (0x00)
#define SH3001_ACT_X_INT_EN (0x40)
#define SH3001_ACT_X_INT_DIS (0x00)
#define SH3001_ACT_Y_INT_EN (0x20)
#define SH3001_ACT_Y_INT_DIS (0x00)
#define SH3001_ACT_Z_INT_EN (0x10)
#define SH3001_ACT_Z_INT_DIS (0x00)
#define SH3001_INACT_AC_MODE (0x08)
#define SH3001_INACT_DC_MODE (0x00)
#define SH3001_INACT_X_INT_EN (0x04)
#define SH3001_INACT_X_INT_DIS (0x00)
#define SH3001_INACT_Y_INT_EN (0x02)
#define SH3001_INACT_Y_INT_DIS (0x00)
#define SH3001_INACT_Z_INT_EN (0x01)
#define SH3001_INACT_Z_INT_DIS (0x00)
#define SH3001_LINK_PRE_STA (0x01)
#define SH3001_LINK_PRE_STA_NO (0x00)
/******************************************************************
* TAP Int Config Macro Definitions
******************************************************************/
#define SH3001_TAP_X_INT_EN (0x08)
#define SH3001_TAP_X_INT_DIS (0x00)
#define SH3001_TAP_Y_INT_EN (0x04)
#define SH3001_TAP_Y_INT_DIS (0x00)
#define SH3001_TAP_Z_INT_EN (0x02)
#define SH3001_TAP_Z_INT_DIS (0x00)
/******************************************************************
* HIGHG Int Config Macro Definitions
******************************************************************/
#define SH3001_HIGHG_ALL_INT_EN (0x80)
#define SH3001_HIGHG_ALL_INT_DIS (0x00)
#define SH3001_HIGHG_X_INT_EN (0x40)
#define SH3001_HIGHG_X_INT_DIS (0x00)
#define SH3001_HIGHG_Y_INT_EN (0x20)
#define SH3001_HIGHG_Y_INT_DIS (0x00)
#define SH3001_HIGHG_Z_INT_EN (0x10)
#define SH3001_HIGHG_Z_INT_DIS (0x00)
/******************************************************************
* LOWG Int Config Macro Definitions
******************************************************************/
#define SH3001_LOWG_ALL_INT_EN (0x01)
#define SH3001_LOWG_ALL_INT_DIS (0x00)
/******************************************************************
* FIFO配置相关宏定义
******************************************************************/
#define SH3001_FIFO_MODE_DIS (0x00)
#define SH3001_FIFO_MODE_FIFO (0x01)
#define SH3001_FIFO_MODE_STREAM (0x02)
#define SH3001_FIFO_MODE_TRIGGER (0x03)
#define SH3001_FIFO_ACC_DOWNS_EN (0x80)
#define SH3001_FIFO_ACC_DOWNS_DIS (0x00)
#define SH3001_FIFO_GYRO_DOWNS_EN (0x08)
#define SH3001_FIFO_GYRO_DOWNS_DIS (0x00)
#define SH3001_FIFO_FREQ_X1_2 (0x00)
#define SH3001_FIFO_FREQ_X1_4 (0x01)
#define SH3001_FIFO_FREQ_X1_8 (0x02)
#define SH3001_FIFO_FREQ_X1_16 (0x03)
#define SH3001_FIFO_FREQ_X1_32 (0x04)
#define SH3001_FIFO_FREQ_X1_64 (0x05)
#define SH3001_FIFO_FREQ_X1_128 (0x06)
#define SH3001_FIFO_FREQ_X1_256 (0x07)
#define SH3001_FIFO_EXT_Z_EN (0x2000)
#define SH3001_FIFO_EXT_Y_EN (0x1000)
#define SH3001_FIFO_EXT_X_EN (0x0080)
#define SH3001_FIFO_TEMPERATURE_EN (0x0040)
#define SH3001_FIFO_GYRO_Z_EN (0x0020)
#define SH3001_FIFO_GYRO_Y_EN (0x0010)
#define SH3001_FIFO_GYRO_X_EN (0x0008)
#define SH3001_FIFO_ACC_Z_EN (0x0004)
#define SH3001_FIFO_ACC_Y_EN (0x0002)
#define SH3001_FIFO_ACC_X_EN (0x0001)
#define SH3001_FIFO_ALL_DIS (0x0000)
/******************************************************************
* 从IIC配置相关宏定义
******************************************************************/
#define SH3001_MI2C_NORMAL_MODE (0x00)
#define SH3001_MI2C_BYPASS_MODE (0x01)
#define SH3001_MI2C_READ_ODR_200HZ (0x00)
#define SH3001_MI2C_READ_ODR_100HZ (0x10)
#define SH3001_MI2C_READ_ODR_50HZ (0x20)
#define SH3001_MI2C_READ_ODR_25HZ (0x30)
#define SH3001_MI2C_FAIL (0x20)
#define SH3001_MI2C_SUCCESS (0x10)
#define SH3001_MI2C_READ_MODE_AUTO (0x40)
#define SH3001_MI2C_READ_MODE_MANUAL (0x00)
/******************************************************************
* 其他宏定义
******************************************************************/
#define SH3001_TRUE (0)
#define SH3001_FALSE (1)
#define SH3001_NORMAL_MODE (0x00)
#define SH3001_SLEEP_MODE (0x01)
#define SH3001_POWERDOWN_MODE (0x02)
#define SH3001_ACC_NORMAL_MODE (0x03)
#define SH3001_CHIP_VERSION_MCC (0x08)
#define SH3001_CHIP_VERSION_MCD (0x10)
#define SH3001_CHIP_VERSION_MCF (0x20)
/***************************************************************************
* 结构体类型定义
****************************************************************************/
typedef struct
{
int8_t cXY;
int8_t cXZ;
int8_t cYX;
int8_t cYZ;
int8_t cZX;
int8_t cZY;
/* 以上六个成员是加速度计Cross Axis误差补偿系数 */
/* 以下七个成员是陀螺仪的误差补偿系数 */
int8_t jX;
int8_t jY;
int8_t jZ;
uint8_t xMulti;
uint8_t yMulti;
uint8_t zMulti;
uint8_t paramP0;
} compCoefType;
/***************************************************************************
* 函数声明
****************************************************************************/
uint8_t sh3001_read_nbytes(uint8_t devaddr, uint8_t regaddr, uint8_t length, uint8_t *readbuf);
uint8_t sh3001_write_nbytes(uint8_t devaddr, uint8_t regaddr, uint8_t length, uint8_t *writebuf);
uint8_t sh3001_switch_powermode(uint8_t powermode);
uint8_t sh3001_init(void);
float sh3001_get_temperature(void);
void sh3001_get_imu_data(short accdata[3], short gyrodata[3]);
void sh3001_get_imu_compdata(short accdata[3], short gyrodata[3]);
#endif
main.c
#include "./SYSTEM/sys/sys.h"
#include "./SYSTEM/usart/usart.h"
#include "./SYSTEM/delay/delay.h"
#include "./BSP/LED/led.h"
#include "./BSP/LCD/lcd.h"
#include "./BSP/SDRAM/sdram.h"
#include "./USMART/usmart.h"
#include "./BSP/SH3001/sh3001.h"
#include <math.h>
/**
* @brief 显示原始数据
* @param x, y : 坐标
* @param title: 标题
* @param val : 值
* @retval 无
*/
void user_show_mag(uint16_t x, uint16_t y, char *title, int16_t val)
{
char buf[20];
sprintf(buf,"%s%d", title, val); /* 格式化输出 */
lcd_fill(x, y, x + 160, y + 16, WHITE); /* 清除上次数据(最多显示20个字符,20*8=160) */
lcd_show_string(x, y, 160, 16, 16, buf, BLUE); /* 显示字符串 */
}
/**
* @brief 显示温度
* @param x, y : 坐标
* @param temp : 温度
* @retval 无
*/
void user_show_temprate(uint16_t x, uint16_t y, float temp)
{
char buf[20];
sprintf(buf,"Temp:%2.1fC", temp); /* 格式化输出 */
lcd_fill(x, y, x + 160, y + 16, WHITE); /* 清除上次数据(最多显示20个字符,20*8=160) */
lcd_show_string(x, y, 160, 16, 16, buf, BLUE); /* 显示字符串 */
}
int main(void)
{
uint8_t t = 0;
float temperature; /* 温度值 */
short acc_data[3]; /* 加速度传感器原始数据 */
short gyro_data[3]; /* 陀螺仪原始数据 */
HAL_Init(); /* 初始化HAL库 */
sys_stm32_clock_init(360, 25, 2, 8); /* 设置时钟,180Mhz */
delay_init(180); /* 延时初始化 */
usart_init(115200); /* 串口初始化 */
usmart_dev.init(90); /* USMART初始化 */
led_init(); /* 初始化LED */
sdram_init(); /* 初始化SDRAM */
lcd_init(); /* 初始化LCD */
while (sh3001_init()) /* 检测不到SH3001 */
{
lcd_show_string(30, 130, 200, 16, 16, "SH3001 Check Failed!", RED);
delay_ms(500);
lcd_show_string(30, 130, 200, 16, 16, "Please Check! ", RED);
delay_ms(500);
LED0_TOGGLE(); /* 红灯闪烁 */
}
lcd_show_string(30, 50, 200, 16, 16, "STM32", RED);
lcd_show_string(30, 70, 200, 16, 16, "SH3001 TEST", RED);
lcd_show_string(30, 90, 200, 16, 16, "ATOM@ALIENTEK", RED);
lcd_show_string(30, 130, 200, 16, 16, " Temp :", RED);
lcd_show_string(30, 150, 200, 16, 16, " ACC_X :", RED);
lcd_show_string(30, 170, 200, 16, 16, " ACC_Y :", RED);
lcd_show_string(30, 190, 200, 16, 16, " ACC_Z :", RED);
lcd_show_string(30, 210, 200, 16, 16, " GYRO_X :", RED);
lcd_show_string(30, 230, 200, 16, 16, " GYRO_Y :", RED);
lcd_show_string(30, 250, 200, 16, 16, " GYRO_Z :", RED);
while (1)
{
delay_ms(10);
t++;
if (t == 20) /* 0.2秒左右更新一次温度/六轴原始值 */
{
temperature = sh3001_get_temperature(); /* 读取温度值 */
user_show_temprate(30, 130, temperature);
sh3001_get_imu_compdata(acc_data, gyro_data);
user_show_mag(30, 150, "ACC_X :", acc_data[0]);
user_show_mag(30, 170, "ACC_Y :", acc_data[1]);
user_show_mag(30, 190, "ACC_Z :", acc_data[2]);
user_show_mag(30, 210, "GYRO_X:", gyro_data[0]);
user_show_mag(30, 230, "GYRO_Y:", gyro_data[1]);
user_show_mag(30, 250, "GYRO_Z:", gyro_data[2]);
t = 0;
LED0_TOGGLE();
}
}
}
六、姿态解算介绍
姿态解算是通过对传感器（如加速度计、陀螺仪和磁力计）读取的实时数据进行综合处理，计算得出飞行器或载体相对于某个固定参考坐标系（如地理坐标系或导航坐标系）的姿态角，这三个姿态角分别是偏航角yaw、俯仰角pitch和横滚角roll。
-
偏航角yaw：描述飞行器沿着自身Z轴（机身纵轴，指向飞行器前方）的旋转角度。当飞行器绕Z轴顺时针或逆时针旋转时，会导致飞行器的前端指向偏离地理坐标系的正北方向，表现为偏航角的变化。
-
俯仰角pitch：描述飞行器绕自身Y轴（机身横轴，指向飞行器左侧）的旋转角度。俯仰角的变化意味着飞行器上下抬头或低头，这会影响到飞行器Z轴相对于地理坐标系的垂直方向（天地方向）的角度。
-
横滚角roll：描述飞行器绕自身X轴（机身立轴，指向飞行器下方）的旋转角度。横滚角的变化意味着飞行器左右翻滚，这会影响飞行器Y轴相对于地理坐标系的水平东西方向的角度。
在导航坐标系中，通常采用东北天（NED，North-East-Down）、东南天（SEU，South-East-Up）或西北天（NWU，North-West-Up）等坐标系，其中地理坐标系中Z轴向上或向下，X轴指向东方或西方，Y轴指向南方或北方。通过计算这三个姿态角，可以明确得知飞行器在三维空间中的确切朝向。
6.1、姿态角
使用传感器测量姿态角的过程主要包括以下几个步骤：
-
原始数据采集：首先通过加速度计、陀螺仪和磁力计（在某些高级应用中可能会加入气压计等）采集飞行器在三维空间中的运动数据，包括加速度、角速度和地磁场信息。
-
原始数据处理：
- 加速度计数据可以用来初步估计载体在静止状态下的姿态角，但由于存在重力分量的影响，需要通过一定的算法（如Madgwick滤波、Mahony滤波或Kalman滤波）去除重力分量并计算出初始姿态角。
- 陀螺仪数据提供了实时的角速度信息，可以快速更新姿态角的变化。
- 磁力计数据用于计算航向角（偏航角yaw），通过地磁场数据矫正载体坐标系相对于地理坐标系的偏航角。
-
欧拉角解算：
- 根据选定的旋转顺序（例如Z-Y-X或Z-X-Y），将传感器数据整合起来，通过一系列数学变换和滤波算法，计算出物体在三维空间中绕各轴的旋转角度，也就是姿态角（航向角yaw、俯仰角pitch、横滚角roll）。
-
实时姿态更新：
- 将每次计算得到的姿态角变化量累加到上一时刻的姿态角上，就可以得到实时的载体姿态。
例如，在Z-Y-X旋转顺序下，首先根据磁力计数据计算航向角yaw，然后根据处理过的加速度计数据计算俯仰角pitch和横滚角roll。之后，通过陀螺仪数据实时更新这三个姿态角的变化，叠加到当前姿态角上，从而实时追踪飞行器的姿态变化。在整个过程中，通常需要使用滤波算法来平滑数据，减少噪声和漂移带来的影响，提高姿态解算的精度和稳定性。
6.2、利用加速度计测量角度（俯仰角与横滚角）
确实，加速度计在载体静止并且水平放置时，由于只受到重力作用，其读数会反映出载体相对于地球引力场的方向。当载体有一定倾角时，加速度计在各个轴上的读数会发生相应变化：
-
当载体发生横滚（Roll）时，X轴会感受到一部分重力分量，其横滚角（φ）可以通过下面的公式计算：
φ = t a n − 1 ( A X , O U T / √ ( A Y , O U T 2 + A Z , O U T 2 ) ) φ = tan^{-1}(A_{X,OUT} / √(A^2_{Y,OUT} + A^2_{Z,OUT})) φ=tan−1(AX,OUT/√(AY,OUT2+AZ,OUT2)) -
当载体发生俯仰（Pitch）时，Y轴也会感受到一部分重力分量，其俯仰角（θ）可以通过下面的公式计算：
θ = t a n − 1 ( A Y , O U T / √ ( A X , O U T 2 + A Z , O U T 2 ) ) θ = tan^{-1}(A_{Y,OUT} / √(A^2_{X,OUT} + A^2_{Z,OUT})) θ=tan−1(AY,OUT/√(AX,OUT2+AZ,OUT2))
这里， A X , O U T A_{X,OUT} AX,OUT、 A Y , O U T A_{Y,OUT} AY,OUT和 A Z , O U T A_{Z,OUT} AZ,OUT分别代表加速度计在X、Y、Z轴上的输出值，单位通常为g（重力加速度）。
需要注意的是，这种方法适用于短时间内小角度变化的测量，因为加速度计直接读数容易受到振动、噪声等因素的影响，长期测量或大角度变化时会出现累积误差。为了实时、准确地测量和跟踪载体的连续姿态变化，通常会结合陀螺仪的数据进行互补滤波或者卡尔曼滤波等算法处理，以抵消加速度计的积分误差并保持角速度信息的连续性。同时，为了获取航向角（Yaw），还需要借助磁力计测量地球磁场方向。
6.3、重力计算欧拉角、方向余弦矩阵
您提到的计算方法是基于三轴加速度计测量到的重力分量 ax、ay、az 来推导载体的欧拉角（横滚角 roll、俯仰角 pitch）。在理想情况下，当载体静止时，加速度计仅测量到地球重力加速度，且不受载体运动和振动影响时，我们可以利用重力分量推算出载体的倾斜角度。
-
由于重力加速度向量始终保持在竖直方向，即 [0, 0, g]，载体绕Z轴（航向角 yaw）旋转不会改变重力加速度在XOY平面上的投影，因此Z轴（偏航角）不影响重力分量在X和Y轴上的值。
-
通过方向余弦矩阵可以将地球重力向量从惯性坐标系转换到载体坐标系，方向余弦矩阵分解为绕三个轴的旋转矩阵相乘，即先绕Z轴旋转角度z（偏航角），再绕Y轴旋转角度y（俯仰角），最后绕X轴旋转角度x（横滚角）。
-
使用以下公式可以从重力分量计算横滚角 roll 和俯仰角 pitch：
- 横滚角 roll：roll = arctan2(ax / az)
- 俯仰角 pitch：pitch = -arcsin(ay)
然而，在实际应用中，这种方法受限于加速度计测量噪声、非线性效应以及长时间积分引起的漂移等问题，往往需要结合陀螺仪数据和磁力计数据，通过互补滤波、卡尔曼滤波等算法进行姿态解算以提高精度。此外，这里的计算方法通常假定载体在静止状态下进行，对于动态运动的情况，需要更加复杂的算法进行处理。
6.4、利用加速度计测量角度缺陷
利用加速度计测量角度的确存在一些固有缺陷：
-
无法直接测量偏航角（Yaw）：
- 如您所指出，加速度计确实无法直接测量载体绕Z轴的旋转，也就是偏航角。由于地球重力始终垂直于地面，不论载体如何绕Z轴旋转，其在Z轴方向上的重力分量保持不变。因此，单纯依赖加速度计无法获取偏航角信息，需要配合磁力计测量地磁场方向以确定航向。
-
高频噪声导致的姿态抖动：
- 加速度计输出受到传感器内部噪声、机械振动以及电磁干扰的影响，特别是在高频段，噪声可能导致姿态角解算结果出现不稳定或虚假波动，这被称为“抖动”。解决这个问题通常需要采用数字滤波技术，如低通滤波、卡尔曼滤波或其他状态估计算法，以平滑数据并提取真实的姿态信息。
-
无法区分重力加速度与动态加速度：
- 加速度计输出的是包括重力加速度和动态加速度在内的总加速度，而在姿态解算中我们关心的是重力分量。当载体移动或加速时，加速度计会同时捕捉到重力加速度和由载体运动产生的动态加速度，这会引入误差。为了解决这个问题，一般会在载体相对静止或匀速直线运动时进行姿态初始化，或者通过融合陀螺仪数据来实时估算姿态变化，以剔除动态加速度的影响。同时，现代惯性导航系统常常采用IMU（惯性测量单元），结合加速度计、陀螺仪和磁力计等多种传感器的数据，通过先进的算法进行联合解算，以提高姿态测量的准确性和稳定性。
6.5、利用陀螺仪检测姿态角
利用陀螺仪检测姿态角的基本思路是：
-
陀螺仪测量角速度：陀螺仪能够检测物体绕各个坐标轴的角速度，其输出通常为模拟信号或数字化后的数值，单位一般是度每秒（deg/s）或弧度每秒（rad/s）。
-
积分获取角度变化：将陀螺仪测得的角速度对时间进行积分，理论上可以获得物体在一段时间内绕某个轴的旋转角度。例如，绕Z轴的旋转角度可以通过以下公式计算：
∠Axz = Zangle_rate * Δt其中，Zangle_rate 是陀螺仪测量到的Z轴角速度经换算得到的真实角速度值，Δt 是积分时间间隔。
-
提高采样频率减小积分误差：通过提高陀螺仪的采样频率，可以更频繁地测量角速度，这样在同样的时间内分割出更多小的时间间隔进行积分，有助于减小积分误差，使得短期姿态估计更为精确。
-
陀螺仪累积误差问题：尽管陀螺仪可以实时提供角速度信息，但其内部固有的漂移现象会导致长时间积分后的姿态角出现误差积累。为了解决这个问题，通常需要与加速度计、磁力计数据相结合，通过互补滤波、卡尔曼滤波等算法进行融合，以消除误差并提供准确的姿态信息。此外，定期进行重新对准或使用具有嵌入式温度补偿和自校准功能的高精度陀螺仪也能有效改善累积误差的问题。
6.6、利用陀螺仪测量角度缺陷
利用陀螺仪测量角度存在一些固有缺陷：
-
零偏（Offset）：陀螺仪在没有任何旋转的情况下也会输出一个非零的角速度值，称为零偏。这意味着即使载体静止，陀螺仪也可能报告出持续的微小旋转，如果将其积分，将会造成累计误差。
-
积分误差（Drift）：正如您举例所示，由于陀螺仪零偏的存在，随着时间的推移，即使在没有实际旋转的情况下，积分计算出的角度也会逐渐增加，这种现象叫做姿态漂移。即使陀螺仪误差仅为0.1°/s，长时间累积后也会导致严重的角度误差。
-
长期稳定性不足：陀螺仪不适合进行长期的绝对角度测量，因为积分误差会随时间快速增长，而且陀螺仪本身还可能存在随机游走噪声等，这也会影响长期测量的准确性。
为克服这些缺陷，惯性导航系统通常会采用以下策略：
- 闭环反馈和校准：定期对陀螺仪进行校准，消除零偏和斜率误差。
- 传感器融合：将陀螺仪数据与加速度计、磁力计等传感器的数据进行融合，利用互补滤波、卡尔曼滤波等算法，结合短时内陀螺仪的精确动态信息和长时间加速度计对重力稳定的静态信息，减小姿态漂移，提高姿态估计的长期稳定性。
- 自适应调整和预测：在复杂环境中，可以采用自适应滤波技术和模型预测等方法，动态调整和优化融合算法，以应对陀螺仪误差随时间的变化。
6.7、传感器数据融合
传感器数据融合是一种通过结合来自不同类型的传感器数据，以提高姿态估计精度和鲁棒性的方法。在姿态检测中，通常会利用加速度计、陀螺仪和磁力计等传感器的数据进行融合。
-
角速度积分与积分漂移：陀螺仪测量角速度并对其积分，可以得到姿态角的变化。但是，由于陀螺仪存在零偏和漂移，长时间积分后会造成姿态角的误差累积。
-
加速度计正交解算：加速度计可以直接测量重力分量，但由于振动、噪声等原因，只能提供相对准确的静态姿态信息，高频动态误差较大。
-
互补滤波：互补滤波是一种简单的传感器融合算法，它利用陀螺仪在高频动态下的优势和加速度计在低频静态下的优势进行互补。通过计算两者之间的误差，并利用误差构建PI控制器，对陀螺仪数据进行补偿。
-
误差表示：误差向量e可以通过陀螺仪测量得到的实际角速度与期望角速度（由加速度计推算得到）的外积表示，然后利用PI控制器调整误差，并将调整后的误差补偿到陀螺仪的测量结果中。
-
PI控制器参数设置：Kp（比例增益）和Ki（积分增益）的选择会影响对陀螺仪和加速度计的信任程度。Kp较大时，系统更倾向于相信加速度计的静态测量结果，有利于消除陀螺仪的积分漂移；Kp较小时，系统更信任陀螺仪的动态响应，能更好地跟踪快速变化的姿态。
四元数则是一种能够高效描述和计算旋转的数学工具，常用于融合后的姿态表示和更新，因其具有避免万向锁问题和便于插值的优点而被广泛应用在传感器融合算法中。在实际应用中，通过不断更新和优化四元数，可以实时准确地估计出载体的姿态信息。
6.8、姿态表示的方式：欧拉角、旋转矩阵、四元数
姿态表示的几种常见方式以及它们的特点和用途。欧拉角、旋转矩阵和四元数都是描述三维空间中旋转的有效方法，各有优缺点：
-
欧拉角：直观且易于理解，但存在万向节死锁问题，且在连续旋转时可能出现不连续现象，不利于实时计算和插值。
-
旋转矩阵：无万向节死锁问题，但占用较多存储空间，且涉及到9个元素，需要更多的计算资源。
-
四元数：四元数是一个高效的旋转表示方法，由四个实数分量（q0, q1, q2, q3）组成，可以避免万向节死锁问题，计算简单且适合实时应用。在单片机资源有限的情况下，相比旋转矩阵和欧拉角，四元数在计算和存储方面的效率更高。
四元数与其对应的旋转矩阵之间存在一种直接映射关系，可以将四元数转换为方向余弦矩阵，但它不是直接从四元数到欧拉角的转换，而是四元数到旋转矩阵的转换表达式。
将四元数转换为欧拉角（偏航角yaw、俯仰角pitch、横滚角roll）的常规方法是解析计算。通常四元数转欧拉角的公式会涉及复杂的三角函数运算，标准转换公式可能会有所不同，以下是一般情况下的转换公式示例（请注意，不同坐标系下的转换公式会有差异）：
roll = atan2(2*(q0*q1 + q2*q3), q0*q0 - q1*q1 - q2*q2 + q3*q3)
pitch = asin(-2*(q1*q3 - q0*q2))
yaw = atan2(2*(q1*q2 + q0*q3), q0*q0 + q1*q1 - q2*q2 - q3*q3)
请根据具体的坐标系约定和传感器输出的四元数顺序调整转换公式。同时，四元数通常写作 q = q0 + q1*i + q2*j + q3*k，其中 (q0, q1, q2, q3) 分别对应四元数的实部和三个虚部。
6.9、四元数的求解
当我们用四元数表示载体的姿态，并且希望通过陀螺仪测量的角速度信息实时更新姿态时，确实需要解决一个关于四元数的时间导数的微分方程。这个微分方程反映了四元数随时间的变化规律，它与陀螺仪读数有关，因为陀螺仪提供了载体相对于惯性参考系的角速度。
四元数 ( Q ) 的形式通常是：
Q
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
Q = q_0 + q_1 i + q_2 j + q_3 k
Q=q0+q1i+q2j+q3k
其中
q
0
=
cos
(
θ
/
2
)
q_0 = \cos(\theta/2)
q0=cos(θ/2)，
(
q
1
,
q
2
,
q
3
)
(q_1, q_2, q_3)
(q1,q2,q3) 是旋转轴方向上的单位向量
ω
^
\hat{\omega}
ω^ 乘以
sin
(
θ
/
2
)
\sin(\theta/2)
sin(θ/2) 的系数，即
(
q
1
,
q
2
,
q
3
)
=
λ
sin
(
θ
/
2
)
(q_1, q_2, q_3) = \lambda \sin(\theta/2)
(q1,q2,q3)=λsin(θ/2)，其中
λ
\lambda
λ 是单位向量，
θ
\theta
θ 是围绕
λ
\lambda
λ 轴的旋转角度。
陀螺仪测量的角速度矢量
ω
=
(
ω
x
,
ω
y
,
ω
z
)
\boldsymbol{\omega} = (\omega_x, \omega_y, \omega_z)
ω=(ωx,ωy,ωz) 可以用来更新四元数的速率，其微分方程简化后的一般形式如您所述：
d
Q
d
t
=
1
2
[
0
−
ω
x
−
ω
y
−
ω
z
ω
x
0
ω
z
−
ω
y
ω
y
−
ω
z
0
ω
x
−
ω
z
ω
y
−
ω
x
0
]
Q
\frac{dQ}{dt} = \frac{1}{2} \left[ \begin{array}{cccc} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y \\ \omega_y & -\omega_z & 0 & \omega_x \\ -\omega_z & \omega_y & -\omega_x & 0 \end{array} \right] Q
dtdQ=21
0ωxωy−ωz−ωx0−ωzωy−ωyωz0−ωx−ωz−ωyωx0
Q
针对这一线性常微分方程，我们可以利用数值积分方法来求解。一阶龙格库塔法是一种简单的数值求解方法，而为了提高精度，可以采用更高阶的龙格库塔法。例如，一阶龙格库塔法的迭代公式简化后的形式如下：
Q ( t + Δ t ) = Q ( t ) + Δ t 2 ⋅ ϕ ( t ) ⋅ Q ( t ) Q(t + \Delta t) = Q(t) + \frac{\Delta t}{2} \cdot \phi(t) \cdot Q(t) Q(t+Δt)=Q(t)+2Δt⋅ϕ(t)⋅Q(t)
其中 ϕ ( t ) \phi(t) ϕ(t) 是上述矩阵与当前时刻的角速度向量 ω ( t ) \boldsymbol{\omega}(t) ω(t) 相结合形成的更新矩阵。
若要使用更高阶的龙格库塔法则会引入更多当前时刻及前一时刻的状态信息，使得近似更加精确。例如，二阶龙格库塔法（也称为改进的 Euler 法）会考虑一次导数的线性插值，而三阶或四阶则会考虑更高次导数的信息，从而获得更好的稳定性与精度。实际应用中，往往会选择足够高阶的方法以确保姿态更新误差在可接受范围内。
七、编程实战2 六轴传感器-姿态解算
源码
sh3001.c
#include "./BSP/IIC/myiic.h"
#include "./BSP/SH3001/sh3001.h"
#include "./SYSTEM/usart/usart.h"
#include "./SYSTEM/delay/delay.h"
#include "./BSP/LCD/lcd.h"
#include <math.h>
static compCoefType g_compcoef; /* compCoefType结构体,用于存储补偿系数 */
static uint8_t g_store_acc_odr; /* 保存ACC ODR临时变量 */
/**
* @brief 从SH3001读取N个字节数据
* @param devaddr : 器件地址
* @param regaddr : 寄存器地址
* @param length : 读取长度
* @param readbuf : 数据存储buf
* @retval 0, 操作成功
* 其他, 操作失败
*/
uint8_t sh3001_read_nbytes(uint8_t devaddr, uint8_t regaddr, uint8_t length, uint8_t *readbuf)
{
uint8_t i;
iic_start(); /* 主机发送起始信号 */
iic_send_byte(devaddr << 1 | 0X00); /* 主机发送写操作地址(器件地址 + 写命令) */
if (iic_wait_ack()) /* 主机等待SH3001响应 */
{
iic_stop(); /* 接收应答信号失败，直接发送停止信号 */
return SH3001_FALSE;
}
iic_send_byte(regaddr); /* 主机发送寄存器地址 */
iic_wait_ack(); /* 主机等待SH3001响应 */
iic_start(); /* 主机重新发送起始信号 */
iic_send_byte(devaddr << 1 | 0X01); /* 主机发送读操作地址(器件地址 + 读命令) */
iic_wait_ack(); /* 主机等待SH3001响应 */
for (i = 0; i < length; i++) /* 循环读取 数据 */
{
readbuf[i] = iic_read_byte(i == (length - 1) ? 0 : 1); /* 主机读取到寄存器的数据并发送ACK(最后一次读发送NACK) */
}
iic_stop(); /* 主机发送停止信号 */
return SH3001_TRUE;
}
/**
* @brief SH3001写入N个字节数据
* @param devaddr : 器件地址
* @param regaddr : 寄存器地址
* @param length : 写入长度
* @param writebuf : 数据存储buf
* @retval 0, 操作成功
* 1, 操作失败
*/
uint8_t sh3001_write_nbytes(uint8_t devaddr, uint8_t regaddr, uint8_t length, uint8_t *writebuf)
{
uint8_t i;
iic_start(); /* 主机发送起始信号 */
iic_send_byte(devaddr << 1 | 0X00); /* 主机发送写操作地址(器件地址 + 写命令) */
iic_wait_ack(); /* 主机等待SH3001响应 */
iic_send_byte(regaddr); /* 主机发送要操作的寄存器地址 */
iic_wait_ack(); /* 主机等待SH3001响应 */
for (i = 0; i < length; i++) /* 连续写入数据 */
{
iic_send_byte(writebuf[i]); /* 主机发送要写入到寄存器的数据 */
if (iic_wait_ack()) /* 主机等待SH3001响应 */
{
iic_stop(); /* 接收应答信号失败，直接发送停止信号 */
return (SH3001_FALSE);
}
}
iic_stop(); /* 主机发送停止信号 */
return (SH3001_TRUE);
}
/**
* @brief 设置加速度计参数
* @param acc_odr acc_range acc_cutofffreq acc_filter_enble
* SH3001_ODR_1000HZ SH3001_ACC_RANGE_16G SH3001_ACC_ODRX040 SH3001_ACC_FILTER_EN
* SH3001_ODR_500HZ SH3001_ACC_RANGE_8G SH3001_ACC_ODRX025 SH3001_ACC_FILTER_DIS
* SH3001_ODR_250HZ SH3001_ACC_RANGE_4G SH3001_ACC_ODRX011
* SH3001_ODR_125HZ SH3001_ACC_RANGE_2G SH3001_ACC_ODRX004
* SH3001_ODR_63HZ
* SH3001_ODR_31HZ
* SH3001_ODR_16HZ
* SH3001_ODR_2000HZ
* SH3001_ODR_4000HZ
* SH3001_ODR_8000HZ
* @retval 无
*/
static void sh3001_acc_config(uint8_t acc_odr, uint8_t acc_range, uint8_t acc_cutofffreq, uint8_t acc_filter_enble)
{
uint8_t regdata = 0;
/* 使能加速度计数字滤波器 */
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_ACC_CONF0, 1, ®data);
regdata |= 0x01;
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_ACC_CONF0, 1, ®data); /* 对SH3001_ACC_CONF0[0]置1，使能数字滤波器 */
/* 设置采样频率 */
g_store_acc_odr = acc_odr;
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_ACC_CONF1, 1, &acc_odr); /* 对SH3001_ACC_CONF1[3:0]设置 */
/* 设置加速度计满量程范围 */
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_ACC_CONF2, 1, &acc_range); /* 对SH3001_ACC_CONF2[2:0]设置 */
/* 设置低通滤波器 */
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_ACC_CONF3, 1, ®data);
regdata &= 0x17; /* 目的：清零要设置位 */
regdata |= (acc_cutofffreq | acc_filter_enble);
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_ACC_CONF3, 1, ®data); /* 对SH3001_ACC_CONF3[7:5]设置低通滤波器截止频率 SH3001_ACC_CONF3[3]设置是否旁路低通滤波器 */
}
/**
* @brief 设置陀螺仪参数
* @param gyro_odr gyro_range_x,y,z gyro_cutofffreq gyro_filter_enble
* SH3001_ODR_1000HZ SH3001_GYRO_RANGE_125 SH3001_GYRO_ODRX00 SH3001_GYRO_FILTER_EN
* SH3001_ODR_500HZ SH3001_GYRO_RANGE_250 SH3001_GYRO_ODRX01 SH3001_GYRO_FILTER_DIS
* SH3001_ODR_250HZ SH3001_GYRO_RANGE_500 SH3001_GYRO_ODRX02
* SH3001_ODR_125HZ SH3001_GYRO_RANGE_1000 SH3001_GYRO_ODRX03
* SH3001_ODR_63HZ SH3001_GYRO_RANGE_2000
* SH3001_ODR_31HZ
* SH3001_ODR_16HZ
* SH3001_ODR_2000HZ
* SH3001_ODR_4000HZ
* SH3001_ODR_8000HZ
* SH3001_ODR_16000HZ
* SH3001_ODR_32000HZ
* @retval 无
*/
static void sh3001_gyro_config(uint8_t gyro_odr, uint8_t gyro_range_x, uint8_t gyro_range_y, uint8_t gyro_range_z, uint8_t gyro_cutofffreq, uint8_t gyro_filter_enble)
{
uint8_t regdata = 0;
/* 使能陀螺仪数字滤波器 */
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_GYRO_CONF0, 1, ®data);
regdata |= 0x01;
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_GYRO_CONF0, 1, ®data); /* SH3001_GYRO_CONF0[0]置1，使能数字滤波器 */
/* 设置采样频率 */
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_GYRO_CONF1, 1, &gyro_odr); /* SH3001_GYRO_CONF1[0:3]设置 */
/* 设置满量程范围 */
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_GYRO_CONF3, 1, &gyro_range_x); /* SH3001_GYRO_CONF3[2:0]设置X轴 */
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_GYRO_CONF4, 1, &gyro_range_y); /* SH3001_GYRO_CONF4[2:0]设置Y轴 */
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_GYRO_CONF5, 1, &gyro_range_z); /* SH3001_GYRO_CONF5[2:0]设置Z轴 */
/* 设置低通滤波器 */
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_GYRO_CONF2, 1, ®data);
regdata &= 0xE3; /* 目的：清零要设置位 */
regdata |= (gyro_cutofffreq | gyro_filter_enble);
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_GYRO_CONF2, 1, ®data); /* SH3001_GYRO_CONF2[3:2]设置低通滤波器截止频率 SH3001_GYRO_CONF2[4]设置是否旁路低通滤波器 */
}
/**
* @brief 设置温度参数
* @param temp_odr
* SH3001_TEMP_ODR_500
* SH3001_TEMP_ODR_250
* SH3001_TEMP_ODR_125
* SH3001_TEMP_ODR_63
* @param temp_enable
* SH3001_TEMP_EN
* SH3001_TEMP_DIS
* @retval 无
*/
static void sh3001_temp_config(uint8_t temp_odr, uint8_t temp_enable)
{
uint8_t regdata = 0;
/* 设置温度传感器参数 */
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_TEMP_CONF0, 1, ®data);
regdata &= 0x4F; /* 目的：清零要设置位 */
regdata |= (temp_odr | temp_enable);
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_TEMP_CONF0, 1, ®data); /* SH3001_TEMP_CONF0[5:4]设置采样频率 SH3001_TEMP_CONF0[7]设置是否使能温度传感器 */
}
/**
* @brief 读取补偿系数
* @param g_compcoef : compCoefType结构体指针
* @retval 无
*/
static void sh3001_comp_init(compCoefType *g_compcoef)
{
uint8_t coefdata[2] = {0};
/* Acc Cross */
sh3001_read_nbytes(SH3001_ADDRESS, 0x81, 2, coefdata);
g_compcoef->cYX = (int8_t)coefdata[0];
g_compcoef->cZX = (int8_t)coefdata[1];
sh3001_read_nbytes(SH3001_ADDRESS, 0x91, 2, coefdata);
g_compcoef->cXY = (int8_t)coefdata[0];
g_compcoef->cZY = (int8_t)coefdata[1];
sh3001_read_nbytes(SH3001_ADDRESS, 0xA1, 2, coefdata);
g_compcoef->cXZ = (int8_t)coefdata[0];
g_compcoef->cYZ = (int8_t)coefdata[1];
/* Gyro Zero */
sh3001_read_nbytes(SH3001_ADDRESS, 0x60, 1, coefdata);
g_compcoef->jX = (int8_t)coefdata[0];
sh3001_read_nbytes(SH3001_ADDRESS, 0x68, 1, coefdata);
g_compcoef->jY = (int8_t)coefdata[0];
sh3001_read_nbytes(SH3001_ADDRESS, 0x70, 1, coefdata);
g_compcoef->jZ = (int8_t)coefdata[0];
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_GYRO_CONF3, 1, coefdata);
coefdata[0] = coefdata[0] & 0x07;
g_compcoef->xMulti = ((coefdata[0] < 2) || (coefdata[0] >= 7)) ? 1 : (1 << (6 - coefdata[0]));
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_GYRO_CONF4, 1, coefdata);
coefdata[0] = coefdata[0] & 0x07;
g_compcoef->yMulti = ((coefdata[0] < 2) || (coefdata[0] >= 7)) ? 1 : (1 << (6 - coefdata[0]));
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_GYRO_CONF5, 1, coefdata);
coefdata[0] = coefdata[0] & 0x07;
g_compcoef->zMulti = ((coefdata[0] < 2) || (coefdata[0] >= 7)) ? 1 : (1 << (6 - coefdata[0]));
sh3001_read_nbytes(SH3001_ADDRESS, 0x2E, 1, coefdata);
g_compcoef->paramP0 = coefdata[0] & 0x1F;
}
/**
* @brief 复位部分内部模块
* @param 无
* @retval 无
*/
static void sh3001_module_reset(void)
{
const uint8_t regaddr[8] = {0xC0, 0xD3, 0xD3, 0xD5, 0xD4, 0xBB, 0xB9, 0xBA};
/* 芯片版本批次：MCC、MCD、MCF */
/* MCC版本配置 */
const uint8_t mcc_regdata_a[8] = {0x38, 0xC6, 0xC1, 0x02, 0x0C, 0x18, 0x18, 0x18};
const uint8_t mcc_regdata_b[8] = {0x3D, 0xC2, 0xC2, 0x00, 0x04, 0x00, 0x00, 0x00};
/* MCD版本配置 */
const uint8_t mcd_regdata_a[8] = {0x38, 0xD6, 0xD1, 0x02, 0x08, 0x18, 0x18, 0x18};
const uint8_t mcd_regdata_b[8] = {0x3D, 0xD2, 0xD2, 0x00, 0x00, 0x00, 0x00, 0x00};
/* MCF版本配置 */
const uint8_t mcf_regdata_a[8] = {0x38, 0x16, 0x11, 0x02, 0x08, 0x18, 0x18, 0x18};
const uint8_t mcf_regdata_b[8] = {0x3E, 0x12, 0x12, 0x00, 0x00, 0x00, 0x00, 0x00};
uint8_t regdata = 0;
uint8_t *regdata_a = (uint8_t *)mcd_regdata_a; /* 默认用MCD配置 */
uint8_t *regdata_b = (uint8_t *)mcd_regdata_b;
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_CHIP_VERSION, 1, ®data); /* 读取 SH3001_CHIP_VERSION */
if (regdata == SH3001_CHIP_VERSION_MCC) /* MCC版本 */
{
regdata_a = (uint8_t *)mcc_regdata_a;
regdata_b = (uint8_t *)mcc_regdata_b;
}
else if (regdata == SH3001_CHIP_VERSION_MCF) /* MCF版本 */
{
regdata_a = (uint8_t *)mcf_regdata_a;
regdata_b = (uint8_t *)mcf_regdata_b;
}
/* Drive Start */
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[0], 1, ®data_a[0]);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[1], 1, ®data_a[1]);
delay_ms(100);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[0], 1, ®data_b[0]);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[1], 1, ®data_b[1]);
delay_ms(50);
/* ADC Resett */
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[2], 1, ®data_a[2]);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[3], 1, ®data_a[3]);
delay_ms(1);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[2], 1, ®data_b[2]);
delay_ms(1);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[3], 1, ®data_b[3]);
delay_ms(50);
/* CVA Resett */
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[4], 1, ®data_a[4]);
delay_ms(10);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[4], 1, ®data_b[4]);
delay_ms(1);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[5], 1, ®data_a[5]);
delay_ms(10);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[6], 1, ®data_a[6]);
delay_ms(10);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[7], 1, ®data_a[7]);
delay_ms(10);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[5], 1, ®data_b[5]);
delay_ms(10);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[6], 1, ®data_b[6]);
delay_ms(10);
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[7], 1, ®data_b[7]);
delay_ms(10);
/* 设置 INT and INT1 脚开漏输出 */
regdata = 0x00;
sh3001_write_nbytes(SH3001_ADDRESS, 0x44, 1, ®data);
}
/**
* @brief 切换SH3001电源模式
* Normal mode: 1.65mA; Sleep mode: 162uA; Acc normal mode:393uA;
* @param powermode
* SH3001_NORMAL_MODE
* SH3001_SLEEP_MODE
* SH3001_POWERDOWN_MODE
* SH3001_ACC_NORMAL_MODE
* @retval SH3001_TRUE 成功
* SH3001_FALSE 异常
*/
uint8_t sh3001_switch_powermode(uint8_t powermode)
{
uint8_t regaddr[10] = {0xCF, 0x22, 0x2F, 0xCB, 0xCE, 0xD3, 0xD4, 0xD5, 0xD6, 0xD7};
uint8_t regdata[10] = {0};
uint8_t i = 0;
uint8_t acc_odr = SH3001_ODR_1000HZ;
if ((powermode != SH3001_NORMAL_MODE) && (powermode != SH3001_SLEEP_MODE) && (powermode != SH3001_POWERDOWN_MODE) && (powermode != SH3001_ACC_NORMAL_MODE))
{
return (SH3001_FALSE);
}
for (i = 0; i < 10; i++)
{
sh3001_read_nbytes(SH3001_ADDRESS, regaddr[i], 1, ®data[i]);
}
switch (powermode)
{
case SH3001_NORMAL_MODE:
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_ACC_CONF1, 1, &g_store_acc_odr); /* 恢复accODR */
regdata[0] = (regdata[0] & 0xF8);
regdata[1] = (regdata[1] & 0x7F);
regdata[2] = (regdata[2] & 0xF7);
regdata[3] = (regdata[3] & 0xF7);
regdata[4] = (regdata[4] & 0xFE);
regdata[5] = (regdata[5] & 0xFC) | 0x02;
regdata[6] = (regdata[6] & 0x9F);
regdata[7] = (regdata[7] & 0xF9);
for (i = 0; i < 8; i++)
{
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[i], 1, ®data[i]);
}
regdata[7] = (regdata[7] & 0x87);
regdata[8] = (regdata[8] & 0x1F);
regdata[9] = (regdata[9] & 0x03);
for (i = 7; i < 10; i++)
{
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[i], 1, ®data[i]);
}
sh3001_module_reset();
return (SH3001_TRUE);
case SH3001_SLEEP_MODE:
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_ACC_CONF1, 1, &g_store_acc_odr); /* 保存当前的acc ODR */
sh3001_write_nbytes(SH3001_ADDRESS, SH3001_ACC_CONF1, 1, &acc_odr); /* 设置acc ODR=1000Hz */
regdata[0] = (regdata[0] & 0xF8) | 0x07;
regdata[1] = (regdata[1] & 0x7F) | 0x80;
regdata[2] = (regdata[2] & 0xF7) | 0x08;
regdata[3] = (regdata[3] & 0xF7) | 0x08;
regdata[4] = (regdata[4] & 0xFE);
regdata[5] = (regdata[5] & 0xFC) | 0x01;
regdata[6] = (regdata[6] & 0x9F);
regdata[7] = (regdata[7] & 0xF9) | 0x06;
for (i = 0; i < 8; i++)
{
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[i], 1, ®data[i]);
}
regdata[7] = (regdata[7] & 0x87);
regdata[8] = (regdata[8] & 0x1F);
regdata[9] = (regdata[9] & 0x03);
for (i = 7; i < 10; i++)
{
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[i], 1, ®data[i]);
}
return (SH3001_TRUE);
case SH3001_POWERDOWN_MODE:
regdata[0] = (regdata[0] & 0xF8);
regdata[1] = (regdata[1] & 0x7F) | 0x80;
regdata[2] = (regdata[2] & 0xF7) | 0x08;
regdata[3] = (regdata[3] & 0xF7) | 0x08;
regdata[4] = (regdata[4] & 0xFE);
regdata[5] = (regdata[5] & 0xFC) | 0x01;
regdata[6] = (regdata[6] & 0x9F) | 0x60;
regdata[7] = (regdata[7] & 0xF9) | 0x06;
for (i = 0; i < 8; i++)
{
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[i], 1, ®data[i]);
}
regdata[7] = (regdata[7] & 0x87);
regdata[8] = (regdata[8] & 0x1F);
regdata[9] = (regdata[9] & 0x03);
for (i = 7; i < 10; i++)
{
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[i], 1, ®data[i]);
}
return (SH3001_TRUE);
case SH3001_ACC_NORMAL_MODE:
regdata[0] = (regdata[0] & 0xF8);
regdata[1] = (regdata[1] & 0x7F);
regdata[2] = (regdata[2] & 0xF7);
regdata[3] = (regdata[3] & 0xF7);
regdata[4] = (regdata[4] | 0x01);
regdata[5] = (regdata[5] & 0xFC) | 0x01;
regdata[6] = (regdata[6] & 0x9F);
regdata[7] = (regdata[7] & 0xF9) | 0x06;
for (i = 0; i < 8; i++)
{
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[i], 1, ®data[i]);
}
regdata[7] = (regdata[7] & 0x87) | 0x78;
regdata[8] = (regdata[8] & 0x1F) | 0xE0;
regdata[9] = (regdata[9] & 0x03) | 0xFC;
for (i = 7; i < 10; i++)
{
sh3001_write_nbytes(SH3001_ADDRESS, regaddr[i], 1, ®data[i]);
}
return (SH3001_TRUE);
default:
return (SH3001_FALSE);
}
}
/**
* @brief 初始化SH3001接口
* @param 无
* @retval SH3001_TRUE, 成功(0)
* SH3001_FALSE, 异常(1)
*/
uint8_t sh3001_init(void)
{
uint8_t i = 0;
uint8_t regdata = 0;
iic_init(); /* 初始化IIC */
/* 读取CHIP ID */
do
{
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_CHIP_ID, 1, ®data); /* 读取器件ID:0x61 */
}while ((regdata != 0x61) && (i++ < 3));
if ((regdata != 0x61))
{
printf("SH3001 CHIP ID:0X%X\r\n", regdata); /* 打印错误的ID */
return SH3001_FALSE;
}
sh3001_module_reset(); /* 重置内部模块 */
/* ACC配置: 500Hz, 16G, cut off Freq(BW)=500*0.25Hz=125Hz, enable filter; */
sh3001_acc_config(SH3001_ODR_500HZ, SH3001_ACC_RANGE_16G, SH3001_ACC_ODRX025, SH3001_ACC_FILTER_EN);
/* GYRO配置: 500Hz, X\Y\Z 2000deg/s, cut off Freq(BW)=181Hz, enable filter; */
sh3001_gyro_config( SH3001_ODR_500HZ, SH3001_GYRO_RANGE_2000, SH3001_GYRO_RANGE_2000, SH3001_GYRO_RANGE_2000, SH3001_GYRO_ODRX00, SH3001_GYRO_FILTER_EN);
/* 温度配置: 输出速率63Hz, 使能温度测量 */
sh3001_temp_config(SH3001_TEMP_ODR_63, SH3001_TEMP_EN);
/* SH3001进入正常工作模式 */
sh3001_switch_powermode(SH3001_NORMAL_MODE);
/* 读取补偿系数 */
sh3001_comp_init(&g_compcoef);
return SH3001_TRUE;
}
/**
* @brief 读温度参数
* @param 无
* @retval 温度值;
*/
float sh3001_get_temperature(void)
{
uint8_t regdata[2] = {0};
uint16_t tempref[2] = {0};
/* 温度数据是无符号的12位 */
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_TEMP_CONF0, 2, ®data[0]); /* SH3001_TEMP_CONF0[3:0]为室温高4位，SH3001_TEMP_CONF1[7:0]为室温低8位 */
tempref[0] = ((uint16_t)(regdata[0] & 0x0F) << 8) | regdata[1];
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_TEMP_ZL, 2, ®data[0]); /* SH3001_TEMP_ZL[7:0]为温度数据低8位，SH3001_TEMP_ZH[3:0]为温度数据高4位 */
tempref[1] = ((uint16_t)(regdata[1] & 0x0F) << 8) | regdata[0];
return ( (((float)(tempref[1] - tempref[0])) / 16.0f) + 25.0f); /* 温度运算公式 */
}
/**
* @brief 读取SH3001陀螺仪和加速度的数据(未作补偿!不推荐,仅供调试)
* @param accdata[3] : acc X,Y,Z;
* @param gyrodata[3] : gyro X,Y,Z;
* @retval 无
*/
void sh3001_get_imu_data(short accdata[3], short gyrodata[3])
{
uint8_t regdata[12] = {0};
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_ACC_XL, 12, regdata);
accdata[0] = ((short)regdata[1] << 8) | regdata[0];
accdata[1] = ((short)regdata[3] << 8) | regdata[2];
accdata[2] = ((short)regdata[5] << 8) | regdata[4];
gyrodata[0] = ((short)regdata[7] << 8) | regdata[6];
gyrodata[1] = ((short)regdata[9] << 8) | regdata[8];
gyrodata[2] = ((short)regdata[11] << 8) | regdata[10];
printf("data: %d %d %d %d %d %d\r\n", accdata[0], accdata[1], accdata[2], gyrodata[0], gyrodata[1], gyrodata[2]);
}
/**
* @brief 读取补偿后SH3001陀螺仪和加速度的数据(推荐使用)
* @param accdata[3] : acc X,Y,Z;
* @param gyrodata[3] : gyro X,Y,Z;
* @retval 无
*/
void sh3001_get_imu_compdata(short accdata[3], short gyrodata[3])
{
uint8_t regdata[15] = {0};
uint8_t paramp;
int acctemp[3], gyrotemp[3];
sh3001_read_nbytes(SH3001_ADDRESS, SH3001_ACC_XL, 15, regdata);
accdata[0] = ((short)regdata[1] << 8) | regdata[0];
accdata[1] = ((short)regdata[3] << 8) | regdata[2];
accdata[2] = ((short)regdata[5] << 8) | regdata[4];
gyrodata[0] = ((short)regdata[7] << 8) | regdata[6];
gyrodata[1] = ((short)regdata[9] << 8) | regdata[8];
gyrodata[2] = ((short)regdata[11] << 8) | regdata[10];
paramp = regdata[14] & 0x1F;
/* 加速度计补偿公式：每个轴原始值还要叠加上其他两个轴及其对应Cross Axis误差补偿系数的乘积 */
acctemp[0] = (int)( accdata[0] + \
accdata[1] * ((float)g_compcoef.cXY / 1024.0f) + \
accdata[2] * ((float)g_compcoef.cXZ / 1024.0f) );
acctemp[1] = (int)( accdata[0] * ((float)g_compcoef.cYX / 1024.0f) + \
accdata[1] + \
accdata[2] * ((float)g_compcoef.cYZ / 1024.0f) );
acctemp[2] = (int)( accdata[0] * ((float)g_compcoef.cZX / 1024.0f) + \
accdata[1] * ((float)g_compcoef.cZY / 1024.0f) + \
accdata[2] );
/* 对非法轴数据做处理 */
if (acctemp[0] > 32767)
{
acctemp[0] = 32767;
}
else if (acctemp[0] < -32768)
{
acctemp[0] = -32768;
}
if (acctemp[1] > 32767)
{
acctemp[1] = 32767;
}
else if (acctemp[1] < -32768)
{
acctemp[1] = -32768;
}
if (acctemp[2] > 32767)
{
acctemp[2] = 32767;
}
else if (acctemp[2] < -32768)
{
acctemp[2] = -32768;
}
accdata[0] = (short)acctemp[0];
accdata[1] = (short)acctemp[1];
accdata[2] = (short)acctemp[2];
/* 陀螺仪补偿公式：每个轴原始值减去对应误差系数乘积 */
gyrotemp[0] = gyrodata[0] - (paramp - g_compcoef.paramP0) * g_compcoef.jX * g_compcoef.xMulti;
gyrotemp[1] = gyrodata[1] - (paramp - g_compcoef.paramP0) * g_compcoef.jY * g_compcoef.yMulti;
gyrotemp[2] = gyrodata[2] - (paramp - g_compcoef.paramP0) * g_compcoef.jZ * g_compcoef.zMulti;
/* 对非法轴数据做处理 */
if (gyrotemp[0] > 32767)
{
gyrotemp[0] = 32767;
}
else if (gyrotemp[0] < -32768)
{
gyrotemp[0] = -32768;
}
if (gyrotemp[1] > 32767)
{
gyrotemp[1] = 32767;
}
else if (gyrotemp[1] < -32768)
{
gyrotemp[1] = -32768;
}
if (gyrotemp[2] > 32767)
{
gyrotemp[2] = 32767;
}
else if (gyrotemp[2] < -32768)
{
gyrotemp[2] = -32768;
}
gyrodata[0] = (short)gyrotemp[0];
gyrodata[1] = (short)gyrotemp[1];
gyrodata[2] = (short)gyrotemp[2];
printf("compdata: %d %d %d %d %d %d\r\n", accdata[0], accdata[1], accdata[2], gyrodata[0], gyrodata[1], gyrodata[2]);
}
sh3001.h
#ifndef __SH3001_H
#define __SH3001_H
#include "./SYSTEM/sys/sys.h"
/* 器件地址 */
#define SH3001_ADDRESS 0x36 /* 由于SDO接GND,SH3001的器件地址为0x36 */
//#define SH3001_WRITE_ADDR 0x6C /* 由器件地址转为写操作地址(0x36 << 1 | 0x0) */
//#define SH3001_READ_ADDR 0x6D /* 由器件地址转为读操作地址(0x36 << 1 | 0x0) */
/******************************************************************
* SH3001寄存器地址
******************************************************************/
#define SH3001_ACC_XL (0x00)
#define SH3001_ACC_XH (0x01)
#define SH3001_ACC_YL (0x02)
#define SH3001_ACC_YH (0x03)
#define SH3001_ACC_ZL (0x04)
#define SH3001_ACC_ZH (0x05)
#define SH3001_GYRO_XL (0x06)
#define SH3001_GYRO_XH (0x07)
#define SH3001_GYRO_YL (0x08)
#define SH3001_GYRO_YH (0x09)
#define SH3001_GYRO_ZL (0x0A)
#define SH3001_GYRO_ZH (0x0B)
#define SH3001_CHIP_ID (0x0F)
#define SH3001_TEMP_ZL (0x0C)
#define SH3001_TEMP_CONF0 (0x20)
#define SH3001_TEMP_CONF1 (0x21)
#define SH3001_ACC_CONF0 (0x22)
#define SH3001_ACC_CONF1 (0x23)
#define SH3001_ACC_CONF2 (0x25)
#define SH3001_ACC_CONF3 (0x26)
#define SH3001_GYRO_CONF0 (0x28)
#define SH3001_GYRO_CONF1 (0x29)
#define SH3001_GYRO_CONF2 (0x2B)
#define SH3001_GYRO_CONF3 (0x8F)
#define SH3001_GYRO_CONF4 (0x9F)
#define SH3001_GYRO_CONF5 (0xAF)
#define SH3001_CHIP_VERSION (0xDD)
#define SH3001_CHIP_ID1 (0xDF)
#define SH3001_AUX_I2C_CONF (0xFD)
/******************************************************************
* 加速度计配置相关宏定义
******************************************************************/
#define SH3001_ODR_1000HZ (0x00)
#define SH3001_ODR_500HZ (0x01)
#define SH3001_ODR_250HZ (0x02)
#define SH3001_ODR_125HZ (0x03)
#define SH3001_ODR_63HZ (0x04)
#define SH3001_ODR_31HZ (0x05)
#define SH3001_ODR_16HZ (0x06)
#define SH3001_ODR_2000HZ (0x08)
#define SH3001_ODR_4000HZ (0x09)
#define SH3001_ODR_8000HZ (0x0A)
#define SH3001_ODR_16000HZ (0x0B)
#define SH3001_ODR_32000HZ (0x0C)
#define SH3001_ACC_RANGE_16G (0x02)
#define SH3001_ACC_RANGE_8G (0x03)
#define SH3001_ACC_RANGE_4G (0x04)
#define SH3001_ACC_RANGE_2G (0x05)
#define SH3001_ACC_ODRX040 (0x00)
#define SH3001_ACC_ODRX025 (0x20)
#define SH3001_ACC_ODRX011 (0x40)
#define SH3001_ACC_ODRX004 (0x60)
#define SH3001_ACC_FILTER_EN (0x00)
#define SH3001_ACC_FILTER_DIS (0x08)
/******************************************************************
* 陀螺仪配置相关宏定义
******************************************************************/
#define SH3001_GYRO_RANGE_125 (0x02)
#define SH3001_GYRO_RANGE_250 (0x03)
#define SH3001_GYRO_RANGE_500 (0x04)
#define SH3001_GYRO_RANGE_1000 (0x05)
#define SH3001_GYRO_RANGE_2000 (0x06)
#define SH3001_GYRO_ODRX00 (0x00)
#define SH3001_GYRO_ODRX01 (0x04)
#define SH3001_GYRO_ODRX02 (0x08)
#define SH3001_GYRO_ODRX03 (0x0C)
#define SH3001_GYRO_FILTER_EN (0x00)
#define SH3001_GYRO_FILTER_DIS (0x10)
/******************************************************************
* 温度配置相关宏定义
******************************************************************/
#define SH3001_TEMP_ODR_500 (0x00)
#define SH3001_TEMP_ODR_250 (0x10)
#define SH3001_TEMP_ODR_125 (0x20)
#define SH3001_TEMP_ODR_63 (0x30)
#define SH3001_TEMP_EN (0x80)
#define SH3001_TEMP_DIS (0x00)
/******************************************************************
* 中断引脚配置相关宏
******************************************************************/
#define SH3001_INT_LOWG (0x8000)
#define SH3001_INT_HIGHG (0x4000)
#define SH3001_INT_INACT (0x2000)
#define SH3001_INT_ACT (0x1000)
#define SH3001_INT_DOUBLE_TAP (0x0800)
#define SH3001_INT_SINGLE_TAP (0x0400)
#define SH3001_INT_FLAT (0x0200)
#define SH3001_INT_ORIENTATION (0x0100)
#define SH3001_INT_FIFO_GYRO (0x0010)
#define SH3001_INT_GYRO_READY (0x0008)
#define SH3001_INT_ACC_FIFO (0x0004)
#define SH3001_INT_ACC_READY (0x0002)
#define SH3001_INT_FREE_FALL (0x0001)
#define SH3001_INT_UP_DOWN_Z (0x0040)
#define SH3001_INT_ENABLE (0x01)
#define SH3001_INT_DISABLE (0x00)
#define SH3001_INT_MAP_INT1 (0x01)
#define SH3001_INT_MAP_INT (0x00)
#define SH3001_INT0_LEVEL_LOW (0x80)
#define SH3001_INT0_LEVEL_HIGH (0x7F)
#define SH3001_INT_NO_LATCH (0x40)
#define SH3001_INT_LATCH (0xBF)
#define SH3001_INT1_LEVEL_LOW (0x20)
#define SH3001_INT1_LEVEL_HIGH (0xDF)
#define SH3001_INT_CLEAR_ANY (0x10)
#define SH3001_INT_CLEAR_STATUS (0xEF)
#define SH3001_INT1_NORMAL (0x04)
#define SH3001_INT1_OD (0xFB)
#define SH3001_INT0_NORMAL (0x01)
#define SH3001_INT0_OD (0xFE)
/******************************************************************
* Orientation Blocking Config Macro Definitions
******************************************************************/
#define SH3001_ORIENT_BLOCK_MODE0 (0x00)
#define SH3001_ORIENT_BLOCK_MODE1 (0x04)
#define SH3001_ORIENT_BLOCK_MODE2 (0x08)
#define SH3001_ORIENT_BLOCK_MODE3 (0x0C)
#define SH3001_ORIENT_SYMM (0x00)
#define SH3001_ORIENT_HIGH_ASYMM (0x01)
#define SH3001_ORIENT_LOW_ASYMM (0x02)
/******************************************************************
* Flat Time Config Macro Definitions
******************************************************************/
#define SH3001_FLAT_TIME_500MS (0x40)
#define SH3001_FLAT_TIME_1000MS (0x80)
#define SH3001_FLAT_TIME_2000MS (0xC0)
/******************************************************************
* 活动和非活动中断相关宏定义
******************************************************************/
#define SH3001_ACT_AC_MODE (0x80)
#define SH3001_ACT_DC_MODE (0x00)
#define SH3001_ACT_X_INT_EN (0x40)
#define SH3001_ACT_X_INT_DIS (0x00)
#define SH3001_ACT_Y_INT_EN (0x20)
#define SH3001_ACT_Y_INT_DIS (0x00)
#define SH3001_ACT_Z_INT_EN (0x10)
#define SH3001_ACT_Z_INT_DIS (0x00)
#define SH3001_INACT_AC_MODE (0x08)
#define SH3001_INACT_DC_MODE (0x00)
#define SH3001_INACT_X_INT_EN (0x04)
#define SH3001_INACT_X_INT_DIS (0x00)
#define SH3001_INACT_Y_INT_EN (0x02)
#define SH3001_INACT_Y_INT_DIS (0x00)
#define SH3001_INACT_Z_INT_EN (0x01)
#define SH3001_INACT_Z_INT_DIS (0x00)
#define SH3001_LINK_PRE_STA (0x01)
#define SH3001_LINK_PRE_STA_NO (0x00)
/******************************************************************
* TAP Int Config Macro Definitions
******************************************************************/
#define SH3001_TAP_X_INT_EN (0x08)
#define SH3001_TAP_X_INT_DIS (0x00)
#define SH3001_TAP_Y_INT_EN (0x04)
#define SH3001_TAP_Y_INT_DIS (0x00)
#define SH3001_TAP_Z_INT_EN (0x02)
#define SH3001_TAP_Z_INT_DIS (0x00)
/******************************************************************
* HIGHG Int Config Macro Definitions
******************************************************************/
#define SH3001_HIGHG_ALL_INT_EN (0x80)
#define SH3001_HIGHG_ALL_INT_DIS (0x00)
#define SH3001_HIGHG_X_INT_EN (0x40)
#define SH3001_HIGHG_X_INT_DIS (0x00)
#define SH3001_HIGHG_Y_INT_EN (0x20)
#define SH3001_HIGHG_Y_INT_DIS (0x00)
#define SH3001_HIGHG_Z_INT_EN (0x10)
#define SH3001_HIGHG_Z_INT_DIS (0x00)
/******************************************************************
* LOWG Int Config Macro Definitions
******************************************************************/
#define SH3001_LOWG_ALL_INT_EN (0x01)
#define SH3001_LOWG_ALL_INT_DIS (0x00)
/******************************************************************
* FIFO配置相关宏定义
******************************************************************/
#define SH3001_FIFO_MODE_DIS (0x00)
#define SH3001_FIFO_MODE_FIFO (0x01)
#define SH3001_FIFO_MODE_STREAM (0x02)
#define SH3001_FIFO_MODE_TRIGGER (0x03)
#define SH3001_FIFO_ACC_DOWNS_EN (0x80)
#define SH3001_FIFO_ACC_DOWNS_DIS (0x00)
#define SH3001_FIFO_GYRO_DOWNS_EN (0x08)
#define SH3001_FIFO_GYRO_DOWNS_DIS (0x00)
#define SH3001_FIFO_FREQ_X1_2 (0x00)
#define SH3001_FIFO_FREQ_X1_4 (0x01)
#define SH3001_FIFO_FREQ_X1_8 (0x02)
#define SH3001_FIFO_FREQ_X1_16 (0x03)
#define SH3001_FIFO_FREQ_X1_32 (0x04)
#define SH3001_FIFO_FREQ_X1_64 (0x05)
#define SH3001_FIFO_FREQ_X1_128 (0x06)
#define SH3001_FIFO_FREQ_X1_256 (0x07)
#define SH3001_FIFO_EXT_Z_EN (0x2000)
#define SH3001_FIFO_EXT_Y_EN (0x1000)
#define SH3001_FIFO_EXT_X_EN (0x0080)
#define SH3001_FIFO_TEMPERATURE_EN (0x0040)
#define SH3001_FIFO_GYRO_Z_EN (0x0020)
#define SH3001_FIFO_GYRO_Y_EN (0x0010)
#define SH3001_FIFO_GYRO_X_EN (0x0008)
#define SH3001_FIFO_ACC_Z_EN (0x0004)
#define SH3001_FIFO_ACC_Y_EN (0x0002)
#define SH3001_FIFO_ACC_X_EN (0x0001)
#define SH3001_FIFO_ALL_DIS (0x0000)
/******************************************************************
* 从IIC配置相关宏定义
******************************************************************/
#define SH3001_MI2C_NORMAL_MODE (0x00)
#define SH3001_MI2C_BYPASS_MODE (0x01)
#define SH3001_MI2C_READ_ODR_200HZ (0x00)
#define SH3001_MI2C_READ_ODR_100HZ (0x10)
#define SH3001_MI2C_READ_ODR_50HZ (0x20)
#define SH3001_MI2C_READ_ODR_25HZ (0x30)
#define SH3001_MI2C_FAIL (0x20)
#define SH3001_MI2C_SUCCESS (0x10)
#define SH3001_MI2C_READ_MODE_AUTO (0x40)
#define SH3001_MI2C_READ_MODE_MANUAL (0x00)
/******************************************************************
* 其他宏定义
******************************************************************/
#define SH3001_TRUE (0)
#define SH3001_FALSE (1)
#define SH3001_NORMAL_MODE (0x00)
#define SH3001_SLEEP_MODE (0x01)
#define SH3001_POWERDOWN_MODE (0x02)
#define SH3001_ACC_NORMAL_MODE (0x03)
#define SH3001_CHIP_VERSION_MCC (0x08)
#define SH3001_CHIP_VERSION_MCD (0x10)
#define SH3001_CHIP_VERSION_MCF (0x20)
/***************************************************************************
* 结构体类型定义
****************************************************************************/
typedef struct
{
int8_t cXY;
int8_t cXZ;
int8_t cYX;
int8_t cYZ;
int8_t cZX;
int8_t cZY;
/* 以上六个成员是加速度计Cross Axis误差补偿系数 */
/* 以下七个成员是陀螺仪的误差补偿系数 */
int8_t jX;
int8_t jY;
int8_t jZ;
uint8_t xMulti;
uint8_t yMulti;
uint8_t zMulti;
uint8_t paramP0;
} compCoefType;
/***************************************************************************
* 函数声明
****************************************************************************/
uint8_t sh3001_read_nbytes(uint8_t devaddr, uint8_t regaddr, uint8_t length, uint8_t *readbuf);
uint8_t sh3001_write_nbytes(uint8_t devaddr, uint8_t regaddr, uint8_t length, uint8_t *writebuf);
uint8_t sh3001_switch_powermode(uint8_t powermode);
uint8_t sh3001_init(void);
float sh3001_get_temperature(void);
void sh3001_get_imu_data(short accdata[3], short gyrodata[3]);
void sh3001_get_imu_compdata(short accdata[3], short gyrodata[3]);
#endif
imu.c
#include "./BSP/SH3001/imu.h"
#include "./BSP/SH3001/sh3001.h"
#include "./SYSTEM/usart/usart.h"
#include "./SYSTEM/delay/delay.h"
#include <math.h>
#define IMU_DELTA_T 0.005f /* 积分周期时间,5ms计算一次 */
#define IMU_M_PI 3.1425f
#define IMU_NEW_WEIGHT 0.35f /* 新值权重 */
#define IMU_OLD_WEIGHT 0.65f /* 旧值权重 */
quater_info_t g_q_info = {1, 0, 0, 0}; /* 全局四元数,roll、pitch和yaw都为0时，对应的四元数值 */
float g_param_kp = 50.0f; /* 用于调整角速度计修整陀螺仪的速度 加速度计(磁力计)的收敛速率比例增益50 */
float g_param_ki = 0.20f; /* 陀螺仪收敛速率的积分增益 0.2 */
short g_acc_avg[3]; /* 加速度平均值 */
short g_gyro_avg[3]; /* 陀螺仪平均值 */
/**
* @brief 开方函数
* @param x : 待开方的值
* @retval 开方结果
*/
static float imu_inv_sqrt(float x)
{
float halfx = 0.5f * x;
float y = x;
long i = *(long *)&y;
i = 0x5f3759df - (i >> 1);
y = *(float *)&i;
y = y * (1.5f - (halfx * y * y));
return y;
}
/**
* @brief 数据转换
* @note 对加速度数据做一阶低通滤波(参考匿名)，对gyro转成弧度每秒(2000dps)
* @param gx, gy, gz : 3轴陀螺仪数据指针
* @param ax, ay, az : 3轴加速度数据指针
* @retval 无
*/
static void imu_data_transform(float *gx, float *gy, float *gz, float *ax, float *ay, float *az)
{
static double lastax = 0;
static double lastay = 0;
static double lastaz = 0;
*ax = *ax * IMU_NEW_WEIGHT + lastax * IMU_OLD_WEIGHT;
*ay = *ay * IMU_NEW_WEIGHT + lastay * IMU_OLD_WEIGHT;
*az = *az * IMU_NEW_WEIGHT + lastaz * IMU_OLD_WEIGHT;
lastax = *ax;
lastay = *ay;
lastaz = *az;
*gx = *gx * IMU_M_PI / 180 / 16.4f; /* 陀螺仪量程为±2000°/s，所以灵敏度为16.4LSB/° */
*gy = *gy * IMU_M_PI / 180 / 16.4f; /* 陀螺仪数据除以16.4得到的是角度，在进行四元数解算时需要将其转化为弧度 */
*gz = *gz * IMU_M_PI / 180 / 16.4f;
}
/**
* @brief 姿态解算融合, 核心算法
* @note 使用的是互补滤波算法，没有使用Kalman滤波算法
* 尽量保证该函数的调用频率为: IMU_DELTA_T , 否则YAW会相应的偏大/偏小
* @param gx, gy, gz : 3轴陀螺仪数据
* @param ax, ay, az : 3轴加速度数据
* @retval 无
*/
static void imu_ahrsupdate_nomagnetic(float gx, float gy, float gz, float ax, float ay, float az)
{
static float i_ex, i_ey, i_ez; /* 误差积分 */
float half_t = 0.25f * IMU_DELTA_T;
float vx, vy, vz; /* 当前的机体坐标系上的重力单位向量 */
float ex, ey, ez; /* 四元数计算值与加速度计测量值的误差 */
float q0 = g_q_info.q0;
float q1 = g_q_info.q1;
float q2 = g_q_info.q2;
float q3 = g_q_info.q3;
float q0q0 = q0 * q0;
float q0q1 = q0 * q1;
float q0q2 = q0 * q2;
float q0q3 = q0 * q3;
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q3q3 = q3 * q3;
float delta_2 = 0;
/* 将加速度计得到的实际重力加速度向量单位化（归一化） */
float norm = imu_inv_sqrt(ax * ax + ay * ay + az * az);
ax = ax * norm;
ay = ay * norm;
az = az * norm;
/* 通过四元数得到理论重力加速度向量g */
/* 理想重力加速度方向在飞行器坐标系中的表示，为四元数表示的旋转矩阵的第三行 */
vx = 2 * (q1q3 - q0q2);
vy = 2 * (q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
/* 向量叉积与陀螺仪角速度补偿问题，https://blog.csdn.net/Leyvi_Hsing/article/details/54293690 */
/* 加速度计读取的实际重力加速度方向向量(ax/ay/az)与理论重力加速度方向向量(vx,vy,vz)的误差，用向量叉乘计算，做陀螺仪的误差补偿 */
ex = ay * vz - az * vy;
ey = az * vx - ax * vz;
ez = ax * vy - ay * vx;
/* 用叉乘误差来做PI修正陀螺仪零偏，补偿角速度
* 通过调节 g_param_kp，g_param_ki 两个参数，
* 可以控制加速度计修正陀螺仪积分姿态的速度。*/
/* 误差积分过程 */
i_ex += IMU_DELTA_T * ex; /* IMU_DELTA_T 积分周期时间 */
i_ey += IMU_DELTA_T * ey;
i_ez += IMU_DELTA_T * ez;
/* 用外积叉乘误差来做PI修正陀螺零偏，即抵消陀螺读数中的偏移量 */
gx = gx + g_param_kp * ex + g_param_ki * i_ex;
gy = gy + g_param_kp * ey + g_param_ki * i_ey;
gz = gz + g_param_kp * ez + g_param_ki * i_ez;
/* 数据修正完成，下面是四元数微分方程 */
/* 四元数微分方程，其中half_t为测量周期的1/4，gx gy gz为陀螺仪角速度，
以下都是已知量，这里使用了一阶龙哥库塔求解四元数微分方程 */
// q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * half_t;
// q1 = q1 + ( q0 * gx + q2 * gz - q3 * gy) * half_t;
// q2 = q2 + ( q0 * gy - q1 * gz + q3 * gx) * half_t;
// q3 = q3 + ( q0 * gz + q1 * gy - q2 * gx) * half_t;
/* 整合四元数率 四元数微分方程 四元数更新算法，二阶毕卡法 */
delta_2 = (2 * half_t * gx) * (2 * half_t * gx) + (2 * half_t * gy) * (2 * half_t * gy) + (2 * half_t * gz) * (2 * half_t * gz);
q0 = (1 - delta_2 / 8) * q0 + (-q1 * gx - q2 * gy - q3 * gz) * half_t;
q1 = (1 - delta_2 / 8) * q1 + (q0 * gx + q2 * gz - q3 * gy) * half_t;
q2 = (1 - delta_2 / 8) * q2 + (q0 * gy - q1 * gz + q3 * gx) * half_t;
q3 = (1 - delta_2 / 8) * q3 + (q0 * gz + q1 * gy - q2 * gx) * half_t;
/* 单位化四元数 保证四元数在迭代过程中保持单位性质 */
norm = imu_inv_sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
g_q_info.q0 = q0 * norm;
g_q_info.q1 = q1 * norm;
g_q_info.q2 = q2 * norm;
g_q_info.q3 = q3 * norm;
}
/**
* @brief 得到姿态解算后的欧拉角
* @param gx, gy, gz : 3轴陀螺仪数据
* @param ax, ay, az : 3轴加速度数据
* @retval 返回值 : 欧拉角
*/
eulerian_angles_t imu_get_eulerian_angles(float gx, float gy, float gz, float ax, float ay, float az)
{
eulerian_angles_t eulerangle;
imu_data_transform(&gx, &gy, &gz, &ax, &ay, &az); /* 数据转换 */
imu_ahrsupdate_nomagnetic(gx, gy, gz, ax, ay, az); /* 姿态解算 */
float q0 = g_q_info.q0;
float q1 = g_q_info.q1;
float q2 = g_q_info.q2;
float q3 = g_q_info.q3;
eulerangle.pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 180 / IMU_M_PI;
eulerangle.roll = -atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 180 / IMU_M_PI;
eulerangle.yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 180 / IMU_M_PI;
/* 可以不用作姿态限度的限制 */
if (eulerangle.roll > 90 || eulerangle.roll < -90)
{
if (eulerangle.pitch > 0)
{
eulerangle.pitch = 180 - eulerangle.pitch;
}
if (eulerangle.pitch < 0)
{
eulerangle.pitch = -(180 + eulerangle.pitch);
}
}
if (eulerangle.yaw > 180)
{
eulerangle.yaw -= 360;
}
else if (eulerangle.yaw < -180)
{
eulerangle.yaw += 360;
}
return eulerangle;
}
/**
* @brief 数据校准
* @note 对数据减去初值
* @param gx, gy, gz : 3轴陀螺仪数据指针
* @param ax, ay, az : 3轴加速度数据指针
* @retval 无
*/
void imu_data_calibration(short *gx, short *gy, short *gz, short *ax, short *ay, short *az)
{
/* 减去初值(去掉零飘) */
*gx -= g_gyro_avg[0];
*gy -= g_gyro_avg[1];
*gz -= g_gyro_avg[2];
*ax -= g_acc_avg[0];
*ay -= g_acc_avg[1];
*az -= (g_acc_avg[2] - 2048); /* 2048是重力加速度理论值 */
}
/**
* @brief 姿态解算初始化
* @note 该函数必须在传感器初始化之后再调用
* @param 无
* @retval 无
*/
void imu_init(void)
{
uint16_t i = 0;
int acc_sum[3] = {0}, gyro_sum[3] = {0};
short acc_data[3]; /* 加速度传感器原始数据 */
short gyro_data[3]; /* 陀螺仪原始数据 */
delay_ms(200);
for (i = 0; i < 250; i++) /* 循环读取250次 取平均 */
{
sh3001_get_imu_compdata(acc_data, gyro_data);
acc_sum[0] += acc_data[0];
acc_sum[1] += acc_data[1];
acc_sum[2] += acc_data[2];
gyro_sum[0] += gyro_data[0];
gyro_sum[1] += gyro_data[1];
gyro_sum[2] += gyro_data[2];
delay_ms(5);
}
g_acc_avg[0] = acc_sum[0] / 250;
g_acc_avg[1] = acc_sum[1] / 250;
g_acc_avg[2] = acc_sum[2] / 250;
g_gyro_avg[0] = gyro_sum[0] / 250;
g_gyro_avg[1] = gyro_sum[1] / 250;
g_gyro_avg[2] = gyro_sum[2] / 250;
}
imu.h
#ifndef __IMU_H
#define __IMU_H
#include "./SYSTEM/sys/sys.h"
/* 四元数结构体类型 */
typedef struct{
float q0;
float q1;
float q2;
float q3;
}quater_info_t;
/* 欧拉角结构体类型 */
typedef struct{
float pitch;
float roll;
float yaw;
}eulerian_angles_t;
/* 函数声明 */
void imu_init(void);
eulerian_angles_t imu_get_eulerian_angles(float gx, float gy, float gz, float ax, float ay, float az);
void imu_data_calibration(short *gx, short *gy, short *gz, short *ax, short *ay, short *az);
#endif
main.c
#include "./SYSTEM/sys/sys.h"
#include "./SYSTEM/usart/usart.h"
#include "./SYSTEM/delay/delay.h"
#include "./BSP/LED/led.h"
#include "./BSP/LCD/lcd.h"
#include "./BSP/KEY/key.h"
#include "./BSP/SDRAM/sdram.h"
#include "./USMART/usmart.h"
#include "./BSP/SH3001/sh3001.h"
#include "./BSP/SH3001/imu.h"
#include <math.h>
/**
* @brief 显示角度
* @param x, y : 坐标
* @param title: 标题
* @param angle: 角度
* @retval 无
*/
void user_show_angle(uint16_t x, uint16_t y, char *title, float angle)
{
char buf[20];
sprintf(buf,"%s%3.1fC", title, angle); /* 格式化输出 */
lcd_fill(x, y, x + 160, y + 16, WHITE); /* 清除上次数据(最多显示20个字符,20*8=160) */
lcd_show_string(x, y, 160, 16, 16, buf, BLUE); /* 显示字符串 */
}
/**
* @brief 显示温度
* @param x, y : 坐标
* @param temp : 温度
* @retval 无
*/
void user_show_temprate(uint16_t x, uint16_t y, float temp)
{
char buf[20];
sprintf(buf," Temp :%2.1fC", temp); /* 格式化输出 */
lcd_fill(x, y, x + 160, y + 16, WHITE); /* 清除上次数据(最多显示20个字符,20*8=160) */
lcd_show_string(x, y, 160, 16, 16, buf, BLUE); /* 显示字符串 */
}
/**
* @brief 串口1发送1个字符
* @param c : 要发送的字符
* @retval 无
*/
void usart1_send_char(uint8_t c)
{
while ((USART_UX->SR & 0X40) == 0); /* 等待上一个字符发送完成 */
USART_UX->DR = (uint8_t)c; /* 将要发送的字符 ch 写入到DR寄存器 */
}
/**
* @brief 传送数据给 ANO_TC匿名科创地面站v4.exe
* @param fun : 功能字. 0XA0~0XAF
* @param data : 数据缓存区,最多28字节!!
* @param len : data区有效数据个数
* @retval 无
*/
void usart1_niming_report(uint8_t fun, uint8_t *data, uint8_t len)
{
uint8_t send_buf[32];
uint8_t i;
if (len > 28)
{
return; /* 最多28字节数据 */
}
send_buf[len + 4] = 0; /* 校验数置零 */
send_buf[0] = 0XAA; /* 帧头 */
send_buf[1] = 0XAA; /* 帧头 */
send_buf[2] = fun; /* 功能字 */
send_buf[3] = len; /* 数据长度 */
for (i = 0; i < len; i++)
{
send_buf[4 + i] = data[i]; /* 复制数据 */
}
for (i = 0; i < len + 4; i++)
{
send_buf[len + 4] += send_buf[i]; /* 计算校验和 */
}
for (i = 0; i < len + 5; i++)
{
usart1_send_char(send_buf[i]); /* 发送数据到串口1 */
}
}
/**
* @brief 发送加速度传感器数据和陀螺仪数据
* @param aacx,aacy,aacz : x,y,z三个方向上面的加速度值
* @param gyrox,gyroy,gyroz : x,y,z三个方向上面的陀螺仪值
* @retval 无
*/
void sh3001_send_data(short aacx, short aacy, short aacz, short gyrox, short gyroy, short gyroz)
{
uint8_t tbuf[18];
tbuf[0] = (aacx >> 8) & 0XFF;
tbuf[1] = aacx & 0XFF;
tbuf[2] = (aacy >> 8) & 0XFF;
tbuf[3] = aacy & 0XFF;
tbuf[4] = (aacz >> 8) & 0XFF;
tbuf[5] = aacz & 0XFF;
tbuf[6] = (gyrox >> 8) & 0XFF;
tbuf[7] = gyrox & 0XFF;
tbuf[8] = (gyroy >> 8) & 0XFF;
tbuf[9] = gyroy & 0XFF;
tbuf[10] = (gyroz >> 8) & 0XFF;
tbuf[11] = gyroz & 0XFF;
tbuf[12] = 0; /* 因为开启MPL后,无法直接读取磁力计数据,所以这里直接屏蔽掉.用0替代. */
tbuf[13] = 0;
tbuf[14] = 0;
tbuf[15] = 0;
tbuf[16] = 0;
tbuf[17] = 0;
usart1_niming_report(0X02, tbuf, 18); /* 自定义帧,0X02 */
}
/**
* @brief 通过串口1上报结算后的姿态数据给电脑
* @param roll : 横滚角.单位0.1度。 -9000 -> 9000 对应 -90.00 -> 90.00度
* @param pitch : 俯仰角.单位 0.1度。-18000 -> 18000 对应 -180.00 -> 180.00 度
* @param yaw : 航向角.单位为0.1度 -18000 -> 18000 对应 -180.00 -> 180.00 度
* @param prs : 气压计高度,单位:cm
* @param fly_mode : 飞行模式
* @param armed : 锁定状态
* @retval 无
*/
void usart1_report_imu(short roll, short pitch, short yaw, int prs, uint8_t fly_mode, uint8_t armed)
{
uint8_t tbuf[12];
tbuf[0] = (roll >> 8) & 0XFF;
tbuf[1] = roll & 0XFF;
tbuf[2] = (pitch >> 8) & 0XFF;
tbuf[3] = pitch & 0XFF;
tbuf[4] = (yaw >> 8) & 0XFF;
tbuf[5] = yaw & 0XFF;
tbuf[6] = (prs >> 24) & 0XFF;
tbuf[7] = (prs >> 16) & 0XFF;
tbuf[8] = (prs >> 8) & 0XFF;
tbuf[9] = prs & 0XFF;
tbuf[10] = fly_mode;
tbuf[11] = armed;
usart1_niming_report(0X01, tbuf, 12); /* 状态帧,0X01 */
}
int main(void)
{
uint8_t t = 0;
eulerian_angles_t e_angles;
float temperature; /* 温度值 */
short acc_data[3]; /* 加速度传感器原始数据 */
short gyro_data[3]; /* 陀螺仪原始数据 */
uint8_t key, report = 1; /* 默认开启上报 */
HAL_Init(); /* 初始化HAL库 */
sys_stm32_clock_init(360, 25, 2, 8); /* 设置时钟,180Mhz */
delay_init(180); /* 延时初始化 */
usart_init(500000); /* 串口初始化 */
usmart_dev.init(90); /* USMART初始化 */
led_init(); /* 初始化LED */
sdram_init(); /* 初始化SDRAM */
lcd_init(); /* 初始化LCD */
key_init(); /* 初始化按键 */
while (sh3001_init()) /* 检测不到SH3001 */
{
lcd_show_string(30, 130, 200, 16, 16, "SH3001 Check Failed!", RED);
delay_ms(500);
lcd_show_string(30, 130, 200, 16, 16, "Please Check! ", RED);
delay_ms(500);
LED0_TOGGLE(); /* 红灯闪烁 */
}
imu_init(); /* 初始化 IMU */
lcd_show_string(30, 50, 200, 16, 16, "STM32", RED);
lcd_show_string(30, 70, 200, 16, 16, "SH3001 TEST", RED);
lcd_show_string(30, 90, 200, 16, 16, "ATOM@ALIENTEK", RED);
lcd_show_string(30, 110, 200, 16, 16, "KEY0:UPLOAD ON/OFF", RED);
while (1)
{
key = key_scan(0);
if (key == KEY0_PRES)
{
report =! report;
if (report)
{
lcd_show_string(30, 210, 200, 16, 16, " UPLOAD ON ", RED);
}
else
{
lcd_show_string(30, 210, 200, 16, 16, " UPLOAD OFF ", RED);
}
}
t++;
/* 读取传感器数据 */
sh3001_get_imu_compdata(acc_data, gyro_data);
/* 数据校准 */
imu_data_calibration(&gyro_data[0], &gyro_data[1], &gyro_data[2],
&acc_data[0], &acc_data[1], &acc_data[2]);
/* 获取欧拉角 */
e_angles = imu_get_eulerian_angles(gyro_data[0], gyro_data[1], gyro_data[2],
acc_data[0], acc_data[1], acc_data[2]);
if (report)
{
sh3001_send_data(acc_data[0], acc_data[1], acc_data[2], gyro_data[0], gyro_data[1], gyro_data[2]); /* 发送加速度+陀螺仪原始数据 */
usart1_report_imu((int)(e_angles.roll * 100), (int)(e_angles.pitch * 100), (int)(e_angles.yaw * 100), 0, 0, 0);
}
if ((t % 50) == 0) /* 0.1秒左右更新一次温度/欧拉角 */
{
temperature = sh3001_get_temperature(); /* 读取温度值 */
user_show_temprate(30, 130, temperature);
user_show_angle(30, 150, "Pitch :", e_angles.pitch);
user_show_angle(30, 170, " Roll :", e_angles.roll);
user_show_angle(30, 190, " Yaw :", e_angles.yaw);
printf("\r\nPITCH:%f\r\n", e_angles.pitch);
printf("ROLL:%f\r\n", e_angles.roll);
printf("YAW:%f\r\n", e_angles.yaw);
LED0_TOGGLE();
}
}
}
八、总结
6080
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
