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

2023/3/13第五次参数飞行测试

Browse Source
This commit is contained in:
lijie
2024-03-14 16:50:56 +08:00
parent 6e701f3ae8
commit c1b5b5fa04
8 changed files with 881 additions and 902 deletions
Download Patch File Download Diff File Expand all files Collapse all files

71
APP/foc.c
View File

@ -3,14 +3,14 @@
// //
#include "foc.h" #include "foc.h"
#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
const float sqrt_3 = 1.7320508075f;
float Voltage_PowerSupply = 12;
const float Voltage_PowerSupply = 12;
float Pole_Pairs = 14; float Pole_Pairs = 14;
float Voltage_Limit = 6.9; float Voltage_Limit = 6.9;
float Velocity_Limit = 100; float Velocity_Limit = 100;
float V_Acc_Limit = 100; float V_Acc_Limit = 100;
float dir = 1; float dir = -1;
FOCTypeDef FOCStruct_X={0,0,0,0,0,0,0,0,0,0,0,0,0,0}; FOCTypeDef FOCStruct_X={0,0,0,0,0,0,0,0,0,0,0,0,0,0};
FOCTypeDef FOCStruct_Y={0,0,0,0,0,0,0,0,0,0,0,0,0,0}; FOCTypeDef FOCStruct_Y={0,0,0,0,0,0,0,0,0,0,0,0,0,0};
@ -22,50 +22,16 @@ void PID_Iint(void)
FOCStruct_X.ZT_KD = 0.5; FOCStruct_X.ZT_KD = 0.5;
FOCStruct_X.V_KP = 0.1; FOCStruct_X.V_KP = 0.1;
FOCStruct_X.V_KI = 1; FOCStruct_X.V_KI = 2;
FOCStruct_X.V_KD = 0; FOCStruct_X.V_KD = 0;
/////////// //////////
FOCStruct_Y.ZT_KP = 3; FOCStruct_Y.ZT_KP = 3.0;
FOCStruct_Y.ZT_KI = 0; FOCStruct_Y.ZT_KI = 0;
FOCStruct_Y.ZT_KD = 0.5; FOCStruct_Y.ZT_KD = 0.5;
FOCStruct_Y.V_KP = 0.1; FOCStruct_Y.V_KP = 0.1;
FOCStruct_Y.V_KI = 1; FOCStruct_Y.V_KI = 1.4;
FOCStruct_Y.V_KD = 0; FOCStruct_Y.V_KD = 0;
// FOCStruct_X.ZT_KP = 4;
// FOCStruct_X.ZT_KI = 0;
// FOCStruct_X.ZT_KD = 0.5;
//
// FOCStruct_X.V_KP = 0.1;
// FOCStruct_X.V_KI = 2;
// FOCStruct_X.V_KD = 0;
////2024/1/2 Y<><59>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// FOCStruct_Y.ZT_KP = 2;
// FOCStruct_Y.ZT_KI = 0;
// FOCStruct_Y.ZT_KD = 0.5;
//
// FOCStruct_Y.V_KP = 0.1;
// FOCStruct_Y.V_KI = 1;
// FOCStruct_Y.V_KD = 0;
////2024/1/23 xY<78><59>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// FOCStruct_X.ZT_KP = 2;
// FOCStruct_X.ZT_KI = 0;
// FOCStruct_X.ZT_KD = 0.2;
//
// FOCStruct_X.V_KP = 0.1;
// FOCStruct_X.V_KI = 1;
// FOCStruct_X.V_KD = 0;
////2024/1/22 Y<><59>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// FOCStruct_Y.ZT_KP = 2;
// FOCStruct_Y.ZT_KI = 0;
// FOCStruct_Y.ZT_KD = 0.5;
//
// FOCStruct_Y.V_KP = 0.1;
// FOCStruct_Y.V_KI = 1;
// FOCStruct_Y.V_KD = 0;
} }
void setPhaseVoltage(unsigned char motor,float Uq, float Ud, double angle_el) void setPhaseVoltage(unsigned char motor,float Uq, float Ud, double angle_el)
{ {
@ -93,8 +59,8 @@ void setPhaseVoltage(unsigned char motor,float Uq, float Ud, double angle_el)
if(Uout<-0.577)Uout=-0.577; if(Uout<-0.577)Uout=-0.577;
sector = (angle_el / _PI_3) + 1; sector = (angle_el / _PI_3) + 1;
T1 = sqrt(3)*sin(sector*_PI_3 - angle_el) * Uout; T1 = sqrt_3*sin(sector*_PI_3 - angle_el) * Uout;
T2 = sqrt(3)*sin(angle_el - (sector-1.0)*_PI_3) * Uout; T2 = sqrt_3*sin(angle_el - (sector-1.0)*_PI_3) * Uout;
T0 = 1 - T1 - T2; T0 = 1 - T1 - T2;
// calculate the duty cycles(times) // calculate the duty cycles(times)
@ -135,14 +101,14 @@ void setPhaseVoltage(unsigned char motor,float Uq, float Ud, double angle_el)
Tb = 0; Tb = 0;
Tc = 0; Tc = 0;
} }
if (motor == motory) if (motor ==motorx )
{ {
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Ta*4800); __HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Ta*4800);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Tb*4800); __HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Tb*4800);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,Tc*4800); __HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,Tc*4800);
} }
else if (motor == motorx) else if (motor == motory)
{ {
__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_1,Ta*4800); __HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_1,Ta*4800);
__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_2,Tb*4800); __HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_2,Tb*4800);
@ -308,8 +274,7 @@ void PWM_Start()
HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_2); HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_2);
HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_3); HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_3);
} }
void moto_gpio_init()
void moto_Init()
{ {
GPIO_InitTypeDef GPIO_InitStruct = {0}; GPIO_InitTypeDef GPIO_InitStruct = {0};
@ -332,10 +297,19 @@ void moto_Init()
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_0,1); HAL_GPIO_WritePin(GPIOB,GPIO_PIN_0,1);
HAL_GPIO_WritePin(GPIOC,GPIO_PIN_9,1); HAL_GPIO_WritePin(GPIOC,GPIO_PIN_9,1);
}
void moto_Init()
{
Voltage_PowerSupply = get_VCC();
Voltage_Limit = Voltage_PowerSupply / sqrt_3 - 0.1;
printf("Voltage_PowerSupply: %f V\r\n",Voltage_PowerSupply);
moto_gpio_init();
PWM_Start(); PWM_Start();
HAL_Delay(1000); HAL_Delay(1000);
alignSensor(motory,&FOCStruct_Y); alignSensor(motory,&FOCStruct_Y); //<2F><><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD>
alignSensor(motorx,&FOCStruct_X); alignSensor(motorx,&FOCStruct_X);
HAL_Delay(50); HAL_Delay(50);
} }
@ -348,7 +322,6 @@ int alignSensor(unsigned char moto,FOCTypeDef * FOCStruct)
float moved; float moved;
float q = 3.5; float q = 3.5;
printf("MOT: Align sensor.\r\n"); printf("MOT: Align sensor.\r\n");
// find natural direction // find natural direction
// move one electrical revolution forward // move one electrical revolution forward
HAL_Delay(100); HAL_Delay(100);

