//#include  "icm42688.h"
//
//#include "kalman.h"
//#include "alldata.h"
//
//static float icm42688_acc_inv = 1, icm42688_gyro_inv = 1;
//
//void Spi_GPIO_Init()
//{
//    GPIO_InitTypeDef GPIO_InitStruct = {0};
//    __HAL_RCC_GPIOB_CLK_ENABLE();
//    /*Configure GPIO pins : PBPin PB12 */
//    GPIO_InitStruct.Pin = GPIO_PIN_12;
//    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
//    GPIO_InitStruct.Pull = GPIO_NOPULL;
//    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
//    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
//}
//
//void Init_ICM42688(void)
//{
//    printf("Init_ICM42688\r\n");
//    HAL_Delay(1000);
//    HAL_Delay(1000);
//    unsigned char model;
//    Spi_GPIO_Init();
//    // 读取陀螺仪型号陀螺仪自检
//    model = 0xff;
//    while(1)
//    {
//        Read_Datas_ICM42688(ICM42688_WHO_AM_I, &model, 1);
//        if(model == 0x47)
//        {
//            printf("model:%x\r\n",model);
//            HAL_Delay(1000);
//            HAL_Delay(1000);
//            break;
//        }
//        ICM42688_DELAY_MS(100);
//    }
//    Write_Data_ICM42688(ICM42688_PWR_MGMT0,0x00);				// 复位设备
//    ICM42688_DELAY_MS(10);															// 操作完PWR—MGMT0寄存器后200us内不能有任何读写寄存器的操作
//    // 设置ICM42688加速度计和陀螺仪的量程和输出速率
//    Set_LowpassFilter_Range_ICM42688(ICM42688_AFS_16G, ICM42688_AODR_32000HZ, ICM42688_GFS_2000DPS, ICM42688_GODR_32000HZ);
//    Write_Data_ICM42688(ICM42688_PWR_MGMT0, 0x0f);      // 设置GYRO_MODE,ACCEL_MODE为低噪声模式
//    ICM42688_DELAY_MS(10);
//}
//
//void Set_LowpassFilter_Range_ICM42688(enum icm42688_afs afs, enum icm42688_aodr aodr, enum icm42688_gfs gfs, enum icm42688_godr godr)
//{
//    Write_Data_ICM42688(ICM42688_ACCEL_CONFIG0, (afs << 5) | (aodr + 1));   // 初始化ACCEL量程和输出速率(p77)
//    Write_Data_ICM42688(ICM42688_GYRO_CONFIG0, (gfs << 5) | (godr + 1));    // 初始化GYRO量程和输出速率(p76)
//
//    switch(afs)
//    {
//        case ICM42688_AFS_2G:
//            icm42688_acc_inv = 2000 / 32768.0f;             // 加速度计量程为:±2g
//            break;
//        case ICM42688_AFS_4G:
//            icm42688_acc_inv = 4000 / 32768.0f;             // 加速度计量程为:±4g
//            break;
//        case ICM42688_AFS_8G:
//            icm42688_acc_inv = 8000 / 32768.0f;             // 加速度计量程为:±8g
//            break;
//        case ICM42688_AFS_16G:
//            icm42688_acc_inv = 16000 / 32768.0f;            // 加速度计量程为:±16g
//            break;
//        default:
//            icm42688_acc_inv = 1;                           // 不转化为实际数据
//            break;
//    }
//    switch(gfs)
//    {
//        case ICM42688_GFS_15_625DPS:
//            icm42688_gyro_inv = 15.625f / 32768.0f;         // 陀螺仪量程为:±15.625dps
//            break;
//        case ICM42688_GFS_31_25DPS:
//            icm42688_gyro_inv = 31.25f / 32768.0f;          // 陀螺仪量程为:±31.25dps
//            break;
//        case ICM42688_GFS_62_5DPS:
//            icm42688_gyro_inv = 62.5f / 32768.0f;           // 陀螺仪量程为:±62.5dps
//            break;
//        case ICM42688_GFS_125DPS:
//            icm42688_gyro_inv = 125.0f / 32768.0f;          // 陀螺仪量程为:±125dps
//            break;
//        case ICM42688_GFS_250DPS:
//            icm42688_gyro_inv = 250.0f / 32768.0f;          // 陀螺仪量程为:±250dps
//            break;
//        case ICM42688_GFS_500DPS:
//            icm42688_gyro_inv = 500.0f / 32768.0f;          // 陀螺仪量程为:±500dps
//            break;
//        case ICM42688_GFS_1000DPS:
//            icm42688_gyro_inv = 1000.0f / 32768.0f;         // 陀螺仪量程为:±1000dps
//            break;
//        case ICM42688_GFS_2000DPS:
//            icm42688_gyro_inv = 2000.0f / 32768.0f;         // 陀螺仪量程为:±2000dps
//            break;
//        default:
//            icm42688_gyro_inv = 1;                          // 不转化为实际数据
//            break;
//    }
//}
//
///**
//*
//* @brief    ICM42688陀螺仪写数据
//* @param    reg                 寄存器
//* @param    data                需要写进该寄存器的数据
//* @return   void
//* @notes    ICM42688.c文件内部调用,用户无需调用尝试
//* Example:  Write_Data_ICM42688(0x00,0x00);
//*
//**/
//static void Write_Data_ICM42688(unsigned char reg, unsigned char dat)
//{
//    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12,0);
//    HAL_SPI_Transmit(&hspi2, &reg, 1, 0xff);
//    HAL_SPI_Transmit(&hspi2, &dat, 1, 0xff);
//    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12,1);
//}
//
///**
//*
//* @brief    ICM42688陀螺仪读数据
//* @param    reg                 寄存器
//* @param    data                把读出的数据存入data
//* @param    num                 数据个数
//* @return   void
//* @notes    ICM42688.c文件内部调用,用户无需调用尝试
//* Example:  Read_Datas_ICM42688(0x00,0x00,1);
//*
//**/
//static void Read_Datas_ICM42688(unsigned char reg, unsigned char *dat, unsigned int num)
//{
//    uint8_t Data;
//    reg |= 0x80;
//    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12,0);
//    HAL_SPI_Transmit(&hspi2, &reg, 1, 0xff );
//    while(num--)
//    {
//        HAL_SPI_Receive(&hspi2,&Data, 1, 0xff);
//        *dat = Data;
//        dat++;
//    }
//    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12,1);
//}
///* 读取MPU6050数据并加滤波 */
////返回值:0,正常
////其他,错误代码
//_st_Mpu ICM42688;
//static volatile int16_t *pMpu = (int16_t *)&ICM42688;
//int16_t MpuOffset[6] = {0};		//MPU6050补偿数值
//
//uint8_t MpuGetData(void)
//{
//    uint8_t i;
//    uint8_t buffer[12];
//    Read_Datas_ICM42688(ICM42688_ACCEL_DATA_X1,buffer,12);
//
//    for(i=0;i<6;i++)
//    {
//        pMpu[i] = (((int16_t)buffer[i<<1] << 8) | buffer[(i<<1)+1])-MpuOffset[i];							/* 将数据整为16bit，并减去水平校准值 */
//        if(i < 3)		/* 角加速度卡尔曼滤波 */
//        {
//            {
//                static struct KalmanFilter EKF[3] = {{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543}};
//                kalmanfiter(&EKF[i],(float)pMpu[i]);
//                pMpu[i] = (int16_t)EKF[i].Out;
//            }
//        }
//        if(i > 2)		/* 角速度一阶互补滤波 */
//        {
//            uint8_t k=i-3;
//            const float factor = 0.15f;
//            static float tBuff[3];
//
//            pMpu[i] = tBuff[k] = tBuff[k] * (1 - factor) + pMpu[i] * factor;
//        }
//    }
//    return 0;
//}















