lijie/foc_Gimbal
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
lijie
2024-01-24 15:56:10 +08:00
commit 2c58355e63
40 changed files with 5524 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

443
IMU/icm42688.c Normal file
View File

@ -0,0 +1,443 @@
#include "icm42688.h"
#include "kalman.h"
#include "alldata.h"
// ICM42688<38><38><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD><EFBFBD><EFBFBD>
float icm42688_acc_x, icm42688_acc_y, icm42688_acc_z ;
// ICM42688<38>Ǽ<EFBFBD><C7BC>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD>
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();
// <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͺ<EFBFBD><CDBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD>
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); // <20><>λ<EFBFBD>豸
ICM42688_DELAY_MS(10); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PWR<57><52>MGMT0<54>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD><EFBFBD>200us<75>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>κζ<CEBA>д<EFBFBD>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>
// <20><><EFBFBD><EFBFBD>ICM42688<38><38><EFBFBD>ٶȼƺ<C8BC><C6BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD><C7B5><EFBFBD><EFBFBD>̺<EFBFBD><CCBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Set_LowpassFilter_Range_ICM42688(ICM42688_AFS_16G, ICM42688_AODR_32000HZ, ICM42688_GFS_2000DPS, ICM42688_GODR_32000HZ);
Write_Data_ICM42688(ICM42688_PWR_MGMT0, 0x0f); // <20><><EFBFBD><EFBFBD>GYRO_MODE,ACCEL_MODEΪ<45><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ
ICM42688_DELAY_MS(10);
}
/**
*
* @brief <20><><EFBFBD><EFBFBD>ICM42688<38><38><EFBFBD><EFBFBD><EFBFBD>Ǽ<EFBFBD><C7BC>ٶ<EFBFBD>
* @param
* @return void
* @notes <20><>λ:g(m/s^2),<2C>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD>
* 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 <20><><EFBFBD><EFBFBD>ICM42688<38><38><EFBFBD><EFBFBD><EFBFBD>ǽǼ<C7BD><C7BC>ٶ<EFBFBD>
* @param
* @return void
* @notes <20><>λΪ:<3A><>/s,<2C>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD>
* 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 <20><><EFBFBD><EFBFBD>ICM42688<38><38><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD>ͨ<EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @param afs // <20><><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>dmx_icm42688.h<>ļ<EFBFBD><C4BC><EFBFBD>ö<EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD>в鿴
* @param aodr // <20><><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>dmx_icm42688.h<>ļ<EFBFBD><C4BC><EFBFBD>ö<EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD>в鿴
* @param gfs // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>dmx_icm42688.h<>ļ<EFBFBD><C4BC><EFBFBD>ö<EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD>в鿴
* @param godr // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>dmx_icm42688.h<>ļ<EFBFBD><C4BC><EFBFBD>ö<EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD>в鿴
* @return void
* @notes ICM42688.c<>ļ<EFBFBD><C4BC>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>,<2C>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ó<EFBFBD><C3B3><EFBFBD>
* 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)); // <20><>ʼ<EFBFBD><CABC>ACCEL<45><4C><EFBFBD>̺<EFBFBD><CCBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(p77)
Write_Data_ICM42688(ICM42688_GYRO_CONFIG0, (gfs << 5) | (godr + 1)); // <20><>ʼ<EFBFBD><CABC>GYRO<52><4F><EFBFBD>̺<EFBFBD><CCBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(p76)
switch(afs)
{
case ICM42688_AFS_2G:
icm42688_acc_inv = 2000 / 32768.0f; // <20><><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD><EFBFBD><EFBFBD>Ϊ:<3A><>2g
break;
case ICM42688_AFS_4G:
icm42688_acc_inv = 4000 / 32768.0f; // <20><><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD><EFBFBD><EFBFBD>Ϊ:<3A><>4g
break;
case ICM42688_AFS_8G:
icm42688_acc_inv = 8000 / 32768.0f; // <20><><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD><EFBFBD><EFBFBD>Ϊ:<3A><>8g
break;
case ICM42688_AFS_16G:
icm42688_acc_inv = 16000 / 32768.0f; // <20><><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD><EFBFBD><EFBFBD>Ϊ:<3A><>16g
break;
default:
icm42688_acc_inv = 1; // <20><>ת<EFBFBD><D7AA>Ϊʵ<CEAA><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
break;
}
switch(gfs)
{
case ICM42688_GFS_15_625DPS:
icm42688_gyro_inv = 15.625f / 32768.0f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ:<3A><>15.625dps
break;
case ICM42688_GFS_31_25DPS:
icm42688_gyro_inv = 31.25f / 32768.0f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ:<3A><>31.25dps
break;
case ICM42688_GFS_62_5DPS:
icm42688_gyro_inv = 62.5f / 32768.0f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ:<3A><>62.5dps
break;
case ICM42688_GFS_125DPS:
icm42688_gyro_inv = 125.0f / 32768.0f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ:<3A><>125dps
break;
case ICM42688_GFS_250DPS:
icm42688_gyro_inv = 250.0f / 32768.0f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ:<3A><>250dps
break;
case ICM42688_GFS_500DPS:
icm42688_gyro_inv = 500.0f / 32768.0f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ:<3A><>500dps
break;
case ICM42688_GFS_1000DPS:
icm42688_gyro_inv = 1000.0f / 32768.0f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ:<3A><>1000dps
break;
case ICM42688_GFS_2000DPS:
icm42688_gyro_inv = 2000.0f / 32768.0f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ:<3A><>2000dps
break;
default:
icm42688_gyro_inv = 1; // <20><>ת<EFBFBD><D7AA>Ϊʵ<CEAA><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
break;
}
}
/**
*
* @brief ICM42688<38><38><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д8bit<69><74><EFBFBD><EFBFBD>
* @param data <20><><EFBFBD><EFBFBD>
* @return void
* @notes ICM42688.c<>ļ<EFBFBD><C4BC>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>,<2C>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ó<EFBFBD><C3B3><EFBFBD>
* 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<38><38><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>8bit<69><74><EFBFBD><EFBFBD>
* @param data <20><><EFBFBD><EFBFBD>
* @return void
* @notes ICM42688.c<>ļ<EFBFBD><C4BC>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>,<2C>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ó<EFBFBD><C3B3><EFBFBD>
* 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<38><38><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD>
* @param reg <20>Ĵ<EFBFBD><C4B4><EFBFBD>
* @param data <20><>Ҫд<D2AA><D0B4><EFBFBD>üĴ<C3BC><C4B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @return void
* @notes ICM42688.c<>ļ<EFBFBD><C4BC>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>,<2C>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ó<EFBFBD><C3B3><EFBFBD>
* 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<38><38><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
* @param reg <20>Ĵ<EFBFBD><C4B4><EFBFBD>
* @param data <20>Ѷ<EFBFBD><D1B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݴ<EFBFBD><DDB4><EFBFBD>data
* @param num <20><><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD>
* @return void
* @notes ICM42688.c<>ļ<EFBFBD><C4BC>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>,<2C>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ó<EFBFBD><C3B3><EFBFBD>
* 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);
}
/* <20><>ȡMPU6050<35><30><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD>˲<EFBFBD> */
//<2F><><EFBFBD><EFBFBD>ֵ:0,<2C><><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
_st_Mpu ICM42688;
static volatile int16_t *pMpu = (int16_t *)&ICM42688;
int16_t MpuOffset[6] = {0}; //MPU6050<35><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
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]; /* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ16bit<69><74><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȥˮƽУ׼ֵ */
if(i < 3) /* <20>Ǽ<EFBFBD><C7BC>ٶȿ<D9B6><C8BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD> */
{
{
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) /* <20><><EFBFBD>ٶ<EFBFBD>һ<EFBFBD>׻<EFBFBD><D7BB><EFBFBD><EFBFBD>˲<EFBFBD> */
{
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));
}
/**
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ŵ<EFBFBD>ƽ
*/
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;
}
}
/**
* <20><>ȡ<EFBFBD><C8A1><EFBFBD>ŵ<EFBFBD>ƽ
*/
uint8_t sys_gpio_pin_get(GPIO_TypeDef *p_gpiox, uint16_t pinx)
{
if (p_gpiox->IDR & pinx)
{
return 1;
}
else
{
return 0;
}
}