diff --git a/APP/foc.c b/APP/foc.c index 2be86b0..925100b 100644 --- a/APP/foc.c +++ b/APP/foc.c @@ -3,14 +3,14 @@ // #include "foc.h" #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) +const float sqrt_3 = 1.7320508075f; - -const float Voltage_PowerSupply = 12; +float Voltage_PowerSupply = 12; float Pole_Pairs = 14; float Voltage_Limit = 6.9; float Velocity_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_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.V_KP = 0.1; - FOCStruct_X.V_KI = 1; + FOCStruct_X.V_KI = 2; FOCStruct_X.V_KD = 0; -/////////// - FOCStruct_Y.ZT_KP = 3; +////////// + FOCStruct_Y.ZT_KP = 3.0; 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_KI = 1.4; 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ÖáЧ¹û²»´í -// 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ÖáЧ¹û²»´í -// 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ÖáЧ¹û²»´í -// 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) { @@ -93,8 +59,8 @@ void setPhaseVoltage(unsigned char motor,float Uq, float Ud, double angle_el) if(Uout<-0.577)Uout=-0.577; sector = (angle_el / _PI_3) + 1; - T1 = sqrt(3)*sin(sector*_PI_3 - angle_el) * Uout; - T2 = sqrt(3)*sin(angle_el - (sector-1.0)*_PI_3) * Uout; + T1 = sqrt_3*sin(sector*_PI_3 - angle_el) * Uout; + T2 = sqrt_3*sin(angle_el - (sector-1.0)*_PI_3) * Uout; T0 = 1 - T1 - T2; // calculate the duty cycles(times) @@ -135,14 +101,14 @@ void setPhaseVoltage(unsigned char motor,float Uq, float Ud, double angle_el) Tb = 0; Tc = 0; } - if (motor == motory) + if (motor ==motorx ) { __HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Ta*4800); __HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Tb*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_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_3); } - -void moto_Init() +void moto_gpio_init() { GPIO_InitTypeDef GPIO_InitStruct = {0}; @@ -332,10 +297,19 @@ void moto_Init() HAL_GPIO_WritePin(GPIOB,GPIO_PIN_0,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(); HAL_Delay(1000); - alignSensor(motory,&FOCStruct_Y); + alignSensor(motory,&FOCStruct_Y); //µç»ú×Ô¼ì alignSensor(motorx,&FOCStruct_X); HAL_Delay(50); } @@ -348,7 +322,6 @@ int alignSensor(unsigned char moto,FOCTypeDef * FOCStruct) float moved; float q = 3.5; printf("MOT: Align sensor.\r\n"); - // find natural direction // move one electrical revolution forward HAL_Delay(100); diff --git a/APP/foc.h b/APP/foc.h index b7b07c7..452f12b 100644 --- a/APP/foc.h +++ b/APP/foc.h @@ -24,9 +24,11 @@ typedef struct double Angle_Prev; unsigned long Time_Prev; }FOCTypeDef; + + extern FOCTypeDef FOCStruct_X; extern FOCTypeDef FOCStruct_Y; - +void setPhaseVoltage(unsigned char motor,float Uq, float Ud, double angle_el); int alignSensor(unsigned char moto,FOCTypeDef * FOCStruct); void PID_Iint(void); 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 Zitai_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float ZT_Error); void Angel_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float Angle_target); - +void moto_gpio_init(); void PWM_Start(); void moto_Init(); + + + #endif //FOC_N_FOC_H diff --git a/APP/mymain.c b/APP/mymain.c index 1864eab..30b4221 100644 --- a/APP/mymain.c +++ b/APP/mymain.c @@ -1,12 +1,12 @@ // -// Created by hu123456 on 2024/1/16. +// Created by lijie on 2024/1/16. // - #include "mymain.h" void mymain() { HAL_UART_Receive_IT(&huart1, (uint8_t *)g_rx_buffer, RXBUFFERSIZE); //¿ªÆô´®¿Ú1½ÓÊÕÖÐ¶Ï + setvbuf(stdout, NULL, _IONBF, 0); //½â¾öprintfûÓÐ\n²»·¢ËÍÎÊÌâ HAL_Delay(2000); led_init(); adc_init(); @@ -15,34 +15,32 @@ void mymain() imu_rest(); moto_Init(); PID_Iint(); - setvbuf(stdout, NULL, _IONBF, 0); //½â¾öprintfûÓÐ\n²»·¢ËÍÎÊÌâ float x = 0,y=0; - unsigned int i =0; + unsigned int a =0; HAL_TIM_Base_Start(&htim5); //Æô¶¯¶¨Ê±Æ÷ -// while(i<2000*1) -// { -//// MpuGetData(); -//// GetAngle(&ICM42688,&Angle); -// Angel_closed_loop(motorx,&FOCStruct_X,3.0); -// Angel_closed_loop(motory,&FOCStruct_Y,2.2); -// i++; -// } -// i=0; + + while(a<2200*6) + { + MpuGetData(); + GetAngle(&ICM42688,&Angle); +// Angel_closed_loop(motorx,&FOCStruct_X,x); +// Angel_closed_loop(motory,&FOCStruct_Y,y); + a++; + } // LED_ON; while(1) { -// MpuGetData(); -// GetAngle(&ICM42688,&Angle); -// x = Angle.pitch; -// y = Angle.roll; -// Zitai_closed_loop(motorx,&FOCStruct_X,x); -// Zitai_closed_loop(motory,&FOCStruct_Y,-y); + MpuGetData(); + GetAngle(&ICM42688,&Angle); + x = Angle.pitch; + y = Angle.roll + 90.f; + Zitai_closed_loop(motorx,&FOCStruct_X,x); + Zitai_closed_loop(motory,&FOCStruct_Y,-y); // commander_run(); // x = getAngle(motorx); // y = getAngle(motory); -// x= get_VCC(); // printf("%fY%f\n",x,y); -// HAL_Delay(2); +// HAL_Delay(10); // speed_closed_loop(motorx,&FOCStruct_X,_2PI*5); // speed_closed_loop(motory,&FOCStruct_Y,_2PI*2); // printf("b%d\n",b); diff --git a/APP/tle5012b.c b/APP/tle5012b.c index 0407d28..d2ba3e1 100644 --- a/APP/tle5012b.c +++ b/APP/tle5012b.c @@ -175,11 +175,12 @@ double getAngle(unsigned char motor) { TLE_CS1_ENABLE; angle_data = ReadTLE5012B_1(READ_ANGLE_VALUE); +// printf("angle_data:%d\n",angle_data); d_angle = angle_data - motory_angle_data_prev; if(fabs(d_angle) > (0.8*cpr) ) motory_full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; motory_angle_data_prev = angle_data; 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 { @@ -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; motorx_angle_data_prev = angle_data; 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; } } diff --git a/Core/Src/spi.c b/Core/Src/spi.c index 3678cd5..6130a4a 100644 --- a/Core/Src/spi.c +++ b/Core/Src/spi.c @@ -87,7 +87,7 @@ void MX_SPI2_Init(void) hspi2.Init.CLKPolarity = SPI_POLARITY_HIGH; hspi2.Init.CLKPhase = SPI_PHASE_2EDGE; 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.TIMode = SPI_TIMODE_DISABLE; hspi2.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; diff --git a/IMU/icm42688.c b/IMU/icm42688.c index cce01b8..5edd75c 100644 --- a/IMU/icm42688.c +++ b/IMU/icm42688.c @@ -1,302 +1,50 @@ -#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) -{ - 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); +// 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) //{ -// 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]); +// 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); //} // -///** -//* -//* @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) @@ -304,222 +52,56 @@ uint8_t MpuGetData(void) // // 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; +// 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; +// 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 ÐèҪд½ø¸Ã¼Ä´æÆ÷µÄÊý¾Ý @@ -530,10 +112,12 @@ uint8_t MpuGetData(void) //**/ //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); +// 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); //} // ///** @@ -549,11 +133,11 @@ uint8_t MpuGetData(void) //**/ //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); +// 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,Õý³£ @@ -564,72 +148,490 @@ uint8_t MpuGetData(void) // //uint8_t MpuGetData(void) //{ -// uint8_t i; -// int res; -// unsigned char buffer[12]; -// Read_Datas_ICM42688(ICM42688_ACCEL_DATA_X1,buffer,12); +// 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) /* ½Ç¼ÓËٶȿ¨¶ûÂüÂ˲¨ */ -// { -// { +// 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; -// } -//} -// -// -// + 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; + } +} + + + diff --git a/IMU/icm42688.h b/IMU/icm42688.h index 35a6cf1..362fc8c 100644 --- a/IMU/icm42688.h +++ b/IMU/icm42688.h @@ -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�ڲ���ַ -#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_ //#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 ; // ����ICM42688���ٶȼ����� -//extern float icm42688_gyro_x, icm42688_gyro_y, icm42688_gyro_z ; // ����ICM42688�Ǽ��ٶ����� -//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 @@ -268,58 +66,7 @@ uint8_t MpuGetData(void); // NUM_ICM42688_GODR //}; // -//void gpio_init(void); // -///** -//* -//* @brief ICM42688�����dz�ʼ�� -//* @param -//* @return void -//* @notes �û����� -//* 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 ���ICM42688�����Ǽ��ٶ� -//* @param -//* @return void -//* @notes ��λ:g(m/s^2),�û����� -//* Example: Get_Acc_ICM42688(); -//* -//**/ -//void Get_Acc_ICM42688(void); -// -///** -//* -//* @brief ���ICM42688�����ǽǼ��ٶ� -//* @param -//* @return void -//* @notes ��λΪ:��/s,�û����� -//* Example: Get_Gyro_ICM42688(); -//* -//**/ -//void Get_Gyro_ICM42688(void); -// -///** -//* -//* @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); -//uint8_t MpuGetData(void); //// ICM42688Bank0�ڲ���ַ //#define ICM42688_DEVICE_CONFIG 0x11 //#define ICM42688_DRIVE_CONFIG 0x13 @@ -400,5 +147,258 @@ uint8_t MpuGetData(void); //#define ICM42688_INTF_CONFIG5 0x7B //#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 ; // ����ICM42688���ٶȼ����� +extern float icm42688_gyro_x, icm42688_gyro_y, icm42688_gyro_z ; // ����ICM42688�Ǽ��ٶ����� +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�����dz�ʼ�� +* @param +* @return void +* @notes �û����� +* 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 ���ICM42688�����Ǽ��ٶ� +* @param +* @return void +* @notes ��λ:g(m/s^2),�û����� +* Example: Get_Acc_ICM42688(); +* +**/ +void Get_Acc_ICM42688(void); + +/** +* +* @brief ���ICM42688�����ǽǼ��ٶ� +* @param +* @return void +* @notes ��λΪ:��/s,�û����� +* Example: Get_Gyro_ICM42688(); +* +**/ +void Get_Gyro_ICM42688(void); + +/** +* +* @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); +uint8_t MpuGetData(void); +// ICM42688Bank0�ڲ���ַ +#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 + diff --git a/IMU/kalman.h b/IMU/kalman.h index ec2ae63..8b9e0ba 100644 --- a/IMU/kalman.h +++ b/IMU/kalman.h @@ -3,12 +3,12 @@ //¿¨¶ûÂüÂ˲¨²ÎÊý½á¹¹Ìå struct KalmanFilter{ - float LastP; //ÉÏÒ»´ÎЭ·½²î + float LastP; //ÉÏÒ»´ÎЭ·½²î float NewP; //×îеÄЭ·½²î - float Out; //¿¨¶ûÂüÊä³ö - float Kg; //¿¨¶ûÂüÔöÒæ - float Q; //¹ý³ÌÔëÉùµÄЭ·½²î - float R; //¹Û²âÔëÉùµÄЭ·½²î + float Out; //¿¨¶ûÂüÊä³ö + float Kg; //¿¨¶ûÂüÔöÒæ + float Q; //¹ý³ÌÔëÉùµÄЭ·½²î + float R; //¹Û²âÔëÉùµÄЭ·½²î }; extern void kalmanfiter(struct KalmanFilter *EKF,float input); //һά¿¨¶ûÂüÂ˲¨