///////////模拟spi版本
#include  "icm42688.h"

#include "kalman.h"
#include "alldata.h"

// ICM42688加速度计数据
float icm42688_acc_x, icm42688_acc_y, icm42688_acc_z  ;
// ICM42688角加速度数据
float icm42688_gyro_x, icm42688_gyro_y, icm42688_gyro_z ;

static float icm42688_acc_inv = 1, icm42688_gyro_inv = 1;

void Spi_GPIO_Init()
{

	GPIO_InitTypeDef GPIO_InitStruct = {0};
	__HAL_RCC_GPIOB_CLK_ENABLE();

  /*Configure GPIO pins : PBPin PB12 */
  GPIO_InitStruct.Pin = GPIO_PIN_12 | GPIO_PIN_13 |GPIO_PIN_15;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

	/*Configure GPIO pins : PBPin PB14 */
  GPIO_InitStruct.Pin = GPIO_PIN_14;
  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
}

void Init_ICM42688(void)
{
	unsigned char time;
	unsigned char model;
	Spi_GPIO_Init();
	// 读取陀螺仪型号陀螺仪自检
	model = 0xff;
	while(1)
	{
			Read_Datas_ICM42688(ICM42688_WHO_AM_I, &model, 1);
			if(model == 0x47)
			{
					// ICM42688
					break;
			}
            ICM42688_DELAY_MS(10);
	}
	Write_Data_ICM42688(ICM42688_PWR_MGMT0,0x00);				// 复位设备
	ICM42688_DELAY_MS(10);															// 操作完PWR—MGMT0寄存器后200us内不能有任何读写寄存器的操作

	// 设置ICM42688加速度计和陀螺仪的量程和输出速率
	Set_LowpassFilter_Range_ICM42688(ICM42688_AFS_16G, ICM42688_AODR_32000HZ, ICM42688_GFS_2000DPS, ICM42688_GODR_32000HZ);

	Write_Data_ICM42688(ICM42688_PWR_MGMT0, 0x0f);      // 设置GYRO_MODE,ACCEL_MODE为低噪声模式
	ICM42688_DELAY_MS(10);
}
/**
*
* @brief    获得ICM42688陀螺仪加速度
* @param
* @return   void
* @notes    单位:g(m/s^2),用户调用
* Example:  Get_Acc_ICM42688();
*
**/
void Get_Acc_ICM42688(void)
{
    unsigned char dat[6];
    Read_Datas_ICM42688(ICM42688_ACCEL_DATA_X1, dat, 6);
    icm42688_acc_x = icm42688_acc_inv * (short int)(((short int)dat[0] << 8) | dat[1]);
    icm42688_acc_y = icm42688_acc_inv * (short int)(((short int)dat[2] << 8) | dat[3]);
    icm42688_acc_z = icm42688_acc_inv * (short int)(((short int)dat[4] << 8) | dat[5]);
}