9
APP/foc.h
View File

@ -24,9 +24,11 @@ typedef struct
double Angle_Prev; double Angle_Prev;
unsigned long Time_Prev; unsigned long Time_Prev;
}FOCTypeDef; }FOCTypeDef;
extern FOCTypeDef FOCStruct_X; extern FOCTypeDef FOCStruct_X;
extern FOCTypeDef FOCStruct_Y; extern FOCTypeDef FOCStruct_Y;
void setPhaseVoltage(unsigned char motor,float Uq, float Ud, double angle_el);
int alignSensor(unsigned char moto,FOCTypeDef * FOCStruct); int alignSensor(unsigned char moto,FOCTypeDef * FOCStruct);
void PID_Iint(void); void PID_Iint(void);
double _normalizeAngle(float angle); double _normalizeAngle(float angle);
@ -34,7 +36,10 @@ double LPF_velocity(double x,FOCTypeDef *FOCStruct);
void speed_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float V_Target); void speed_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float V_Target);
void Zitai_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float ZT_Error); void Zitai_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float ZT_Error);
void Angel_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float Angle_target); void Angel_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float Angle_target);
void moto_gpio_init();
void PWM_Start(); void PWM_Start();
void moto_Init(); void moto_Init();
#endif //FOC_N_FOC_H #endif //FOC_N_FOC_H

40
APP/mymain.c
View File

