638 lines
18 KiB
C
638 lines
18 KiB
C
//#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);
|
||
// 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)
|
||
//{
|
||
// unsigned char command[2];
|
||
// command[0] = reg;
|
||
// command[1] = dat;
|
||
// HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12,0);
|
||
// HAL_SPI_Transmit(&hspi2,command, 2, 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)
|
||
//{
|
||
// reg |= 0x80;
|
||
// HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12,0);
|
||
// HAL_SPI_Transmit(&hspi2,®, 1, 0xff );
|
||
// HAL_SPI_Receive(&hspi2,dat, num, 0xff);
|
||
// 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);
|
||
printf("model:%x\r\n",model);
|
||
if(model == 0x47)
|
||
{
|
||
printf("model:%x\r\n",model);
|
||
// 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补偿数值
|
||
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}};
|
||
|
||
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;
|
||
}
|
||
}
|
||
|
||
|
||
|