/**
*
* @brief    获得ICM42688陀螺仪角加速度
* @param
* @return   void
* @notes    单位为:°/s,用户调用
* Example:  Get_Gyro_ICM42688();
*
**/
void Get_Gyro_ICM42688(void)
{
    unsigned char dat[6];
    Read_Datas_ICM42688(ICM42688_GYRO_DATA_X1, dat, 6);
    icm42688_gyro_x = icm42688_gyro_inv * (short int)(((short int)dat[0] << 8) | dat[1]);
    icm42688_gyro_y = icm42688_gyro_inv * (short int)(((short int)dat[2] << 8) | dat[3]);
    icm42688_gyro_z = icm42688_gyro_inv * (short int)(((short int)dat[4] << 8) | dat[5]);
}

/**
*
* @brief    设置ICM42688陀螺仪低通滤波器带宽和量程
* @param    afs                 // 加速度计量程,可在dmx_icm42688.h文件里枚举定义中查看
* @param    aodr                // 加速度计输出速率,可在dmx_icm42688.h文件里枚举定义中查看
* @param    gfs                 // 陀螺仪量程,可在dmx_icm42688.h文件里枚举定义中查看
* @param    godr                // 陀螺仪输出速率,可在dmx_icm42688.h文件里枚举定义中查看
* @return   void
* @notes    ICM42688.c文件内部调用,用户无需调用尝试
* Example:  Set_LowpassFilter_Range_ICM42688(ICM42688_AFS_16G,ICM42688_AODR_32000HZ,ICM42688_GFS_2000DPS,ICM42688_GODR_32000HZ);
*
**/
void Set_LowpassFilter_Range_ICM42688(enum icm42688_afs afs, enum icm42688_aodr aodr, enum icm42688_gfs gfs, enum icm42688_godr godr)
{
    Write_Data_ICM42688(ICM42688_ACCEL_CONFIG0, (afs << 5) | (aodr + 1));   // 初始化ACCEL量程和输出速率(p77)
    Write_Data_ICM42688(ICM42688_GYRO_CONFIG0, (gfs << 5) | (godr + 1));    // 初始化GYRO量程和输出速率(p76)

    switch(afs)
    {
    case ICM42688_AFS_2G:
        icm42688_acc_inv = 2000 / 32768.0f;             // 加速度计量程为:±2g
        break;
    case ICM42688_AFS_4G:
        icm42688_acc_inv = 4000 / 32768.0f;             // 加速度计量程为:±4g
        break;
    case ICM42688_AFS_8G:
        icm42688_acc_inv = 8000 / 32768.0f;             // 加速度计量程为:±8g
        break;
    case ICM42688_AFS_16G:
        icm42688_acc_inv = 16000 / 32768.0f;            // 加速度计量程为:±16g
        break;
    default:
        icm42688_acc_inv = 1;                           // 不转化为实际数据
        break;
    }
    switch(gfs)
    {
    case ICM42688_GFS_15_625DPS:
        icm42688_gyro_inv = 15.625f / 32768.0f;         // 陀螺仪量程为:±15.625dps
        break;
    case ICM42688_GFS_31_25DPS:
        icm42688_gyro_inv = 31.25f / 32768.0f;          // 陀螺仪量程为:±31.25dps
        break;
    case ICM42688_GFS_62_5DPS:
        icm42688_gyro_inv = 62.5f / 32768.0f;           // 陀螺仪量程为:±62.5dps
        break;
    case ICM42688_GFS_125DPS:
        icm42688_gyro_inv = 125.0f / 32768.0f;          // 陀螺仪量程为:±125dps
        break;
    case ICM42688_GFS_250DPS:
        icm42688_gyro_inv = 250.0f / 32768.0f;          // 陀螺仪量程为:±250dps
        break;
    case ICM42688_GFS_500DPS:
        icm42688_gyro_inv = 500.0f / 32768.0f;          // 陀螺仪量程为:±500dps
        break;
    case ICM42688_GFS_1000DPS:
        icm42688_gyro_inv = 1000.0f / 32768.0f;         // 陀螺仪量程为:±1000dps
        break;
    case ICM42688_GFS_2000DPS:
        icm42688_gyro_inv = 2000.0f / 32768.0f;         // 陀螺仪量程为:±2000dps
        break;
    default:
        icm42688_gyro_inv = 1;                          // 不转化为实际数据
        break;
    }
}