@ -1,12 +1,12 @@
// //
// Created by hu123456 on 2024/1/16. // Created by lijie on 2024/1/16.
// //
#include "mymain.h" #include "mymain.h"
void mymain() void mymain()
{ {
HAL_UART_Receive_IT(&huart1, (uint8_t *)g_rx_buffer, RXBUFFERSIZE); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD> HAL_UART_Receive_IT(&huart1, (uint8_t *)g_rx_buffer, RXBUFFERSIZE); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
setvbuf(stdout, NULL, _IONBF, 0); //<2F><><EFBFBD><EFBFBD>printfû<66><C3BB>\n<><6E><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
HAL_Delay(2000); HAL_Delay(2000);
led_init(); led_init();
adc_init(); adc_init();
@ -15,34 +15,32 @@ void mymain()
imu_rest(); imu_rest();
moto_Init(); moto_Init();
PID_Iint(); PID_Iint();
setvbuf(stdout, NULL, _IONBF, 0); //<2F><><EFBFBD><EFBFBD>printfû<66><C3BB>\n<><6E><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float x = 0,y=0; float x = 0,y=0;
unsigned int i =0; unsigned int a =0;
HAL_TIM_Base_Start(&htim5); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1> HAL_TIM_Base_Start(&htim5); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
// while(i<2000*1)
// { while(a<2200*6)
//// MpuGetData(); {
//// GetAngle(&ICM42688,&Angle); MpuGetData();
// Angel_closed_loop(motorx,&FOCStruct_X,3.0); GetAngle(&ICM42688,&Angle);
// Angel_closed_loop(motory,&FOCStruct_Y,2.2); // Angel_closed_loop(motorx,&FOCStruct_X,x);
// i++; // Angel_closed_loop(motory,&FOCStruct_Y,y);
// } a++;
// i=0; }
// LED_ON; // LED_ON;
while(1) while(1)
{ {
// MpuGetData(); MpuGetData();
// GetAngle(&ICM42688,&Angle); GetAngle(&ICM42688,&Angle);
// x = Angle.pitch; x = Angle.pitch;
// y = Angle.roll; y = Angle.roll + 90.f;
// Zitai_closed_loop(motorx,&FOCStruct_X,x); Zitai_closed_loop(motorx,&FOCStruct_X,x);
// Zitai_closed_loop(motory,&FOCStruct_Y,-y); Zitai_closed_loop(motory,&FOCStruct_Y,-y);
// commander_run(); // commander_run();
// x = getAngle(motorx); // x = getAngle(motorx);
// y = getAngle(motory); // y = getAngle(motory);
// x= get_VCC();
// printf("%fY%f\n",x,y); // printf("%fY%f\n",x,y);
// HAL_Delay(2); // HAL_Delay(10);
// speed_closed_loop(motorx,&FOCStruct_X,_2PI*5); // speed_closed_loop(motorx,&FOCStruct_X,_2PI*5);
// speed_closed_loop(motory,&FOCStruct_Y,_2PI*2); // speed_closed_loop(motory,&FOCStruct_Y,_2PI*2);
// printf("b%d\n",b); // printf("b%d\n",b);

5
APP/tle5012b.c
View File

@ -175,11 +175,12 @@ double getAngle(unsigned char motor)
{ {
TLE_CS1_ENABLE; TLE_CS1_ENABLE;
angle_data = ReadTLE5012B_1(READ_ANGLE_VALUE); angle_data = ReadTLE5012B_1(READ_ANGLE_VALUE);
// printf("angle_data:%d\n",angle_data);
d_angle = angle_data - motory_angle_data_prev; d_angle = angle_data - motory_angle_data_prev;
if(fabs(d_angle) > (0.8*cpr) ) motory_full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; if(fabs(d_angle) > (0.8*cpr) ) motory_full_rotation_offset += d_angle > 0 ? -_2PI : _2PI;
motory_angle_data_prev = angle_data; motory_angle_data_prev = angle_data;
TLE_CS1_DISABLE; TLE_CS1_DISABLE;
return (motory_full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ; return (motory_full_rotation_offset + ( angle_data / (float)cpr) * _2PI) /2.f;
} }
else else
{ {
@ -189,7 +190,7 @@ double getAngle(unsigned char motor)
if(fabs(d_angle) > (0.8*cpr) ) motorx_full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; if(fabs(d_angle) > (0.8*cpr) ) motorx_full_rotation_offset += d_angle > 0 ? -_2PI : _2PI;
motorx_angle_data_prev = angle_data; motorx_angle_data_prev = angle_data;
TLE_CS2_DISABLE; TLE_CS2_DISABLE;
return (motorx_full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ; return (motorx_full_rotation_offset + ( angle_data / (float)cpr) * _2PI) /2.f;
} }
} }

2
Core/Src/spi.c
View File

@ -87,7 +87,7 @@ void MX_SPI2_Init(void)
hspi2.Init.CLKPolarity = SPI_POLARITY_HIGH; hspi2.Init.CLKPolarity = SPI_POLARITY_HIGH;
hspi2.Init.CLKPhase = SPI_PHASE_2EDGE; hspi2.Init.CLKPhase = SPI_PHASE_2EDGE;
hspi2.Init.NSS = SPI_NSS_SOFT; hspi2.Init.NSS = SPI_NSS_SOFT;
hspi2.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8; hspi2.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_16;
hspi2.Init.FirstBit = SPI_FIRSTBIT_MSB; hspi2.Init.FirstBit = SPI_FIRSTBIT_MSB;
hspi2.Init.TIMode = SPI_TIMODE_DISABLE; hspi2.Init.TIMode = SPI_TIMODE_DISABLE;
hspi2.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; hspi2.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;

1136
IMU/icm42688.c
View File

File diff suppressed because it is too large Load Diff

510
IMU/icm42688.h
View File

@ -1,212 +1,10 @@
#ifndef _DMX_ICM42688_H_
#define _DMX_ICM42688_H_
#include "stm32h7xx_hal.h"
#include "mymain.h"
#define ICM42688_DELAY_MS(time) (HAL_Delay(time))
enum icm42688_afs
{
ICM42688_AFS_16G,// default
ICM42688_AFS_8G,
ICM42688_AFS_4G,
ICM42688_AFS_2G,
NUM_ICM42688__AFS
};
enum icm42688_aodr
{
ICM42688_AODR_32000HZ,
ICM42688_AODR_16000HZ,
ICM42688_AODR_8000HZ,
ICM42688_AODR_4000HZ,
ICM42688_AODR_2000HZ,
ICM42688_AODR_1000HZ,// default
ICM42688_AODR_200HZ,
ICM42688_AODR_100HZ,
ICM42688_AODR_50HZ,
ICM42688_AODR_25HZ,
ICM42688_AODR_12_5HZ,
ICM42688_AODR_6_25HZ,
ICM42688_AODR_3_125HZ,
ICM42688_AODR_1_5625HZ,
ICM42688_AODR_500HZ,
NUM_ICM42688_AODR
};
enum icm42688_gfs
{
ICM42688_GFS_2000DPS,// default
ICM42688_GFS_1000DPS,
ICM42688_GFS_500DPS,
ICM42688_GFS_250DPS,
ICM42688_GFS_125DPS,
ICM42688_GFS_62_5DPS,
ICM42688_GFS_31_25DPS,
ICM42688_GFS_15_625DPS,
NUM_ICM42688_GFS
};
enum icm42688_godr
{
ICM42688_GODR_32000HZ,
ICM42688_GODR_16000HZ,
ICM42688_GODR_8000HZ,
ICM42688_GODR_4000HZ,
ICM42688_GODR_2000HZ,
ICM42688_GODR_1000HZ,// default
ICM42688_GODR_200HZ,
ICM42688_GODR_100HZ,
ICM42688_GODR_50HZ,
ICM42688_GODR_25HZ,
ICM42688_GODR_12_5HZ,
ICM42688_GODR_X0HZ,
ICM42688_GODR_X1HZ,
ICM42688_GODR_X2HZ,
ICM42688_GODR_500HZ,
NUM_ICM42688_GODR
};
// ICM42688Bank0<6B>ڲ<EFBFBD><DAB2><EFBFBD>ַ
#define ICM42688_DEVICE_CONFIG 0x11
#define ICM42688_DRIVE_CONFIG 0x13
#define ICM42688_INT_CONFIG 0x14
#define ICM42688_FIFO_CONFIG 0x16
#define ICM42688_TEMP_DATA1 0x1D
#define ICM42688_TEMP_DATA0 0x1E
#define ICM42688_ACCEL_DATA_X1 0x1F
#define ICM42688_ACCEL_DATA_X0 0x20
#define ICM42688_ACCEL_DATA_Y1 0x21
#define ICM42688_ACCEL_DATA_Y0 0x22
#define ICM42688_ACCEL_DATA_Z1 0x23
#define ICM42688_ACCEL_DATA_Z0 0x24
#define ICM42688_GYRO_DATA_X1 0x25
#define ICM42688_GYRO_DATA_X0 0x26
#define ICM42688_GYRO_DATA_Y1 0x27
#define ICM42688_GYRO_DATA_Y0 0x28
#define ICM42688_GYRO_DATA_Z1 0x29
#define ICM42688_GYRO_DATA_Z0 0x2A
#define ICM42688_TMST_FSYNCH 0x2B
#define ICM42688_TMST_FSYNCL 0x2C
#define ICM42688_INT_STATUS 0x2D
#define ICM42688_FIFO_COUNTH 0x2E
#define ICM42688_FIFO_COUNTL 0x2F
#define ICM42688_FIFO_DATA 0x30
#define ICM42688_APEX_DATA0 0x31
#define ICM42688_APEX_DATA1 0x32
#define ICM42688_APEX_DATA2 0x33
#define ICM42688_APEX_DATA3 0x34
#define ICM42688_APEX_DATA4 0x35
#define ICM42688_APEX_DATA5 0x36
#define ICM42688_INT_STATUS2 0x37
#define ICM42688_INT_STATUS3 0x38
#define ICM42688_SIGNAL_PATH_RESET 0x4B
#define ICM42688_INTF_CONFIG0 0x4C
#define ICM42688_INTF_CONFIG1 0x4D
#define ICM42688_PWR_MGMT0 0x4E
#define ICM42688_GYRO_CONFIG0 0x4F
#define ICM42688_ACCEL_CONFIG0 0x50
#define ICM42688_GYRO_CONFIG1 0x51
#define ICM42688_GYRO_ACCEL_CONFIG0 0x52
#define ICM42688_ACCEL_CONFIG1 0x53
#define ICM42688_TMST_CONFIG 0x54
#define ICM42688_APEX_CONFIG0 0x56
#define ICM42688_SMD_CONFIG 0x57
#define ICM42688_FIFO_CONFIG1 0x5F
#define ICM42688_FIFO_CONFIG2 0x60
#define ICM42688_FIFO_CONFIG3 0x61
#define ICM42688_FSYNC_CONFIG 0x62
#define ICM42688_INT_CONFIG0 0x63
#define ICM42688_INT_CONFIG1 0x64
#define ICM42688_INT_SOURCE0 0x65
#define ICM42688_INT_SOURCE1 0x66
#define ICM42688_INT_SOURCE3 0x68
#define ICM42688_INT_SOURCE4 0x69
#define ICM42688_FIFO_LOST_PKT0 0x6C
#define ICM42688_FIFO_LOST_PKT1 0x6D
#define ICM42688_SELF_TEST_CONFIG 0x70
#define ICM42688_WHO_AM_I 0x75
#define ICM42688_REG_BANK_SEL 0x76 // Banks
#define ICM42688_SENSOR_CONFIG0 0x03
#define ICM42688_GYRO_CONFIG_STATIC2 0x0B
#define ICM42688_GYRO_CONFIG_STATIC3 0x0C
#define ICM42688_GYRO_CONFIG_STATIC4 0x0D
#define ICM42688_GYRO_CONFIG_STATIC5 0x0E
#define ICM42688_GYRO_CONFIG_STATIC6 0x0F
#define ICM42688_GYRO_CONFIG_STATIC7 0x10
#define ICM42688_GYRO_CONFIG_STATIC8 0x11
#define ICM42688_GYRO_CONFIG_STATIC9 0x12
#define ICM42688_GYRO_CONFIG_STATIC10 0x13
#define ICM42688_XG_ST_DATA 0x5F
#define ICM42688_YG_ST_DATA 0x60
#define ICM42688_ZG_ST_DATA 0x61
#define ICM42688_TMSTVAL0 0x62
#define ICM42688_TMSTVAL1 0x63
#define ICM42688_TMSTVAL2 0x64
#define ICM42688_INTF_CONFIG4 0x7A
#define ICM42688_INTF_CONFIG5 0x7B
#define ICM42688_INTF_CONFIG6 0x7C
static void Read_Datas_ICM42688(unsigned char reg, unsigned char *dat, unsigned int num);
static void Write_Data_ICM42688(unsigned char reg, unsigned char dat);
void Init_ICM42688(void);
//void sys_gpio_pin_set(GPIO_TypeDef *p_gpiox, uint16_t pinx, uint8_t status);
void Set_LowpassFilter_Range_ICM42688(enum icm42688_afs afs, enum icm42688_aodr aodr, enum icm42688_gfs gfs, enum icm42688_godr godr);
uint8_t MpuGetData(void);
#endif
//
//#ifndef _DMX_ICM42688_H_ //#ifndef _DMX_ICM42688_H_
//#define _DMX_ICM42688_H_ //#define _DMX_ICM42688_H_
//#include "stm32h7xx_hal.h" //#include "stm32h7xx_hal.h"
//#include "mymain.h" //#include "mymain.h"
//#define ICM42688_DELAY_MS(time) (HAL_Delay(time)) //#define ICM42688_DELAY_MS(time) (HAL_Delay(time))
// //
//extern float icm42688_acc_x, icm42688_acc_y, icm42688_acc_z ; // <20><><EFBFBD><EFBFBD>ICM42688<38><38><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD><EFBFBD><EFBFBD> //
//extern float icm42688_gyro_x, icm42688_gyro_y, icm42688_gyro_z ; // <20><><EFBFBD><EFBFBD>ICM42688<38>Ǽ<EFBFBD><C7BC>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD>
//static void Read_Datas_ICM42688(unsigned char reg, unsigned char *dat, unsigned int num);
//static void Write_Data_ICM42688(unsigned char reg, unsigned char dat);
//enum icm42688_afs //enum icm42688_afs
//{ //{
// ICM42688_AFS_16G,// default // ICM42688_AFS_16G,// default
@ -268,58 +66,7 @@ uint8_t MpuGetData(void);
// NUM_ICM42688_GODR // NUM_ICM42688_GODR
//}; //};
// //
//void gpio_init(void);
// //
///**
//*
//* @brief ICM42688<38><38><EFBFBD><EFBFBD><EFBFBD>dz<EFBFBD>ʼ<EFBFBD><CABC>
//* @param
//* @return void
//* @notes <20>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD>
//* Example: Init_ICM42688();
//*
//**/
//void Init_ICM42688(void);
////void get_IMU(float accx,float accy,float accz,float *pitch,float *roll);
//void get_IMU(float *pitch,float *roll);
//uint8_t sys_gpio_pin_get(GPIO_TypeDef *p_gpiox, uint16_t pinx);
//void sys_gpio_pin_set(GPIO_TypeDef *p_gpiox, uint16_t pinx, uint8_t status);
///**
//*
//* @brief <20><><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);
//
///**
//*
//* @brief <20><><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);
//
///**
//*
//* @brief <20><><EFBFBD><EFBFBD>ICM42688<38><38><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD>ͨ<EFBFBD>˲<EFBFBD><CBB2><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>,<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>,<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><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);
//uint8_t MpuGetData(void);
//// ICM42688Bank0<6B>ڲ<EFBFBD><DAB2><EFBFBD>ַ //// ICM42688Bank0<6B>ڲ<EFBFBD><DAB2><EFBFBD>ַ
//#define ICM42688_DEVICE_CONFIG 0x11 //#define ICM42688_DEVICE_CONFIG 0x11
//#define ICM42688_DRIVE_CONFIG 0x13 //#define ICM42688_DRIVE_CONFIG 0x13
@ -400,5 +147,258 @@ uint8_t MpuGetData(void);
//#define ICM42688_INTF_CONFIG5 0x7B //#define ICM42688_INTF_CONFIG5 0x7B
//#define ICM42688_INTF_CONFIG6 0x7C //#define ICM42688_INTF_CONFIG6 0x7C
// //
//#endif
// //
//static void Read_Datas_ICM42688(unsigned char reg, unsigned char *dat, unsigned int num);
//static void Write_Data_ICM42688(unsigned char reg, unsigned char dat);
//void Init_ICM42688(void);
////void sys_gpio_pin_set(GPIO_TypeDef *p_gpiox, uint16_t pinx, uint8_t status);
//void Set_LowpassFilter_Range_ICM42688(enum icm42688_afs afs, enum icm42688_aodr aodr, enum icm42688_gfs gfs, enum icm42688_godr godr);
//uint8_t MpuGetData(void);
//
//
//#endif
#ifndef _DMX_ICM42688_H_
#define _DMX_ICM42688_H_
#include "stm32h7xx_hal.h"
#include "mymain.h"
#define ICM42688_DELAY_MS(time) (HAL_Delay(time))
extern float icm42688_acc_x, icm42688_acc_y, icm42688_acc_z ; // <20><><EFBFBD><EFBFBD>ICM42688<38><38><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD><EFBFBD><EFBFBD>
extern float icm42688_gyro_x, icm42688_gyro_y, icm42688_gyro_z ; // <20><><EFBFBD><EFBFBD>ICM42688<38>Ǽ<EFBFBD><C7BC>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD>
static void Read_Datas_ICM42688(unsigned char reg, unsigned char *dat, unsigned int num);
static void Write_Data_ICM42688(unsigned char reg, unsigned char dat);
enum icm42688_afs
{
ICM42688_AFS_16G,// default
ICM42688_AFS_8G,
ICM42688_AFS_4G,
ICM42688_AFS_2G,
NUM_ICM42688__AFS
};
enum icm42688_aodr
{
ICM42688_AODR_32000HZ,
ICM42688_AODR_16000HZ,
ICM42688_AODR_8000HZ,
ICM42688_AODR_4000HZ,
ICM42688_AODR_2000HZ,
ICM42688_AODR_1000HZ,// default
ICM42688_AODR_200HZ,
ICM42688_AODR_100HZ,
ICM42688_AODR_50HZ,
ICM42688_AODR_25HZ,
ICM42688_AODR_12_5HZ,
ICM42688_AODR_6_25HZ,
ICM42688_AODR_3_125HZ,
ICM42688_AODR_1_5625HZ,
ICM42688_AODR_500HZ,
NUM_ICM42688_AODR
};
enum icm42688_gfs
{
ICM42688_GFS_2000DPS,// default
ICM42688_GFS_1000DPS,
ICM42688_GFS_500DPS,
ICM42688_GFS_250DPS,
ICM42688_GFS_125DPS,
ICM42688_GFS_62_5DPS,
ICM42688_GFS_31_25DPS,
ICM42688_GFS_15_625DPS,
NUM_ICM42688_GFS
};
enum icm42688_godr
{
ICM42688_GODR_32000HZ,
ICM42688_GODR_16000HZ,
ICM42688_GODR_8000HZ,
ICM42688_GODR_4000HZ,
ICM42688_GODR_2000HZ,
ICM42688_GODR_1000HZ,// default
ICM42688_GODR_200HZ,
ICM42688_GODR_100HZ,
ICM42688_GODR_50HZ,
ICM42688_GODR_25HZ,
ICM42688_GODR_12_5HZ,
ICM42688_GODR_X0HZ,
ICM42688_GODR_X1HZ,
ICM42688_GODR_X2HZ,
ICM42688_GODR_500HZ,
NUM_ICM42688_GODR
};
void gpio_init(void);
/**
*
* @brief ICM42688<38><38><EFBFBD><EFBFBD><EFBFBD>dz<EFBFBD>ʼ<EFBFBD><CABC>
* @param
* @return void
* @notes <20>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD>
* Example: Init_ICM42688();
*
**/
void Init_ICM42688(void);
//void get_IMU(float accx,float accy,float accz,float *pitch,float *roll);
void get_IMU(float *pitch,float *roll);
uint8_t sys_gpio_pin_get(GPIO_TypeDef *p_gpiox, uint16_t pinx);
void sys_gpio_pin_set(GPIO_TypeDef *p_gpiox, uint16_t pinx, uint8_t status);
/**
*
* @brief <20><><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);
/**
*
* @brief <20><><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);
/**
*
* @brief <20><><EFBFBD><EFBFBD>ICM42688<38><38><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD>ͨ<EFBFBD>˲<EFBFBD><CBB2><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>,<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>,<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><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);
uint8_t MpuGetData(void);
// ICM42688Bank0<6B>ڲ<EFBFBD><DAB2><EFBFBD>ַ
#define ICM42688_DEVICE_CONFIG 0x11
#define ICM42688_DRIVE_CONFIG 0x13
#define ICM42688_INT_CONFIG 0x14
#define ICM42688_FIFO_CONFIG 0x16
#define ICM42688_TEMP_DATA1 0x1D
#define ICM42688_TEMP_DATA0 0x1E
#define ICM42688_ACCEL_DATA_X1 0x1F
#define ICM42688_ACCEL_DATA_X0 0x20
#define ICM42688_ACCEL_DATA_Y1 0x21
#define ICM42688_ACCEL_DATA_Y0 0x22
#define ICM42688_ACCEL_DATA_Z1 0x23
#define ICM42688_ACCEL_DATA_Z0 0x24
#define ICM42688_GYRO_DATA_X1 0x25
#define ICM42688_GYRO_DATA_X0 0x26
#define ICM42688_GYRO_DATA_Y1 0x27
#define ICM42688_GYRO_DATA_Y0 0x28
#define ICM42688_GYRO_DATA_Z1 0x29
#define ICM42688_GYRO_DATA_Z0 0x2A
#define ICM42688_TMST_FSYNCH 0x2B
#define ICM42688_TMST_FSYNCL 0x2C
#define ICM42688_INT_STATUS 0x2D
#define ICM42688_FIFO_COUNTH 0x2E
#define ICM42688_FIFO_COUNTL 0x2F
#define ICM42688_FIFO_DATA 0x30
#define ICM42688_APEX_DATA0 0x31
#define ICM42688_APEX_DATA1 0x32
#define ICM42688_APEX_DATA2 0x33
#define ICM42688_APEX_DATA3 0x34
#define ICM42688_APEX_DATA4 0x35
#define ICM42688_APEX_DATA5 0x36
#define ICM42688_INT_STATUS2 0x37
#define ICM42688_INT_STATUS3 0x38
#define ICM42688_SIGNAL_PATH_RESET 0x4B
#define ICM42688_INTF_CONFIG0 0x4C
#define ICM42688_INTF_CONFIG1 0x4D
#define ICM42688_PWR_MGMT0 0x4E
#define ICM42688_GYRO_CONFIG0 0x4F
#define ICM42688_ACCEL_CONFIG0 0x50
#define ICM42688_GYRO_CONFIG1 0x51
#define ICM42688_GYRO_ACCEL_CONFIG0 0x52
#define ICM42688_ACCEL_CONFIG1 0x53
#define ICM42688_TMST_CONFIG 0x54
#define ICM42688_APEX_CONFIG0 0x56
#define ICM42688_SMD_CONFIG 0x57
#define ICM42688_FIFO_CONFIG1 0x5F
#define ICM42688_FIFO_CONFIG2 0x60
#define ICM42688_FIFO_CONFIG3 0x61
#define ICM42688_FSYNC_CONFIG 0x62
#define ICM42688_INT_CONFIG0 0x63
#define ICM42688_INT_CONFIG1 0x64
#define ICM42688_INT_SOURCE0 0x65
#define ICM42688_INT_SOURCE1 0x66
#define ICM42688_INT_SOURCE3 0x68
#define ICM42688_INT_SOURCE4 0x69
#define ICM42688_FIFO_LOST_PKT0 0x6C
#define ICM42688_FIFO_LOST_PKT1 0x6D
#define ICM42688_SELF_TEST_CONFIG 0x70
#define ICM42688_WHO_AM_I 0x75
#define ICM42688_REG_BANK_SEL 0x76 // Banks
#define ICM42688_SENSOR_CONFIG0 0x03
#define ICM42688_GYRO_CONFIG_STATIC2 0x0B
#define ICM42688_GYRO_CONFIG_STATIC3 0x0C
#define ICM42688_GYRO_CONFIG_STATIC4 0x0D
#define ICM42688_GYRO_CONFIG_STATIC5 0x0E
#define ICM42688_GYRO_CONFIG_STATIC6 0x0F
#define ICM42688_GYRO_CONFIG_STATIC7 0x10
#define ICM42688_GYRO_CONFIG_STATIC8 0x11
#define ICM42688_GYRO_CONFIG_STATIC9 0x12
#define ICM42688_GYRO_CONFIG_STATIC10 0x13
#define ICM42688_XG_ST_DATA 0x5F
#define ICM42688_YG_ST_DATA 0x60
#define ICM42688_ZG_ST_DATA 0x61
#define ICM42688_TMSTVAL0 0x62
#define ICM42688_TMSTVAL1 0x63
#define ICM42688_TMSTVAL2 0x64
#define ICM42688_INTF_CONFIG4 0x7A
#define ICM42688_INTF_CONFIG5 0x7B
#define ICM42688_INTF_CONFIG6 0x7C
#endif

10
IMU/kalman.h
View File

@ -3,12 +3,12 @@
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><E1B9B9> //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><E1B9B9>
struct KalmanFilter{ struct KalmanFilter{
float LastP; //<2F><>һ<EFBFBD><D2BB>Э<EFBFBD><D0AD><EFBFBD><EFBFBD> float LastP; //<2F><>һ<EFBFBD><D2BB>Э<EFBFBD><D0AD><EFBFBD><EFBFBD>
float NewP; //<2F><><EFBFBD>µ<EFBFBD>Э<EFBFBD><D0AD><EFBFBD><EFBFBD> float NewP; //<2F><><EFBFBD>µ<EFBFBD>Э<EFBFBD><D0AD><EFBFBD><EFBFBD>
float Out; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> float Out; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float Kg; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> float Kg; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float Q; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><D0AD><EFBFBD><EFBFBD> float Q; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><D0AD><EFBFBD><EFBFBD>
float R; //<2F>۲<EFBFBD><DBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><D0AD><EFBFBD><EFBFBD> float R; //<2F>۲<EFBFBD><DBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><D0AD><EFBFBD><EFBFBD>
}; };
extern void kalmanfiter(struct KalmanFilter *EKF,float input); //һά<D2BB><CEAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD> extern void kalmanfiter(struct KalmanFilter *EKF,float input); //һά<D2BB><CEAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD>