mpu9250九轴算法-纯c语言

基于mpu9250的九轴算法，融合了重力加速度和磁力计来实现四元素换欧拉角的算法，可以避免因为高速运动而产生的不稳定，是属于纯c语言的源代码，不依赖于任何外库。

#include “icm20602.h“
#include “imu.h“

#if defined（USE_IIC）

static ST_ICM20602_FIFO g_astRxIcmBuf;
static ST_ICM20602 st_icm20602;

//======================================================================
//@fn function  :ICM20602_Set_Rate
//@Brief        :设置采样率ucrate HZ
//@Constraint   :无
//@Param        :[in]: usrate   -- 采样频率
//@Return       :uint8_t
//@Retval       :
//======================================================================
static uint8_t ICM20602_Set_Rate（uint16_t usrate）
{
    uint8_t ucdata = 0;
    if（usrate > 1000）   usrate = 1000;
    if（usrate < 4）      usrate = 4;
    ucdata = 1000 / usrate - 1;
    return ucdata;
}

//======================================================================
//@fn function  :ICM20602_Set_Gyro_LPF
//@Brief        :设置数字低通滤波器
//@Constraint   :无
//@Param        :[in]: usrate   -- 采样频率
//@Return       :uint8_t
//@Retval       :
//======================================================================
static uint8_t ICM20602_Set_Gyro_LPF（uint16_t usrate）
{
    uint16_t uslpf = usrate / 2;
    uint8_t ucdata = 0;
		if（uslpf >= 250）        ucdata=7;
		else if（uslpf >= 176）   ucdata=1;
		else if（uslpf >= 92）    ucdata=2;
		else if（uslpf >= 41）    ucdata=3;
		else if（uslpf >= 20）    ucdata=4;
		else if（uslpf >= 10）    ucdata=5;
		else ucdata=6;  
    return ucdata;
}

//======================================================================
//@fn function  :ICM20602_Set_Acc_LPF
//@Brief        :设置数字低通滤波器
//@Constraint   :无
//@Param        :[in]: usrate   -- 采样频率
//@Return       :uint8_t
//@Retval       :
//======================================================================
static uint8_t ICM20602_Set_Acc_LPF（uint16_t usrate）
{
    uint16_t uslpf = usrate / 2;
    uint8_t ucdata = 0;
		if（uslpf >= 218）        ucdata=7;
		else if（uslpf >= 99）    ucdata=2;
		else if（uslpf >= 44）    ucdata=3;
		else if（uslpf >= 21）    ucdata=4;
		else if（uslpf >= 10）    ucdata=5;
		else ucdata=6; 
    return ucdata;
}

//======================================================================
//@fn function  :ICM20602_Init
//@Brief        :icm20602初始化
//@Constraint   :无
//@Param        :[in]: 
//@Return       :void
//@Retval       :
//======================================================================
    HAL_StatusTypeDef res;
extern I2C_HandleTypeDef hi2c1;
void ICM20602_Init（void）
{
    uint16_t usrate = 500;         //500
    uint8_t uidata = 0x00;
    PWR_MGMT_1:
    if（HAL_OK != （res = HAL_I2C_Mem_Write（&hi2c1DEF_AD0_0DEF_PWR_MGMT_1I2C_MEMADD_SIZE_8BIT&uidata11000）））             //复位
    {
        printf（“PWR_MGMT_1 Init err...\n“）;
        goto PWR_MGMT_1;
    }
    uidata = 0x03;
    PATH_RESET:
    if（HAL_OK != HAL_I2C_Mem_Write（&hi2c1DEF_AD0_0DEF_SIGNAL_PATH_RESETI2C_MEMADD_SIZE_8BIT&uidata11000））      //复位
    {
        printf（“PATH_RESET Init err...\n“）;
        goto PATH_RESET;
    }
    uidata = 3

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2018-01-02 10:23  9轴算法\
     文件         794  2017-03-29 23:02  9轴算法\ctrl.h
     文件        8294  2017-04-22 05:22  9轴算法\icm20602.c
     文件        3178  2017-03-10 23:37  9轴算法\icm20602.h
     文件       14871  2017-04-22 05:20  9轴算法\imu.c
     文件         516  2017-02-13 04:51  9轴算法\imu.h
     文件        4373  2017-03-18 02:51  9轴算法\lsm.c
     文件         260  2017-02-16 23:36  9轴算法\lsm.h
     文件        4486  2017-10-30 14:01  9轴算法\tags
     目录           0  2017-11-15 10:24  9轴算法\四元数解算欧拉角\
     文件      113983  2017-11-15 10:19  9轴算法\四元数解算欧拉角\1归一重力和磁力初始化四元数.png
     文件      134541  2017-11-15 10:20  9轴算法\四元数解算欧拉角\2根据参考向量求重力向量.png
     文件      131672  2017-11-15 10:21  9轴算法\四元数解算欧拉角\3磁力向量解析.png
     文件      153032  2017-11-15 10:22  9轴算法\四元数解算欧拉角\3磁力向量转换.png
     文件      111591  2017-11-15 10:22  9轴算法\四元数解算欧拉角\4利用向量算出误差，再用误差校正陀螺仪，再用校正后的值更新四元数.png
     文件       57789  2017-11-15 10:24  9轴算法\四元数解算欧拉角\把四元数规范后转换成欧拉角.png