/**
*
* @brief    ICM42688陀螺仪写8bit数据
* @param    data                数据
* @return   void
* @notes    ICM42688.c文件内部调用,用户无需调用尝试
* Example:  Write_8bit_ICM42688(0x00);
*
**/
static void Write_8bit_ICM42688(unsigned char dat)
{
    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
    if(0x80 & dat)
    {
        sys_gpio_pin_set(GPIOB,GPIO_PIN_15,1);
    }
    else
    {
        sys_gpio_pin_set(GPIOB,GPIO_PIN_15,0);
    }
    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
    dat <<= 1;


    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
    if(0x80 & dat)
    {
        sys_gpio_pin_set(GPIOB,GPIO_PIN_15,1);
    }
    else
    {
        sys_gpio_pin_set(GPIOB,GPIO_PIN_15,0);
    }
    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
    dat <<= 1;


    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
    if(0x80 & dat)
    {
        sys_gpio_pin_set(GPIOB,GPIO_PIN_15,1);
    }
    else
    {
        sys_gpio_pin_set(GPIOB,GPIO_PIN_15,0);
    }
    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
    dat <<= 1;


    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
    if(0x80 & dat)
    {
        sys_gpio_pin_set(GPIOB,GPIO_PIN_15,1);
    }
    else
    {
        sys_gpio_pin_set(GPIOB,GPIO_PIN_15,0);
    }
    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
    dat <<= 1;


    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
    if(0x80 & dat)
    {
        sys_gpio_pin_set(GPIOB,GPIO_PIN_15,1);
    }
    else
    {
        sys_gpio_pin_set(GPIOB,GPIO_PIN_15,0);
    }
    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
    dat <<= 1;


    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
    if(0x80 & dat)
    {
        sys_gpio_pin_set(GPIOB,GPIO_PIN_15,1);
    }
    else
    {
        sys_gpio_pin_set(GPIOB,GPIO_PIN_15,0);
    }
    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
    dat <<= 1;

    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
    if(0x80 & dat)
    {
        sys_gpio_pin_set(GPIOB,GPIO_PIN_15,1);
    }
    else
    {
        sys_gpio_pin_set(GPIOB,GPIO_PIN_15,0);
    }
    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
    dat <<= 1;

    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
    if(0x80 & dat)
    {
        sys_gpio_pin_set(GPIOB,GPIO_PIN_15,1);
    }
    else
    {
        sys_gpio_pin_set(GPIOB,GPIO_PIN_15,0);
    }
    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
    dat <<= 1;
}

/**
*
* @brief    ICM42688陀螺仪读8bit数据
* @param    data                数据
* @return   void
* @notes    ICM42688.c文件内部调用,用户无需调用尝试
* Example:  Read_8bit_ICM42688(dat);
*
**/
static void Read_8bit_ICM42688(unsigned char *dat)
{
    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
    *dat = *dat << 1;
    *dat |=sys_gpio_pin_get(GPIOB,GPIO_PIN_14);
    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);


    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
    *dat = *dat << 1;
    *dat |=sys_gpio_pin_get(GPIOB,GPIO_PIN_14);
    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);

    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
    *dat = *dat << 1;
    *dat |=sys_gpio_pin_get(GPIOB,GPIO_PIN_14);
    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);

    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
    *dat = *dat << 1;
    *dat |=sys_gpio_pin_get(GPIOB,GPIO_PIN_14);
    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);

    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
    *dat = *dat << 1;
    *dat |=sys_gpio_pin_get(GPIOB,GPIO_PIN_14);
    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);

    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
    *dat = *dat << 1;
    *dat |=sys_gpio_pin_get(GPIOB,GPIO_PIN_14);
    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);

    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
    *dat = *dat << 1;
    *dat |=sys_gpio_pin_get(GPIOB,GPIO_PIN_14);
    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);

    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
    *dat = *dat << 1;
    *dat |=sys_gpio_pin_get(GPIOB,GPIO_PIN_14);
    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
}

/**
*
* @brief    ICM42688陀螺仪写数据
* @param    reg                 寄存器
* @param    data                需要写进该寄存器的数据
* @return   void
* @notes    ICM42688.c文件内部调用,用户无需调用尝试
* Example:  Write_Data_ICM42688(0x00,0x00);
*
**/
static void Write_Data_ICM42688(unsigned char reg, unsigned char dat)
{
    sys_gpio_pin_set(GPIOB,GPIO_PIN_12,0);
    Write_8bit_ICM42688(reg);
    Write_8bit_ICM42688(dat);
    sys_gpio_pin_set(GPIOB,GPIO_PIN_12,1);
}

/**
*
* @brief    ICM42688陀螺仪读数据
* @param    reg                 寄存器
* @param    data                把读出的数据存入data
* @param    num                 数据个数
* @return   void
* @notes    ICM42688.c文件内部调用,用户无需调用尝试
* Example:  Read_Datas_ICM42688(0x00,0x00,1);
*
**/
static void Read_Datas_ICM42688(unsigned char reg, unsigned char *dat, unsigned int num)
{
    sys_gpio_pin_set(GPIOB,GPIO_PIN_12,0);
    reg |= 0x80;
    Write_8bit_ICM42688(reg);
    while(num--) Read_8bit_ICM42688(dat++);
    sys_gpio_pin_set(GPIOB,GPIO_PIN_12,1);
}
/* 读取MPU6050数据并加滤波 */
//返回值:0,正常
//其他,错误代码
_st_Mpu ICM42688;
static volatile int16_t *pMpu = (int16_t *)&ICM42688;
int16_t MpuOffset[6] = {0};		//MPU6050补偿数值

uint8_t MpuGetData(void)
{
	uint8_t i;
	int res;
	unsigned char buffer[12];
	Read_Datas_ICM42688(ICM42688_ACCEL_DATA_X1,buffer,12);

	for(i=0;i<6;i++)
	{
		pMpu[i] = (((int16_t)buffer[i<<1] << 8) | buffer[(i<<1)+1])-MpuOffset[i];							/* 将数据整为16bit，并减去水平校准值 */
		if(i < 3)		/* 角加速度卡尔曼滤波 */
		{
			{
				static struct KalmanFilter EKF[3] = {{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543}};
				kalmanfiter(&EKF[i],(float)pMpu[i]);
				pMpu[i] = (int16_t)EKF[i].Out;
			}
		}
		if(i > 2)		/* 角速度一阶互补滤波 */
		{
			uint8_t k=i-3;
			const float factor = 0.15f;
			static float tBuff[3];

			pMpu[i] = tBuff[k] = tBuff[k] * (1 - factor) + pMpu[i] * factor;
		}
	}
	return 0;
}

void get_IMU(float *pitch,float *roll)
{
    *roll  = atan2(ICM42688.accY, ICM42688.accZ) * 180.0 / _PI;
    *pitch = atan2(-ICM42688.accX, sqrt(ICM42688.accX * ICM42688.accY + ICM42688.accZ * ICM42688.accZ)) * 180.0 / _PI;

//    *roll  = atan2(ICM42688.accY, ICM42688.accZ);
//    *pitch = atan2(-ICM42688.accX, sqrt(ICM42688.accX * ICM42688.accY + ICM42688.accZ * ICM42688.accZ));
}

/**
设置引脚电平
 */
void sys_gpio_pin_set(GPIO_TypeDef *p_gpiox, uint16_t pinx, uint8_t status)
{
    if (status & 0X01)
    {
        p_gpiox->BSRR |= pinx;
    }
    else
    {
        p_gpiox->BSRR |= (uint32_t)pinx << 16;
    }
}

/**
 * 读取引脚电平
 */
uint8_t sys_gpio_pin_get(GPIO_TypeDef *p_gpiox, uint16_t pinx)
{
    if (p_gpiox->IDR & pinx)
    {
        return 1;
    }
    else
    {
        return 0;
    }
}